Dominik Święch / Mbed 2 deprecated JakKrisowy_

.

Dependencies:   HCSR Ser mbed

Fork of JakKrisowy by Dominik Święch

main.cpp@1:1e01b12063cd, 2016-07-14 (annotated)

Committer:
yruiewyrui3
Date:
Thu Jul 14 13:31:01 2016 +0000
Revision:
1:1e01b12063cd
Parent:
0:f3a3f80e3202 
.;

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