Diff: motion.h

Branch:

svoe

Revision:

22:14e85f2068c7

Parent:

20:e73f49ba5001

Child:

23:bc05a104be10

--- a/motion.h	Sat Mar 23 16:39:46 2019 +0000
+++ b/motion.h	Tue Jul 23 12:55:23 2019 +0000
@@ -5,6 +5,7 @@
 bool tupic = 0;
 //int corrected_obstacle [100];
 bool infinite_flag = 0;
+int fail_counter = 0;
 
 /*void go(float distance_m, bool brake = 1){
     target.path = current.path + distance_m; 
@@ -14,23 +15,32 @@
 //        wifi.printf("%.2f %.2f %.2f %.2f;",target_path,current_path,target_path,current_speed);}
     }*/
     
-void correct_obstacle(){
+void correct_obstacle(){ //gabariti
     for(int point = 0;point <= 12;point ++){
-        corrected_obstacle[point] = obstacle[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])) corrected_obstacle[point] = obstacle[test_point]; break;
-                case 2: if((obstacle[test_point] < 30) && (obstacle[test_point] < obstacle[point])) corrected_obstacle[point] = 0.85 * obstacle[test_point];break;
-                case 3: if((obstacle[test_point] < 20) && (obstacle[test_point] < obstacle[point])) corrected_obstacle[point] = 0.7 * obstacle[test_point];break;
-                case 4: if((obstacle[test_point] < 18) && (obstacle[test_point] < obstacle[point])) corrected_obstacle[point] = 0.5 * obstacle[test_point];break;
+                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;
+        }
     }
     
     
@@ -40,25 +50,28 @@
         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 = max_dist - 30; if (tm > 0.5) tm = 0.5;
+        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");
+    //wifi.printf("!***==========***!\r\n");
 }   
     
 void vstavai(){
@@ -72,7 +85,56 @@
     set_motor_speed(0,0);
     target.x = current.x;
     target.y = current.y;
+    target.path = current.path;
     target.azimuth = current.azimuth;
+    fail_counter++;
+    }
+    
+void free_walk(){
+    if (motion_mode == GO){
+        if (timeout_counter-- == 0) {
+            target.path = current.path - 0.2;
+            timeout_counter = 250;
+            }
+        }
+        
+    if (delay_counter-- == 3) {  
+            glaz.startContinuous(100);
+            echo_start();
+            }
+        if (delay_counter == 0) {
+            if(motion_mode > STOP) {if(coord_ready) {motion_mode++;if (motion_mode > GO) motion_mode = STOP; delay2_counter = 15;}}
+            else{
+                if (delay2_counter > 0) delay2_counter--;//delay before full stop and scan
+                else{   
+                    obstacle_glaz[serva_counter] = glaz.read()/10;//cm //Function
+                    glaz.stopContinuous();
+                    obstacle_echo[serva_counter] = echo_cm;
+                    if ((obstacle_glaz[serva_counter] > obstacle_echo[serva_counter]) && (obstacle_echo[serva_counter] < 100))
+                        obstacle[serva_counter] = obstacle_echo[serva_counter]; 
+                    else obstacle[serva_counter] = obstacle_glaz[serva_counter];
+                    if(serva_counter < 12)serva(-90 + (serva_counter++)*15); 
+                    else{
+                        serva_counter = 0; 
+                        serva(-90);
+                        correct_obstacle();
+                        analyze_obstacle();
+                        echo_transmit(13);
+                        //wifi.putc(current.x * 10); wifi.putc(current.y * 10); wifi.putc(current.azimuth * 40); wifi.putc(target.x * 10); wifi.putc(target.y * 10); wifi.putc(target.azimuth * 40);             
+                        motion_mode = ROTATE;
+                        //motion_timeout = 250; 
+                        //wait(2);
+                        }    
+                    }
+                }
+            delay_counter = 7;
+            }
+    
+    }
+    
+void motion_1D(){
+    static int k;
+    if (k++ > 2400) k = 0; else { target.path = traj_1D[int(k/200)];}
     }
     
     /*if (max_dist < 0.3) tupic = 1;