Diff: main.cpp

Revision:

20:6a9062d54eb0

Parent:

19:dbc5fbad4975

Parent:

17:caf393b63e27

Child:

21:62154d644531

--- a/main.cpp	Mon Apr 03 17:01:13 2017 +0000
+++ b/main.cpp	Mon Apr 03 17:07:37 2017 +0000
@@ -22,7 +22,7 @@
 float temp;
 float d2;
 
-bool tooClose = false;
+//bool tooClose = false;
 
 int leftMm;
 int frontMm;
@@ -34,11 +34,16 @@
 
 int speed=999; // Max speed at beggining of movement
 
-//Target example x,y values
+//target exemple x y theta
 float target_x=46.8, target_y=78.6, target_angle=1.57;
 
+
 //Timeout time;
 int main(){
+    target_x=200*rand();
+    target_y=200*rand();
+    target_angle=3.1416*2*rand()-3.1416;
+    
     i2c1.frequency(100000);
     initRobot(); //Initializing the robot
     pc.baud(9600); // baud for the pc communication
@@ -61,12 +66,10 @@
     /*alpha = atan2((target_y-Y),(target_x-X))-theta;
     alpha = atan(sin(alpha)/cos(alpha));
     rho = dist(X, Y, target_x, target_y);
-    
     beta = -alpha-theta+target_angle;*/
     for (int i = 0; i<10; i++) {
         randomizeAndMap();
     }
-
     //Stop at the end
     leftMotor(1,0);
     rightMotor(1,0);
@@ -124,14 +127,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);
@@ -185,7 +188,7 @@
         wait(0.2);
         //Timer stuff
         t.stop();
-    } while(d2>1 & !tooClose);
+    } while(d2>1);
     
     if (tooClose) {
         computeObstacle();