Diff: main.cpp

Revision:

76:a0c09fd62be1

Parent:

74:88be86f83d17

Child:

78:7c7cefbe1772

--- a/main.cpp	Mon Apr 13 17:14:24 2015 +0000
+++ b/main.cpp	Mon Apr 13 17:34:50 2015 +0000
@@ -90,7 +90,7 @@
     
     #ifdef PLAN_A
         Asserv<float> instanceAsserv(&odometry);
-        instanceAsserv.setGoal( (float)200,(float)0, (float)0);
+        instanceAsserv.setGoal(0.0,0.0,0);
         
         char state = 0;
         
@@ -111,7 +111,27 @@
             motorR.setSpeed(0.08+(phi_r/phi_max));
             motorL.setSpeed(0.06+(phi_l/phi_max));
             
-            if(state == 0 && instanceAsserv.
+            if(state == 0 && instanceAsserv.isArrivedRho())
+            {
+                state = 1;
+                logger.printf("Arrive en 0,0 !!!!!");
+                wait(5);
+                instanceAsserv.setGoal(200.0,200.0,0);
+            }
+            else if(state == 1 && instanceAsserv.isArrivedRho())
+            {
+                state = 2;
+                logger.printf("Arrive en 200,200 !!!!!");
+                wait(5);
+                instanceAsserv.setGoal(0.0,200.0,0);
+            }
+            else if(state == 2 && instanceAsserv.isArrivedRho())
+            {
+                state = 0;
+                logger.printf("Arrive en 0,200 !!!!!");
+                wait(5);
+                instanceAsserv.setGoal(0.0,0.0,0);
+            }
             
         }
     #else