Dominik Święch
/
JakKrisowy
.
Diff: main.cpp
- Revision:
- 0:f3a3f80e3202
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Thu Jul 07 12:56:54 2016 +0000 @@ -0,0 +1,243 @@ +//*****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); + } +} \ No newline at end of file