Diff: main.cpp

Revision:

10:6c3653c53eca

Parent:

9:4053b5217339

Child:

11:31564089b41c

--- a/main.cpp	Thu Oct 24 14:21:22 2019 +0000
+++ b/main.cpp	Fri Oct 25 11:19:43 2019 +0000
@@ -5,10 +5,12 @@
 #include "VMA306.h"
 #include "Pixy.h"
 #include "PID.h"
+#include "motor_state.h"
 
 #define PI  3.1415926535898
 
 Serial      pc          (PA_2, PA_3, 115200);
+TIM
 PID         motor       (TIM4, TIM3, PA_9, PA_8, PC_9, PC_8, PC_6, PC_5);
 
 CMPS03      boussole    (PC_4);
@@ -21,7 +23,7 @@
 
 PIXY        pixy        (PA_0, PA_1, 115200);
 
-InterruptIn bp          (PC_13);
+DigitalIn   bp          (PC_13);
 
 DigitalOut  led1        (PA_5);
 DigitalOut  led2        (PD_2);
@@ -36,20 +38,72 @@
 
 Timer       temps;
 
+motor_state current_state = IDLE;
+
 int main ()
 {
+    int numberOfObjects=0, dummy=0;
+    T_pixyNMBloc file; 
     motor.resetPosition();
-    double x=0, y, theta = 0;
-
+    double x=0, y=0, theta = 0;
+    motor.setProportionnalValue(2.0);
+    motor.setintegralValue(0.4);
+    motor.setDerivativeValue(1.0);
+    
     while (1) {
-        double speed_L = 50, speed_R = 50;
-        while (x <= 200)
+        if (pixy.checkNewImage()) {
+            pixy.detectedObject(&numberOfObjects,&dummy);
+            if (numberOfObjects==1) {
+                file=pixy.getNMBloc();
+                if (file.x >160){
+                }
+            }
+        }else numberOfObjects = 0;
+            
+        double speed_L = -100, speed_R = -100;
+        motor.getPosition(&x, &y, &theta);
+        pc.printf("\r x is %.2lf, y is %.2lf, theta is %.2lf",x,y,theta);
+        switch (current_state) 
         {
-            motor.setSpeed(speed_L,speed_R);
-            motor.getPosition(&x, &y, &theta);
-            pc.printf("\r x = %lf", x);
-        } 
-        motor.setSpeed(0,0);
+        case IDLE : 
+            motor.setSpeed(0,0);
+            if (bp == 0) current_state = FORWARD;
+            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
+            break;
+        }
     }
     
     return 0;