Diff: main.cpp

Revision:

18:ede80eb1e6d5

Parent:

17:631f978121c2

--- a/main.cpp	Thu Oct 31 09:52:11 2019 +0000
+++ b/main.cpp	Thu Oct 31 12:27:29 2019 +0000
@@ -36,7 +36,7 @@
 
 Timer       timer;
 
-typedef enum {IDLE,WAIT1,WAIT2,HOW,GET,TURN,FOLLOW} T_state;
+typedef enum {IDLE,WAIT1,WAIT2,HOW,GET,TURN,ALIGN,FOLLOW} T_state;
 
 main ()
 {
@@ -48,7 +48,7 @@
     while (1) {
         motor.getPosition(&x,&y,&theta);
         motor.setSpeed(VL,VR);
-        //float distance= ultraSon.readUSB();
+        float distance= ultraSon.readUSB();
         switch (state) {
             case IDLE:
                 if (pixy.checkPixy()==0)state= WAIT1;
@@ -81,12 +81,24 @@
                 else state = WAIT2;
 
                 break;
+            case ALIGN:
+                VL=NMBloc.x-159;
+                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;
+                // if(distance>20)state=WAIT2;
                 VL=-100;
                 VR=-100;
                 break;
+                  
+                
         }
     }
 }