Diff: main.cpp

Revision:

0:86c5a1f6e21d

Child:

1:81896d606b4e

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Tue Jun 05 08:39:37 2018 +0000
@@ -0,0 +1,83 @@
+#include "mbed.h"
+#include "Nucleo_Encoder_16_bits.h"
+#include "math.h"
+#include "PID.h"
+
+PID         motor       (TIM4, TIM3, PA_9, PA_8, PC_9, PC_8, PC_6, PC_5, PA_5);
+
+Serial      pc          (PA_2, PA_3, 921600);                                   // Create a serial link to PC for communication
+
+//DigitalOut              led1        (PA_5);                                     // Added Led1 for test purpose
+DigitalOut              led2        (PD_2);                                     // Added Led2 for test purpose
+DigitalOut              disquette   (PA_12);                                    // Added baloon destructor command (without it, you might see baloon destructor motor be set to full speed)
+
+main ()
+{
+    int                 etatmvt = 0;
+    double              x, y, theta, vG, vD;
+
+    pc.printf ("\n\rHelloWorld\n");
+//    led1 = 1;
+    led2 = 0;
+    disquette = 0;
+    
+    motor.resetPosition();
+
+    wait (5);
+
+    while (1) {
+
+        // Square danse !!!
+        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", etatmvt, x, y, theta, 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;
+                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;
+        }
+
+//        led1 = !led1;
+        led2 = !led2;
+        wait (0.2);
+    }
+}
\ No newline at end of file