![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
Fork of JakKrisowy_ by
main.cpp@0:f3a3f80e3202, 2016-07-07 (annotated)
- Committer:
- yruiewyrui3
- Date:
- Thu Jul 07 12:56:54 2016 +0000
- Revision:
- 0:f3a3f80e3202
- Child:
- 1:1e01b12063cd
Jak Krisowy
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 | |
yruiewyrui3 | 0:f3a3f80e3202 | 19 | const int BufferSize=15; |
yruiewyrui3 | 0:f3a3f80e3202 | 20 | char buffer1[BufferSize]; |
yruiewyrui3 | 0:f3a3f80e3202 | 21 | char buffer2[BufferSize]; |
yruiewyrui3 | 0:f3a3f80e3202 | 22 | char camove[2]; |
yruiewyrui3 | 0:f3a3f80e3202 | 23 | int wall=30; // odleglosc od sciany w cm |
yruiewyrui3 | 0:f3a3f80e3202 | 24 | int act_vertical=0, act_horizontal=0, move=0, val=0; |
yruiewyrui3 | 0:f3a3f80e3202 | 25 | Timer t, t_sonar; |
yruiewyrui3 | 0:f3a3f80e3202 | 26 | long distance; |
yruiewyrui3 | 0:f3a3f80e3202 | 27 | volatile unsigned int speed_car = 0; |
yruiewyrui3 | 0:f3a3f80e3202 | 28 | |
yruiewyrui3 | 0:f3a3f80e3202 | 29 | //*****funkcje***** |
yruiewyrui3 | 0:f3a3f80e3202 | 30 | |
yruiewyrui3 | 0:f3a3f80e3202 | 31 | void cleanBuffer1() // czyszczaca bufor |
yruiewyrui3 | 0:f3a3f80e3202 | 32 | { |
yruiewyrui3 | 0:f3a3f80e3202 | 33 | for(int i=0; i<=BufferSize; i++) |
yruiewyrui3 | 0:f3a3f80e3202 | 34 | buffer1[i]=NULL; |
yruiewyrui3 | 0:f3a3f80e3202 | 35 | } |
yruiewyrui3 | 0:f3a3f80e3202 | 36 | |
yruiewyrui3 | 0:f3a3f80e3202 | 37 | void cleanBuffer2() // czyszczaca bufor2 |
yruiewyrui3 | 0:f3a3f80e3202 | 38 | { |
yruiewyrui3 | 0:f3a3f80e3202 | 39 | for(int i=0; i<=BufferSize; i++) |
yruiewyrui3 | 0:f3a3f80e3202 | 40 | buffer2[i]=NULL; |
yruiewyrui3 | 0:f3a3f80e3202 | 41 | } |
yruiewyrui3 | 0:f3a3f80e3202 | 42 | |
yruiewyrui3 | 0:f3a3f80e3202 | 43 | int what_command(char* buffer1){ // rozpoznajaca komende |
yruiewyrui3 | 0:f3a3f80e3202 | 44 | if ((buffer1[0]=='f') && (distance>30)) |
yruiewyrui3 | 0:f3a3f80e3202 | 45 | return 1; |
yruiewyrui3 | 0:f3a3f80e3202 | 46 | else if ((buffer1[0]=='t') && (distance>30)) |
yruiewyrui3 | 0:f3a3f80e3202 | 47 | if (buffer1[2]=='l') |
yruiewyrui3 | 0:f3a3f80e3202 | 48 | return 2; |
yruiewyrui3 | 0:f3a3f80e3202 | 49 | else |
yruiewyrui3 | 0:f3a3f80e3202 | 50 | return 3; |
yruiewyrui3 | 0:f3a3f80e3202 | 51 | else if (buffer1[0]=='b') |
yruiewyrui3 | 0:f3a3f80e3202 | 52 | return 4; |
yruiewyrui3 | 0:f3a3f80e3202 | 53 | else if (buffer1[0]=='S') |
yruiewyrui3 | 0:f3a3f80e3202 | 54 | return 5; |
yruiewyrui3 | 0:f3a3f80e3202 | 55 | else if (buffer1[0]=='l') |
yruiewyrui3 | 0:f3a3f80e3202 | 56 | return 6; |
yruiewyrui3 | 0:f3a3f80e3202 | 57 | else if (buffer1[0]=='r') |
yruiewyrui3 | 0:f3a3f80e3202 | 58 | return 7; |
yruiewyrui3 | 0:f3a3f80e3202 | 59 | else if (buffer1[0]=='v') |
yruiewyrui3 | 0:f3a3f80e3202 | 60 | if (buffer1[4]=='l') |
yruiewyrui3 | 0:f3a3f80e3202 | 61 | return 8; |
yruiewyrui3 | 0:f3a3f80e3202 | 62 | else |
yruiewyrui3 | 0:f3a3f80e3202 | 63 | return 10; |
yruiewyrui3 | 0:f3a3f80e3202 | 64 | else if (buffer1[0]=='h') |
yruiewyrui3 | 0:f3a3f80e3202 | 65 | if (buffer1[4]=='l') |
yruiewyrui3 | 0:f3a3f80e3202 | 66 | return 9; |
yruiewyrui3 | 0:f3a3f80e3202 | 67 | else |
yruiewyrui3 | 0:f3a3f80e3202 | 68 | return 11; |
yruiewyrui3 | 0:f3a3f80e3202 | 69 | else if (buffer1[0]=='_') |
yruiewyrui3 | 0:f3a3f80e3202 | 70 | return 12; |
yruiewyrui3 | 0:f3a3f80e3202 | 71 | else |
yruiewyrui3 | 0:f3a3f80e3202 | 72 | return 0; |
yruiewyrui3 | 0:f3a3f80e3202 | 73 | } |
yruiewyrui3 | 0:f3a3f80e3202 | 74 | |
yruiewyrui3 | 0:f3a3f80e3202 | 75 | void hard_stop() // zatrzymujaca robota |
yruiewyrui3 | 0:f3a3f80e3202 | 76 | { |
yruiewyrui3 | 0:f3a3f80e3202 | 77 | eng_left.move(0); |
yruiewyrui3 | 0:f3a3f80e3202 | 78 | eng_right.move(0); |
yruiewyrui3 | 0:f3a3f80e3202 | 79 | } |
yruiewyrui3 | 0:f3a3f80e3202 | 80 | |
yruiewyrui3 | 0:f3a3f80e3202 | 81 | int map(int value, int from_min, int from_max, int to_min, int to_max) // mapujaca wartosc do skretu |
yruiewyrui3 | 0:f3a3f80e3202 | 82 | { |
yruiewyrui3 | 0:f3a3f80e3202 | 83 | return (value - from_min) * (to_max - to_min)/(from_max - from_min) + to_min; |
yruiewyrui3 | 0:f3a3f80e3202 | 84 | } |
yruiewyrui3 | 0:f3a3f80e3202 | 85 | |
yruiewyrui3 | 0:f3a3f80e3202 | 86 | void move_camera_down(int val) // sterujaca kamera |
yruiewyrui3 | 0:f3a3f80e3202 | 87 | { |
yruiewyrui3 | 0:f3a3f80e3202 | 88 | act_vertical = act_vertical + val; |
yruiewyrui3 | 0:f3a3f80e3202 | 89 | if(act_vertical>90){ |
yruiewyrui3 | 0:f3a3f80e3202 | 90 | act_vertical=90; |
yruiewyrui3 | 0:f3a3f80e3202 | 91 | } |
yruiewyrui3 | 0:f3a3f80e3202 | 92 | cam_vertical.position(act_vertical); |
yruiewyrui3 | 0:f3a3f80e3202 | 93 | } |
yruiewyrui3 | 0:f3a3f80e3202 | 94 | |
yruiewyrui3 | 0:f3a3f80e3202 | 95 | void move_camera_up(int val) // sterujaca kamera |
yruiewyrui3 | 0:f3a3f80e3202 | 96 | { |
yruiewyrui3 | 0:f3a3f80e3202 | 97 | act_vertical = act_vertical - val; |
yruiewyrui3 | 0:f3a3f80e3202 | 98 | if(act_vertical<-90){ |
yruiewyrui3 | 0:f3a3f80e3202 | 99 | act_vertical=-90; |
yruiewyrui3 | 0:f3a3f80e3202 | 100 | } |
yruiewyrui3 | 0:f3a3f80e3202 | 101 | cam_vertical.position(act_vertical); |
yruiewyrui3 | 0:f3a3f80e3202 | 102 | } |
yruiewyrui3 | 0:f3a3f80e3202 | 103 | |
yruiewyrui3 | 0:f3a3f80e3202 | 104 | void move_camera_right(int val) // sterujaca kamera |
yruiewyrui3 | 0:f3a3f80e3202 | 105 | { |
yruiewyrui3 | 0:f3a3f80e3202 | 106 | act_horizontal = act_horizontal - val; |
yruiewyrui3 | 0:f3a3f80e3202 | 107 | if(act_horizontal<-90){ |
yruiewyrui3 | 0:f3a3f80e3202 | 108 | act_horizontal=-90; |
yruiewyrui3 | 0:f3a3f80e3202 | 109 | } |
yruiewyrui3 | 0:f3a3f80e3202 | 110 | cam_horizontal.position(act_horizontal); |
yruiewyrui3 | 0:f3a3f80e3202 | 111 | } |
yruiewyrui3 | 0:f3a3f80e3202 | 112 | |
yruiewyrui3 | 0:f3a3f80e3202 | 113 | void move_camera_left(int val) // sterujaca kamera |
yruiewyrui3 | 0:f3a3f80e3202 | 114 | { |
yruiewyrui3 | 0:f3a3f80e3202 | 115 | act_horizontal = act_horizontal + val; |
yruiewyrui3 | 0:f3a3f80e3202 | 116 | if(act_horizontal>90){ |
yruiewyrui3 | 0:f3a3f80e3202 | 117 | act_horizontal=90; |
yruiewyrui3 | 0:f3a3f80e3202 | 118 | } |
yruiewyrui3 | 0:f3a3f80e3202 | 119 | cam_horizontal.position(act_horizontal); |
yruiewyrui3 | 0:f3a3f80e3202 | 120 | } |
yruiewyrui3 | 0:f3a3f80e3202 | 121 | |
yruiewyrui3 | 0:f3a3f80e3202 | 122 | void set_camera_zero() // zerujaca kamere |
yruiewyrui3 | 0:f3a3f80e3202 | 123 | { |
yruiewyrui3 | 0:f3a3f80e3202 | 124 | cam_horizontal.position(-5); |
yruiewyrui3 | 0:f3a3f80e3202 | 125 | cam_vertical.position(-20); |
yruiewyrui3 | 0:f3a3f80e3202 | 126 | } |
yruiewyrui3 | 0:f3a3f80e3202 | 127 | |
yruiewyrui3 | 0:f3a3f80e3202 | 128 | void set_val() // odczytująca wartosc |
yruiewyrui3 | 0:f3a3f80e3202 | 129 | { |
yruiewyrui3 | 0:f3a3f80e3202 | 130 | for (int q=0; q<3; q++){ |
yruiewyrui3 | 0:f3a3f80e3202 | 131 | buffer2[q]=buffer1[q+8]; |
yruiewyrui3 | 0:f3a3f80e3202 | 132 | } |
yruiewyrui3 | 0:f3a3f80e3202 | 133 | sscanf(buffer2, "%3d", &val); |
yruiewyrui3 | 0:f3a3f80e3202 | 134 | } |
yruiewyrui3 | 0:f3a3f80e3202 | 135 | |
yruiewyrui3 | 0:f3a3f80e3202 | 136 | int main(){ |
yruiewyrui3 | 0:f3a3f80e3202 | 137 | stm.baud(115200); |
yruiewyrui3 | 0:f3a3f80e3202 | 138 | cleanBuffer1(); |
yruiewyrui3 | 0:f3a3f80e3202 | 139 | cleanBuffer2(); |
yruiewyrui3 | 0:f3a3f80e3202 | 140 | set_camera_zero(); |
yruiewyrui3 | 0:f3a3f80e3202 | 141 | t_sonar.start(); |
yruiewyrui3 | 0:f3a3f80e3202 | 142 | |
yruiewyrui3 | 0:f3a3f80e3202 | 143 | while(true){ |
yruiewyrui3 | 0:f3a3f80e3202 | 144 | stm.printf("Poczatek while \n"); |
yruiewyrui3 | 0:f3a3f80e3202 | 145 | if(t.read_ms()>500) //jesli przez ponad 500ms nie ma nowej ramki, zatrzymujemy robota |
yruiewyrui3 | 0:f3a3f80e3202 | 146 | { |
yruiewyrui3 | 0:f3a3f80e3202 | 147 | eng_left.move(0); |
yruiewyrui3 | 0:f3a3f80e3202 | 148 | eng_right.move(0); |
yruiewyrui3 | 0:f3a3f80e3202 | 149 | } |
yruiewyrui3 | 0:f3a3f80e3202 | 150 | if(t_sonar.read_ms()>1000) //odczyt odleglosci co 1,2s |
yruiewyrui3 | 0:f3a3f80e3202 | 151 | { |
yruiewyrui3 | 0:f3a3f80e3202 | 152 | distance = sensor.distance(); |
yruiewyrui3 | 0:f3a3f80e3202 | 153 | stm.printf("%d\n",distance); |
yruiewyrui3 | 0:f3a3f80e3202 | 154 | |
yruiewyrui3 | 0:f3a3f80e3202 | 155 | t_sonar.stop(); |
yruiewyrui3 | 0:f3a3f80e3202 | 156 | t_sonar.reset(); |
yruiewyrui3 | 0:f3a3f80e3202 | 157 | t_sonar.start(); |
yruiewyrui3 | 0:f3a3f80e3202 | 158 | } |
yruiewyrui3 | 0:f3a3f80e3202 | 159 | while(stm.readable()){ |
yruiewyrui3 | 0:f3a3f80e3202 | 160 | stm.gets(buffer1, 13); |
yruiewyrui3 | 0:f3a3f80e3202 | 161 | break;} |
yruiewyrui3 | 0:f3a3f80e3202 | 162 | |
yruiewyrui3 | 0:f3a3f80e3202 | 163 | stm.printf("bufor pierwszy: \n"); |
yruiewyrui3 | 0:f3a3f80e3202 | 164 | |
yruiewyrui3 | 0:f3a3f80e3202 | 165 | for (int f=0; f<20; f++) |
yruiewyrui3 | 0:f3a3f80e3202 | 166 | { |
yruiewyrui3 | 0:f3a3f80e3202 | 167 | stm.printf("%c", buffer1[f]); |
yruiewyrui3 | 0:f3a3f80e3202 | 168 | } |
yruiewyrui3 | 0:f3a3f80e3202 | 169 | |
yruiewyrui3 | 0:f3a3f80e3202 | 170 | stm.printf("\n"); |
yruiewyrui3 | 0:f3a3f80e3202 | 171 | |
yruiewyrui3 | 0:f3a3f80e3202 | 172 | set_val(); |
yruiewyrui3 | 0:f3a3f80e3202 | 173 | |
yruiewyrui3 | 0:f3a3f80e3202 | 174 | stm.printf("%i", val); |
yruiewyrui3 | 0:f3a3f80e3202 | 175 | |
yruiewyrui3 | 0:f3a3f80e3202 | 176 | t.stop(); |
yruiewyrui3 | 0:f3a3f80e3202 | 177 | t.reset(); |
yruiewyrui3 | 0:f3a3f80e3202 | 178 | t.start(); |
yruiewyrui3 | 0:f3a3f80e3202 | 179 | switch (what_command(buffer1)) |
yruiewyrui3 | 0:f3a3f80e3202 | 180 | { |
yruiewyrui3 | 0:f3a3f80e3202 | 181 | case 1: |
yruiewyrui3 | 0:f3a3f80e3202 | 182 | eng_left.move(val); |
yruiewyrui3 | 0:f3a3f80e3202 | 183 | eng_right.move(val); |
yruiewyrui3 | 0:f3a3f80e3202 | 184 | set_camera_zero(); |
yruiewyrui3 | 0:f3a3f80e3202 | 185 | speed_car = val; |
yruiewyrui3 | 0:f3a3f80e3202 | 186 | break; |
yruiewyrui3 | 0:f3a3f80e3202 | 187 | case 2: |
yruiewyrui3 | 0:f3a3f80e3202 | 188 | eng_right.move(speed_car); |
yruiewyrui3 | 0:f3a3f80e3202 | 189 | eng_left.move(speed_car - map(val, 0, 255, 0, speed_car)); |
yruiewyrui3 | 0:f3a3f80e3202 | 190 | cam_horizontal.position(15); |
yruiewyrui3 | 0:f3a3f80e3202 | 191 | cam_vertical.position(0); |
yruiewyrui3 | 0:f3a3f80e3202 | 192 | break; |
yruiewyrui3 | 0:f3a3f80e3202 | 193 | case 3: |
yruiewyrui3 | 0:f3a3f80e3202 | 194 | eng_left.move(speed_car); |
yruiewyrui3 | 0:f3a3f80e3202 | 195 | eng_right.move(speed_car - map(val, 0, 255, 0, speed_car)); |
yruiewyrui3 | 0:f3a3f80e3202 | 196 | cam_horizontal.position(0); |
yruiewyrui3 | 0:f3a3f80e3202 | 197 | cam_vertical.position(-35); |
yruiewyrui3 | 0:f3a3f80e3202 | 198 | break; |
yruiewyrui3 | 0:f3a3f80e3202 | 199 | case 4: |
yruiewyrui3 | 0:f3a3f80e3202 | 200 | eng_left.move(-val); |
yruiewyrui3 | 0:f3a3f80e3202 | 201 | eng_right.move(-val); |
yruiewyrui3 | 0:f3a3f80e3202 | 202 | set_camera_zero(); |
yruiewyrui3 | 0:f3a3f80e3202 | 203 | speed_car = val; |
yruiewyrui3 | 0:f3a3f80e3202 | 204 | break; |
yruiewyrui3 | 0:f3a3f80e3202 | 205 | case 5: |
yruiewyrui3 | 0:f3a3f80e3202 | 206 | hard_stop(); |
yruiewyrui3 | 0:f3a3f80e3202 | 207 | speed_car = 0; |
yruiewyrui3 | 0:f3a3f80e3202 | 208 | break; |
yruiewyrui3 | 0:f3a3f80e3202 | 209 | case 6: |
yruiewyrui3 | 0:f3a3f80e3202 | 210 | eng_left.move(-val); |
yruiewyrui3 | 0:f3a3f80e3202 | 211 | eng_right.move(val); |
yruiewyrui3 | 0:f3a3f80e3202 | 212 | break; |
yruiewyrui3 | 0:f3a3f80e3202 | 213 | case 7: |
yruiewyrui3 | 0:f3a3f80e3202 | 214 | eng_left.move(val); |
yruiewyrui3 | 0:f3a3f80e3202 | 215 | eng_right.move(-val); |
yruiewyrui3 | 0:f3a3f80e3202 | 216 | break; |
yruiewyrui3 | 0:f3a3f80e3202 | 217 | case 8: |
yruiewyrui3 | 0:f3a3f80e3202 | 218 | move_camera_down(val); |
yruiewyrui3 | 0:f3a3f80e3202 | 219 | break; |
yruiewyrui3 | 0:f3a3f80e3202 | 220 | case 9: |
yruiewyrui3 | 0:f3a3f80e3202 | 221 | move_camera_left(val); |
yruiewyrui3 | 0:f3a3f80e3202 | 222 | break; |
yruiewyrui3 | 0:f3a3f80e3202 | 223 | case 10: |
yruiewyrui3 | 0:f3a3f80e3202 | 224 | move_camera_up(val); |
yruiewyrui3 | 0:f3a3f80e3202 | 225 | break; |
yruiewyrui3 | 0:f3a3f80e3202 | 226 | case 11: |
yruiewyrui3 | 0:f3a3f80e3202 | 227 | move_camera_right(val); |
yruiewyrui3 | 0:f3a3f80e3202 | 228 | break; |
yruiewyrui3 | 0:f3a3f80e3202 | 229 | case 12: |
yruiewyrui3 | 0:f3a3f80e3202 | 230 | set_camera_zero(); |
yruiewyrui3 | 0:f3a3f80e3202 | 231 | break; |
yruiewyrui3 | 0:f3a3f80e3202 | 232 | default: |
yruiewyrui3 | 0:f3a3f80e3202 | 233 | stm.printf("unknown_command \n"); |
yruiewyrui3 | 0:f3a3f80e3202 | 234 | break; |
yruiewyrui3 | 0:f3a3f80e3202 | 235 | } |
yruiewyrui3 | 0:f3a3f80e3202 | 236 | cleanBuffer1(); |
yruiewyrui3 | 0:f3a3f80e3202 | 237 | cleanBuffer2(); |
yruiewyrui3 | 0:f3a3f80e3202 | 238 | |
yruiewyrui3 | 0:f3a3f80e3202 | 239 | stm.printf("Koniec while \n"); |
yruiewyrui3 | 0:f3a3f80e3202 | 240 | |
yruiewyrui3 | 0:f3a3f80e3202 | 241 | wait_ms(2000); |
yruiewyrui3 | 0:f3a3f80e3202 | 242 | } |
yruiewyrui3 | 0:f3a3f80e3202 | 243 | } |