Diff: source/Positioning.cpp

Revision:

72:4e8a151d804e

Parent:

71:ddf4eb5c3081

Child:

79:92b9d083322d

--- a/source/Positioning.cpp	Thu Apr 20 11:37:24 2017 +0000
+++ b/source/Positioning.cpp	Fri Apr 21 12:12:57 2017 +0000
@@ -84,10 +84,13 @@
         last_dist_r = 100;
         last_dist_l = 100;
 
-        deg_r = -50;
-        deg_l = 50;
+  //      deg_r = -50;
+    //    deg_l = 50;
+        
+        deg_r = 0;
+        deg_l = 0;
 
-        set_servo_position(0,deg_l);    //servo sensor left
+        set_servo_position(0,-deg_l);    //servo sensor left
         set_servo_position(2,-deg_r);   //servo sensor right
         current_coord.x = 0;
         current_coord.y = 0;
@@ -96,13 +99,17 @@
     } else {
 
         if(t2 > 0.2f) {
+            
             t2.reset();
-            float dist_r = getDistanceIR(4);
-            float dist_l = getDistanceIR(0);
+            float dist_r = getDistanceIR(0);
+            float dist_l = getDistanceIR(4);
+            printf("distances %f || %f\r\n",dist_l, dist_r);
 
             //right
             if(dist_r < last_dist_r) {
                 last_dist_r = dist_r;
+                
+                
                 deg_r += 5;
                 set_servo_position(2,-deg_r);
             } else if(direction_r == 0) {
@@ -121,13 +128,12 @@
                 while (direction_r >= 360) direction_r -= 360;
                 while (direction_r < 0) direction_r += 360;
             }
-
-
+/*
             //left
             if(dist_l < last_dist_l) {
                 last_dist_l = dist_l;
                 deg_l -= 5;
-                set_servo_position(0,deg_l);
+                set_servo_position(0,-deg_l);
             } else if(direction_l == 0) {
 
                 dist_r *= 100;
@@ -155,7 +161,7 @@
                 //finished
                 return 17;
 
-            }
+            }*/
         }
     }
     // not finished