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?
User | Revision | Line number | New 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 | } |