Justyna Guzik
/
golonkaczosniur
Fork of JakKrisowy_ by
main.cpp
- Committer:
- yruiewyrui3
- Date:
- 2016-07-07
- Revision:
- 0:f3a3f80e3202
- Child:
- 1:1e01b12063cd
File content as of revision 0:f3a3f80e3202:
//*****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=15; char buffer1[BufferSize]; char buffer2[BufferSize]; char camove[2]; int wall=30; // odleglosc od sciany w cm int act_vertical=0, act_horizontal=0, move=0, val=0; Timer t, t_sonar; 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 if ((buffer1[0]=='f') && (distance>30)) return 1; else if ((buffer1[0]=='t') && (distance>30)) if (buffer1[2]=='l') return 2; else return 3; else 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 return 0; } 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(-5); cam_vertical.position(-20); } void set_val() // odczytująca wartosc { for (int q=0; q<3; q++){ buffer2[q]=buffer1[q+8]; } sscanf(buffer2, "%3d", &val); } int main(){ stm.baud(115200); cleanBuffer1(); cleanBuffer2(); set_camera_zero(); t_sonar.start(); while(true){ stm.printf("Poczatek while \n"); if(t.read_ms()>500) //jesli przez ponad 500ms nie ma nowej ramki, zatrzymujemy robota { eng_left.move(0); eng_right.move(0); } if(t_sonar.read_ms()>1000) //odczyt odleglosci co 1,2s { distance = sensor.distance(); stm.printf("%d\n",distance); t_sonar.stop(); t_sonar.reset(); t_sonar.start(); } while(stm.readable()){ stm.gets(buffer1, 13); break;} stm.printf("bufor pierwszy: \n"); for (int f=0; f<20; f++) { stm.printf("%c", buffer1[f]); } stm.printf("\n"); set_val(); stm.printf("%i", val); t.stop(); t.reset(); t.start(); switch (what_command(buffer1)) { case 1: eng_left.move(val); eng_right.move(val); set_camera_zero(); speed_car = val; break; case 2: eng_right.move(speed_car); eng_left.move(speed_car - map(val, 0, 255, 0, speed_car)); cam_horizontal.position(15); cam_vertical.position(0); break; case 3: eng_left.move(speed_car); eng_right.move(speed_car - map(val, 0, 255, 0, speed_car)); cam_horizontal.position(0); cam_vertical.position(-35); 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(); stm.printf("Koniec while \n"); wait_ms(2000); } }