svoe
Dependencies: mbed mbed-STM32F103C8T6 MPU6050_1
Diff: echo.h
- Branch:
- svoe
- Revision:
- 23:bc05a104be10
- Parent:
- 22:14e85f2068c7
diff -r 14e85f2068c7 -r bc05a104be10 echo.h --- a/echo.h Tue Jul 23 12:55:23 2019 +0000 +++ b/echo.h Tue Aug 06 14:13:55 2019 +0000 @@ -15,6 +15,7 @@ int max_dist_angle; int front_dist; + Timer echo_timer; InterruptIn echo(PB_12); DigitalOut sonar_triger(PB_13); @@ -48,6 +49,10 @@ //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); @@ -71,9 +76,136 @@ 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 echo_full_scan(int steps){ //scan by wheels (calculate speed) + +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(){