Diff: main.cpp

Revision:

15:415c38c93b89

Parent:

14:899c6d29c0a8

Child:

16:c0b098c83233

--- a/main.cpp	Fri Oct 25 13:53:17 2019 +0000
+++ b/main.cpp	Tue Oct 29 16:48:03 2019 +0000
@@ -43,7 +43,7 @@
 int main ()
 {
     int numberOfObjects=0, dummy=0;
-    T_pixyNMBloc file; 
+    T_pixyNMBloc file;
     motor.resetPosition();
     double x=0, y=0, theta = 0;
     motor.setProportionnalValue(2.0);
@@ -51,85 +51,106 @@
     motor.setDerivativeValue(1.0);
     double speed_L = -200, speed_R = -200;
     while (1) {
-        
+
         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 = TURN_RIGHT;
-            break;
-         
-         case TURN_RIGHT :
-             motor.setSpeed(-speed_L/10.0,speed_R/10.0);// turn right
-            
-            if (pixy.checkNewImage()) {
-            pixy.detectedObject(&numberOfObjects,&dummy);
-            if (numberOfObjects==1) {
-                current_state = LOOK_FOR_FILE;
+        pc.printf("\r state is %d x is %.2lf, y is %.2lf, theta is %.2lf",current_state,x,y,theta);
+        switch (current_state) {
+            case IDLE :
+                motor.setSpeed(0,0);
+                if (bp == 0) current_state = TURN_RIGHT;
+                break;
+
+            case TURN_RIGHT :
+                motor.setSpeed(-speed_L/10.0,speed_R/10.0);// turn right
+
+                if (pixy.checkNewImage()) {
+                    pixy.detectedObject(&numberOfObjects,&dummy);
+                    if (numberOfObjects==1) {
+                        current_state = LOOK_FOR_FILE;
+                    }
                 }
-            }
-            break;   
-            
-          case SAFEMODE :
-            motor.setSpeed(0,0);
-            if (ultraSon.readUSB() >= 20) current_state = FORWARD;
-            if (ultraSon.readUSB() <= 15) current_state = BACKWARD;
-            break;            
-            
-        case 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 : 
+                if (x<=-1400) current_state = STUPID1;
+                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();
-                pc.printf("\r x = %d", file.x);
-                if (file.x >165){
-                    motor.setSpeed(-speed_L/10.0,speed_R/10.0);// turn right
-                    pc.printf(" - Turning right");
-                }
-                else if (file.x <155){
-                    motor.setSpeed(speed_L/10.0,-speed_R/10.0);// turn left
-                    pc.printf(" - Turning left");
+                break;
+
+            case LOOK_FOR_FILE :
+                if (pixy.checkNewImage()) {
+                    pixy.detectedObject(&numberOfObjects,&dummy);
+                    if (numberOfObjects==1) {
+                        file=pixy.getNMBloc();
+                        pc.printf("\r x = %d", file.x);
+                        if (file.x >165) {
+                            motor.setSpeed(-speed_L/10.0,speed_R/10.0);// turn right
+                            pc.printf(" - Turning right");
+                        } else if (file.x <155) {
+                            motor.setSpeed(speed_L/10.0,-speed_R/10.0);// turn left
+                            pc.printf(" - Turning left");
+                        } else {
+                            motor.setSpeed(0,0);
+                            motor.resetPosition();
+                            current_state = FORWARD;
+                        }
+                    } else {
+                        //motor.setSpeed(-speed_L,speed_R);// turn right
+                        pc.printf(" - No object");
+                    }
                 }
-                else {
-                    motor.setSpeed(0,0);
-                    motor.resetPosition();
-                    current_state = FORWARD;
-                }
-            }
-            else {
-                motor.setSpeed(-speed_L,speed_R);// turn right
-                pc.printf(" - No object");
-                }
-            }
-            else 
-                {
-                    numberOfObjects = 0;
-                    //motor.setSpeed(-speed_L,speed_R);// turn right
-                   // motor.setSpeed(0,0);    
-            } // end of pixy.checkNewImage
-                              
-            break;
-    
-        default : 
-            //do something
-            break;
+                break;
+                
+                case STUPID1 :
+                    wait(0.2);
+                    current_state = HARVEST1;
+                    break;
+                    
+                case STUPID2 :
+                    wait(0.2);
+                    current_state = HARVEST2;
+                    break;
+                    
+                case STUPID3 :
+                    wait(0.2);
+                    current_state = HARVEST3;
+                    break;
+                    
+                
+                
+                case HARVEST1 :
+                    motor.setSpeed(-speed_L,0.0);
+                    if (theta < (-2*PI)) current_state = STUPID2;
+                break;
+
+                case HARVEST2 :
+                    motor.setSpeed(0.0,-speed_R);
+                    if (theta > 0) current_state = STUPID3;
+                break;
+
+                case HARVEST3 :
+                    motor.setSpeed(speed_L,-speed_R);
+                    if (theta > (2*PI)) current_state = IDLE;
+                break;
+ 
+             default :
+                //do something
+                break;
         }
     }
-    
+
     return 0;
 }
\ No newline at end of file