Diff: main.cpp

Revision:

12:548cdc49cdba

Parent:

11:31564089b41c

Child:

13:f81c8451d601

diff -r 31564089b41c -r 548cdc49cdba main.cpp
--- a/main.cpp	Fri Oct 25 12:40:27 2019 +0000
+++ b/main.cpp	Fri Oct 25 13:18:15 2019 +0000
@@ -51,7 +51,46 @@
     motor.setDerivativeValue(1.0);
     double speed_L = -50, speed_R = -50;
     while (1) {
-        if (pixy.checkNewImage()) {
+        
+        motor.getPosition(&x, &y, &theta);
+        pc.printf("\r x is %.2lf, y is %.2lf, theta is %.2lf",x,y,theta);
+        switch (current_state) 
+        {
+        case IDLE : 
+            motor.setSpeed(0,0);
+            if (bp == 0) current_state = LOOK_FOR_FILE;
+            break;
+         
+         case TURN_RIGHT :
+            motor.setSpeed(speed_L,-speed_R);
+            
+            if (theta <= -PI/3) {
+                motor.resetPosition();
+                current_state = FORWARD;
+                }
+            break;   
+            
+          case SAFEMODE :
+            motor.setSpeed(0,0);
+            if (ultraSon.readUSB() >= 20) current_state = FORWARD;
+            if (ultraSon.readUSB() <= 15) current_state = BACKWARD;
+            break;            
+            
+        case FORWARD : 
+                motor.setSpeed(speed_L,speed_R);
+                if (ultraSon.readUSB() <= 20) {
+                    current_state = SAFEMODE;
+                }
+                if (x<=-2000) current_state = IDLE;
+            break;
+            
+        case BACKWARD : 
+                motor.setSpeed(-speed_L,-speed_R);
+                if (ultraSon.readUSB() >= 15) current_state = SAFEMODE;
+            break;
+            
+        case LOOK_FOR_FILE :               
+           if (pixy.checkNewImage()) {
             pixy.detectedObject(&numberOfObjects,&dummy);
             if (numberOfObjects==1) {
                 file=pixy.getNMBloc();
@@ -66,58 +105,22 @@
                 }
                 else {
                     motor.setSpeed(0,0);
+                    current_state = FORWARD;
                 }
             }
             else {
-                motor.setSpeed(0,0);   
+                motor.setSpeed(-speed_L,speed_R);// turn right
                 pc.printf(" - No object");
                 }
-        }
-        else 
-            {
-                numberOfObjects = 0;
-               // motor.setSpeed(0,0);    
             }
-        //continue;    
-
-        motor.getPosition(&x, &y, &theta);
-        pc.printf("\r x is %.2lf, y is %.2lf, theta is %.2lf",x,y,theta);
-        switch (current_state) 
-        {
-        case IDLE : 
-            motor.setSpeed(0,0);
-            if (bp == 0) current_state = FORWARD;
+            else 
+                {
+                    numberOfObjects = 0;
+                    //motor.setSpeed(-speed_L,speed_R);// turn right
+                   // motor.setSpeed(0,0);    
+            } // end of pixy.checkNewImage
+                              
             break;
-         
-         case TURN_RIGHT :
-            motor.setSpeed(speed_L,-speed_R);
-            
-            if (theta <= -PI/3) {
-                motor.resetPosition();
-                current_state = FORWARD;
-                }
-            break;   
-            
-          case SAFEMODE :
-            motor.setSpeed(0,0);
-            if (ultraSon.readUSB() >= 20) current_state = FORWARD;
-            break;            
-            
-        case FORWARD : 
-                motor.setSpeed(speed_L,speed_R);
-                if (ultraSon.readUSB() <= 20) {
-                    current_state = SAFEMODE;
-                }
-                if (x<=-1000) current_state = IDLE;
-            break;
-            
-        case BACKWARD : 
-                motor.setSpeed(-speed_L,-speed_R);
-                if (x <= 0) {
-                    current_state = IDLE;
-                }
-            break;
-            
     
         default : 
             //do something