Dima Dudin / Mbed 2 deprecated DD_Robot

svoe

Dependencies:   mbed mbed-STM32F103C8T6 MPU6050_1

echo.h@23:bc05a104be10, 2019-08-06 (annotated)

Committer:
dima285
Date:
Tue Aug 06 14:13:55 2019 +0000
Branch:
svoe
Revision:
23:bc05a104be10
Parent:
22:14e85f2068c7 
06.08.19

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
dima285 23:bc05a104be10 18
Stas285 0:e9488589a8ee 19 Timer echo_timer;
Stas285 0:e9488589a8ee 20 InterruptIn echo(PB_12);
Stas285 0:e9488589a8ee 21 DigitalOut sonar_triger(PB_13);
Stas285 0:e9488589a8ee 22
Stas285 1:e2a6e523bf1f 23 void echo_begin(){ //begin pulsewidth measurement
Stas285 0:e9488589a8ee 24 echo_timer.reset();
Stas285 0:e9488589a8ee 25 }
Stas285 0:e9488589a8ee 26
Stas285 1:e2a6e523bf1f 27 void echo_end(){ //end pulsewidth measurement
Stas285 0:e9488589a8ee 28 echo_cm = echo_timer.read_us()/58;
Stas285 0:e9488589a8ee 29 }
Stas285 0:e9488589a8ee 30
Stas285 0:e9488589a8ee 31 void echo_start(){
Stas285 0:e9488589a8ee 32 sonar_triger = 1;
Stas285 0:e9488589a8ee 33 tmf=log(1.0);//delay 5us
Stas285 1:e2a6e523bf1f 34 tmf=log(2.0);//delay 5us
Stas285 0:e9488589a8ee 35 sonar_triger = 0;//sonar start
Stas285 0:e9488589a8ee 36 }
Stas285 0:e9488589a8ee 37
Stas285 0:e9488589a8ee 38 void echo_init(){
Stas285 0:e9488589a8ee 39 echo.rise(&echo_begin);
Stas285 0:e9488589a8ee 40 echo.fall(&echo_end);
Stas285 0:e9488589a8ee 41 echo_timer.start();
Stas285 0:e9488589a8ee 42 }
Stas285 0:e9488589a8ee 43
Stas285 1:e2a6e523bf1f 44 void echo_transmit(int steps_number) { //printf is not recommended for interrupts
Stas285 1:e2a6e523bf1f 45 int tmstep;
Stas285 1:e2a6e523bf1f 46 wifi.putc(0xff); //sync symbol
dima285 22:14e85f2068c7 47 for(tmstep = 0; tmstep < steps_number; tmstep ++){wifi.putc((gabarit_obstacle[tmstep] < 500) ? gabarit_obstacle[tmstep]/2 : 250);}
dima285 17:bd6b6ac89e0e 48 //for(tmstep = 0; tmstep < steps_number; tmstep ++){wifi.putc((obstacle_echo[tmstep] < 500) ? obstacle_echo[tmstep]/2 : 250);}
dima285 19:2fe650d29823 49 //for(tmstep = 0; tmstep < steps_number; tmstep ++){wifi.putc((obstacle[tmstep] < 500) ? obstacle[tmstep]/2 : 250);}
dima285 17:bd6b6ac89e0e 50 for(tmstep = 0; tmstep < steps_number; tmstep ++){wifi.putc((corrected_obstacle[tmstep] < 500) ? corrected_obstacle[tmstep]/2 : 250);}
Stas285 1:e2a6e523bf1f 51 }
dima285 23:bc05a104be10 52
dima285 23:bc05a104be10 53 void transmit_360(){
dima285 23:bc05a104be10 54
dima285 23:bc05a104be10 55 }
Stas285 1:e2a6e523bf1f 56
Stas285 1:e2a6e523bf1f 57 void echo_scan(){ // rewrite with argument: n or step //will not work in interrupt ??
Stas285 1:e2a6e523bf1f 58 serva(-90);
Stas285 1:e2a6e523bf1f 59 wait_ms(300);
Stas285 1:e2a6e523bf1f 60 for(int point = 0;point <= 12;point ++){
Stas285 1:e2a6e523bf1f 61 serva(point*15-90);
Stas285 1:e2a6e523bf1f 62 wait_ms(50);
Stas285 1:e2a6e523bf1f 63 echo_start();
Stas285 1:e2a6e523bf1f 64 wait_ms(50);
dima285 17:bd6b6ac89e0e 65 obstacle[point] = echo_cm;}
Stas285 1:e2a6e523bf1f 66 }
Stas285 1:e2a6e523bf1f 67
Stas285 1:e2a6e523bf1f 68 void echo_step(bool transmit = 0){
dima285 17:bd6b6ac89e0e 69 obstacle[echo_old_old_step] = echo_cm;
Stas285 1:e2a6e523bf1f 70 //echo_transmit(echo_current_step);
Stas285 1:e2a6e523bf1f 71 echo_old_old_step = echo_old_step;
Stas285 1:e2a6e523bf1f 72 echo_old_step = echo_current_step;
Stas285 0:e9488589a8ee 73 echo_current_step += echo_current_dir;
Stas285 4:904b737ef08a 74 if (echo_current_step > 36) {echo_current_step = 35 ; echo_current_dir = -1; /*echo_transmit(37);*/}
Stas285 4:904b737ef08a 75 if (echo_current_step < 0) {echo_current_step = 1 ; echo_current_dir = 1; /*echo_transmit(37);*/}
Stas285 1:e2a6e523bf1f 76 serva(echo_current_step*5-90);
Stas285 4:904b737ef08a 77 echo_start(); //the last to exit ticker interrupt ASAP
Stas285 0:e9488589a8ee 78 }
dima285 23:bc05a104be10 79 void correct_obstacle(){ //gabariti
dima285 23:bc05a104be10 80 for(int point = 0;point <= 12;point ++){
dima285 23:bc05a104be10 81 gabarit_obstacle[point] = obstacle[point];
dima285 23:bc05a104be10 82 }
dima285 23:bc05a104be10 83 for(int point = 0;point <= 12;point ++){
dima285 23:bc05a104be10 84 for(int test_point = 0;test_point <= 12;test_point ++){
dima285 23:bc05a104be10 85 switch(abs(test_point - point)){
dima285 23:bc05a104be10 86 case 1: if((obstacle[test_point] < 60) && (obstacle[test_point] < obstacle[point])) gabarit_obstacle[point] = obstacle[test_point]; break;
dima285 23:bc05a104be10 87 case 2: if((obstacle[test_point] < 30) && (obstacle[test_point] < obstacle[point])) gabarit_obstacle[point] = 0.85 * obstacle[test_point];break;
dima285 23:bc05a104be10 88 case 3: if((obstacle[test_point] < 20) && (obstacle[test_point] < obstacle[point])) gabarit_obstacle[point] = 0.7 * obstacle[test_point];break;
dima285 23:bc05a104be10 89 case 4: if((obstacle[test_point] < 18) && (obstacle[test_point] < obstacle[point])) gabarit_obstacle[point] = 0.5 * obstacle[test_point];break;
dima285 23:bc05a104be10 90 }
dima285 23:bc05a104be10 91 }
dima285 23:bc05a104be10 92 }
dima285 23:bc05a104be10 93 /*for(int i = 0;i <= 12;i++){
dima285 23:bc05a104be10 94 obstacle[i] = corrected_obstacle[i];
dima285 23:bc05a104be10 95 }*/
dima285 23:bc05a104be10 96 for(int point = 0;point <= 12;point ++){
dima285 23:bc05a104be10 97 corrected_obstacle[point] = 0;
dima285 23:bc05a104be10 98 float tm = 0;
dima285 23:bc05a104be10 99 for(int test_point = 0;test_point <= 12;test_point ++){
dima285 23:bc05a104be10 100 corrected_obstacle[point] += gabarit_obstacle[test_point]/(abs(point-test_point)+1);
dima285 23:bc05a104be10 101 tm += 1.0/(abs(point-test_point)+1);
dima285 23:bc05a104be10 102 }
dima285 23:bc05a104be10 103 corrected_obstacle[point] /= tm;
dima285 23:bc05a104be10 104 }
dima285 23:bc05a104be10 105 }
dima285 23:bc05a104be10 106
dima285 23:bc05a104be10 107 void correct_obstacle_360(){ //gabariti
dima285 23:bc05a104be10 108 int tmtp;
dima285 23:bc05a104be10 109 for(int point = 0;point <= 71;point ++){
dima285 23:bc05a104be10 110 gabarit_obstacle[point] = obstacle[point];
dima285 23:bc05a104be10 111 }
dima285 23:bc05a104be10 112 for(int point = 0;point <= 71;point ++){
dima285 23:bc05a104be10 113 for(int test_point = point - 12;test_point <= point + 12 ;test_point ++){
dima285 23:bc05a104be10 114 if (test_point < 0) tmtp = 72 + test_point; else tmtp = test_point;
dima285 23:bc05a104be10 115 if((obstacle[tmtp] < (half_axis + 0.05) * 100 / sin(abs(tmtp - point) * deg5_rad) && (obstacle[tmtp] < obstacle[point]))){
dima285 23:bc05a104be10 116 gabarit_obstacle[point] = cos(abs(tmtp - point) * deg5_rad) * obstacle[tmtp];
dima285 23:bc05a104be10 117 }
dima285 23:bc05a104be10 118 }
dima285 23:bc05a104be10 119 }
dima285 23:bc05a104be10 120
dima285 23:bc05a104be10 121 for(int point = 0;point <= 71;point ++){
dima285 23:bc05a104be10 122 corrected_obstacle[point] = 0;
dima285 23:bc05a104be10 123 float tm = 0;
dima285 23:bc05a104be10 124 for(int test_point = 0;test_point <= 71;test_point ++){
dima285 23:bc05a104be10 125 corrected_obstacle[point] += gabarit_obstacle[test_point]/(abs(point-test_point)+1);
dima285 23:bc05a104be10 126 tm += 1.0/(abs(point-test_point)+1);
dima285 23:bc05a104be10 127 }
dima285 23:bc05a104be10 128 corrected_obstacle[point] /= tm;
dima285 23:bc05a104be10 129 }
dima285 23:bc05a104be10 130 }
dima285 23:bc05a104be10 131
dima285 23:bc05a104be10 132
dima285 23:bc05a104be10 133 void analyze_obstacle(){
dima285 23:bc05a104be10 134 min_dist = 400; max_dist = 0; front_dist = corrected_obstacle[6];
dima285 23:bc05a104be10 135 for(int point = 2;point <= 10;point ++){
dima285 23:bc05a104be10 136 if((corrected_obstacle[point]<min_dist) && (corrected_obstacle[point] > 0)) {min_dist=corrected_obstacle[point];min_dist_angle = point*15-90;}
dima285 23:bc05a104be10 137 if(corrected_obstacle[point]>max_dist) {max_dist=corrected_obstacle[point];max_dist_angle = point*15-90;}
dima285 23:bc05a104be10 138 }
dima285 23:bc05a104be10 139 if (max_dist > 300) max_dist = 300;
dima285 23:bc05a104be10 140 if (min_dist > 10) {
dima285 23:bc05a104be10 141 float tm = float(max_dist - 30) / 100; //if (tm > 0.5) tm = 0.5;
dima285 23:bc05a104be10 142 target.azimuth = current.azimuth + (max_dist_angle)/57.3; if (current.azimuth > pi) current.azimuth -= 2*pi; if (current.azimuth < -pi) current.azimuth += 2*pi;
dima285 23:bc05a104be10 143 target.x = current.x + cos(target.azimuth) * tm;//(max_dist - 200);
dima285 23:bc05a104be10 144 target.y = current.y + sin(target.azimuth) * tm;//(max_dist - 200);
dima285 23:bc05a104be10 145 target.path = current.path + tm;
dima285 23:bc05a104be10 146 timeout_counter = 250;
dima285 23:bc05a104be10 147 }
dima285 23:bc05a104be10 148 else if(min_dist > 0){
dima285 23:bc05a104be10 149 target.azimuth = current.azimuth + (min_dist_angle-15)/57.3 ; if (current.azimuth > pi) current.azimuth -= 2*pi; if (current.azimuth < -pi) current.azimuth += 2*pi;
dima285 23:bc05a104be10 150 target.x = current.x - 0.01*(cos(target.azimuth) * 20);//(max_dist - 200);
dima285 23:bc05a104be10 151 target.y = current.y - 0.01*(sin(target.azimuth) * 20);//(max_dist - 200);
dima285 23:bc05a104be10 152 target.path = current.path - 0.2;
dima285 23:bc05a104be10 153 timeout_counter = 250;
dima285 23:bc05a104be10 154 }
dima285 23:bc05a104be10 155 delta.path = target.path - current.path;
dima285 23:bc05a104be10 156 delta.azimuth = target.azimuth - current.azimuth; if(delta.azimuth > pi) delta.azimuth -= 2*pi; if(delta.azimuth < -pi) delta.azimuth += 2*pi;
dima285 23:bc05a104be10 157 delta.x = target.x - current.x;
dima285 23:bc05a104be10 158 delta.y = target.y - current.y;
dima285 23:bc05a104be10 159 if (delta.path > 0) target.dir = 1; else target.dir = 0;
dima285 23:bc05a104be10 160 //wifi.printf("!***==========***!\r\n");
dima285 23:bc05a104be10 161 }
dima285 23:bc05a104be10 162
dima285 23:bc05a104be10 163 void analize_obstacle_360(){
dima285 23:bc05a104be10 164 min_dist = 400; max_dist = 0; front_dist = corrected_obstacle[0];
dima285 23:bc05a104be10 165 for(int point = 0;point <= 71;point ++){
dima285 23:bc05a104be10 166 if((corrected_obstacle[point] < min_dist) && (corrected_obstacle[point] > 0)) {min_dist = corrected_obstacle[point]; min_dist_angle = array_360_rad[point];}
dima285 23:bc05a104be10 167 if(corrected_obstacle[point] > max_dist) {max_dist=corrected_obstacle[point]; max_dist_angle = array_360_rad[point];}
dima285 23:bc05a104be10 168 }
dima285 23:bc05a104be10 169 if (max_dist < 30){/*vse ploho*/}
dima285 23:bc05a104be10 170 target.azimuth = max_dist_angle;
dima285 23:bc05a104be10 171 target.path = current.path + (max_dist - 30) / 100.0;
dima285 23:bc05a104be10 172 }
dima285 17:bd6b6ac89e0e 173
dima285 23:bc05a104be10 174
dima285 23:bc05a104be10 175 void rt_scan(){ //realtime scan
dima285 23:bc05a104be10 176 int tm_echo;
dima285 23:bc05a104be10 177 int tm_glaz;
dima285 23:bc05a104be10 178
dima285 23:bc05a104be10 179 tm_glaz = glaz.read()/10;//cm //read glaz
dima285 23:bc05a104be10 180 glaz.stopContinuous();
dima285 23:bc05a104be10 181 tm_echo = echo_cm; //read echo
dima285 23:bc05a104be10 182 if ((tm_glaz > tm_echo) && (tm_echo < 100)) //echo/glaz selection
dima285 23:bc05a104be10 183 current.echo_cm = tm_echo;
dima285 23:bc05a104be10 184 else current.echo_cm = tm_glaz;
dima285 23:bc05a104be10 185
dima285 23:bc05a104be10 186 glaz.startContinuous(100); //start new measurement
dima285 23:bc05a104be10 187 echo_start();
Stas285 1:e2a6e523bf1f 188
dima285 23:bc05a104be10 189 switch (motion_mode){ //set new coord
dima285 23:bc05a104be10 190 case STOP:{break;}
dima285 23:bc05a104be10 191 case GO:{break;}
dima285 23:bc05a104be10 192 case ROTATE:{ //SCAN 360
dima285 23:bc05a104be10 193 if (scan_360_flag == 1) {
dima285 23:bc05a104be10 194 array_360_cm[scan_360_counter] = current.echo_cm; //save current data
dima285 23:bc05a104be10 195 array_360_rad[scan_360_counter] = current.azimuth;
dima285 23:bc05a104be10 196 target.azimuth = current.azimuth + deg5_rad; //set new azimuth
dima285 23:bc05a104be10 197 wifi.putc(current.echo_cm/2);
dima285 23:bc05a104be10 198 wifi.putc(current.azimuth * pi2_byte);
dima285 23:bc05a104be10 199 if (scan_360_counter++ > 70) { //increment counter
dima285 23:bc05a104be10 200 scan_360_counter = 0; //finish scan
dima285 23:bc05a104be10 201 scan_360_flag = 0;
dima285 23:bc05a104be10 202 correct_obstacle_360();
dima285 23:bc05a104be10 203 analize_obstacle_360();
dima285 23:bc05a104be10 204 }
dima285 23:bc05a104be10 205
dima285 23:bc05a104be10 206 }
dima285 23:bc05a104be10 207 break;}
dima285 23:bc05a104be10 208 }
dima285 17:bd6b6ac89e0e 209 }
dima285 17:bd6b6ac89e0e 210
dima285 17:bd6b6ac89e0e 211 void glaz_init(){
dima285 17:bd6b6ac89e0e 212 glaz.setTimeout(500);
dima285 17:bd6b6ac89e0e 213 if (!glaz.init())
dima285 17:bd6b6ac89e0e 214 {
dima285 17:bd6b6ac89e0e 215 wifi.printf("Failed to detect and initialize sensor!");
dima285 17:bd6b6ac89e0e 216 }
dima285 17:bd6b6ac89e0e 217 glaz.setDistanceMode(VL53L1X::Long);
dima285 17:bd6b6ac89e0e 218 glaz.setMeasurementTimingBudget(50000);
dima285 19:2fe650d29823 219 //glaz.startContinuous(50);
Stas285 0:e9488589a8ee 220 }