svoe
Dependencies: mbed mbed-STM32F103C8T6 MPU6050_1
echo.h
- Committer:
- dima285
- Date:
- 2019-02-10
- Revision:
- 17:bd6b6ac89e0e
- Parent:
- 4:904b737ef08a
- Child:
- 19:2fe650d29823
File content as of revision 17:bd6b6ac89e0e:
int obstacle_glaz[100];//13 steps [-90;+90] by 15 deg int obstacle_echo[100];//13 steps [-90;+90] by 15 deg int obstacle[100];//13 steps [-90;+90] by 15 deg int corrected_obstacle [100]; 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; int min_dist; int min_dist_angle; int max_dist; int max_dist_angle; int front_dist; Timer echo_timer; InterruptIn echo(PB_12); DigitalOut sonar_triger(PB_13); void echo_begin(){ //begin pulsewidth measurement echo_timer.reset(); } void echo_end(){ //end pulsewidth measurement echo_cm = echo_timer.read_us()/58; } void echo_start(){ sonar_triger = 1; tmf=log(1.0);//delay 5us tmf=log(2.0);//delay 5us sonar_triger = 0;//sonar start } void echo_init(){ echo.rise(&echo_begin); echo.fall(&echo_end); echo_timer.start(); } 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_glaz[tmstep] < 500) ? obstacle_glaz[tmstep]/2 : 250);} //for(tmstep = 0; tmstep < steps_number; tmstep ++){wifi.putc((obstacle_echo[tmstep] < 500) ? obstacle_echo[tmstep]/2 : 250);} for(tmstep = 0; tmstep < steps_number; tmstep ++){wifi.putc((obstacle[tmstep] < 500) ? obstacle[tmstep]/2 : 250);} for(tmstep = 0; tmstep < steps_number; tmstep ++){wifi.putc((corrected_obstacle[tmstep] < 500) ? corrected_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 > 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(); //the last to exit ticker interrupt ASAP } void echo_full_scan(int steps){ //scan by wheels (calculate speed) } void glaz_init(){ glaz.setTimeout(500); if (!glaz.init()) { wifi.printf("Failed to detect and initialize sensor!"); } glaz.setDistanceMode(VL53L1X::Long); glaz.setMeasurementTimingBudget(50000); glaz.startContinuous(50); }