Justyna Guzik
/
golonkaczosniur
Fork of JakKrisowy_ by
main.cpp
- Committer:
- jguzik
- Date:
- 2016-07-19
- Revision:
- 2:d1dec651ff13
- Parent:
- 1:1e01b12063cd
File content as of revision 2:d1dec651ff13:
//*****biblioteki***** #include "mbed.h" #include "Engine.h" #include "Servo.h" #include "HCSR04.h" //*****piny***** 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); //*****deklaracja zmiennych***** const int BufferSize=18; char buffer1[BufferSize]; char buffer2[BufferSize]; char buffer3[BufferSize]; char camove[2]; char tablicaf[4]; int wall=30; // odleglosc od sciany w cm int act_vertical=0, act_horizontal=0, move=0, val=0; Timer t, t_sonar, t_fidu; long distance; volatile unsigned int speed_car = 0; //*****funkcje***** void cleanBuffer1() // czyszczaca bufor { for(int i=0; i<=BufferSize; i++) buffer1[i]=NULL; } void cleanBuffer2() // czyszczaca bufor2 { for(int i=0; i<=BufferSize; i++) buffer2[i]=NULL; } int what_command(char* buffer1){ // rozpoznajaca komende /*forward 1 t_leftt 2 t_right 3 backkkk 4 STOPPPP 5 leftttt 6 righttt 7 v_m_lef 8 h_m_lef 9 v_m_rig 10 h_m_rig 11 _m_zero 12 unknown command*/ if (buffer1[0]=='b') return 4; else if (buffer1[0]=='S') return 5; else if (buffer1[0]=='l') return 6; else if (buffer1[0]=='r') return 7; else if (buffer1[0]=='v'){ if (buffer1[4]=='l') return 8; else return 10;} else if (buffer1[0]=='h'){ if (buffer1[4]=='l') return 9; else return 11;} else if (buffer1[0]=='_') return 12; else if (distance<30){ if (buffer1[0]=='f') return 5; else if(buffer1[0]=='b') return 4; else if (buffer1[0]=='l') return 6; else if (buffer1[0]=='r') return 7; //return 5; } else if ((buffer1[0]=='f')) return 1; else if ((buffer1[0]=='t')){ if (buffer1[2]=='l') return 2; else return 3;} else return 0; } 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); tab[3]= ';'; return tab; } void hard_stop() // zatrzymujaca robota { eng_left.move(0); eng_right.move(0); } int map(int value, int from_min, int from_max, int to_min, int to_max) // mapujaca wartosc do skretu { return (value - from_min) * (to_max - to_min)/(from_max - from_min) + to_min; } void move_camera_down(int val) // sterujaca kamera { act_vertical = act_vertical + val; if(act_vertical>90){ act_vertical=90; } cam_vertical.position(act_vertical); } void move_camera_up(int val) // sterujaca kamera { act_vertical = act_vertical - val; if(act_vertical<-90){ act_vertical=-90; } cam_vertical.position(act_vertical); } void move_camera_right(int val) // sterujaca kamera { act_horizontal = act_horizontal - val; if(act_horizontal<-90){ act_horizontal=-90; } cam_horizontal.position(act_horizontal); } void move_camera_left(int val) // sterujaca kamera { act_horizontal = act_horizontal + val; if(act_horizontal>90){ act_horizontal=90; } cam_horizontal.position(act_horizontal); } void set_camera_zero() // zerujaca kamere { cam_horizontal.position(-8); cam_vertical.position(-20); act_horizontal=-8; act_vertical=-20; } void set_val() // odczytująca wartosc { for (int q=0; q<3; q++){ buffer2[q]=buffer1[q+8]; } sscanf(buffer2, "%3d", &val); } void go_slower(){ if(distance<50) val=0.5*val; if(distance<30) val=0.2*val; } void distance2array(int dist, char * dist_array){ if(dist<10){ dist_array[0]='0'; dist_array[1]='0'; dist_array[2]=dist+48;} else if(dist>=10 && dist<100){ dist_array[0]='0'; dist_array[1]=dist/10+48; dist_array[2]=dist%10+48;} else { dist_array[0]=dist/100+48; dist_array[1]=dist%100/10+48; dist_array[2]=dist%10+48;} } int main(){ char dist_array[3]; stm.baud(115200); cleanBuffer1(); cleanBuffer2(); set_camera_zero(); t_sonar.start(); t_fidu.start(); int i=0; while(true){ //stm.printf("Poczatek petli nr %d \n", i); if(t.read_ms()>200) //jesli przez ponad 200ms nie ma nowej ramki, zatrzymujemy robota { eng_left.move(0); eng_right.move(0); } if(t_sonar.read_ms()>100) //odczyt odleglosci co 0,4s { distance = sensor.distance(); //stm.printf("distance z czujnika: %d\n",distance); t_sonar.stop(); t_sonar.reset(); t_sonar.start(); } while(stm.readable()){ /*stm.gets(buffer1, 13); stm.puts(buffer1); break;}*/ for (int x=0; x<12; x++){ if(t_fidu.read_ms()>1000){ //stm.printf("urywam;"); t_fidu.stop(); t_fidu.reset(); break;} buffer1[x]=stm.getc(); if(buffer1[x]==';'){ t_fidu.stop(); t_fidu.reset(); t_fidu.start(); break;} } distance2array(distance, dist_array); distance2array(distance, buffer3); for(int i=0; i<3; i++){ buffer3[i]=dist_array[i];} for(int i=0; i<12; i++){ buffer3[i+3]=buffer1[i];} stm.puts(buffer3); break;} set_val(); t.stop(); t.reset(); t.start(); switch (what_command(buffer1)) { /*forward 1 t_leftt 2 t_right 3 backkkk 4 STOPPPP 5 leftttt 6 righttt 7 v_m_lef 8 h_m_lef 9 v_m_rig 10 h_m_rig 11 _m_zero 12 unknown command*/ case 1: go_slower(); eng_left.move(val); eng_right.move(val); set_camera_zero(); speed_car = val; break; case 2: go_slower(); eng_right.move(speed_car); eng_left.move(speed_car - map(val, 0, 100, 0, speed_car)); cam_horizontal.position(7); cam_vertical.position(-20); break; case 3: go_slower(); eng_left.move(speed_car); eng_right.move(speed_car - map(val, 0, 100, 0, speed_car)); cam_horizontal.position(-23); cam_vertical.position(-20); break; case 4: eng_left.move(-val); eng_right.move(-val); set_camera_zero(); speed_car = -val; break; case 5: hard_stop(); speed_car = 0; break; case 6: eng_left.move(-val); eng_right.move(val); break; case 7: eng_left.move(val); eng_right.move(-val); break; case 8: move_camera_down(val); break; case 9: move_camera_left(val); break; case 10: move_camera_up(val); break; case 11: move_camera_right(val); break; case 12: set_camera_zero(); break; default: //stm.printf("unknown_command \n"); break; } cleanBuffer1(); cleanBuffer2(); i++; //wait_ms(200); } }