Diff: main.cpp

Revision:

94:5c37bcf73d14

Parent:

85:8e95432d99d3

Child:

99:9f4f3e46c2f0

--- a/main.cpp	Fri Apr 24 11:12:44 2015 +0000
+++ b/main.cpp	Thu Apr 30 16:03:55 2015 +0000
@@ -1,8 +1,7 @@
 #include "mbed.h"
 
-#define PLAN_B
+#define PLAN_A
 
-//#define OUT_USB
 #include "defines.h"
 
 #include "QEI.h"
@@ -36,7 +35,8 @@
     AX12 test(AX12_TX,AX12_RX,5,250000);
     test.setMode(0);
     
-    /*while(1)
+#ifdef PLAN_B
+    while(1)
     {
         test.setMaxTorque(0x1FF);
         test.setGoal(0);
@@ -46,7 +46,8 @@
         test.setGoal(90);
         logger.printf("180\r\n");
         wait(4);
-    }*/
+    }
+#endif
     
     PwmOut pw1(PWM_MOT1);
     DigitalOut dir1(DIR_MOT1);
@@ -96,7 +97,9 @@
     
     #ifdef PLAN_A
         Asserv<float> instanceAsserv(&odometry);
-        instanceAsserv.setGoal((float)0.0,(float)0.0,(float)PI/2);
+        //instanceAsserv.setGoal((float)0.0,(float)0.0,(float)0);
+        instanceAsserv.setGoal(300.0f,200.0f,0.0f);
+        logger.printf("GOAL SET... RUNNING!\r\n");
         
         char state = 0;
         
@@ -113,36 +116,39 @@
             float phi_r = (float)instanceAsserv.getPhiR();
             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));
+            motorR.setSpeed(0.08f+(phi_r/phi_max));
+            motorL.setSpeed(0.08f+(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(200.0,200.0,0);
+                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.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 !!!!!");
+                logger.printf("Arrive en 0,200 !!!!!\r\n");
                 wait(5);
-                instanceAsserv.setGoal(0.0,0.0,0);
+                instanceAsserv.setGoal(0.0f,0.0f,0.0f);
             }
 #endif
+            
         }
     #else
         aserv_planB asserv(odometry,motorL,motorR);
-        asserv.setGoal(45,45,0);
+        asserv.setGoal(1,-5000,0);
         
         t.start();
         t.reset();
@@ -150,20 +156,20 @@
         while(!testOdo)
         {
             //Parametrage des coef par bluetooth
-            /*while(logger.readable())
-            {
+            while(logger.readable()) {
                 char c = logger.getc();
                 if(c=='a') asserv.Kd += 0.001;
                 else if(c=='z') asserv.Kd -= 0.001;
                 logger.printf("%f\n\r",asserv.Kd);
-            }*/
+            }
             
             //Asservissement :
             float dt = t.read();
             t.reset();
             odometry.update(dt);
             asserv.update(dt);
-            wait_ms(10);            
+            
+            wait_ms(100);            
         }
     #endif
 }