Diff: motion.h

Branch:

svoe

Revision:

23:bc05a104be10

Parent:

22:14e85f2068c7

--- a/motion.h	Tue Jul 23 12:55:23 2019 +0000
+++ b/motion.h	Tue Aug 06 14:13:55 2019 +0000
@@ -7,6 +7,8 @@
 bool infinite_flag = 0;
 int fail_counter = 0;
 
+
+
 /*void go(float distance_m, bool brake = 1){
     target.path = current.path + distance_m; 
 //    wifi.printf("%.2f %.2f %.2f %.2f;",target_path,current_path,distance_m,current_speed);
@@ -15,64 +17,7 @@
 //        wifi.printf("%.2f %.2f %.2f %.2f;",target_path,current_path,target_path,current_speed);}
     }*/
     
-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 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 vstavai(){
     //motor_speed[0] = 0;
@@ -91,7 +36,30 @@
     }
     
 void free_walk(){
-    if (motion_mode == GO){
+    
+    switch (motion_mode){
+        case ROTATE:{
+            if(coord_ready && !scan_360_flag) motion_mode = GO;
+            break;}
+        case GO:{
+            if (timeout_counter-- == 0) {
+                target.path = current.path - 0.2;
+                timeout_counter = 250;
+            }
+            if (current.echo_cm < 30) target.path = current.path + current.echo_cm/100.0 - 0.15;
+            if (coord_ready) motion_mode = STOP;
+            break;}
+        case STOP:{
+            wifi.putc(current.x * 20); wifi.putc(current.y * 20);
+            scan_360_counter = 0;
+            scan_360_flag = 1;
+            motion_mode = ROTATE;
+            break;}
+        }
+    
+    
+    
+    /*if (motion_mode == GO){
         if (timeout_counter-- == 0) {
             target.path = current.path - 0.2;
             timeout_counter = 250;
@@ -128,7 +96,7 @@
                     }
                 }
             delay_counter = 7;
-            }
+            }*/
     
     }