programme de test de la bibliothèque d'asservissement PID
Dependencies: Encoder_Nucleo_16_bits mbed
main.cpp
- Committer:
- haarkon
- Date:
- 2018-06-05
- Revision:
- 0:86c5a1f6e21d
- Child:
- 1:81896d606b4e
File content as of revision 0:86c5a1f6e21d:
#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); } }