Diff: main.cpp

Revision:

17:631f978121c2

Parent:

16:08bd2d5d21fb

Child:

18:ede80eb1e6d5

--- a/main.cpp	Thu Oct 31 09:00:36 2019 +0000
+++ b/main.cpp	Thu Oct 31 09:52:11 2019 +0000
@@ -36,17 +36,19 @@
 
 Timer       timer;
 
-typedef enum {IDLE,WAIT1,WAIT2,HOW,GET,TURN} T_state;
+typedef enum {IDLE,WAIT1,WAIT2,HOW,GET,TURN,FOLLOW} T_state;
 
 main ()
 {
     T_pixyNMBloc NMBloc;
     int nbNM,nbCC;
-    double VL=0,VR=0;
+    double x,y,theta,VL=0,VR=0;
     T_state  state= IDLE;
     timer.start();
     while (1) {
+        motor.getPosition(&x,&y,&theta);
         motor.setSpeed(VL,VR);
+        //float distance= ultraSon.readUSB();
         switch (state) {
             case IDLE:
                 if (pixy.checkPixy()==0)state= WAIT1;
@@ -75,8 +77,16 @@
                 VR=159-NMBloc.x;
                 if(pixy.checkNewImage()) state=HOW;
                 if(timer.read()>0.5f) state= WAIT2;
+                if(NMBloc.x > 155 && NMBloc.x < 165) state= FOLLOW;
+                else state = WAIT2;
 
                 break;
+
+            case FOLLOW :
+                if(x<-300)state= WAIT2;
+                VL=-100;
+                VR=-100;
+                break;
         }
     }
 }