Ephantus Mugo / Mbed 2 deprecated Kenya_2019

Test code 29-10-2019

Dependencies:   mbed RC_Servo VMA306 PID CNY70 CMPS03 GP2A Pixy

Files at this revision

API Documentation at this revision

Revision 18:ede80eb1e6d5, committed 2019-10-31

Comitter:
ephymugo
Date:
Thu Oct 31 12:27:29 2019 +0000
Parent:
17:631f978121c2
Commit message:
ALIGNING ROBOT

Changed in this revision

main.cpp Show annotated file Show diff for this revision Revisions of this file 
diff -r 631f978121c2 -r ede80eb1e6d5 main.cpp
--- 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;
+                  
+                
         }
     }
 }