Dependencies: czosnekengine_17_06 Servo_fullrange mbed
Fork of CZOSN_z_czujnikiem_17_06 by
main.cpp
- Committer:
- p683
- Date:
- 2016-06-29
- Revision:
- 4:5e3f03e75645
- Parent:
- 3:d78e7f6477d8
File content as of revision 4:5e3f03e75645:
#include "mbed.h" #include "Engine.h" #include "Servo.h" #include "hcsr04.h" Serial stm(PA_2, PA_3); 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); HCSR04 sensor(PB_9, PB_8, 11770); const int BufferSize=10; 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; void cleanBuffer() { for(int i=0; i<=BufferSize; i++) buffer[i]=NULL; } 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; } //**************************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((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)buffer[i]) < 48 || ((int)buffer[i]) > 57) return false; if(((int)buffer[i+4]) < 48 || ((int)buffer[i+4]) > 57) return false; } return true; } void actualize_speed() { //*******PRZYPISANIE DO TABLICY LEWEJ LICZBY*********// for(int j=0; j<=2; j++){ left[j]=buffer[j+2]; } //*******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(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 actualize_servo_val() { //*******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() { cam_horizontal.position(act_horizontal); cam_vertical.position(act_vertical); } int main(){ int i = 0; cleanBuffer(); stm.baud(115200); t_sonar.start(); while(true){ 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)//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(); } if(buffer[9] == NULL){ buffer[i] = stm.getc(); if(i==0){ if(buffer[i]=='@') //zaczynamy zapelniac bufor jak dostaniemy @ ++i; } 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()){ t.stop(); t.reset(); t.start(); } if(buffer[9]=='$'){ actualize_speed(); if(distance>25) move_wheels(); else if (distance<=25){ if(l<=0 && r<=0 || l*r<=0){ move_wheels(); } else{ eng_left.move(0); eng_right.move(0); } } } else if(buffer[9]=='&'){ actualize_servo_val(); move_camera(); } i=0; cleanBuffer(); } }