Diff: main.cpp

Revision:

11:31564089b41c

Parent:

10:6c3653c53eca

Child:

12:548cdc49cdba

--- a/main.cpp	Fri Oct 25 11:19:43 2019 +0000
+++ b/main.cpp	Fri Oct 25 12:40:27 2019 +0000
@@ -10,7 +10,7 @@
 #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 +21,7 @@
 
 VMA306      ultraSon    (PB_15, PA_6, PB_14, PC_7, PB_13, PB_2);
 
-PIXY        pixy        (PA_0, PA_1, 115200);
+PIXY        pixy        (PA_0, PA_1, 50000);
 
 DigitalIn   bp          (PC_13);
 
@@ -49,18 +49,37 @@
     motor.setProportionnalValue(2.0);
     motor.setintegralValue(0.4);
     motor.setDerivativeValue(1.0);
-    
+    double speed_L = -50, speed_R = -50;
     while (1) {
         if (pixy.checkNewImage()) {
             pixy.detectedObject(&numberOfObjects,&dummy);
             if (numberOfObjects==1) {
                 file=pixy.getNMBloc();
-                if (file.x >160){
+                pc.printf("\r x = %d", file.x);
+                if (file.x >165){
+                    motor.setSpeed(-speed_L,speed_R);// turn right
+                    pc.printf(" - Turning right");
+                }
+                else if (file.x <155){
+                    motor.setSpeed(speed_L,-speed_R);// turn left
+                    pc.printf(" - Turning left");
+                }
+                else {
+                    motor.setSpeed(0,0);
                 }
             }
-        }else numberOfObjects = 0;
-            
-        double speed_L = -100, speed_R = -100;
+            else {
+                motor.setSpeed(0,0);   
+                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)