FRC - Hackathon / Mbed 2 deprecated TestPID

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?

UserRevisionLine numberNew 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 }