Diff: main.cpp

Revision:

13:f81c8451d601

Parent:

12:548cdc49cdba

Child:

14:899c6d29c0a8

--- a/main.cpp	Fri Oct 25 13:18:15 2019 +0000
+++ b/main.cpp	Fri Oct 25 13:24:47 2019 +0000
@@ -58,16 +58,18 @@
         {
         case IDLE : 
             motor.setSpeed(0,0);
-            if (bp == 0) current_state = LOOK_FOR_FILE;
+            if (bp == 0) current_state = TURN_RIGHT;
             break;
          
          case TURN_RIGHT :
-            motor.setSpeed(speed_L,-speed_R);
+             motor.setSpeed(-speed_L,speed_R);// turn right
             
-            if (theta <= -PI/3) {
-                motor.resetPosition();
-                current_state = FORWARD;
+            if (pixy.checkNewImage()) {
+            pixy.detectedObject(&numberOfObjects,&dummy);
+            if (numberOfObjects==1) {
+                current_state = LOOK_FOR_FILE;
                 }
+            }
             break;   
             
           case SAFEMODE :
@@ -105,6 +107,7 @@
                 }
                 else {
                     motor.setSpeed(0,0);
+                    motor.resetPosition();
                     current_state = FORWARD;
                 }
             }