Diff: main.cpp

Revision:

14:d58f2bdbf42e

Parent:

13:41f75c132135

Child:

16:ff73cc7b3156

--- a/main.cpp	Mon Mar 27 16:42:53 2017 +0000
+++ b/main.cpp	Mon Mar 27 16:49:49 2017 +0000
@@ -19,7 +19,7 @@
 float temp;
 float d2;
 
-bool tooClose = false;
+//bool tooClose = false;
 
 int leftMm;
 int frontMm;
@@ -85,14 +85,14 @@
         t.reset();
         t.start();
         
+        //Updating X,Y and theta with the odometry values
+        Odometria();
+        
         updateSonarValues();
         if (leftMm < 100 || frontMm < 100 || rightMm < 100) {
-            tooClose = true;    
+            break;    
         }
         
-        //Updating X,Y and theta with the odometry values
-        Odometria();
-        
         alpha = atan2((target_y-Y),(target_x-X))-theta;
         alpha = atan(sin(alpha)/cos(alpha));
         rho = dist(X, Y, target_x, target_y);
@@ -145,7 +145,7 @@
         wait(0.2);
         //Timer stuff
         t.stop();
-    } while(d2>1 & !tooClose);
+    } while(d2>1);
     
     return 0;
 }
\ No newline at end of file