Diff: main.cpp

Revision:

95:33520ead3b94

Parent:

92:b403f2724252

--- a/main.cpp	Wed Apr 29 16:13:53 2015 +0000
+++ b/main.cpp	Thu Apr 30 16:39:35 2015 +0000
@@ -85,7 +85,7 @@
     #ifdef PLAN_A
         Asserv<float> instanceAsserv(&odometry);
         //instanceAsserv.setGoal((float)0.0,(float)0.0,(float)0);
-        instanceAsserv.setGoal(300.0f,200.0f,0.0f);
+        instanceAsserv.setGoal((float)0.0f,(float)0.0f,(float)0.0f);
         logger.printf("GOAL SET... RUNNING!\r\n");
         
         char state = 0;
@@ -97,6 +97,7 @@
         {
             float dt = t.read();
             t.reset();
+            t.start();
             
             odometry.update(dt);
             instanceAsserv.update(dt);
@@ -104,33 +105,50 @@
             float phi_l = (float)instanceAsserv.getPhiL();
             float phi_max = (float)instanceAsserv.getPhiMax();
                         
+            logger.printf("STATE = %d \r\n", state);
             
-            motorR.setSpeed(0.08+(phi_r/phi_max));
-            motorL.setSpeed(0.08+(phi_l/phi_max));
-            #define test
-            #ifdef test            
-                if(state == 0 && instanceAsserv.isArrivedRho())
-                {
-                    state = 1;
-                    logger.printf("Arrive en 0,0 !!!!!\r\n");
-                    wait(5);
-                    instanceAsserv.setGoal(300.0f,200.0f,0.0f);
-                }
-                else if(state == 1 && instanceAsserv.isArrivedRho())
-                {
-                    state = 2;
-                    logger.printf("Arrive en 200,200 !!!!!\r\n");
-                    wait(5);
-                    instanceAsserv.setGoal(0.0f,300.0f,0.0f);
-                }
-                else if(state == 2 && instanceAsserv.isArrivedRho())
-                {
-                    state = 0;
-                    logger.printf("Arrive en 0,200 !!!!!\r\n");
-                    wait(5);
-                    instanceAsserv.setGoal(0.0f,0.0f,0.0f);
-                }
-            #endif
+            motorR.setSpeed( (phi_r > (float)0? (float)+0.08f : (float)-0.08f) +(phi_r/phi_max));
+            motorL.setSpeed( (phi_l > (float)0? (float)+0.08f : (float)-0.08f) +(phi_l/phi_max));
+#define test
+#ifdef test            
+            if(state == 0 && instanceAsserv.isArrivedRho())
+            {
+                state = 1;
+                logger.printf("Arrive en 0,0 !!!!!\r\n");
+                motorR.setSpeed( (float)0);
+                motorL.setSpeed( (float)0);
+                wait(5);
+                instanceAsserv.setGoal((float)300.0f,(float)0.0f,(float)0.0f);
+            }
+            else if(state == 1 && instanceAsserv.isArrivedRho())
+            {
+                state = 2;
+                logger.printf("Arrive en 200,0 !!!!!\r\n");
+                motorR.setSpeed( (float)0);
+                motorL.setSpeed( (float)0);
+                wait(5);
+                instanceAsserv.setGoal((float)200.0f,(float)200.0f,(float)0.0f);
+            }
+            else if(state == 2 && instanceAsserv.isArrivedRho())
+            {
+                state = 3;
+                logger.printf("Arrive en 200,200 !!!!!\r\n");
+                motorR.setSpeed( (float)0);
+                motorL.setSpeed( (float)0);
+                wait(5);
+                instanceAsserv.setGoal((float)0.0f,(float)200.0f,(float)0.0f);
+            }            
+            else if(state == 3 && instanceAsserv.isArrivedRho())
+            {
+                state = 0;
+                logger.printf("Arrive en 0,200 !!!!!\r\n");
+                motorR.setSpeed( (float)0);
+                motorL.setSpeed( (float)0);
+                wait(5);
+                instanceAsserv.setGoal((float)0.0f,(float)0.0f,(float)0.0f);
+            }
+#endif
+            
         }
     #else
         aserv_planB asserv(odometry,motorL,motorR);