Diff: main.cpp

Revision:

89:d05001d85a02

Parent:

88:77d6ced9977e

Child:

90:294b16267109

--- a/main.cpp	Sat Apr 18 08:40:24 2015 +0000
+++ b/main.cpp	Thu Apr 23 16:29:31 2015 +0000
@@ -35,6 +35,7 @@
     AX12 test(AX12_TX,AX12_RX,5,250000);
     test.setMode(0);
     
+#ifdef PLAN_B
     while(1)
     {
         test.setMaxTorque(0x1FF);
@@ -46,6 +47,7 @@
         logger.printf("180\r\n");
         wait(4);
     }
+#endif
     
     PwmOut pw1(PWM_MOT1);
     DigitalOut dir1(DIR_MOT1);
@@ -95,7 +97,8 @@
     
     #ifdef PLAN_A
         Asserv<float> instanceAsserv(&odometry);
-        instanceAsserv.setGoal((float)0.0,(float)0.0,(float)PI/4);
+        instanceAsserv.setGoal((float)100.0,(float)-150.0,(float)PI/4);
+        logger.printf("GOAL SET... RUNNING!\r\n");
         
         char state = 0;
         
@@ -113,8 +116,10 @@
             float phi_l = (float)instanceAsserv.getPhiL();
             float phi_max = (float)instanceAsserv.getPhiMax();
             
-            motorR.setSpeed(/*0.08+*/(phi_r/phi_max));
-            motorL.setSpeed(/*0.06+*/(phi_l/phi_max));
+            //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
 #ifdef test            
             if(state == 0 && instanceAsserv.isArrivedRho())