svoe
Dependencies: mbed mbed-STM32F103C8T6 MPU6050_1
echo.h@22:14e85f2068c7, 2019-07-23 (annotated)
- Committer:
- dima285
- Date:
- Tue Jul 23 12:55:23 2019 +0000
- Branch:
- svoe
- Revision:
- 22:14e85f2068c7
- Parent:
- 19:2fe650d29823
- Child:
- 23:bc05a104be10
23.07.2019
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
dima285 | 17:bd6b6ac89e0e | 1 | int obstacle_glaz[100];//13 steps [-90;+90] by 15 deg |
dima285 | 17:bd6b6ac89e0e | 2 | int obstacle_echo[100];//13 steps [-90;+90] by 15 deg |
Stas285 | 1:e2a6e523bf1f | 3 | int obstacle[100];//13 steps [-90;+90] by 15 deg |
dima285 | 17:bd6b6ac89e0e | 4 | int corrected_obstacle [100]; |
dima285 | 22:14e85f2068c7 | 5 | int gabarit_obstacle [100]; |
Stas285 | 1:e2a6e523bf1f | 6 | int echo_current_step;//[0;36], (step=18) => 0 deg |
Stas285 | 1:e2a6e523bf1f | 7 | int echo_old_step; |
Stas285 | 1:e2a6e523bf1f | 8 | int echo_old_old_step; |
Stas285 | 0:e9488589a8ee | 9 | int echo_current_dir = -1; |
Stas285 | 0:e9488589a8ee | 10 | int echo_cm; |
Stas285 | 0:e9488589a8ee | 11 | float tmf; |
Stas285 | 0:e9488589a8ee | 12 | int min_dist; |
Stas285 | 0:e9488589a8ee | 13 | int min_dist_angle; |
Stas285 | 0:e9488589a8ee | 14 | int max_dist; |
Stas285 | 0:e9488589a8ee | 15 | int max_dist_angle; |
Stas285 | 0:e9488589a8ee | 16 | int front_dist; |
Stas285 | 0:e9488589a8ee | 17 | |
Stas285 | 0:e9488589a8ee | 18 | Timer echo_timer; |
Stas285 | 0:e9488589a8ee | 19 | InterruptIn echo(PB_12); |
Stas285 | 0:e9488589a8ee | 20 | DigitalOut sonar_triger(PB_13); |
Stas285 | 0:e9488589a8ee | 21 | |
Stas285 | 1:e2a6e523bf1f | 22 | void echo_begin(){ //begin pulsewidth measurement |
Stas285 | 0:e9488589a8ee | 23 | echo_timer.reset(); |
Stas285 | 0:e9488589a8ee | 24 | } |
Stas285 | 0:e9488589a8ee | 25 | |
Stas285 | 1:e2a6e523bf1f | 26 | void echo_end(){ //end pulsewidth measurement |
Stas285 | 0:e9488589a8ee | 27 | echo_cm = echo_timer.read_us()/58; |
Stas285 | 0:e9488589a8ee | 28 | } |
Stas285 | 0:e9488589a8ee | 29 | |
Stas285 | 0:e9488589a8ee | 30 | void echo_start(){ |
Stas285 | 0:e9488589a8ee | 31 | sonar_triger = 1; |
Stas285 | 0:e9488589a8ee | 32 | tmf=log(1.0);//delay 5us |
Stas285 | 1:e2a6e523bf1f | 33 | tmf=log(2.0);//delay 5us |
Stas285 | 0:e9488589a8ee | 34 | sonar_triger = 0;//sonar start |
Stas285 | 0:e9488589a8ee | 35 | } |
Stas285 | 0:e9488589a8ee | 36 | |
Stas285 | 0:e9488589a8ee | 37 | void echo_init(){ |
Stas285 | 0:e9488589a8ee | 38 | echo.rise(&echo_begin); |
Stas285 | 0:e9488589a8ee | 39 | echo.fall(&echo_end); |
Stas285 | 0:e9488589a8ee | 40 | echo_timer.start(); |
Stas285 | 0:e9488589a8ee | 41 | } |
Stas285 | 0:e9488589a8ee | 42 | |
Stas285 | 1:e2a6e523bf1f | 43 | void echo_transmit(int steps_number) { //printf is not recommended for interrupts |
Stas285 | 1:e2a6e523bf1f | 44 | int tmstep; |
Stas285 | 1:e2a6e523bf1f | 45 | wifi.putc(0xff); //sync symbol |
dima285 | 22:14e85f2068c7 | 46 | for(tmstep = 0; tmstep < steps_number; tmstep ++){wifi.putc((gabarit_obstacle[tmstep] < 500) ? gabarit_obstacle[tmstep]/2 : 250);} |
dima285 | 17:bd6b6ac89e0e | 47 | //for(tmstep = 0; tmstep < steps_number; tmstep ++){wifi.putc((obstacle_echo[tmstep] < 500) ? obstacle_echo[tmstep]/2 : 250);} |
dima285 | 19:2fe650d29823 | 48 | //for(tmstep = 0; tmstep < steps_number; tmstep ++){wifi.putc((obstacle[tmstep] < 500) ? obstacle[tmstep]/2 : 250);} |
dima285 | 17:bd6b6ac89e0e | 49 | for(tmstep = 0; tmstep < steps_number; tmstep ++){wifi.putc((corrected_obstacle[tmstep] < 500) ? corrected_obstacle[tmstep]/2 : 250);} |
Stas285 | 1:e2a6e523bf1f | 50 | } |
Stas285 | 1:e2a6e523bf1f | 51 | |
Stas285 | 1:e2a6e523bf1f | 52 | void echo_scan(){ // rewrite with argument: n or step //will not work in interrupt ?? |
Stas285 | 1:e2a6e523bf1f | 53 | serva(-90); |
Stas285 | 1:e2a6e523bf1f | 54 | wait_ms(300); |
Stas285 | 1:e2a6e523bf1f | 55 | for(int point = 0;point <= 12;point ++){ |
Stas285 | 1:e2a6e523bf1f | 56 | serva(point*15-90); |
Stas285 | 1:e2a6e523bf1f | 57 | wait_ms(50); |
Stas285 | 1:e2a6e523bf1f | 58 | echo_start(); |
Stas285 | 1:e2a6e523bf1f | 59 | wait_ms(50); |
dima285 | 17:bd6b6ac89e0e | 60 | obstacle[point] = echo_cm;} |
Stas285 | 1:e2a6e523bf1f | 61 | } |
Stas285 | 1:e2a6e523bf1f | 62 | |
Stas285 | 1:e2a6e523bf1f | 63 | void echo_step(bool transmit = 0){ |
dima285 | 17:bd6b6ac89e0e | 64 | obstacle[echo_old_old_step] = echo_cm; |
Stas285 | 1:e2a6e523bf1f | 65 | //echo_transmit(echo_current_step); |
Stas285 | 1:e2a6e523bf1f | 66 | echo_old_old_step = echo_old_step; |
Stas285 | 1:e2a6e523bf1f | 67 | echo_old_step = echo_current_step; |
Stas285 | 0:e9488589a8ee | 68 | echo_current_step += echo_current_dir; |
Stas285 | 4:904b737ef08a | 69 | if (echo_current_step > 36) {echo_current_step = 35 ; echo_current_dir = -1; /*echo_transmit(37);*/} |
Stas285 | 4:904b737ef08a | 70 | if (echo_current_step < 0) {echo_current_step = 1 ; echo_current_dir = 1; /*echo_transmit(37);*/} |
Stas285 | 1:e2a6e523bf1f | 71 | serva(echo_current_step*5-90); |
Stas285 | 4:904b737ef08a | 72 | echo_start(); //the last to exit ticker interrupt ASAP |
Stas285 | 0:e9488589a8ee | 73 | } |
dima285 | 17:bd6b6ac89e0e | 74 | |
Stas285 | 1:e2a6e523bf1f | 75 | void echo_full_scan(int steps){ //scan by wheels (calculate speed) |
Stas285 | 1:e2a6e523bf1f | 76 | |
dima285 | 17:bd6b6ac89e0e | 77 | } |
dima285 | 17:bd6b6ac89e0e | 78 | |
dima285 | 17:bd6b6ac89e0e | 79 | void glaz_init(){ |
dima285 | 17:bd6b6ac89e0e | 80 | glaz.setTimeout(500); |
dima285 | 17:bd6b6ac89e0e | 81 | if (!glaz.init()) |
dima285 | 17:bd6b6ac89e0e | 82 | { |
dima285 | 17:bd6b6ac89e0e | 83 | wifi.printf("Failed to detect and initialize sensor!"); |
dima285 | 17:bd6b6ac89e0e | 84 | } |
dima285 | 17:bd6b6ac89e0e | 85 | glaz.setDistanceMode(VL53L1X::Long); |
dima285 | 17:bd6b6ac89e0e | 86 | glaz.setMeasurementTimingBudget(50000); |
dima285 | 19:2fe650d29823 | 87 | //glaz.startContinuous(50); |
Stas285 | 0:e9488589a8ee | 88 | } |