Diff: main.cpp

Revision:

15:a0a88aeb4fe1

Parent:

14:899c6d29c0a8

Child:

16:08bd2d5d21fb

--- a/main.cpp	Fri Oct 25 13:53:17 2019 +0000
+++ b/main.cpp	Thu Oct 31 07:45:20 2019 +0000
@@ -5,12 +5,10 @@
 #include "VMA306.h"
 #include "Pixy.h"
 #include "PID.h"
-#include "motor_state.h"
 
 #define PI  3.1415926535898
 
 Serial      pc          (PA_2, PA_3, 115200);
-
 PID         motor       (TIM4, TIM3, PA_9, PA_8, PC_9, PC_8, PC_6, PC_5);
 
 CMPS03      boussole    (PC_4);
@@ -21,9 +19,9 @@
 
 VMA306      ultraSon    (PB_15, PA_6, PB_14, PC_7, PB_13, PB_2);
 
-PIXY        pixy        (PA_0, PA_1, 50000);
+PIXY        pixy        (PA_0, PA_1, 115200);
 
-DigitalIn   bp          (PC_13);
+InterruptIn button          (PC_13);
 
 DigitalOut  led1        (PA_5);
 DigitalOut  led2        (PD_2);
@@ -37,99 +35,42 @@
 DigitalIn   unused7     (PC_0, PullUp);
 
 Timer       temps;
+typedef enum {IDLE,FORWARD,REVERSE} T_state;
 
-motor_state current_state = IDLE;
-
-int main ()
+main ()
 {
-    int numberOfObjects=0, dummy=0;
-    T_pixyNMBloc file; 
+    double x,y,theta, VL=0, VR=0;
+    T_state  state= IDLE;
     motor.resetPosition();
-    double x=0, y=0, theta = 0;
-    motor.setProportionnalValue(2.0);
-    motor.setintegralValue(0.4);
-    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;
-                }
-            }
-            break;   
-            
-          case SAFEMODE :
-            motor.setSpeed(0,0);
-            if (ultraSon.readUSB() >= 20) current_state = FORWARD;
-            if (ultraSon.readUSB() <= 15) current_state = BACKWARD;
-            break;            
+        pc.printf("\r x is %lf ",x);
+        motor.getPosition(&x,&y,&theta);
+        motor.setSpeed (VL,VR);
+        switch (state) {
+            case IDLE:
+                if (button==0)state= FORWARD;
+                VL=0;
+                VR=0;
+                break;
+            case FORWARD:
+                if (x<-300) state= REVERSE;
+                VL=-100;
+                VR=-100;
+                break;
+            case REVERSE:
+                if (x>0) state= IDLE;
+                VL=100;
+                VR=100;
+                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();
-                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 
-                {
-                    numberOfObjects = 0;
-                    //motor.setSpeed(-speed_L,speed_R);// turn right
-                   // motor.setSpeed(0,0);    
-            } // end of pixy.checkNewImage
-                              
-            break;
-    
-        default : 
-            //do something
-            break;
-        }
-    }
-    
-    return 0;
-}
\ No newline at end of file
+
+
+        }// switch
+    }// while 1
+
+
+}// main
+
+
+