Diff: main.cpp

Revision:

107:a6e498b5706c

Parent:

106:05096985d1b2

Child:

111:c8a1129691da

--- a/main.cpp	Tue May 05 05:05:07 2015 +0000
+++ b/main.cpp	Tue May 05 12:08:20 2015 +0000
@@ -1,6 +1,6 @@
 #include "mbed.h"
 
-#define PLAN_B
+#define PLAN_A
 #define OUT_USB
 
 #include "defines.h"
@@ -69,7 +69,7 @@
     QEI qei_right(PB_3,PA_10,NC,1024*4,QEI::X4_ENCODING);
     QEI qei_left(PB_4,PB_5,NC,1024*4,QEI::X4_ENCODING);
     
-    Odometry odometry(&qei_left,&qei_right,63.84/2.,63.65/2.,252);
+    Odometry2 odometry(&qei_left,&qei_right,63.84/2.,63.65/2.,252);
 #else
     QEI qei_left(ODO_G_A,ODO_G_B,NC,1024*4,QEI::X4_ENCODING);
     QEI qei_right(ODO_D_A,ODO_D_B,NC,1024*4,QEI::X4_ENCODING);
@@ -176,14 +176,34 @@
     bool continuer = true;
     
     #ifdef PLAN_A
-        instanceAsserv.setGoal(300.0f,200.0f,0.0f);
+        instanceAsserv.setGoal(0.0f,0.0f,(float)PI/4);
         logger.printf("GOAL SET... RUNNING!\r\n");
         
         char state = 0;
         
         while(continuer)
         {
-            #define test
+            //#define test
+            #ifndef test
+                if(state == 0 && instanceAsserv.isArrived())
+                {
+                    state = 1;
+                    logger.printf("Arrive en 0,0 !!!!!\r\n");
+                    motorR.setSpeed(0.0f);
+                    motorL.setSpeed(0.0f);
+                    wait(5);
+                    instanceAsserv.setGoal(0.0f,0.0f,0.0f);
+                }
+                else if(state == 1 && instanceAsserv.isArrived())
+                {
+                    state = 0;
+                    logger.printf("Arrive en 0,200 !!!!!\r\n");
+                    motorR.setSpeed(0.0f);
+                    motorL.setSpeed(0.0f);
+                    wait(5);
+                    instanceAsserv.setGoal(0.0f,0.0f,0.0f);
+                }
+            #endif
             #ifdef test            
                 if(state == 0 && instanceAsserv.isArrivedRho())
                 {