Diff: main.cpp

Revision:

90:294b16267109

Parent:

89:d05001d85a02

Child:

91:c0b436c52ede

--- a/main.cpp	Thu Apr 23 16:29:31 2015 +0000
+++ b/main.cpp	Thu Apr 23 18:20:35 2015 +0000
@@ -97,7 +97,8 @@
     
     #ifdef PLAN_A
         Asserv<float> instanceAsserv(&odometry);
-        instanceAsserv.setGoal((float)100.0,(float)-150.0,(float)PI/4);
+        //instanceAsserv.setGoal((float)0.0,(float)0.0,(float)0);
+        instanceAsserv.setGoal(500.0f,200.0f,0.0f);
         logger.printf("GOAL SET... RUNNING!\r\n");
         
         char state = 0;
@@ -118,30 +119,30 @@
             
             //logger.printf("phi r = %3.f ; phi_l = %3.f \r\n", phi_r,phi_l);
             
-            motorR.setSpeed(/*0.08*/+(phi_r/phi_max));
-            motorL.setSpeed(/*0.08*/+(phi_l/phi_max));
-//#define test
+            motorR.setSpeed(0.01+(phi_r/phi_max));
+            motorL.setSpeed(0.01+(phi_l/phi_max));
+#define test
 #ifdef test            
             if(state == 0 && instanceAsserv.isArrivedRho())
             {
                 state = 1;
                 logger.printf("Arrive en 0,0 !!!!!");
                 wait(5);
-                instanceAsserv.setGoal(200.0,200.0,0);
+                instanceAsserv.setGoal(500.0f,200.0f,0.0f);
             }
             else if(state == 1 && instanceAsserv.isArrivedRho())
             {
                 state = 2;
                 logger.printf("Arrive en 200,200 !!!!!");
                 wait(5);
-                instanceAsserv.setGoal(0.0,200.0,0);
+                instanceAsserv.setGoal(0.0f,300.0f,0.0f);
             }
             else if(state == 2 && instanceAsserv.isArrivedRho())
             {
                 state = 0;
                 logger.printf("Arrive en 0,200 !!!!!");
                 wait(5);
-                instanceAsserv.setGoal(0.0,0.0,0);
+                instanceAsserv.setGoal(0.0f,0.0f,0.0f);
             }
 #endif