Diff: main.cpp

Revision:

91:c0b436c52ede

Parent:

90:294b16267109

Child:

92:b403f2724252

--- a/main.cpp	Thu Apr 23 18:20:35 2015 +0000
+++ b/main.cpp	Fri Apr 24 10:24:09 2015 +0000
@@ -20,8 +20,8 @@
 /*----------------------------------------------------------------------------------------------*/
 /*Serial*/    
 
-Serial logger(OUT_TX, OUT_RX); // tx, rx
-//Serial logger(USBTX,USBRX);
+//Serial logger(OUT_TX, OUT_RX); // tx, rx
+Serial logger(USBTX,USBRX);
 //logger.baud((int)115200);
 /*----------------------------------------------------------------------------------------------*/
 
@@ -98,7 +98,7 @@
     #ifdef PLAN_A
         Asserv<float> instanceAsserv(&odometry);
         //instanceAsserv.setGoal((float)0.0,(float)0.0,(float)0);
-        instanceAsserv.setGoal(500.0f,200.0f,0.0f);
+        instanceAsserv.setGoal(300.0f,200.0f,0.0f);
         logger.printf("GOAL SET... RUNNING!\r\n");
         
         char state = 0;
@@ -116,31 +116,30 @@
             float phi_r = (float)instanceAsserv.getPhiR();
             float phi_l = (float)instanceAsserv.getPhiL();
             float phi_max = (float)instanceAsserv.getPhiMax();
-            
-            //logger.printf("phi r = %3.f ; phi_l = %3.f \r\n", phi_r,phi_l);
+                        
             
-            motorR.setSpeed(0.01+(phi_r/phi_max));
-            motorL.setSpeed(0.01+(phi_l/phi_max));
+            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 !!!!!");
+                logger.printf("Arrive en 0,0 !!!!!\r\n");
                 wait(5);
-                instanceAsserv.setGoal(500.0f,200.0f,0.0f);
+                instanceAsserv.setGoal(300.0f,200.0f,0.0f);
             }
             else if(state == 1 && instanceAsserv.isArrivedRho())
             {
                 state = 2;
-                logger.printf("Arrive en 200,200 !!!!!");
+                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 !!!!!");
+                logger.printf("Arrive en 0,200 !!!!!\r\n");
                 wait(5);
                 instanceAsserv.setGoal(0.0f,0.0f,0.0f);
             }