svoe
Dependencies: mbed mbed-STM32F103C8T6 MPU6050_1
echo.h
- Committer:
- dima285
- Date:
- 2019-08-06
- Branch:
- svoe
- Revision:
- 23:bc05a104be10
- Parent:
- 22:14e85f2068c7
File content as of revision 23:bc05a104be10:
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 gabarit_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((gabarit_obstacle[tmstep] < 500) ? gabarit_obstacle[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 transmit_360(){ } 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 correct_obstacle(){ //gabariti for(int point = 0;point <= 12;point ++){ gabarit_obstacle[point] = obstacle[point]; } for(int point = 0;point <= 12;point ++){ for(int test_point = 0;test_point <= 12;test_point ++){ switch(abs(test_point - point)){ case 1: if((obstacle[test_point] < 60) && (obstacle[test_point] < obstacle[point])) gabarit_obstacle[point] = obstacle[test_point]; break; case 2: if((obstacle[test_point] < 30) && (obstacle[test_point] < obstacle[point])) gabarit_obstacle[point] = 0.85 * obstacle[test_point];break; case 3: if((obstacle[test_point] < 20) && (obstacle[test_point] < obstacle[point])) gabarit_obstacle[point] = 0.7 * obstacle[test_point];break; case 4: if((obstacle[test_point] < 18) && (obstacle[test_point] < obstacle[point])) gabarit_obstacle[point] = 0.5 * obstacle[test_point];break; } } } /*for(int i = 0;i <= 12;i++){ obstacle[i] = corrected_obstacle[i]; }*/ for(int point = 0;point <= 12;point ++){ corrected_obstacle[point] = 0; float tm = 0; for(int test_point = 0;test_point <= 12;test_point ++){ corrected_obstacle[point] += gabarit_obstacle[test_point]/(abs(point-test_point)+1); tm += 1.0/(abs(point-test_point)+1); } corrected_obstacle[point] /= tm; } } void correct_obstacle_360(){ //gabariti int tmtp; for(int point = 0;point <= 71;point ++){ gabarit_obstacle[point] = obstacle[point]; } for(int point = 0;point <= 71;point ++){ for(int test_point = point - 12;test_point <= point + 12 ;test_point ++){ if (test_point < 0) tmtp = 72 + test_point; else tmtp = test_point; if((obstacle[tmtp] < (half_axis + 0.05) * 100 / sin(abs(tmtp - point) * deg5_rad) && (obstacle[tmtp] < obstacle[point]))){ gabarit_obstacle[point] = cos(abs(tmtp - point) * deg5_rad) * obstacle[tmtp]; } } } for(int point = 0;point <= 71;point ++){ corrected_obstacle[point] = 0; float tm = 0; for(int test_point = 0;test_point <= 71;test_point ++){ corrected_obstacle[point] += gabarit_obstacle[test_point]/(abs(point-test_point)+1); tm += 1.0/(abs(point-test_point)+1); } corrected_obstacle[point] /= tm; } } void analyze_obstacle(){ min_dist = 400; max_dist = 0; front_dist = corrected_obstacle[6]; for(int point = 2;point <= 10;point ++){ if((corrected_obstacle[point]<min_dist) && (corrected_obstacle[point] > 0)) {min_dist=corrected_obstacle[point];min_dist_angle = point*15-90;} if(corrected_obstacle[point]>max_dist) {max_dist=corrected_obstacle[point];max_dist_angle = point*15-90;} } if (max_dist > 300) max_dist = 300; if (min_dist > 10) { float tm = float(max_dist - 30) / 100; //if (tm > 0.5) tm = 0.5; 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; target.x = current.x + cos(target.azimuth) * tm;//(max_dist - 200); target.y = current.y + sin(target.azimuth) * tm;//(max_dist - 200); target.path = current.path + tm; timeout_counter = 250; } else if(min_dist > 0){ 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; target.x = current.x - 0.01*(cos(target.azimuth) * 20);//(max_dist - 200); target.y = current.y - 0.01*(sin(target.azimuth) * 20);//(max_dist - 200); target.path = current.path - 0.2; timeout_counter = 250; } delta.path = target.path - current.path; delta.azimuth = target.azimuth - current.azimuth; if(delta.azimuth > pi) delta.azimuth -= 2*pi; if(delta.azimuth < -pi) delta.azimuth += 2*pi; delta.x = target.x - current.x; delta.y = target.y - current.y; if (delta.path > 0) target.dir = 1; else target.dir = 0; //wifi.printf("!***==========***!\r\n"); } void analize_obstacle_360(){ min_dist = 400; max_dist = 0; front_dist = corrected_obstacle[0]; for(int point = 0;point <= 71;point ++){ if((corrected_obstacle[point] < min_dist) && (corrected_obstacle[point] > 0)) {min_dist = corrected_obstacle[point]; min_dist_angle = array_360_rad[point];} if(corrected_obstacle[point] > max_dist) {max_dist=corrected_obstacle[point]; max_dist_angle = array_360_rad[point];} } if (max_dist < 30){/*vse ploho*/} target.azimuth = max_dist_angle; target.path = current.path + (max_dist - 30) / 100.0; } void rt_scan(){ //realtime scan int tm_echo; int tm_glaz; tm_glaz = glaz.read()/10;//cm //read glaz glaz.stopContinuous(); tm_echo = echo_cm; //read echo if ((tm_glaz > tm_echo) && (tm_echo < 100)) //echo/glaz selection current.echo_cm = tm_echo; else current.echo_cm = tm_glaz; glaz.startContinuous(100); //start new measurement echo_start(); switch (motion_mode){ //set new coord case STOP:{break;} case GO:{break;} case ROTATE:{ //SCAN 360 if (scan_360_flag == 1) { array_360_cm[scan_360_counter] = current.echo_cm; //save current data array_360_rad[scan_360_counter] = current.azimuth; target.azimuth = current.azimuth + deg5_rad; //set new azimuth wifi.putc(current.echo_cm/2); wifi.putc(current.azimuth * pi2_byte); if (scan_360_counter++ > 70) { //increment counter scan_360_counter = 0; //finish scan scan_360_flag = 0; correct_obstacle_360(); analize_obstacle_360(); } } break;} } } 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); }