Diff: main.cpp

Revision:

7:6d58adc26b78

Parent:

5:6afed0e241cb

Child:

8:7b0fa8a914c0

--- a/main.cpp	Tue Jun 05 08:06:57 2018 +0000
+++ b/main.cpp	Tue Jun 05 13:55:04 2018 +0000
@@ -9,8 +9,10 @@
 
 #define PI  3.1415926535898
 
+#define NSpeed  100
+
 Serial      pc          (PA_2, PA_3, 921600);
-//PID         motor       (TIM4, TIM3, PA_9, PA_8, PC_9, PC_8, PC_6, PC_5);
+PID         motor       (TIM4, TIM3, PA_9, PA_8, PC_9, PC_8, PC_6, PC_5);
 
 CMPS03      boussole    (PC_4);
 
@@ -21,13 +23,14 @@
 GP2A        ld1         (PA_4, 20, 150, 55);
 GP2A        sd1         (PB_0, 5, 80, 10);
 
-RC_Servo    ballon      (PA_10, 0);
-RC_Servo    verrou      (PA_11, 0);
+RC_Servo    ballon      (PB_10, 0);
+RC_Servo    verrou      (PA_15, 0);
 
 VMA306      UltraSon    (PB_15, PA_6, PB_14, PC_7, PB_13, PB_2);
 
 PIXY        Pixy        (PA_0, PA_1, 230400);
 
+DigitalIn   bp          (PC_13);
 
 DigitalOut  led1        (PA_5);
 DigitalOut  led2        (PD_2);
@@ -40,7 +43,7 @@
     double              pos = 0.5;
     T_pixyCCBloc        CCBuf;
     T_pixyNMBloc        NMBuf;
-    double              x, y, theta;
+    double              x, y, theta, vG, vD;
 
     pc.printf ("\n\rHelloWorld\n");
     led1 = 1;
@@ -101,51 +104,71 @@
         }
 
         // Square danse !!!
-        /*motor.getPosition (&x,&y, &theta);
+        motor.getPosition (&x,&y, &theta);
+        motor.getSpeed (&vG, &vD);
+
+        pc.printf ("\rEtape = %d : x = %5.1lf, y = %5.1lf, theta = %5.1lf - speedG = %5.1lf, speedD = %5.1lf\n", etatmvt, x, y, 180*theta/PI, vG, vD);
+
         switch (etatmvt) {
             case 0 :
                 // On avance de 50cm (coordonnés X = 500, Y = 0)
-                motor.setSpeed (400,400);
-                if (x >= 500) etatmvt = 1;
+                motor.setSpeed (NSpeed,NSpeed);
+                if (x > 300) {
+                    etatmvt = 1;
+                }
                 break;
             case 1 :
                 // On tourne à droite de 90° (sens antitrigo => négatif) => Angle = -PI/2
-                motor.setSpeed (400,-400);
-                if (theta <= -PI/2.0) etatmvt = 2;
+                motor.setSpeed (NSpeed,-NSpeed);
+                if (theta < (-PI/2.0)) {
+                    etatmvt = 2;
+                }
                 break;
             case 2 :
                 // On avance de 50cm (coordonnés X = 500, Y = -500)
-                motor.setSpeed (400,400);
-                if (y <= -500) etatmvt = 3;
+                motor.setSpeed (NSpeed,NSpeed);
+                if (y < -300)  {
+                    etatmvt = 3;
+                }
                 break;
             case 3 :
                 // On tourne à droite de 90° (sens antitrigo => négatif) => Angle = -PI (Attention comme -PI = +PI, on teste le rebouclage)
-                motor.setSpeed (400,-400);
-                if (theta >= 0) etatmvt = 4;
+                motor.setSpeed (NSpeed,-NSpeed);
+                if (theta > 0)  {
+                    etatmvt = 4;
+                }
                 break;
             case 4 :
                 // On avance de 50cm (coordonnés X = 0, Y = -500)
-                motor.setSpeed (400,400);
-                if (x <= 0) etatmvt = 5;
+                motor.setSpeed (NSpeed,NSpeed);
+                if (x < 0)  {
+                    etatmvt = 5;
+                }
                 break;
             case 5 :
                 // On tourne à droite de 90° (sens antitrigo => négatif) => Angle = PI/2
-                motor.setSpeed (400,-400);
-                if (theta <= PI/2.0) etatmvt = 6;
+                motor.setSpeed (NSpeed,-NSpeed);
+                if (theta < (PI/2.0))  {
+                    etatmvt = 6;
+                }
                 break;
             case 6 :
                 // On avance de 50cm (coordonnés X = 0, Y = 0)
-                motor.setSpeed (400,400);
-                if (y <= 0) etatmvt = 7;
+                motor.setSpeed (NSpeed,NSpeed);
+                if (y > 0)  {
+                    etatmvt = 7;
+                }
                 break;
             case 7 :
                 // On tourne à droite de 90° (sens antitrigo => négatif) => Angle = 0
-                motor.setSpeed (400,-400);
-                if (theta <= 0) etatmvt = 0;
+                motor.setSpeed (NSpeed,-NSpeed);
+                if (theta < 0) {
+                    etatmvt = 0;
+                }
                 break;
         }
-*/
-        pc.printf ("\n");
+
+        pc.printf ("\n\n");
         led1 = !led1;
         led2 = !led2;
         wait (0.2);