svoe
Dependencies: mbed mbed-STM32F103C8T6 MPU6050_1
Diff: echo.h
- Revision:
- 1:e2a6e523bf1f
- Parent:
- 0:e9488589a8ee
- Child:
- 4:904b737ef08a
--- a/echo.h Tue May 02 16:50:18 2017 +0000 +++ b/echo.h Sat May 06 18:21:55 2017 +0000 @@ -1,5 +1,7 @@ -int obstacle[20];//13 steps [-90;+90] by 15 deg -int echo_current_step;//[0;12], step = 6 => 0 deg +int obstacle[100];//13 steps [-90;+90] by 15 deg +int echo_current_step;//[0;36], (step=18) => 0 deg +int echo_old_step; +int echo_old_old_step; int echo_current_dir = -1; int echo_cm; float tmf; @@ -9,34 +11,22 @@ int max_dist_angle; int front_dist; - - Timer echo_timer; InterruptIn echo(PB_12); DigitalOut sonar_triger(PB_13); -void echo_begin(){ +void echo_begin(){ //begin pulsewidth measurement echo_timer.reset(); } -void echo_end(){ +void echo_end(){ //end pulsewidth measurement echo_cm = echo_timer.read_us()/58; } -void search(){ - serva(-80); - wait_ms(200); - for(int point = 0;point <= 12;point ++){ - serva(point*15-90); - wait_ms(100); - obstacle[point]=echo_cm;} - } - void echo_start(){ sonar_triger = 1; tmf=log(1.0);//delay 5us - tmf=log(1.0);//delay 5us - //for(int i = 0; i<2000; i++) {tmi++;} + tmf=log(2.0);//delay 5us sonar_triger = 0;//sonar start } @@ -46,15 +36,35 @@ echo_timer.start(); } -void echo_step(){ - obstacle[echo_current_step]=echo_cm; +void echo_transmit(int steps_number) { //printf is not recommended for interrupts + int tmstep; + wifi.putc(0xff); //sync symbol + for(tmstep=0;tmstep<steps_number;tmstep++){wifi.putc((obstacle[tmstep]<500) ? obstacle[tmstep]/2 : 250);} + } + +void echo_scan(){ // rewrite with argument: n or step //will not work in interrupt ?? + serva(-90); + wait_ms(300); + for(int point = 0;point <= 12;point ++){ + serva(point*15-90); + wait_ms(50); + echo_start(); + wait_ms(50); + obstacle[point]=echo_cm;} + } + +void echo_step(bool transmit = 0){ + obstacle[echo_old_old_step]=echo_cm; + //echo_transmit(echo_current_step); + echo_old_old_step = echo_old_step; + echo_old_step = echo_current_step; echo_current_step += echo_current_dir; - if (echo_current_step > 12) {echo_current_step = 11 ; echo_current_dir = -1;} - if (echo_current_step < 0) {echo_current_step = 1 ; echo_current_dir = 1;} - serva(echo_current_step*15-90); + if (echo_current_step > 36) {echo_current_step = 35 ; echo_current_dir = -1; echo_transmit(37);} + if (echo_current_step < 0) {echo_current_step = 1 ; echo_current_dir = 1; echo_transmit(37);} + serva(echo_current_step*5-90); echo_start(); } - + bool analyze_obstacle(){ min_dist = 400;max_dist = 0; front_dist = 400; for(int point = 0;point <= 12;point ++){ @@ -63,4 +73,8 @@ if((obstacle[point]*abs(point-6)*3<15)&&(obstacle[point]<front_dist)) {front_dist=obstacle[point];} } if(front_dist < 20) return(1); else return(0); + } + +void echo_full_scan(int steps){ //scan by wheels (calculate speed) + } \ No newline at end of file