programme de test de la bibliothèque d'asservissement PID
Dependencies: Encoder_Nucleo_16_bits mbed
Diff: main.cpp
- Revision:
- 1:81896d606b4e
- Parent:
- 0:86c5a1f6e21d
diff -r 86c5a1f6e21d -r 81896d606b4e main.cpp --- a/main.cpp Tue Jun 05 08:39:37 2018 +0000 +++ b/main.cpp Wed Jun 06 20:28:39 2018 +0000 @@ -3,11 +3,11 @@ #include "math.h" #include "PID.h" -PID motor (TIM4, TIM3, PA_9, PA_8, PC_9, PC_8, PC_6, PC_5, PA_5); +PID motor (TIM4, TIM3, PA_9, PA_8, PC_9, PC_8, PC_6, PC_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 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) @@ -20,7 +20,7 @@ // led1 = 1; led2 = 0; disquette = 0; - + motor.resetPosition(); wait (5); @@ -30,54 +30,141 @@ // 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); + + pc.printf ("\rEtape = %d : x = %5.1lf, y = %5.1lf, theta = %5.1lf - speedG = %5.1lf, speedD = %5.1lf", 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 (300,300); + if (x > 500) { + etatmvt = 1; + pc.printf("\n"); + } 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 (150,-150); + if (theta < (-PI/2.0)) { + etatmvt = 2; + pc.printf("\n"); + } break; case 2 : // On avance de 50cm (coordonnés X = 500, Y = -500) - motor.setSpeed (400,400); - if (y <= -500) etatmvt = 3; + motor.setSpeed (300,300); + if (y < -500) { + etatmvt = 3; + pc.printf("\n"); + } 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 (150,-150); + if (theta > 0) { + etatmvt = 4; + pc.printf("\n"); + } break; case 4 : // On avance de 50cm (coordonnés X = 0, Y = -500) - motor.setSpeed (400,400); - if (x <= 0) etatmvt = 5; + motor.setSpeed (300,300); + if (x < 0) { + etatmvt = 5; + pc.printf("\n"); + } 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 (200,-200); + if (theta < (PI/2.0)) { + etatmvt = 6; + pc.printf("\n"); + } break; case 6 : // On avance de 50cm (coordonnés X = 0, Y = 0) - motor.setSpeed (400,400); - if (y <= 0) etatmvt = 7; + motor.setSpeed (300,300); + if (y > 0) { + etatmvt = 7; + pc.printf("\n"); + } 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 (200,-200); + if (theta < 0) { + etatmvt = 8; + pc.printf("\n"); + } + break; + case 8 : + // On avance de 50cm (coordonnés X = 500, Y = 0) + motor.setSpeed (300,300); + if (x > 500) { + etatmvt = 9; + pc.printf("\n"); + } + break; + case 9 : + // On tourne à droite de 90° (sens antitrigo => négatif) => Angle = -PI/2 + motor.setSpeed (-150,150); + if (theta > (PI/2.0)) { + etatmvt = 10; + pc.printf("\n"); + } + break; + case 10 : + // On avance de 50cm (coordonnés X = 500, Y = -500) + motor.setSpeed (300,300); + if (y > 500) { + etatmvt = 11; + pc.printf("\n"); + } + break; + case 11 : + // On tourne à droite de 90° (sens antitrigo => négatif) => Angle = -PI (Attention comme -PI = +PI, on teste le rebouclage) + motor.setSpeed (-150,150); + if (theta < 0) { + etatmvt = 12; + pc.printf("\n"); + } + break; + case 12 : + // On avance de 50cm (coordonnés X = 0, Y = -500) + motor.setSpeed (300,300); + if (x < 0) { + etatmvt = 13; + pc.printf("\n"); + } + break; + case 13 : + // On tourne à droite de 90° (sens antitrigo => négatif) => Angle = PI/2 + motor.setSpeed (-200,200); + if (theta > (-PI/2.0)) { + etatmvt = 14; + pc.printf("\n"); + } + break; + case 14 : + // On avance de 50cm (coordonnés X = 0, Y = 0) + motor.setSpeed (300,300); + if (y < 0) { + etatmvt = 15; + pc.printf("\n"); + } + break; + case 15 : + // On tourne à droite de 90° (sens antitrigo => négatif) => Angle = 0 + motor.setSpeed (-200,200); + if (theta > 0) { + etatmvt = 0; + pc.printf("\n"); + } break; } - -// led1 = !led1; + led1 = !led1; led2 = !led2; - wait (0.2); + wait (0.005); } } \ No newline at end of file