Hardware components | ||||||
| × | 1 | ||||
| × | 1 | ||||
| × | 5 | ||||
| × | 1 |
This project was built during classes of the Basics of Mechatronics in my second year of studying Mechatronical Engineering on the AGH University of Science and Technology.
This project is built with the Lego Mindstorms. We also used an ultrasonic sensor, HC-SR04, and a Bluetooth module for wireless communication. The catapult uses 2 Lego engines to move in all directions. The other 3 engines are used for reloading, changing shot angle and for shooting.
The main requirement of our project was to hit, with a ball, a target located in front of the device. Additionally, the catapult can find and hit a box in an empty room. It could also be manually operated using a mobile phone.
A few photos of how the catapult looks:
Video of the device working in automatic mode:
Device working in half-automatic mode (the catapult finds distance to the target located in front of it and calculate the angle of its shot):
Reloading and shooting (zoom in):
#include <hFramework.h>
#include <DistanceSensor.h>
#include <math.h>
#include "hCloudClient.h"
using namespace hModules;
DistanceSensor sens(hSens5);
double Vo=3; // zalozona predkosc wylotowa kulki
double Ho=0.3; // wysokosc z ktorej kulka jest wystrzeliwywana
int tablica[100]; // mapa zasiegu
int iloscobrotow(float); //przelicza kat obrotu katapulty na kat obrotu silnikow napdzajcych
void obrotKatapulty(int); //obraca silniki o zadany kat
int policzkat(int); //na podstawie odleglosci wylicza kat strzalu
void uzupelnijTablice(); //stworzenie mapy zasiegu
float dystans(); //zczytuje odlegosc do przeszkody z wykorzystaniem czujnika ultradzwiekowego
void ustawKatapulte(int); //obraca silnikiem sterujacym katem strzalu
void strzal(); //laduje kulke i strzela
int zczytywanieLiczb(); //zczytuje sygnal z serialu i zamienia go na trzycyforwa liczbe
void jazdaKatapulty(int); //jazda katapulty do przodu
void skrecanieKatapulty(int); //obrt katapulty w miejscu
void zatrzymanieKatapulty();//wylaczenie silnikow napedowych
void hMain()
{
sys.setLogDev(&Serial);
hExt1.serial.init(9600, ODD, ONE); //okreslenie sposobu odczytywania danych z serialu
printf("Zaczynamy\n\r");
uzupelnijTablice();
float II=2;
char wybor;
char wybor2;
for (;;)
{
// podstawowe menu wyboru
printf("Wybierz tryb: \n\r");
printf("1. Automatyczny \n\r");
printf("2. Strzal na zadana odleglosc \n\r");
printf("3. Strzal przed siebie \n\r");
printf("4. Reczny \n\r");
printf("Podaj 1, 2, 3 lub 4 \n\r");
wybor=hExt1.serial.getch(); //odczytanie decyzji uytkownika
switch(wybor)
{
case '1':
{
LED2.on();
LED1.on();
int wskaznik=0;
for (;;)
{
float dist = dystans(); //odczytanie odleglosci do przeszkody z czujnika
float kat=10;
int obroty=iloscobrotow(kat); // obrot o staly kat
if (dist>200) /*jesli najblisza przeszkoda jest dalej niz 200cm czujnik nie daje w peni wiarygodnych wynikow, katapulta obraca sie wiec o staly kat az do znalezienia blizeszej przeszkody */
{
obrotKatapulty(obroty);
printf("Jestesmy poza zasiegiem wiarygodnego dzialania czujnika\n\r");
sys.delay(100);
}
else if (dist<=200)
{
printf("Jestesmy w zasiegu \n\r");
for (;;)
{
float dist2=dist;
obrotKatapulty(obroty);
dist=dystans();
if (dist>dist2+3 || dist<dist2-3) /* porownanie odlegosci do przeszkody z obecnego kroku z odlegocia z poprzedniego kroku, jezli nastpila zauwazalna zmiana interpretuejmy to jako pocztek naszego celu */
{
printf("Znalazlem cel++++++++++++++++++++++\n\r");
for(;;)
{
printf("Krok : %d \n\r",wskaznik);
obrotKatapulty(obroty);
wskaznik++; // do sprawdzania czy cel jest pudelkiem - w przyblizeniu szerokosc pudelka
dist2=dist;
dist=dystans();
if ((dist>dist2+3 || dist<dist2-3)&&(wskaznik>2)) // wskaznik wiekszy niz 2 - kwalfikuje cel jako pudelko
/* jeli po wykonaniu przzynajmniej 3 krokw znowu nastpia duza
zmiana odlegoci, interpretujemy to jako koniec celu */
{
printf("Cel sie skonczyl++++++++++++++++++ \n\r");
break;
}
}
printf("Celuje i strzelam \n\r");
obrotKatapulty(-obroty*wskaznik/II); /*katapulta cofa sie o polowe krokow ktore wykonala, aby celowac w srodek celu*/
dist=dystans()+22; // katapulta ma trafi do pudeka wic musi strzela dalej niz znajduje sie przednia krawedz pudelka
int katstrzalu=policzkat(dist);
ustawKatapulte(katstrzalu);
sys.delay(500);
strzal();
obrotKatapulty(obroty*wskaznik/II); //ustawienie si na kocu celu, aby moc zacz poszukiwania nastpnego celu
break;
}
}
break;
}
}
}
break;
case '2':
{
LED2.off();
LED1.on();
LED3.on();
printf("Podaj odleglosc strzalu (100-300cm) \n\r");
int odleglosc=zczytywanieLiczb();
printf("Celuje i strzelam \n\r");
int katstrzalu=policzkat(odleglosc);
ustawKatapulte(katstrzalu);
sys.delay(500);
strzal();
}
break;
case '3':
{
LED2.on();
LED1.off();
LED3.off();
printf("Celuje i strzelam \n\r");
float dist=dystans()+22;
int katstrzalu=policzkat(dist);
ustawKatapulte(katstrzalu);
sys.delay(500);
strzal();
}
break;
case '4':
{
LED3.on();
for(;;)
{
wybor2=hExt1.serial.getch();
if (wybor2=='b')
break;
switch(wybor2)
{
case 'w':
{
int moc = zczytywanieLiczb();
jazdaKatapulty(moc);
}
break;
case 's':
{
int moc = -zczytywanieLiczb();
jazdaKatapulty(moc);
}
break;
case 'a':
{
int moc = zczytywanieLiczb();
skrecanieKatapulty(moc);
}
break;
case 'd':
{
int moc = -zczytywanieLiczb();
skrecanieKatapulty(moc);
}
break;
case 'x':
{
int odleglosc=zczytywanieLiczb();
printf("Celuje i strzelam \n\r");
int katstrzalu=policzkat(odleglosc);
ustawKatapulte(katstrzalu);
sys.delay(500);
strzal();
}
break;
case 'p':
{
zatrzymanieKatapulty();
}
break;
default:
{
printf("Cos poszlo nie tak\n\r");
}
}
}
}
default:
{
printf("Wpisales zly znak \n\r");
}
}
LED2.off();
LED1.off();
LED3.off();
}
}
int iloscobrotow(float kat)
{
int katobrotu=round(kat*16);
/* jeden stopien obrotu katapulty
to 8 stopni obortu jednego silnika w przod, drugiego w tyl */
return katobrotu;
}
void obrotKatapulty(int obroty)
{
hMot1.rotRel(obroty, 1000, 0);
hMot2.rotRel(-(obroty), 1000, 1);
}
int policzkat(int dist)
{
int katstrzalu;
if (dist>200)
{
printf("Nie mam tak duzego zasiegu, strzlam na maksymalna odleglosc \n\r");
katstrzalu=100;
}
else if (dist<50)
{
printf("Nie strzele tak blisko, strzelam na minimalna odleglosc \n\r");
katstrzalu=0;
}
else
{
for (int i=0; i<100; i++)
{
if (dist>=tablica[i] && dist<tablica[i+1]) //iteracyjne sprawdzenie w jakim przedziale miesci sie zadana odleglosc
katstrzalu=i; //i wyznaczenie na tej podstawie kata strzau
}
}
printf("Kat strzalu powinien wynosic: %d \n\r",katstrzalu);
return katstrzalu;
}
void ustawKatapulte(int kat)
{
int katStrzalu=kat;
printf("Kat strzalu %d \n\r ", katStrzalu);
printf("Silnik obroci sie o %d \n\r",katStrzalu*180);
hMot5.rotRel(katStrzalu*180, 1000, 1);
}
void strzal()
{
//sys.delay(1000);
hMot4.rotRel(1600,1000,1); // nacig spustu do poowy, specjalnie dla Asi, 1600: to jest kt obrotu silnika!!!
hMot6.rotRel(-400,300,1); // obrot 1 jeziorka
sys.delay(100);
hMot6.rotRel(400,300,1); // obrot 2 jeziorka
sys.delay(1000); // czekamy a kulka sie uspokoi
hMot4.rotRel(1600,1000,1); // dokoczenie nacigu
hMot4.rotRel(-5000,1000,1); // powrt na pocztek
hMot4.rotAbs(0,1000,1); // powrt do pozycji zero
hMot5.rotAbs(0,1000,1); // powrt do poziomu(tlok)
}
void uzupelnijTablice()
{
float zasieg;
float zasieg2;
int zasieg3;
for (int i=0; i<100; i++)
{
zasieg= 0.45*(Vo*cos(i*M_PI/(180*4))*(Vo*sin(i*M_PI/(180*4))/9.81 + sqrt(2*Ho/9.81 + Vo*Vo*sin(i*M_PI/(180*4))*sin(i*M_PI/(180*4))/9.81*9.81)));
zasieg2=zasieg*100;
zasieg3=(int)zasieg2;
tablica[i]=zasieg3;
//printf("Tablica %d : %d\n\r",i,zasieg3);
}
//printf("zasieg dla 45 stopni w metrach %f w centymetrach %f zaokraglone %d \n\r",zasieg,zasieg2,zasieg3);
}
void jazdaKatapulty(int moc)
{
if (moc > 0)
{
hMot1.rotRel(-99999999, moc, 0);
hMot2.rotRel(-99999999, moc, 0);
}
if (moc < 0)
{
moc=-moc;
hMot1.rotRel(99999999, moc, 0);
hMot2.rotRel(99999999, moc, 0);
}
}
void skrecanieKatapulty(int moc)
{
if (moc > 0)
{
hMot1.rotRel(-99999999, moc, 0);
hMot2.rotRel(99999999, moc, 0);
}
if (moc < 0)
{
moc=-moc;
hMot1.rotRel(99999999, moc, 0);
hMot2.rotRel(-99999999, moc, 0);
}
}
void zatrzymanieKatapulty()
{
hMot1.resetEncoderCnt();
hMot1.rotAbs(0,1000,1);
hMot2.resetEncoderCnt();
hMot2.rotAbs(0,1000,1);
}
float dystans()
{
float dystans[20];
float dist;
float suma=0;
float a=20;
float srednia=0;
float blad[20];
float sumabledow=0;
float BSK=0;
for (int i=0;i<6;i++)
{
suma=0;
sumabledow=0;
BSK=0;
for (int i=0;i<20;i++)
{
dist=sens.getDistance();
//printf("Dystans= %f \n\r",dist);
dystans[i]=dist;
suma=suma+dist;
}
srednia=suma/a;
for (int j=0;j<20;j++)
{
blad[j]=dystans[j]-srednia;
sumabledow=sumabledow+blad[j]*blad[j];
}
BSK=sumabledow/(a*srednia);
printf("Odleglosc: %f \n\r",srednia);
//printf("Blad wzgledny wynosi: %f \n\r",BSK);
if (BSK<0.01)
break;
}
return srednia;
}
int zczytywanieLiczb()
{
char znak[3];
int liczba[3];
int nrPetli=0;
int wartosc=0;
for (;;)
{
for(;;)
{
znak[nrPetli]= hExt1.serial.getch();
liczba[nrPetli]=(int)znak[nrPetli]-48;
if ((liczba[nrPetli]<0) || (liczba[nrPetli]>9))
{
printf("Wpisales niedozowolone znaki, sprobuj jeszcze raz \n\r");
nrPetli=-1;
}
if (nrPetli==2)
break;
nrPetli++;
}
wartosc= liczba[0]*100+liczba[1]*10+liczba[2];
if (wartosc<1000)
{
break;
}
printf("cel jest poza zasiegiem, nie strzelimy tak daleko \n\r");
printf("podaj nowa wartosc \n\r");
nrPetli=0;
}
return wartosc;
}
Comments
Please log in or sign up to comment.