Dima Dudin
/
DD_Robot
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Dependencies: mbed mbed-STM32F103C8T6 MPU6050_1
Embed:
(wiki syntax)
Show/hide line numbers
echo.h
00001 int obstacle_glaz[100];//13 steps [-90;+90] by 15 deg 00002 int obstacle_echo[100];//13 steps [-90;+90] by 15 deg 00003 int obstacle[100];//13 steps [-90;+90] by 15 deg 00004 int corrected_obstacle [100]; 00005 int gabarit_obstacle [100]; 00006 int echo_current_step;//[0;36], (step=18) => 0 deg 00007 int echo_old_step; 00008 int echo_old_old_step; 00009 int echo_current_dir = -1; 00010 int echo_cm; 00011 float tmf; 00012 int min_dist; 00013 int min_dist_angle; 00014 int max_dist; 00015 int max_dist_angle; 00016 int front_dist; 00017 00018 00019 Timer echo_timer; 00020 InterruptIn echo(PB_12); 00021 DigitalOut sonar_triger(PB_13); 00022 00023 void echo_begin(){ //begin pulsewidth measurement 00024 echo_timer.reset(); 00025 } 00026 00027 void echo_end(){ //end pulsewidth measurement 00028 echo_cm = echo_timer.read_us()/58; 00029 } 00030 00031 void echo_start(){ 00032 sonar_triger = 1; 00033 tmf=log(1.0);//delay 5us 00034 tmf=log(2.0);//delay 5us 00035 sonar_triger = 0;//sonar start 00036 } 00037 00038 void echo_init(){ 00039 echo.rise(&echo_begin); 00040 echo.fall(&echo_end); 00041 echo_timer.start(); 00042 } 00043 00044 void echo_transmit(int steps_number) { //printf is not recommended for interrupts 00045 int tmstep; 00046 wifi.putc(0xff); //sync symbol 00047 for(tmstep = 0; tmstep < steps_number; tmstep ++){wifi.putc((gabarit_obstacle[tmstep] < 500) ? gabarit_obstacle[tmstep]/2 : 250);} 00048 //for(tmstep = 0; tmstep < steps_number; tmstep ++){wifi.putc((obstacle_echo[tmstep] < 500) ? obstacle_echo[tmstep]/2 : 250);} 00049 //for(tmstep = 0; tmstep < steps_number; tmstep ++){wifi.putc((obstacle[tmstep] < 500) ? obstacle[tmstep]/2 : 250);} 00050 for(tmstep = 0; tmstep < steps_number; tmstep ++){wifi.putc((corrected_obstacle[tmstep] < 500) ? corrected_obstacle[tmstep]/2 : 250);} 00051 } 00052 00053 void transmit_360(){ 00054 00055 } 00056 00057 void echo_scan(){ // rewrite with argument: n or step //will not work in interrupt ?? 00058 serva(-90); 00059 wait_ms(300); 00060 for(int point = 0;point <= 12;point ++){ 00061 serva(point*15-90); 00062 wait_ms(50); 00063 echo_start(); 00064 wait_ms(50); 00065 obstacle[point] = echo_cm;} 00066 } 00067 00068 void echo_step(bool transmit = 0){ 00069 obstacle[echo_old_old_step] = echo_cm; 00070 //echo_transmit(echo_current_step); 00071 echo_old_old_step = echo_old_step; 00072 echo_old_step = echo_current_step; 00073 echo_current_step += echo_current_dir; 00074 if (echo_current_step > 36) {echo_current_step = 35 ; echo_current_dir = -1; /*echo_transmit(37);*/} 00075 if (echo_current_step < 0) {echo_current_step = 1 ; echo_current_dir = 1; /*echo_transmit(37);*/} 00076 serva(echo_current_step*5-90); 00077 echo_start(); //the last to exit ticker interrupt ASAP 00078 } 00079 void correct_obstacle(){ //gabariti 00080 for(int point = 0;point <= 12;point ++){ 00081 gabarit_obstacle[point] = obstacle[point]; 00082 } 00083 for(int point = 0;point <= 12;point ++){ 00084 for(int test_point = 0;test_point <= 12;test_point ++){ 00085 switch(abs(test_point - point)){ 00086 case 1: if((obstacle[test_point] < 60) && (obstacle[test_point] < obstacle[point])) gabarit_obstacle[point] = obstacle[test_point]; break; 00087 case 2: if((obstacle[test_point] < 30) && (obstacle[test_point] < obstacle[point])) gabarit_obstacle[point] = 0.85 * obstacle[test_point];break; 00088 case 3: if((obstacle[test_point] < 20) && (obstacle[test_point] < obstacle[point])) gabarit_obstacle[point] = 0.7 * obstacle[test_point];break; 00089 case 4: if((obstacle[test_point] < 18) && (obstacle[test_point] < obstacle[point])) gabarit_obstacle[point] = 0.5 * obstacle[test_point];break; 00090 } 00091 } 00092 } 00093 /*for(int i = 0;i <= 12;i++){ 00094 obstacle[i] = corrected_obstacle[i]; 00095 }*/ 00096 for(int point = 0;point <= 12;point ++){ 00097 corrected_obstacle[point] = 0; 00098 float tm = 0; 00099 for(int test_point = 0;test_point <= 12;test_point ++){ 00100 corrected_obstacle[point] += gabarit_obstacle[test_point]/(abs(point-test_point)+1); 00101 tm += 1.0/(abs(point-test_point)+1); 00102 } 00103 corrected_obstacle[point] /= tm; 00104 } 00105 } 00106 00107 void correct_obstacle_360(){ //gabariti 00108 int tmtp; 00109 for(int point = 0;point <= 71;point ++){ 00110 gabarit_obstacle[point] = obstacle[point]; 00111 } 00112 for(int point = 0;point <= 71;point ++){ 00113 for(int test_point = point - 12;test_point <= point + 12 ;test_point ++){ 00114 if (test_point < 0) tmtp = 72 + test_point; else tmtp = test_point; 00115 if((obstacle[tmtp] < (half_axis + 0.05) * 100 / sin(abs(tmtp - point) * deg5_rad) && (obstacle[tmtp] < obstacle[point]))){ 00116 gabarit_obstacle[point] = cos(abs(tmtp - point) * deg5_rad) * obstacle[tmtp]; 00117 } 00118 } 00119 } 00120 00121 for(int point = 0;point <= 71;point ++){ 00122 corrected_obstacle[point] = 0; 00123 float tm = 0; 00124 for(int test_point = 0;test_point <= 71;test_point ++){ 00125 corrected_obstacle[point] += gabarit_obstacle[test_point]/(abs(point-test_point)+1); 00126 tm += 1.0/(abs(point-test_point)+1); 00127 } 00128 corrected_obstacle[point] /= tm; 00129 } 00130 } 00131 00132 00133 void analyze_obstacle(){ 00134 min_dist = 400; max_dist = 0; front_dist = corrected_obstacle[6]; 00135 for(int point = 2;point <= 10;point ++){ 00136 if((corrected_obstacle[point]<min_dist) && (corrected_obstacle[point] > 0)) {min_dist=corrected_obstacle[point];min_dist_angle = point*15-90;} 00137 if(corrected_obstacle[point]>max_dist) {max_dist=corrected_obstacle[point];max_dist_angle = point*15-90;} 00138 } 00139 if (max_dist > 300) max_dist = 300; 00140 if (min_dist > 10) { 00141 float tm = float(max_dist - 30) / 100; //if (tm > 0.5) tm = 0.5; 00142 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; 00143 target.x = current.x + cos(target.azimuth) * tm;//(max_dist - 200); 00144 target.y = current.y + sin(target.azimuth) * tm;//(max_dist - 200); 00145 target.path = current.path + tm; 00146 timeout_counter = 250; 00147 } 00148 else if(min_dist > 0){ 00149 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; 00150 target.x = current.x - 0.01*(cos(target.azimuth) * 20);//(max_dist - 200); 00151 target.y = current.y - 0.01*(sin(target.azimuth) * 20);//(max_dist - 200); 00152 target.path = current.path - 0.2; 00153 timeout_counter = 250; 00154 } 00155 delta.path = target.path - current.path; 00156 delta.azimuth = target.azimuth - current.azimuth; if(delta.azimuth > pi) delta.azimuth -= 2*pi; if(delta.azimuth < -pi) delta.azimuth += 2*pi; 00157 delta.x = target.x - current.x; 00158 delta.y = target.y - current.y; 00159 if (delta.path > 0) target.dir = 1; else target.dir = 0; 00160 //wifi.printf("!***==========***!\r\n"); 00161 } 00162 00163 void analize_obstacle_360(){ 00164 min_dist = 400; max_dist = 0; front_dist = corrected_obstacle[0]; 00165 for(int point = 0;point <= 71;point ++){ 00166 if((corrected_obstacle[point] < min_dist) && (corrected_obstacle[point] > 0)) {min_dist = corrected_obstacle[point]; min_dist_angle = array_360_rad[point];} 00167 if(corrected_obstacle[point] > max_dist) {max_dist=corrected_obstacle[point]; max_dist_angle = array_360_rad[point];} 00168 } 00169 if (max_dist < 30){/*vse ploho*/} 00170 target.azimuth = max_dist_angle; 00171 target.path = current.path + (max_dist - 30) / 100.0; 00172 } 00173 00174 00175 void rt_scan(){ //realtime scan 00176 int tm_echo; 00177 int tm_glaz; 00178 00179 tm_glaz = glaz.read()/10;//cm //read glaz 00180 glaz.stopContinuous(); 00181 tm_echo = echo_cm; //read echo 00182 if ((tm_glaz > tm_echo) && (tm_echo < 100)) //echo/glaz selection 00183 current.echo_cm = tm_echo; 00184 else current.echo_cm = tm_glaz; 00185 00186 glaz.startContinuous(100); //start new measurement 00187 echo_start(); 00188 00189 switch (motion_mode){ //set new coord 00190 case STOP:{break;} 00191 case GO:{break;} 00192 case ROTATE:{ //SCAN 360 00193 if (scan_360_flag == 1) { 00194 array_360_cm[scan_360_counter] = current.echo_cm; //save current data 00195 array_360_rad[scan_360_counter] = current.azimuth; 00196 target.azimuth = current.azimuth + deg5_rad; //set new azimuth 00197 wifi.putc(current.echo_cm/2); 00198 wifi.putc(current.azimuth * pi2_byte); 00199 if (scan_360_counter++ > 70) { //increment counter 00200 scan_360_counter = 0; //finish scan 00201 scan_360_flag = 0; 00202 correct_obstacle_360(); 00203 analize_obstacle_360(); 00204 } 00205 00206 } 00207 break;} 00208 } 00209 } 00210 00211 void glaz_init(){ 00212 glaz.setTimeout(500); 00213 if (!glaz.init()) 00214 { 00215 wifi.printf("Failed to detect and initialize sensor!"); 00216 } 00217 glaz.setDistanceMode(VL53L1X::Long); 00218 glaz.setMeasurementTimingBudget(50000); 00219 //glaz.startContinuous(50); 00220 }
Generated on Thu Jul 28 2022 12:08:13 by
1.7.2
