Dima Dudin / Mbed 2 deprecated DD_Robot

svoe

Dependencies:   mbed mbed-STM32F103C8T6 MPU6050_1

echo.h@1:e2a6e523bf1f, 2017-05-06 (annotated)

Committer:
Stas285
Date:
Sat May 06 18:21:55 2017 +0000
Revision:
1:e2a6e523bf1f
Parent:
0:e9488589a8ee
Child:
4:904b737ef08a
wifi + echo + rc

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Stas285 1:e2a6e523bf1f 1 int obstacle[100];//13 steps [-90;+90] by 15 deg
Stas285 1:e2a6e523bf1f 2 int echo_current_step;//[0;36], (step=18) => 0 deg
Stas285 1:e2a6e523bf1f 3 int echo_old_step;
Stas285 1:e2a6e523bf1f 4 int echo_old_old_step;
Stas285 0:e9488589a8ee 5 int echo_current_dir = -1;
Stas285 0:e9488589a8ee 6 int echo_cm;
Stas285 0:e9488589a8ee 7 float tmf;
Stas285 0:e9488589a8ee 8 int min_dist;
Stas285 0:e9488589a8ee 9 int min_dist_angle;
Stas285 0:e9488589a8ee 10 int max_dist;
Stas285 0:e9488589a8ee 11 int max_dist_angle;
Stas285 0:e9488589a8ee 12 int front_dist;
Stas285 0:e9488589a8ee 13
Stas285 0:e9488589a8ee 14 Timer echo_timer;
Stas285 0:e9488589a8ee 15 InterruptIn echo(PB_12);
Stas285 0:e9488589a8ee 16 DigitalOut sonar_triger(PB_13);
Stas285 0:e9488589a8ee 17
Stas285 1:e2a6e523bf1f 18 void echo_begin(){ //begin pulsewidth measurement
Stas285 0:e9488589a8ee 19 echo_timer.reset();
Stas285 0:e9488589a8ee 20 }
Stas285 0:e9488589a8ee 21
Stas285 1:e2a6e523bf1f 22 void echo_end(){ //end pulsewidth measurement
Stas285 0:e9488589a8ee 23 echo_cm = echo_timer.read_us()/58;
Stas285 0:e9488589a8ee 24 }
Stas285 0:e9488589a8ee 25
Stas285 0:e9488589a8ee 26 void echo_start(){
Stas285 0:e9488589a8ee 27 sonar_triger = 1;
Stas285 0:e9488589a8ee 28 tmf=log(1.0);//delay 5us
Stas285 1:e2a6e523bf1f 29 tmf=log(2.0);//delay 5us
Stas285 0:e9488589a8ee 30 sonar_triger = 0;//sonar start
Stas285 0:e9488589a8ee 31 }
Stas285 0:e9488589a8ee 32
Stas285 0:e9488589a8ee 33 void echo_init(){
Stas285 0:e9488589a8ee 34 echo.rise(&echo_begin);
Stas285 0:e9488589a8ee 35 echo.fall(&echo_end);
Stas285 0:e9488589a8ee 36 echo_timer.start();
Stas285 0:e9488589a8ee 37 }
Stas285 0:e9488589a8ee 38
Stas285 1:e2a6e523bf1f 39 void echo_transmit(int steps_number) { //printf is not recommended for interrupts
Stas285 1:e2a6e523bf1f 40 int tmstep;
Stas285 1:e2a6e523bf1f 41 wifi.putc(0xff); //sync symbol
Stas285 1:e2a6e523bf1f 42 for(tmstep=0;tmstep<steps_number;tmstep++){wifi.putc((obstacle[tmstep]<500) ? obstacle[tmstep]/2 : 250);}
Stas285 1:e2a6e523bf1f 43 }
Stas285 1:e2a6e523bf1f 44
Stas285 1:e2a6e523bf1f 45 void echo_scan(){ // rewrite with argument: n or step //will not work in interrupt ??
Stas285 1:e2a6e523bf1f 46 serva(-90);
Stas285 1:e2a6e523bf1f 47 wait_ms(300);
Stas285 1:e2a6e523bf1f 48 for(int point = 0;point <= 12;point ++){
Stas285 1:e2a6e523bf1f 49 serva(point*15-90);
Stas285 1:e2a6e523bf1f 50 wait_ms(50);
Stas285 1:e2a6e523bf1f 51 echo_start();
Stas285 1:e2a6e523bf1f 52 wait_ms(50);
Stas285 1:e2a6e523bf1f 53 obstacle[point]=echo_cm;}
Stas285 1:e2a6e523bf1f 54 }
Stas285 1:e2a6e523bf1f 55
Stas285 1:e2a6e523bf1f 56 void echo_step(bool transmit = 0){
Stas285 1:e2a6e523bf1f 57 obstacle[echo_old_old_step]=echo_cm;
Stas285 1:e2a6e523bf1f 58 //echo_transmit(echo_current_step);
Stas285 1:e2a6e523bf1f 59 echo_old_old_step = echo_old_step;
Stas285 1:e2a6e523bf1f 60 echo_old_step = echo_current_step;
Stas285 0:e9488589a8ee 61 echo_current_step += echo_current_dir;
Stas285 1:e2a6e523bf1f 62 if (echo_current_step > 36) {echo_current_step = 35 ; echo_current_dir = -1; echo_transmit(37);}
Stas285 1:e2a6e523bf1f 63 if (echo_current_step < 0) {echo_current_step = 1 ; echo_current_dir = 1; echo_transmit(37);}
Stas285 1:e2a6e523bf1f 64 serva(echo_current_step*5-90);
Stas285 0:e9488589a8ee 65 echo_start();
Stas285 0:e9488589a8ee 66 }
Stas285 1:e2a6e523bf1f 67
Stas285 0:e9488589a8ee 68 bool analyze_obstacle(){
Stas285 0:e9488589a8ee 69 min_dist = 400;max_dist = 0; front_dist = 400;
Stas285 0:e9488589a8ee 70 for(int point = 0;point <= 12;point ++){
Stas285 0:e9488589a8ee 71 if(obstacle[point]<min_dist) {min_dist=obstacle[point];min_dist_angle = point*15-90;}
Stas285 0:e9488589a8ee 72 if(obstacle[point]>max_dist) {max_dist=obstacle[point];max_dist_angle = point*15-90;}
Stas285 0:e9488589a8ee 73 if((obstacle[point]*abs(point-6)*3<15)&&(obstacle[point]<front_dist)) {front_dist=obstacle[point];}
Stas285 0:e9488589a8ee 74 }
Stas285 0:e9488589a8ee 75 if(front_dist < 20) return(1); else return(0);
Stas285 1:e2a6e523bf1f 76 }
Stas285 1:e2a6e523bf1f 77
Stas285 1:e2a6e523bf1f 78 void echo_full_scan(int steps){ //scan by wheels (calculate speed)
Stas285 1:e2a6e523bf1f 79
Stas285 0:e9488589a8ee 80 }