svoe
Dependencies: mbed mbed-STM32F103C8T6 MPU6050_1
echo.h@4:904b737ef08a, 2017-05-14 (annotated)
- Committer:
- Stas285
- Date:
- Sun May 14 11:28:29 2017 +0000
- Revision:
- 4:904b737ef08a
- Parent:
- 1:e2a6e523bf1f
- Child:
- 17:bd6b6ac89e0e
+Gyro(draft)
Who changed what in which revision?
User | Revision | Line number | New 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 | 4:904b737ef08a | 62 | if (echo_current_step > 36) {echo_current_step = 35 ; echo_current_dir = -1; /*echo_transmit(37);*/} |
Stas285 | 4:904b737ef08a | 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 | 4:904b737ef08a | 65 | echo_start(); //the last to exit ticker interrupt ASAP |
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 | } |