Karol JanickiJan GracaJoanna KamińskaPaulina GłąbałaDawid Furdzik
Published © GPL3+

Autonomous Catapult

A device that finds close targets and shoots to them with plastic balls.

IntermediateShowcase (no instructions)10 hours936
Autonomous Catapult

Things used in this project

Hardware components

RoboCORE
Husarion RoboCORE
×1
HC-06 Bluetooth Module
×1
LEGO engine
×5
Ultrasonic sensor HC SR04
×1

Story

Read more

Code

Main

C/C++
#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;

}

Credits

Karol Janicki
1 project • 1 follower
Contact
Jan Graca
1 project • 1 follower
Contact
Joanna Kamińska
1 project • 1 follower
Contact
Paulina Głąbała
1 project • 1 follower
Contact
Dawid Furdzik
1 project • 1 follower
Contact

Comments

Please log in or sign up to comment.