Diff: main.cpp

Revision:

16:08bd2d5d21fb

Parent:

15:a0a88aeb4fe1

Child:

17:631f978121c2

--- a/main.cpp	Thu Oct 31 07:45:20 2019 +0000
+++ b/main.cpp	Thu Oct 31 09:00:36 2019 +0000
@@ -34,43 +34,49 @@
 DigitalIn   unused6     (PC_1, PullUp);
 DigitalIn   unused7     (PC_0, PullUp);
 
-Timer       temps;
-typedef enum {IDLE,FORWARD,REVERSE} T_state;
+Timer       timer;
+
+typedef enum {IDLE,WAIT1,WAIT2,HOW,GET,TURN} T_state;
 
 main ()
 {
-    double x,y,theta, VL=0, VR=0;
+    T_pixyNMBloc NMBloc;
+    int nbNM,nbCC;
+    double VL=0,VR=0;
     T_state  state= IDLE;
-    motor.resetPosition();
-
+    timer.start();
     while (1) {
-        pc.printf("\r x is %lf ",x);
-        motor.getPosition(&x,&y,&theta);
-        motor.setSpeed (VL,VR);
+        motor.setSpeed(VL,VR);
         switch (state) {
             case IDLE:
-                if (button==0)state= FORWARD;
+                if (pixy.checkPixy()==0)state= WAIT1;
+                break;
+            case WAIT1:
+                if (button==0) state= WAIT2;
+                break;
+            case WAIT2:
                 VL=0;
                 VR=0;
+                if (pixy.checkNewImage()) state= HOW;
+
                 break;
-            case FORWARD:
-                if (x<-300) state= REVERSE;
-                VL=-100;
-                VR=-100;
-                break;
-            case REVERSE:
-                if (x>0) state= IDLE;
-                VL=100;
-                VR=100;
+            case HOW:
+                pixy.detectedObject(&nbNM,&nbCC);
+                if(nbNM==1)state= GET;
+                else state= WAIT2;
                 break;
-            
-
-
-        }// switch
-    }// while 1
+            case GET:
+                NMBloc=pixy.getNMBloc();
+                state=TURN;
+                timer.reset();
+                break;
+            case TURN:
+                VL=NMBloc.x-159;
+                VR=159-NMBloc.x;
+                if(pixy.checkNewImage()) state=HOW;
+                if(timer.read()>0.5f) state= WAIT2;
 
-
-}// main
-
-
-
+                break;
+        }
+    }
+}