Justyna Guzik
Fork of
JakKrisowy_
by 
Dominik Święch
main.cpp@2:d1dec651ff13, 2016-07-19 (annotated)
- Committer:
- jguzik
- Date:
- Tue Jul 19 13:10:16 2016 +0000
- Revision:
- 2:d1dec651ff13
- Parent:
- 1:1e01b12063cd
Who changed what in which revision?
| User | Revision | Line number | New contents of line |
|---|---|---|---|
| yruiewyrui3 | 0:f3a3f80e3202 | 1 | //*****biblioteki***** |
| yruiewyrui3 | 0:f3a3f80e3202 | 2 | |
| yruiewyrui3 | 0:f3a3f80e3202 | 3 | #include "mbed.h" |
| yruiewyrui3 | 0:f3a3f80e3202 | 4 | #include "Engine.h" |
| yruiewyrui3 | 0:f3a3f80e3202 | 5 | #include "Servo.h" |
| yruiewyrui3 | 0:f3a3f80e3202 | 6 | #include "HCSR04.h" |
| yruiewyrui3 | 0:f3a3f80e3202 | 7 | |
| yruiewyrui3 | 0:f3a3f80e3202 | 8 | //*****piny***** |
| yruiewyrui3 | 0:f3a3f80e3202 | 9 | |
| yruiewyrui3 | 0:f3a3f80e3202 | 10 | Serial stm(PA_2, PA_3); |
| yruiewyrui3 | 0:f3a3f80e3202 | 11 | Servo cam_horizontal(PB_6); |
| yruiewyrui3 | 0:f3a3f80e3202 | 12 | Servo cam_vertical(PC_7); |
| yruiewyrui3 | 0:f3a3f80e3202 | 13 | Engine eng_left = Engine(PB_13, PB_4, PB_10); |
| yruiewyrui3 | 0:f3a3f80e3202 | 14 | Engine eng_right = Engine(PB_14, PB_5, PB_3); |
| yruiewyrui3 | 0:f3a3f80e3202 | 15 | HCSR04 sensor(PB_9, PB_8, 11770); |
| yruiewyrui3 | 0:f3a3f80e3202 | 16 | |
| yruiewyrui3 | 0:f3a3f80e3202 | 17 | //*****deklaracja zmiennych***** |
| yruiewyrui3 | 0:f3a3f80e3202 | 18 | |
| jguzik | 2:d1dec651ff13 | 19 | const int BufferSize=18; |
| yruiewyrui3 | 0:f3a3f80e3202 | 20 | char buffer1[BufferSize]; |
| yruiewyrui3 | 0:f3a3f80e3202 | 21 | char buffer2[BufferSize]; |
| jguzik | 2:d1dec651ff13 | 22 | char buffer3[BufferSize]; |
| yruiewyrui3 | 0:f3a3f80e3202 | 23 | char camove[2]; |
| yruiewyrui3 | 1:1e01b12063cd | 24 | char tablicaf[4]; |
| yruiewyrui3 | 0:f3a3f80e3202 | 25 | int wall=30; // odleglosc od sciany w cm |
| yruiewyrui3 | 0:f3a3f80e3202 | 26 | int act_vertical=0, act_horizontal=0, move=0, val=0; |
| yruiewyrui3 | 1:1e01b12063cd | 27 | Timer t, t_sonar, t_fidu; |
| yruiewyrui3 | 0:f3a3f80e3202 | 28 | long distance; |
| yruiewyrui3 | 0:f3a3f80e3202 | 29 | volatile unsigned int speed_car = 0; |
| yruiewyrui3 | 0:f3a3f80e3202 | 30 | |
| yruiewyrui3 | 0:f3a3f80e3202 | 31 | //*****funkcje***** |
| yruiewyrui3 | 0:f3a3f80e3202 | 32 | |
| yruiewyrui3 | 0:f3a3f80e3202 | 33 | void cleanBuffer1() // czyszczaca bufor |
| yruiewyrui3 | 0:f3a3f80e3202 | 34 | { |
| yruiewyrui3 | 0:f3a3f80e3202 | 35 | for(int i=0; i<=BufferSize; i++) |
| yruiewyrui3 | 0:f3a3f80e3202 | 36 | buffer1[i]=NULL; |
| yruiewyrui3 | 0:f3a3f80e3202 | 37 | } |
| yruiewyrui3 | 0:f3a3f80e3202 | 38 | |
| yruiewyrui3 | 0:f3a3f80e3202 | 39 | void cleanBuffer2() // czyszczaca bufor2 |
| yruiewyrui3 | 0:f3a3f80e3202 | 40 | { |
| yruiewyrui3 | 0:f3a3f80e3202 | 41 | for(int i=0; i<=BufferSize; i++) |
| yruiewyrui3 | 0:f3a3f80e3202 | 42 | buffer2[i]=NULL; |
| yruiewyrui3 | 0:f3a3f80e3202 | 43 | } |
| yruiewyrui3 | 0:f3a3f80e3202 | 44 | |
| yruiewyrui3 | 0:f3a3f80e3202 | 45 | int what_command(char* buffer1){ // rozpoznajaca komende |
| jguzik | 2:d1dec651ff13 | 46 | |
| jguzik | 2:d1dec651ff13 | 47 | /*forward 1 |
| jguzik | 2:d1dec651ff13 | 48 | t_leftt 2 |
| jguzik | 2:d1dec651ff13 | 49 | t_right 3 |
| jguzik | 2:d1dec651ff13 | 50 | backkkk 4 |
| jguzik | 2:d1dec651ff13 | 51 | STOPPPP 5 |
| jguzik | 2:d1dec651ff13 | 52 | leftttt 6 |
| jguzik | 2:d1dec651ff13 | 53 | righttt 7 |
| jguzik | 2:d1dec651ff13 | 54 | v_m_lef 8 |
| jguzik | 2:d1dec651ff13 | 55 | h_m_lef 9 |
| jguzik | 2:d1dec651ff13 | 56 | v_m_rig 10 |
| jguzik | 2:d1dec651ff13 | 57 | h_m_rig 11 |
| jguzik | 2:d1dec651ff13 | 58 | _m_zero 12 |
| jguzik | 2:d1dec651ff13 | 59 | unknown command*/ |
| jguzik | 2:d1dec651ff13 | 60 | |
| jguzik | 2:d1dec651ff13 | 61 | |
| jguzik | 2:d1dec651ff13 | 62 | if (buffer1[0]=='b') |
| yruiewyrui3 | 0:f3a3f80e3202 | 63 | return 4; |
| yruiewyrui3 | 0:f3a3f80e3202 | 64 | else if (buffer1[0]=='S') |
| yruiewyrui3 | 0:f3a3f80e3202 | 65 | return 5; |
| yruiewyrui3 | 0:f3a3f80e3202 | 66 | else if (buffer1[0]=='l') |
| yruiewyrui3 | 0:f3a3f80e3202 | 67 | return 6; |
| yruiewyrui3 | 0:f3a3f80e3202 | 68 | else if (buffer1[0]=='r') |
| yruiewyrui3 | 0:f3a3f80e3202 | 69 | return 7; |
| jguzik | 2:d1dec651ff13 | 70 | else if (buffer1[0]=='v'){ |
| yruiewyrui3 | 0:f3a3f80e3202 | 71 | if (buffer1[4]=='l') |
| yruiewyrui3 | 0:f3a3f80e3202 | 72 | return 8; |
| yruiewyrui3 | 0:f3a3f80e3202 | 73 | else |
| jguzik | 2:d1dec651ff13 | 74 | return 10;} |
| jguzik | 2:d1dec651ff13 | 75 | else if (buffer1[0]=='h'){ |
| yruiewyrui3 | 0:f3a3f80e3202 | 76 | if (buffer1[4]=='l') |
| yruiewyrui3 | 0:f3a3f80e3202 | 77 | return 9; |
| yruiewyrui3 | 0:f3a3f80e3202 | 78 | else |
| jguzik | 2:d1dec651ff13 | 79 | return 11;} |
| yruiewyrui3 | 0:f3a3f80e3202 | 80 | else if (buffer1[0]=='_') |
| yruiewyrui3 | 0:f3a3f80e3202 | 81 | return 12; |
| jguzik | 2:d1dec651ff13 | 82 | else if (distance<30){ |
| jguzik | 2:d1dec651ff13 | 83 | if (buffer1[0]=='f') |
| jguzik | 2:d1dec651ff13 | 84 | return 5; |
| jguzik | 2:d1dec651ff13 | 85 | else if(buffer1[0]=='b') |
| jguzik | 2:d1dec651ff13 | 86 | return 4; |
| jguzik | 2:d1dec651ff13 | 87 | else if (buffer1[0]=='l') |
| jguzik | 2:d1dec651ff13 | 88 | return 6; |
| jguzik | 2:d1dec651ff13 | 89 | else if (buffer1[0]=='r') |
| jguzik | 2:d1dec651ff13 | 90 | return 7; |
| jguzik | 2:d1dec651ff13 | 91 | //return 5; |
| jguzik | 2:d1dec651ff13 | 92 | } |
| jguzik | 2:d1dec651ff13 | 93 | |
| jguzik | 2:d1dec651ff13 | 94 | else if ((buffer1[0]=='f')) |
| jguzik | 2:d1dec651ff13 | 95 | return 1; |
| jguzik | 2:d1dec651ff13 | 96 | else if ((buffer1[0]=='t')){ |
| jguzik | 2:d1dec651ff13 | 97 | if (buffer1[2]=='l') |
| jguzik | 2:d1dec651ff13 | 98 | return 2; |
| jguzik | 2:d1dec651ff13 | 99 | else |
| jguzik | 2:d1dec651ff13 | 100 | return 3;} |
| yruiewyrui3 | 0:f3a3f80e3202 | 101 | else |
| yruiewyrui3 | 0:f3a3f80e3202 | 102 | return 0; |
| yruiewyrui3 | 0:f3a3f80e3202 | 103 | } |
| yruiewyrui3 | 0:f3a3f80e3202 | 104 | |
| yruiewyrui3 | 1:1e01b12063cd | 105 | char* intToChar(int value, char tab[3]) |
| yruiewyrui3 | 1:1e01b12063cd | 106 | { |
| yruiewyrui3 | 1:1e01b12063cd | 107 | tab[0]= (char)(value/100 + 48); |
| yruiewyrui3 | 1:1e01b12063cd | 108 | tab[1]= (char)((value-(100*(value/100)))/10 + 48); |
| yruiewyrui3 | 1:1e01b12063cd | 109 | tab[2]= (char)(value-(100*(value/100))-(10*((value-(100*(value/100)))/10)) + 48); |
| yruiewyrui3 | 1:1e01b12063cd | 110 | tab[3]= ';'; |
| yruiewyrui3 | 1:1e01b12063cd | 111 | return tab; |
| yruiewyrui3 | 1:1e01b12063cd | 112 | } |
| yruiewyrui3 | 1:1e01b12063cd | 113 | |
| yruiewyrui3 | 0:f3a3f80e3202 | 114 | void hard_stop() // zatrzymujaca robota |
| yruiewyrui3 | 0:f3a3f80e3202 | 115 | { |
| yruiewyrui3 | 0:f3a3f80e3202 | 116 | eng_left.move(0); |
| yruiewyrui3 | 0:f3a3f80e3202 | 117 | eng_right.move(0); |
| yruiewyrui3 | 0:f3a3f80e3202 | 118 | } |
| yruiewyrui3 | 0:f3a3f80e3202 | 119 | |
| yruiewyrui3 | 0:f3a3f80e3202 | 120 | int map(int value, int from_min, int from_max, int to_min, int to_max) // mapujaca wartosc do skretu |
| yruiewyrui3 | 0:f3a3f80e3202 | 121 | { |
| yruiewyrui3 | 0:f3a3f80e3202 | 122 | return (value - from_min) * (to_max - to_min)/(from_max - from_min) + to_min; |
| yruiewyrui3 | 0:f3a3f80e3202 | 123 | } |
| yruiewyrui3 | 0:f3a3f80e3202 | 124 | |
| yruiewyrui3 | 0:f3a3f80e3202 | 125 | void move_camera_down(int val) // sterujaca kamera |
| yruiewyrui3 | 0:f3a3f80e3202 | 126 | { |
| yruiewyrui3 | 0:f3a3f80e3202 | 127 | act_vertical = act_vertical + val; |
| yruiewyrui3 | 0:f3a3f80e3202 | 128 | if(act_vertical>90){ |
| yruiewyrui3 | 0:f3a3f80e3202 | 129 | act_vertical=90; |
| yruiewyrui3 | 0:f3a3f80e3202 | 130 | } |
| yruiewyrui3 | 0:f3a3f80e3202 | 131 | cam_vertical.position(act_vertical); |
| yruiewyrui3 | 0:f3a3f80e3202 | 132 | } |
| yruiewyrui3 | 0:f3a3f80e3202 | 133 | |
| yruiewyrui3 | 0:f3a3f80e3202 | 134 | void move_camera_up(int val) // sterujaca kamera |
| yruiewyrui3 | 0:f3a3f80e3202 | 135 | { |
| yruiewyrui3 | 0:f3a3f80e3202 | 136 | act_vertical = act_vertical - val; |
| yruiewyrui3 | 0:f3a3f80e3202 | 137 | if(act_vertical<-90){ |
| yruiewyrui3 | 0:f3a3f80e3202 | 138 | act_vertical=-90; |
| yruiewyrui3 | 0:f3a3f80e3202 | 139 | } |
| yruiewyrui3 | 0:f3a3f80e3202 | 140 | cam_vertical.position(act_vertical); |
| yruiewyrui3 | 0:f3a3f80e3202 | 141 | } |
| yruiewyrui3 | 0:f3a3f80e3202 | 142 | |
| yruiewyrui3 | 0:f3a3f80e3202 | 143 | void move_camera_right(int val) // sterujaca kamera |
| yruiewyrui3 | 0:f3a3f80e3202 | 144 | { |
| yruiewyrui3 | 0:f3a3f80e3202 | 145 | act_horizontal = act_horizontal - val; |
| yruiewyrui3 | 0:f3a3f80e3202 | 146 | if(act_horizontal<-90){ |
| yruiewyrui3 | 0:f3a3f80e3202 | 147 | act_horizontal=-90; |
| yruiewyrui3 | 0:f3a3f80e3202 | 148 | } |
| yruiewyrui3 | 0:f3a3f80e3202 | 149 | cam_horizontal.position(act_horizontal); |
| yruiewyrui3 | 0:f3a3f80e3202 | 150 | } |
| yruiewyrui3 | 0:f3a3f80e3202 | 151 | |
| yruiewyrui3 | 0:f3a3f80e3202 | 152 | void move_camera_left(int val) // sterujaca kamera |
| yruiewyrui3 | 0:f3a3f80e3202 | 153 | { |
| yruiewyrui3 | 0:f3a3f80e3202 | 154 | act_horizontal = act_horizontal + val; |
| yruiewyrui3 | 0:f3a3f80e3202 | 155 | if(act_horizontal>90){ |
| yruiewyrui3 | 0:f3a3f80e3202 | 156 | act_horizontal=90; |
| yruiewyrui3 | 0:f3a3f80e3202 | 157 | } |
| yruiewyrui3 | 0:f3a3f80e3202 | 158 | cam_horizontal.position(act_horizontal); |
| yruiewyrui3 | 0:f3a3f80e3202 | 159 | } |
| yruiewyrui3 | 0:f3a3f80e3202 | 160 | |
| yruiewyrui3 | 0:f3a3f80e3202 | 161 | void set_camera_zero() // zerujaca kamere |
| yruiewyrui3 | 0:f3a3f80e3202 | 162 | { |
| yruiewyrui3 | 1:1e01b12063cd | 163 | cam_horizontal.position(-8); |
| yruiewyrui3 | 1:1e01b12063cd | 164 | cam_vertical.position(-20); |
| yruiewyrui3 | 1:1e01b12063cd | 165 | act_horizontal=-8; |
| yruiewyrui3 | 1:1e01b12063cd | 166 | act_vertical=-20; |
| yruiewyrui3 | 0:f3a3f80e3202 | 167 | } |
| yruiewyrui3 | 0:f3a3f80e3202 | 168 | |
| yruiewyrui3 | 0:f3a3f80e3202 | 169 | void set_val() // odczytująca wartosc |
| yruiewyrui3 | 0:f3a3f80e3202 | 170 | { |
| yruiewyrui3 | 0:f3a3f80e3202 | 171 | for (int q=0; q<3; q++){ |
| yruiewyrui3 | 0:f3a3f80e3202 | 172 | buffer2[q]=buffer1[q+8]; |
| yruiewyrui3 | 0:f3a3f80e3202 | 173 | } |
| yruiewyrui3 | 0:f3a3f80e3202 | 174 | sscanf(buffer2, "%3d", &val); |
| yruiewyrui3 | 0:f3a3f80e3202 | 175 | } |
| yruiewyrui3 | 0:f3a3f80e3202 | 176 | |
| jguzik | 2:d1dec651ff13 | 177 | void go_slower(){ |
| jguzik | 2:d1dec651ff13 | 178 | if(distance<50) |
| jguzik | 2:d1dec651ff13 | 179 | val=0.5*val; |
| jguzik | 2:d1dec651ff13 | 180 | if(distance<30) |
| jguzik | 2:d1dec651ff13 | 181 | val=0.2*val; |
| jguzik | 2:d1dec651ff13 | 182 | } |
| jguzik | 2:d1dec651ff13 | 183 | |
| jguzik | 2:d1dec651ff13 | 184 | void distance2array(int dist, char * dist_array){ |
| jguzik | 2:d1dec651ff13 | 185 | if(dist<10){ |
| jguzik | 2:d1dec651ff13 | 186 | dist_array[0]='0'; |
| jguzik | 2:d1dec651ff13 | 187 | dist_array[1]='0'; |
| jguzik | 2:d1dec651ff13 | 188 | dist_array[2]=dist+48;} |
| jguzik | 2:d1dec651ff13 | 189 | else if(dist>=10 && dist<100){ |
| jguzik | 2:d1dec651ff13 | 190 | dist_array[0]='0'; |
| jguzik | 2:d1dec651ff13 | 191 | dist_array[1]=dist/10+48; |
| jguzik | 2:d1dec651ff13 | 192 | dist_array[2]=dist%10+48;} |
| jguzik | 2:d1dec651ff13 | 193 | else { |
| jguzik | 2:d1dec651ff13 | 194 | dist_array[0]=dist/100+48; |
| jguzik | 2:d1dec651ff13 | 195 | dist_array[1]=dist%100/10+48; |
| jguzik | 2:d1dec651ff13 | 196 | dist_array[2]=dist%10+48;} |
| jguzik | 2:d1dec651ff13 | 197 | } |
| jguzik | 2:d1dec651ff13 | 198 | |
| yruiewyrui3 | 0:f3a3f80e3202 | 199 | int main(){ |
| jguzik | 2:d1dec651ff13 | 200 | char dist_array[3]; |
| yruiewyrui3 | 0:f3a3f80e3202 | 201 | stm.baud(115200); |
| yruiewyrui3 | 0:f3a3f80e3202 | 202 | cleanBuffer1(); |
| yruiewyrui3 | 0:f3a3f80e3202 | 203 | cleanBuffer2(); |
| yruiewyrui3 | 0:f3a3f80e3202 | 204 | set_camera_zero(); |
| yruiewyrui3 | 0:f3a3f80e3202 | 205 | t_sonar.start(); |
| yruiewyrui3 | 1:1e01b12063cd | 206 | t_fidu.start(); |
| yruiewyrui3 | 1:1e01b12063cd | 207 | int i=0; |
| yruiewyrui3 | 0:f3a3f80e3202 | 208 | |
| yruiewyrui3 | 0:f3a3f80e3202 | 209 | while(true){ |
| yruiewyrui3 | 1:1e01b12063cd | 210 | //stm.printf("Poczatek petli nr %d \n", i); |
| jguzik | 2:d1dec651ff13 | 211 | if(t.read_ms()>200) //jesli przez ponad 200ms nie ma nowej ramki, zatrzymujemy robota |
| yruiewyrui3 | 0:f3a3f80e3202 | 212 | { |
| yruiewyrui3 | 0:f3a3f80e3202 | 213 | eng_left.move(0); |
| yruiewyrui3 | 0:f3a3f80e3202 | 214 | eng_right.move(0); |
| yruiewyrui3 | 0:f3a3f80e3202 | 215 | } |
| jguzik | 2:d1dec651ff13 | 216 | if(t_sonar.read_ms()>100) //odczyt odleglosci co 0,4s |
| yruiewyrui3 | 0:f3a3f80e3202 | 217 | { |
| yruiewyrui3 | 0:f3a3f80e3202 | 218 | distance = sensor.distance(); |
| yruiewyrui3 | 1:1e01b12063cd | 219 | //stm.printf("distance z czujnika: %d\n",distance); |
| yruiewyrui3 | 0:f3a3f80e3202 | 220 | |
| yruiewyrui3 | 0:f3a3f80e3202 | 221 | t_sonar.stop(); |
| yruiewyrui3 | 0:f3a3f80e3202 | 222 | t_sonar.reset(); |
| yruiewyrui3 | 0:f3a3f80e3202 | 223 | t_sonar.start(); |
| yruiewyrui3 | 0:f3a3f80e3202 | 224 | } |
| yruiewyrui3 | 0:f3a3f80e3202 | 225 | while(stm.readable()){ |
| yruiewyrui3 | 1:1e01b12063cd | 226 | |
| yruiewyrui3 | 1:1e01b12063cd | 227 | /*stm.gets(buffer1, 13); |
| yruiewyrui3 | 1:1e01b12063cd | 228 | stm.puts(buffer1); |
| yruiewyrui3 | 1:1e01b12063cd | 229 | break;}*/ |
| jguzik | 2:d1dec651ff13 | 230 | |
| yruiewyrui3 | 1:1e01b12063cd | 231 | for (int x=0; x<12; x++){ |
| yruiewyrui3 | 1:1e01b12063cd | 232 | if(t_fidu.read_ms()>1000){ |
| jguzik | 2:d1dec651ff13 | 233 | //stm.printf("urywam;"); |
| yruiewyrui3 | 1:1e01b12063cd | 234 | t_fidu.stop(); |
| yruiewyrui3 | 1:1e01b12063cd | 235 | t_fidu.reset(); |
| yruiewyrui3 | 1:1e01b12063cd | 236 | break;} |
| yruiewyrui3 | 1:1e01b12063cd | 237 | buffer1[x]=stm.getc(); |
| yruiewyrui3 | 1:1e01b12063cd | 238 | if(buffer1[x]==';'){ |
| yruiewyrui3 | 1:1e01b12063cd | 239 | t_fidu.stop(); |
| yruiewyrui3 | 1:1e01b12063cd | 240 | t_fidu.reset(); |
| yruiewyrui3 | 1:1e01b12063cd | 241 | t_fidu.start(); |
| yruiewyrui3 | 1:1e01b12063cd | 242 | break;} |
| yruiewyrui3 | 1:1e01b12063cd | 243 | } |
| jguzik | 2:d1dec651ff13 | 244 | distance2array(distance, dist_array); |
| jguzik | 2:d1dec651ff13 | 245 | distance2array(distance, buffer3); |
| jguzik | 2:d1dec651ff13 | 246 | for(int i=0; i<3; i++){ |
| jguzik | 2:d1dec651ff13 | 247 | buffer3[i]=dist_array[i];} |
| jguzik | 2:d1dec651ff13 | 248 | for(int i=0; i<12; i++){ |
| jguzik | 2:d1dec651ff13 | 249 | buffer3[i+3]=buffer1[i];} |
| jguzik | 2:d1dec651ff13 | 250 | |
| jguzik | 2:d1dec651ff13 | 251 | |
| jguzik | 2:d1dec651ff13 | 252 | stm.puts(buffer3); |
| yruiewyrui3 | 0:f3a3f80e3202 | 253 | break;} |
| yruiewyrui3 | 0:f3a3f80e3202 | 254 | |
| yruiewyrui3 | 0:f3a3f80e3202 | 255 | set_val(); |
| yruiewyrui3 | 0:f3a3f80e3202 | 256 | t.stop(); |
| yruiewyrui3 | 0:f3a3f80e3202 | 257 | t.reset(); |
| yruiewyrui3 | 0:f3a3f80e3202 | 258 | t.start(); |
| yruiewyrui3 | 0:f3a3f80e3202 | 259 | switch (what_command(buffer1)) |
| yruiewyrui3 | 0:f3a3f80e3202 | 260 | { |
| jguzik | 2:d1dec651ff13 | 261 | |
| jguzik | 2:d1dec651ff13 | 262 | /*forward 1 |
| jguzik | 2:d1dec651ff13 | 263 | t_leftt 2 |
| jguzik | 2:d1dec651ff13 | 264 | t_right 3 |
| jguzik | 2:d1dec651ff13 | 265 | backkkk 4 |
| jguzik | 2:d1dec651ff13 | 266 | STOPPPP 5 |
| jguzik | 2:d1dec651ff13 | 267 | leftttt 6 |
| jguzik | 2:d1dec651ff13 | 268 | righttt 7 |
| jguzik | 2:d1dec651ff13 | 269 | v_m_lef 8 |
| jguzik | 2:d1dec651ff13 | 270 | h_m_lef 9 |
| jguzik | 2:d1dec651ff13 | 271 | v_m_rig 10 |
| jguzik | 2:d1dec651ff13 | 272 | h_m_rig 11 |
| jguzik | 2:d1dec651ff13 | 273 | _m_zero 12 |
| jguzik | 2:d1dec651ff13 | 274 | unknown command*/ |
| jguzik | 2:d1dec651ff13 | 275 | |
| yruiewyrui3 | 0:f3a3f80e3202 | 276 | case 1: |
| jguzik | 2:d1dec651ff13 | 277 | go_slower(); |
| yruiewyrui3 | 0:f3a3f80e3202 | 278 | eng_left.move(val); |
| yruiewyrui3 | 0:f3a3f80e3202 | 279 | eng_right.move(val); |
| yruiewyrui3 | 0:f3a3f80e3202 | 280 | set_camera_zero(); |
| yruiewyrui3 | 0:f3a3f80e3202 | 281 | speed_car = val; |
| yruiewyrui3 | 0:f3a3f80e3202 | 282 | break; |
| yruiewyrui3 | 0:f3a3f80e3202 | 283 | case 2: |
| jguzik | 2:d1dec651ff13 | 284 | go_slower(); |
| yruiewyrui3 | 0:f3a3f80e3202 | 285 | eng_right.move(speed_car); |
| yruiewyrui3 | 1:1e01b12063cd | 286 | eng_left.move(speed_car - map(val, 0, 100, 0, speed_car)); |
| yruiewyrui3 | 1:1e01b12063cd | 287 | cam_horizontal.position(7); |
| yruiewyrui3 | 1:1e01b12063cd | 288 | cam_vertical.position(-20); |
| yruiewyrui3 | 0:f3a3f80e3202 | 289 | break; |
| yruiewyrui3 | 0:f3a3f80e3202 | 290 | case 3: |
| jguzik | 2:d1dec651ff13 | 291 | go_slower(); |
| yruiewyrui3 | 0:f3a3f80e3202 | 292 | eng_left.move(speed_car); |
| yruiewyrui3 | 1:1e01b12063cd | 293 | eng_right.move(speed_car - map(val, 0, 100, 0, speed_car)); |
| yruiewyrui3 | 1:1e01b12063cd | 294 | cam_horizontal.position(-23); |
| yruiewyrui3 | 1:1e01b12063cd | 295 | cam_vertical.position(-20); |
| yruiewyrui3 | 0:f3a3f80e3202 | 296 | break; |
| yruiewyrui3 | 0:f3a3f80e3202 | 297 | case 4: |
| yruiewyrui3 | 0:f3a3f80e3202 | 298 | eng_left.move(-val); |
| yruiewyrui3 | 0:f3a3f80e3202 | 299 | eng_right.move(-val); |
| yruiewyrui3 | 0:f3a3f80e3202 | 300 | set_camera_zero(); |
| yruiewyrui3 | 1:1e01b12063cd | 301 | speed_car = -val; |
| yruiewyrui3 | 0:f3a3f80e3202 | 302 | break; |
| yruiewyrui3 | 0:f3a3f80e3202 | 303 | case 5: |
| yruiewyrui3 | 0:f3a3f80e3202 | 304 | hard_stop(); |
| yruiewyrui3 | 0:f3a3f80e3202 | 305 | speed_car = 0; |
| yruiewyrui3 | 0:f3a3f80e3202 | 306 | break; |
| yruiewyrui3 | 0:f3a3f80e3202 | 307 | case 6: |
| yruiewyrui3 | 0:f3a3f80e3202 | 308 | eng_left.move(-val); |
| yruiewyrui3 | 0:f3a3f80e3202 | 309 | eng_right.move(val); |
| yruiewyrui3 | 0:f3a3f80e3202 | 310 | break; |
| yruiewyrui3 | 0:f3a3f80e3202 | 311 | case 7: |
| yruiewyrui3 | 0:f3a3f80e3202 | 312 | eng_left.move(val); |
| yruiewyrui3 | 0:f3a3f80e3202 | 313 | eng_right.move(-val); |
| yruiewyrui3 | 0:f3a3f80e3202 | 314 | break; |
| yruiewyrui3 | 0:f3a3f80e3202 | 315 | case 8: |
| yruiewyrui3 | 0:f3a3f80e3202 | 316 | move_camera_down(val); |
| yruiewyrui3 | 0:f3a3f80e3202 | 317 | break; |
| yruiewyrui3 | 0:f3a3f80e3202 | 318 | case 9: |
| yruiewyrui3 | 0:f3a3f80e3202 | 319 | move_camera_left(val); |
| yruiewyrui3 | 0:f3a3f80e3202 | 320 | break; |
| yruiewyrui3 | 0:f3a3f80e3202 | 321 | case 10: |
| yruiewyrui3 | 0:f3a3f80e3202 | 322 | move_camera_up(val); |
| yruiewyrui3 | 0:f3a3f80e3202 | 323 | break; |
| yruiewyrui3 | 0:f3a3f80e3202 | 324 | case 11: |
| yruiewyrui3 | 0:f3a3f80e3202 | 325 | move_camera_right(val); |
| yruiewyrui3 | 0:f3a3f80e3202 | 326 | break; |
| yruiewyrui3 | 0:f3a3f80e3202 | 327 | case 12: |
| yruiewyrui3 | 1:1e01b12063cd | 328 | set_camera_zero(); |
| yruiewyrui3 | 0:f3a3f80e3202 | 329 | break; |
| yruiewyrui3 | 0:f3a3f80e3202 | 330 | default: |
| yruiewyrui3 | 1:1e01b12063cd | 331 | //stm.printf("unknown_command \n"); |
| yruiewyrui3 | 0:f3a3f80e3202 | 332 | break; |
| yruiewyrui3 | 0:f3a3f80e3202 | 333 | } |
| yruiewyrui3 | 0:f3a3f80e3202 | 334 | cleanBuffer1(); |
| yruiewyrui3 | 0:f3a3f80e3202 | 335 | cleanBuffer2(); |
| yruiewyrui3 | 0:f3a3f80e3202 | 336 | |
| yruiewyrui3 | 1:1e01b12063cd | 337 | i++; |
| yruiewyrui3 | 0:f3a3f80e3202 | 338 | |
| yruiewyrui3 | 1:1e01b12063cd | 339 | //wait_ms(200); |
| yruiewyrui3 | 0:f3a3f80e3202 | 340 | } |
| yruiewyrui3 | 0:f3a3f80e3202 | 341 | } |