Dima Dudin / Mbed 2 deprecated DD_Robot

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?

UserRevisionLine numberNew 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 }