Dependencies: czosnekengine_17_06 Servo_fullrange mbed
Fork of CZOSN_ostateczny_29_06 by
Diff: main.cpp
- Revision:
- 4:5e3f03e75645
- Parent:
- 3:d78e7f6477d8
- Child:
- 5:87151d829108
diff -r d78e7f6477d8 -r 5e3f03e75645 main.cpp --- a/main.cpp Fri Jun 17 08:58:05 2016 +0000 +++ b/main.cpp Wed Jun 29 09:04:34 2016 +0000 @@ -2,208 +2,199 @@ #include "Engine.h" #include "Servo.h" #include "hcsr04.h" - -//*************** Deklaracja wszystkich portów potrzebnych do obsługi************** + Serial stm(PA_2, PA_3); -Servo cam_poziom(PB_6); -Servo cam_pion(PC_7); +Servo cam_horizontal(PB_6); +Servo cam_vertical(PC_7); Engine eng_left = Engine(PB_13, PB_4, PB_10); Engine eng_right = Engine(PB_14, PB_5, PB_3); -//*************** Deklaracja zmiennych globalnych, tablic, bufora na ramke znaków ************** HCSR04 sensor(PB_9, PB_8, 11770); const int BufferSize=10; -char bufor[BufferSize]; -char prawa[3]; -char lewa[3]; -char poziom[2]; -char pion[2]; -int l=0; -int p=0; -int po=0; -int pi=0; +char buffer[BufferSize]; +char right[3]; +char left[3]; +char add_horizontal[2]; +char add_vertical[2]; +int act_vertical=0, act_horizontal=0; +int l=0, r=0, h=0, v=0; Timer t, t_sonar; -long distance=0; - -//*************** Funkcja czyszczaca bufor ***************** -void cleanBuffer(char *buffor) - { - for(int i=0; i<BufferSize; i++) - buffor[i]=NULL; - buffor[BufferSize]=NULL; +long distance; + + +void cleanBuffer() +{ + for(int i=0; i<=BufferSize; i++) + buffer[i]=NULL; } -//***************OPIS FUNKCJI isCorrectPacket******************************************************// -// funkcja sprawdza czy ramka spelnia wymagania protokolu: @ZCCCZCCC$ lub &ZCCCZCCC$ gdzie Z={+/-} C={0,1,...9} // -//*******************KONIEC OPISU******************************************************************// +char* intToChar(int value, char tab[3]) +{ + tab[0]= (char)(value/100 + 48); + tab[1]= (char)((value-(100*(value/100)))/10 + 48); + tab[2]= (char)(value-(100*(value/100))-(10*((value-(100*(value/100)))/10)) + 48); + return tab; +} -bool isCorrectPacket(char *buffor){ - if (bufor[0]!='@') - { - // stm.printf("ZLY ZNAK POCZATKU\n"); - stm.printf("blad\n"); - return false; - } - if((buffor[1] != '+') && (buffor[1] != '-')) - { - // stm.printf("ZLY ZNAK +/- LEWY\n"); - stm.printf("blad\n"); - return false; - } - if((buffor[5] != '+') && (buffor[5] != '-')) - { - // stm.printf("ZLY ZNAK +/- PRAWY\n"); - stm.printf("blad\n"); +//**************************OPIS FUNKCJI isCorrectPacket******************************************************// +// funkcja sprawdza czy ramka spelnia wymagania protokolu: @ZCCCZCCC$ lub &ZCCCZCCC$ gdzie Z={+/-} C={0,1,...9} + +bool isCorrectPacket(){ + if (buffer[0]!='@') return false; - } - if((bufor[9]!='$')&&(bufor[9]!='&')) - { - // stm.printf("ZLY ZNAK KONCA\n"); - stm.printf("blad\n"); + + if((buffer[1] != '+') && (buffer[1] != '-')) return false; - } + + if((buffer[5] != '+') && (buffer[5] != '-')) + return false; + + if((buffer[9]!='$')&&(buffer[9]!='&')) + return false; + for(int i=2; i< 5; i++){ - if(((int)buffor[i]) < 48 || ((int)buffor[i]) > 57) - { - // stm.printf("NA LEWE KOLO: NIE LICZBA\n"); - stm.printf("blad\n"); - return false; - } - if(((int)buffor[i+4]) < 48 || ((int)buffor[i+4]) > 57) - { - // stm.printf("NA PRAWE KOLO: NIE LICZBA\n"); - stm.printf("blad\n"); - return false; - } + if(((int)buffer[i]) < 48 || ((int)buffer[i]) > 57) + return false; + if(((int)buffer[i+4]) < 48 || ((int)buffer[i+4]) > 57) + return false; } - //stm.printf("pakiet poprawny\n");] + return true; } -void calculate_velocity() + +void actualize_speed() { //*******PRZYPISANIE DO TABLICY LEWEJ LICZBY*********// for(int j=0; j<=2; j++){ - lewa[j]=bufor[j+2]; - } - //*******PRZYPISANIE DO TABLICY PRAWEJ LICZBY*********// - for(int k=0; k<=2; k++){ - prawa[k]=bufor[k+6]; + left[j]=buffer[j+2]; } - //*******KONWERSJA CHAROW NA INTY*********// - sscanf(lewa, "%3d", &l); - sscanf(prawa, "%3d", &p); - //********KOREKCJA***************// - if(abs(l)>100){ + //*******PRZYPISANIE DO TABLICY PRAWEJ LICZBY********// + for(int k=0; k<=2; k++){ + right[k]=buffer[k+6]; + } + //*******KONWERSJA CHAROW NA INTY********************// + sscanf(left, "%3d", &l); + sscanf(right, "%3d", &r); + if(abs(l)>100) l=100; - } - if(abs(p)>100){ - p=100; - } - //************WYPISYWANIE*******************// - for(int j=0; j<=BufferSize; j++){ - stm.putc(bufor[j]); - } - //******KOREKCJA ZNAKU***********// - if(bufor[1]=='-') l=-l; - if(bufor[5]=='-') p=-p; + + if(abs(r)>100) + r=100; + + if(buffer[1]=='-') l=-l; + if(buffer[5]=='-') r=-r; +} + +void move_wheels() +{ + eng_left.move(l); + eng_right.move(r); } - -void move_wheels() + +void actualize_servo_val() { - eng_left.move(l); - eng_right.move(p); + //*******PRZYPISANIE DO KAMERY POZIOM*********// + for(int m=0; m<2; m++){ + add_horizontal[m]=buffer[m+3]; + } + //*******PRZYPISANIE DO KAMERY PION**********// + for(int n=0; n<2; n++){ + add_vertical[n]=buffer[n+7]; + } + //*******KONWERSJA CHAROW NA INTY************// + sscanf(add_horizontal, "%2d", &h); + sscanf(add_vertical, "%2d", &v); + + if(buffer[1]=='-') h=-h; + if(buffer[5]=='-') v=-v; + + act_horizontal = act_horizontal + h; + act_vertical = act_vertical + v; + + if(act_horizontal>90) + act_horizontal=90; + else if (act_horizontal<-90) + act_horizontal=-90; + + if(act_vertical>90) + act_vertical=90; + else if (act_vertical<-90) + act_vertical=-90; } void move_camera() { - //*******PRZYPISANIE DO KAMERY POZIOM*********// - for(int m=0; m<2; m++){ - poziom[m]=bufor[m+3]; - } - //*******PRZYPISANIE DO KAMERY PION*********// - for(int n=0; n<2; n++){ - pion[n]=bufor[n+3]; - } - //*******KONWERSJA CHAROW NA INTY*********// - sscanf(poziom, "%2d", &po); - sscanf(pion, "%2d", &pi); - //********KOREKCJA***************// - if(abs(po)>90){ - po=90; - } - if(abs(pi)>90){ - pi=90; - } - //************WYPISYWANIE*******************// - for(int j=0; j<=BufferSize; j++){ - stm.putc(bufor[j]); - } - if(bufor[1]=='-') po=-po; - if(bufor[5]=='-') pi=-pi; - //**********RUCH KAMERĄ******// - cam_poziom.position(po); - cam_pion.position(pi); + cam_horizontal.position(act_horizontal); + cam_vertical.position(act_vertical); + } + + int main(){ int i = 0; - cleanBuffer(bufor); + cleanBuffer(); stm.baud(115200); - t_sonar.start(); + t_sonar.start(); while(true){ - if(t.read_ms()>200){ //jesli przez ponad 200ms nie ma nowej ramki, zatrzymujemy robota + if(t.read_ms()>500)//jesli przez ponad 200ms nie ma nowej ramki, zatrzymujemy robota + { eng_left.move(0); eng_right.move(0); } - if(t_sonar.read_ms()>1200){ - + if(t_sonar.read_ms()>1200)//dczyt odleglosci co 1,2s + { + distance = sensor.distance(); + stm.printf("%d\n",distance); + /*intToChar(distance, tablicaf); + for (int f=0; f<3; f++) + { + stm.putc(tablicaf[f]); + }*/ t_sonar.stop(); t_sonar.reset(); - t_sonar.start(); - distance = sensor.distance(); - stm.printf("dystans %d \n",distance); - } - - if(bufor[9] == NULL){ - bufor[i] = stm.getc(); + t_sonar.start(); + } + if(buffer[9] == NULL){ + buffer[i] = stm.getc(); if(i==0){ - if(bufor[i]=='@') //zaczynamy zapelniac bufor jak dostaniemy @ - ++i; + if(buffer[i]=='@') //zaczynamy zapelniac bufor jak dostaniemy @ + ++i; } - else if(bufor[i]=='@'){ // i != 0 //interpretujemy jakby potencjalny poczatek ramki - cleanBuffer(bufor); - bufor[0]='@'; - i=1; //bo zerowy znak '@' juz zczytal + else if(buffer[i]=='@'){ // i != 0 //interpretujemy jakby potencjalny poczatek ramki + cleanBuffer(); + buffer[0]='@'; + i=1; //bo zerowy znak '@' juz sczytal } else i++; - continue; - } - if(isCorrectPacket(bufor)){ + continue; + } + if(isCorrectPacket()){ t.stop(); t.reset(); - t.start(); + t.start(); } - if(bufor[9]=='$'){ - calculate_velocity(); - if(distance>25){ + if(buffer[9]=='$'){ + actualize_speed(); + if(distance>25) move_wheels(); - } else if (distance<=25){ - if(l<=0 && p<=0 || l*p<=0){ + if(l<=0 && r<=0 || l*r<=0){ move_wheels(); } else{ - eng_left.move(0); - eng_right.move(0); + eng_left.move(0); + eng_right.move(0); } } } - else if(bufor[9]=='&'){ - move_camera(); + else if(buffer[9]=='&'){ + actualize_servo_val(); + move_camera(); } i=0; - cleanBuffer(bufor); - } -} \ No newline at end of file + cleanBuffer(); + } +}