Diff: main.cpp

Revision:

4:5038b4cd1088

Parent:

3:0221daeeaa86

Child:

5:6afed0e241cb

--- a/main.cpp	Thu May 31 17:47:43 2018 +0000
+++ b/main.cpp	Tue Jun 05 07:29:54 2018 +0000
@@ -5,8 +5,12 @@
 #include "RC_Servo.h"
 #include "VMA306.h"
 #include "Pixy.h"
+#include "PID.h"
+
+#define PI  3.1415926535898
 
 Serial      pc          (PA_2, PA_3, 921600);
+PID         motor       (TIM4, TIM3, PA_9, PA_8, PC_9, PC_8, PC_6, PC_5);
 
 CMPS03      boussole    (PC_4);
 
@@ -17,28 +21,33 @@
 GP2A        ld1         (PA_4, 20, 150, 55);
 GP2A        sd1         (PB_0, 5, 80, 10);
 
-RC_Servo    ballon      (PB_10, 0);
-RC_Servo    verrou      (PA_15, 0);
+RC_Servo    ballon      (PA_10, 0);
+RC_Servo    verrou      (PA_11, 0);
 
 VMA306      UltraSon    (PB_15, PA_6, PB_14, PC_7, PB_13, PB_2);
 
 PIXY        Pixy        (PA_0, PA_1, 230400);
 
+
 DigitalOut  led1        (PA_5);
 DigitalOut  led2        (PD_2);
 DigitalOut  disquette   (PA_12);
 
 main ()
 {
+    int                 etatmvt = 0;
     int                 sens = 1, nbiter = 0, nbCC, nbNM;
     double              pos = 0.5;
     T_pixyCCBloc        CCBuf;
     T_pixyNMBloc        NMBuf;
+    double              x, y, theta;
 
     pc.printf ("\n\rHelloWorld\n");
     led1 = 1;
     led2 = 0;
     disquette = 0;
+    
+    motor.resetPosition();
 
     wait (5);
 
@@ -58,7 +67,7 @@
         pc.printf("\rGP2 courte distance = %5.2f\n",sd1.getDistance ());
 
         pc.printf("\rpos = %2.1lf\n",pos);
-        ballon.write (pos);
+        /*ballon.write (pos);
         verrou.write (pos);
         nbiter++;
         if (nbiter%5==0) {
@@ -69,7 +78,7 @@
                 pos-=0.1;
                 if (pos<0.1) sens = 1;
             }
-        }
+        }*/
 
         if (UltraSon.isUSGReady())     pc.printf ("\rusG = %5.2lf -", UltraSon.readUSG());
         if (UltraSon.isUSBReady())     pc.printf ("\r\t\t usB = %5.2lf -", UltraSon.readUSB());
@@ -91,6 +100,51 @@
             }
         }
 
+        // Square danse !!!
+        motor.getPosition (&x,&y, &theta);
+        switch (etatmvt) {
+            case 0 :
+                // On avance de 50cm (coordonnés X = 500, Y = 0)
+                motor.setSpeed (400,400);
+                if (x >= 500) 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;
+                break;
+            case 2 :
+                // On avance de 50cm (coordonnés X = 500, Y = -500)
+                motor.setSpeed (400,400);
+                if (y <= -500) 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;
+                break;
+            case 4 :
+                // On avance de 50cm (coordonnés X = 0, Y = -500)
+                motor.setSpeed (400,400);
+                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;
+                break;
+            case 6 :
+                // On avance de 50cm (coordonnés X = 0, Y = 0)
+                motor.setSpeed (400,400);
+                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;
+                break;
+        }
+
         pc.printf ("\n");
         led1 = !led1;
         led2 = !led2;