Justyna Guzik / Mbed 2 deprecated golonkaczosniur

Dependencies:   HCSR Ser mbed

Fork of JakKrisowy_ by Dominik Święch

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?

UserRevisionLine numberNew 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 }