Dima Dudin / Mbed 2 deprecated DD_Robot

svoe

Dependencies:   mbed mbed-STM32F103C8T6 MPU6050_1

echo.h@0:e9488589a8ee, 2017-05-02 (annotated)

Committer:
Stas285
Date:
Tue May 02 16:50:18 2017 +0000
Revision:
0:e9488589a8ee
Child:
1:e2a6e523bf1f
debil

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Stas285 0:e9488589a8ee 1 int obstacle[20];//13 steps [-90;+90] by 15 deg
Stas285 0:e9488589a8ee 2 int echo_current_step;//[0;12], step = 6 => 0 deg
Stas285 0:e9488589a8ee 3 int echo_current_dir = -1;
Stas285 0:e9488589a8ee 4 int echo_cm;
Stas285 0:e9488589a8ee 5 float tmf;
Stas285 0:e9488589a8ee 6 int min_dist;
Stas285 0:e9488589a8ee 7 int min_dist_angle;
Stas285 0:e9488589a8ee 8 int max_dist;
Stas285 0:e9488589a8ee 9 int max_dist_angle;
Stas285 0:e9488589a8ee 10 int front_dist;
Stas285 0:e9488589a8ee 11
Stas285 0:e9488589a8ee 12
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 0:e9488589a8ee 18 void echo_begin(){
Stas285 0:e9488589a8ee 19 echo_timer.reset();
Stas285 0:e9488589a8ee 20 }
Stas285 0:e9488589a8ee 21
Stas285 0:e9488589a8ee 22 void echo_end(){
Stas285 0:e9488589a8ee 23 echo_cm = echo_timer.read_us()/58;
Stas285 0:e9488589a8ee 24 }
Stas285 0:e9488589a8ee 25
Stas285 0:e9488589a8ee 26 void search(){
Stas285 0:e9488589a8ee 27 serva(-80);
Stas285 0:e9488589a8ee 28 wait_ms(200);
Stas285 0:e9488589a8ee 29 for(int point = 0;point <= 12;point ++){
Stas285 0:e9488589a8ee 30 serva(point*15-90);
Stas285 0:e9488589a8ee 31 wait_ms(100);
Stas285 0:e9488589a8ee 32 obstacle[point]=echo_cm;}
Stas285 0:e9488589a8ee 33 }
Stas285 0:e9488589a8ee 34
Stas285 0:e9488589a8ee 35 void echo_start(){
Stas285 0:e9488589a8ee 36 sonar_triger = 1;
Stas285 0:e9488589a8ee 37 tmf=log(1.0);//delay 5us
Stas285 0:e9488589a8ee 38 tmf=log(1.0);//delay 5us
Stas285 0:e9488589a8ee 39 //for(int i = 0; i<2000; i++) {tmi++;}
Stas285 0:e9488589a8ee 40 sonar_triger = 0;//sonar start
Stas285 0:e9488589a8ee 41 }
Stas285 0:e9488589a8ee 42
Stas285 0:e9488589a8ee 43 void echo_init(){
Stas285 0:e9488589a8ee 44 echo.rise(&echo_begin);
Stas285 0:e9488589a8ee 45 echo.fall(&echo_end);
Stas285 0:e9488589a8ee 46 echo_timer.start();
Stas285 0:e9488589a8ee 47 }
Stas285 0:e9488589a8ee 48
Stas285 0:e9488589a8ee 49 void echo_step(){
Stas285 0:e9488589a8ee 50 obstacle[echo_current_step]=echo_cm;
Stas285 0:e9488589a8ee 51 echo_current_step += echo_current_dir;
Stas285 0:e9488589a8ee 52 if (echo_current_step > 12) {echo_current_step = 11 ; echo_current_dir = -1;}
Stas285 0:e9488589a8ee 53 if (echo_current_step < 0) {echo_current_step = 1 ; echo_current_dir = 1;}
Stas285 0:e9488589a8ee 54 serva(echo_current_step*15-90);
Stas285 0:e9488589a8ee 55 echo_start();
Stas285 0:e9488589a8ee 56 }
Stas285 0:e9488589a8ee 57
Stas285 0:e9488589a8ee 58 bool analyze_obstacle(){
Stas285 0:e9488589a8ee 59 min_dist = 400;max_dist = 0; front_dist = 400;
Stas285 0:e9488589a8ee 60 for(int point = 0;point <= 12;point ++){
Stas285 0:e9488589a8ee 61 if(obstacle[point]<min_dist) {min_dist=obstacle[point];min_dist_angle = point*15-90;}
Stas285 0:e9488589a8ee 62 if(obstacle[point]>max_dist) {max_dist=obstacle[point];max_dist_angle = point*15-90;}
Stas285 0:e9488589a8ee 63 if((obstacle[point]*abs(point-6)*3<15)&&(obstacle[point]<front_dist)) {front_dist=obstacle[point];}
Stas285 0:e9488589a8ee 64 }
Stas285 0:e9488589a8ee 65 if(front_dist < 20) return(1); else return(0);
Stas285 0:e9488589a8ee 66 }