Diff: main.cpp

Revision:

111:c8a1129691da

Parent:

110:7e71e5cd8197

Parent:

107:a6e498b5706c

Child:

114:be06d518b4a7

--- a/main.cpp	Tue May 05 16:52:09 2015 +0000
+++ b/main.cpp	Tue May 05 16:55:12 2015 +0000
@@ -71,7 +71,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);
@@ -280,7 +280,27 @@
         
         while(continuer)
         {
-            #define test
+            //#define test
+            #ifndef test
+                if(state == 0 && asserv.isArrived())
+                {
+                    state = 1;
+                    logger.printf("Arrive en 0,0 !!!!!\r\n");
+                    motorR.setSpeed(0.0f);
+                    motorL.setSpeed(0.0f);
+                    wait(5);
+                    asserv.setGoal(0.0f,0.0f,0.0f);
+                }
+                else if(state == 1 && asserv.isArrived())
+                {
+                    state = 0;
+                    logger.printf("Arrive en 0,200 !!!!!\r\n");
+                    motorR.setSpeed(0.0f);
+                    motorL.setSpeed(0.0f);
+                    wait(5);
+                    asserv.setGoal(0.0f,0.0f,0.0f);
+                }
+            #endif
             #ifdef test            
                 if(state == 0 && asserv.isArrivedRho())
                 {