Diff: echo.h

Branch:

svoe

Revision:

23:bc05a104be10

Parent:

22:14e85f2068c7

--- 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(){