programme de test de la bibliothèque d'asservissement PID
Dependencies: Encoder_Nucleo_16_bits mbed
main.cpp@0:86c5a1f6e21d, 2018-06-05 (annotated)
- Committer:
- haarkon
- Date:
- Tue Jun 05 08:39:37 2018 +0000
- Revision:
- 0:86c5a1f6e21d
- Child:
- 1:81896d606b4e
testlib (not functionning)
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
haarkon | 0:86c5a1f6e21d | 1 | #include "mbed.h" |
haarkon | 0:86c5a1f6e21d | 2 | #include "Nucleo_Encoder_16_bits.h" |
haarkon | 0:86c5a1f6e21d | 3 | #include "math.h" |
haarkon | 0:86c5a1f6e21d | 4 | #include "PID.h" |
haarkon | 0:86c5a1f6e21d | 5 | |
haarkon | 0:86c5a1f6e21d | 6 | PID motor (TIM4, TIM3, PA_9, PA_8, PC_9, PC_8, PC_6, PC_5, PA_5); |
haarkon | 0:86c5a1f6e21d | 7 | |
haarkon | 0:86c5a1f6e21d | 8 | Serial pc (PA_2, PA_3, 921600); // Create a serial link to PC for communication |
haarkon | 0:86c5a1f6e21d | 9 | |
haarkon | 0:86c5a1f6e21d | 10 | //DigitalOut led1 (PA_5); // Added Led1 for test purpose |
haarkon | 0:86c5a1f6e21d | 11 | DigitalOut led2 (PD_2); // Added Led2 for test purpose |
haarkon | 0:86c5a1f6e21d | 12 | DigitalOut disquette (PA_12); // Added baloon destructor command (without it, you might see baloon destructor motor be set to full speed) |
haarkon | 0:86c5a1f6e21d | 13 | |
haarkon | 0:86c5a1f6e21d | 14 | main () |
haarkon | 0:86c5a1f6e21d | 15 | { |
haarkon | 0:86c5a1f6e21d | 16 | int etatmvt = 0; |
haarkon | 0:86c5a1f6e21d | 17 | double x, y, theta, vG, vD; |
haarkon | 0:86c5a1f6e21d | 18 | |
haarkon | 0:86c5a1f6e21d | 19 | pc.printf ("\n\rHelloWorld\n"); |
haarkon | 0:86c5a1f6e21d | 20 | // led1 = 1; |
haarkon | 0:86c5a1f6e21d | 21 | led2 = 0; |
haarkon | 0:86c5a1f6e21d | 22 | disquette = 0; |
haarkon | 0:86c5a1f6e21d | 23 | |
haarkon | 0:86c5a1f6e21d | 24 | motor.resetPosition(); |
haarkon | 0:86c5a1f6e21d | 25 | |
haarkon | 0:86c5a1f6e21d | 26 | wait (5); |
haarkon | 0:86c5a1f6e21d | 27 | |
haarkon | 0:86c5a1f6e21d | 28 | while (1) { |
haarkon | 0:86c5a1f6e21d | 29 | |
haarkon | 0:86c5a1f6e21d | 30 | // Square danse !!! |
haarkon | 0:86c5a1f6e21d | 31 | motor.getPosition (&x,&y, &theta); |
haarkon | 0:86c5a1f6e21d | 32 | motor.getSpeed (&vG, &vD); |
haarkon | 0:86c5a1f6e21d | 33 | |
haarkon | 0:86c5a1f6e21d | 34 | 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); |
haarkon | 0:86c5a1f6e21d | 35 | |
haarkon | 0:86c5a1f6e21d | 36 | switch (etatmvt) { |
haarkon | 0:86c5a1f6e21d | 37 | case 0 : |
haarkon | 0:86c5a1f6e21d | 38 | // On avance de 50cm (coordonnés X = 500, Y = 0) |
haarkon | 0:86c5a1f6e21d | 39 | motor.setSpeed (400,400); |
haarkon | 0:86c5a1f6e21d | 40 | if (x >= 500) etatmvt = 1; |
haarkon | 0:86c5a1f6e21d | 41 | break; |
haarkon | 0:86c5a1f6e21d | 42 | case 1 : |
haarkon | 0:86c5a1f6e21d | 43 | // On tourne à droite de 90° (sens antitrigo => négatif) => Angle = -PI/2 |
haarkon | 0:86c5a1f6e21d | 44 | motor.setSpeed (400,-400); |
haarkon | 0:86c5a1f6e21d | 45 | if (theta <= -PI/2.0) etatmvt = 2; |
haarkon | 0:86c5a1f6e21d | 46 | break; |
haarkon | 0:86c5a1f6e21d | 47 | case 2 : |
haarkon | 0:86c5a1f6e21d | 48 | // On avance de 50cm (coordonnés X = 500, Y = -500) |
haarkon | 0:86c5a1f6e21d | 49 | motor.setSpeed (400,400); |
haarkon | 0:86c5a1f6e21d | 50 | if (y <= -500) etatmvt = 3; |
haarkon | 0:86c5a1f6e21d | 51 | break; |
haarkon | 0:86c5a1f6e21d | 52 | case 3 : |
haarkon | 0:86c5a1f6e21d | 53 | // On tourne à droite de 90° (sens antitrigo => négatif) => Angle = -PI (Attention comme -PI = +PI, on teste le rebouclage) |
haarkon | 0:86c5a1f6e21d | 54 | motor.setSpeed (400,-400); |
haarkon | 0:86c5a1f6e21d | 55 | if (theta >= 0) etatmvt = 4; |
haarkon | 0:86c5a1f6e21d | 56 | break; |
haarkon | 0:86c5a1f6e21d | 57 | case 4 : |
haarkon | 0:86c5a1f6e21d | 58 | // On avance de 50cm (coordonnés X = 0, Y = -500) |
haarkon | 0:86c5a1f6e21d | 59 | motor.setSpeed (400,400); |
haarkon | 0:86c5a1f6e21d | 60 | if (x <= 0) etatmvt = 5; |
haarkon | 0:86c5a1f6e21d | 61 | break; |
haarkon | 0:86c5a1f6e21d | 62 | case 5 : |
haarkon | 0:86c5a1f6e21d | 63 | // On tourne à droite de 90° (sens antitrigo => négatif) => Angle = PI/2 |
haarkon | 0:86c5a1f6e21d | 64 | motor.setSpeed (400,-400); |
haarkon | 0:86c5a1f6e21d | 65 | if (theta <= PI/2.0) etatmvt = 6; |
haarkon | 0:86c5a1f6e21d | 66 | break; |
haarkon | 0:86c5a1f6e21d | 67 | case 6 : |
haarkon | 0:86c5a1f6e21d | 68 | // On avance de 50cm (coordonnés X = 0, Y = 0) |
haarkon | 0:86c5a1f6e21d | 69 | motor.setSpeed (400,400); |
haarkon | 0:86c5a1f6e21d | 70 | if (y <= 0) etatmvt = 7; |
haarkon | 0:86c5a1f6e21d | 71 | break; |
haarkon | 0:86c5a1f6e21d | 72 | case 7 : |
haarkon | 0:86c5a1f6e21d | 73 | // On tourne à droite de 90° (sens antitrigo => négatif) => Angle = 0 |
haarkon | 0:86c5a1f6e21d | 74 | motor.setSpeed (400,-400); |
haarkon | 0:86c5a1f6e21d | 75 | if (theta <= 0) etatmvt = 0; |
haarkon | 0:86c5a1f6e21d | 76 | break; |
haarkon | 0:86c5a1f6e21d | 77 | } |
haarkon | 0:86c5a1f6e21d | 78 | |
haarkon | 0:86c5a1f6e21d | 79 | // led1 = !led1; |
haarkon | 0:86c5a1f6e21d | 80 | led2 = !led2; |
haarkon | 0:86c5a1f6e21d | 81 | wait (0.2); |
haarkon | 0:86c5a1f6e21d | 82 | } |
haarkon | 0:86c5a1f6e21d | 83 | } |