programme de test de la bibliothèque d'asservissement PID
Dependencies: Encoder_Nucleo_16_bits mbed
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