Diff: motion.h

Revision:

17:bd6b6ac89e0e

Parent:

15:960b922433d1

Child:

19:2fe650d29823

--- a/motion.h	Sat Dec 01 14:31:24 2018 +0000
+++ b/motion.h	Sun Feb 10 12:05:16 2019 +0000
@@ -2,7 +2,8 @@
 //float accel (defined in motor.h)
 float speed = 1;//cm/s
 float radius = 100;
-
+bool tupic = 0;
+//int corrected_obstacle [100];
 bool infinite_flag = 0;
 
 void go(float distance_m, bool brake = 1){
@@ -12,6 +13,41 @@
 //        gyro_process();
 //        wifi.printf("%.2f %.2f %.2f %.2f;",target_path,current_path,target_path,current_speed);}
     }
+    
+void correct_obstacle(){
+    for(int point = 1;point <= 12;point ++){
+        for(int test_point = 1;test_point <= 12;test_point ++){
+            switch(abs(test_point - point)){
+                case 1: if((obstacle[test_point] < 40) && (obstacle[test_point] < obstacle[point])) corrected_obstacle[point] = obstacle[test_point];break;
+                case 2: if((obstacle[test_point] < 20) && (obstacle[test_point] < obstacle[point])) corrected_obstacle[point] = 0.85 * obstacle[test_point];break;
+                case 3: if((obstacle[test_point] < 15) && (obstacle[test_point] < obstacle[point])) corrected_obstacle[point] = 0.7 * obstacle[test_point];break;
+                case 4: if((obstacle[test_point] < 12) && (obstacle[test_point] < obstacle[point])) corrected_obstacle[point] = 0.5 * obstacle[test_point];break;
+                }
+            }
+        }
+    /*for(int i = 0;i <= 12;i++){
+       obstacle[i] = corrected_obstacle[i];
+        }*/
+    }
+    
+    
+void analyze_obstacle(){
+    min_dist = 400; max_dist = 0; front_dist = obstacle[6];
+    for(int point = 0;point <= 12;point ++){
+        if(obstacle[point]<min_dist) {min_dist=obstacle[point];min_dist_angle = point*15-90;}
+        if(obstacle[point]>max_dist) {max_dist=obstacle[point];max_dist_angle = point*15-90;}
+        }
+    target.azimuth = current.azimuth + max_dist_angle/57.3;
+    target.x = current.x + 0.01*(cos(target.azimuth) * 25);//(max_dist - 200);
+    target.y = current.y + 0.01*(sin(target.azimuth) * 25);//(max_dist - 200);
+    /*if (max_dist < 0.3) tupic = 1;
+    if (tupic == 1) {
+        target.azimuth += pi * (rand() % 10 + 5)/10; if (target.azimuth > pi) target.azimuth -= pi;
+        target.x = current.x;
+        target.y = current.y;
+        }     */
+    }
+    
 /*    
 void go_no_wait(int cm, bool brake = 1){
     if(cm != 0) {