FRC - Hackathon / Mbed 2 deprecated TestPID

programme de test de la bibliothèque d'asservissement PID

Dependencies:   Encoder_Nucleo_16_bits mbed

main.cpp@1:81896d606b4e, 2018-06-06 (annotated)

Committer:
haarkon
Date:
Wed Jun 06 20:28:39 2018 +0000
Revision:
1:81896d606b4e
Parent:
0:86c5a1f6e21d 
Test des fonction de l'asservissement

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 1:81896d606b4e 6 PID motor (TIM4, TIM3, PA_9, PA_8, PC_9, PC_8, PC_6, PC_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 1:81896d606b4e 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 1:81896d606b4e 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 1:81896d606b4e 33
haarkon 1:81896d606b4e 34 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);
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 1:81896d606b4e 39 motor.setSpeed (300,300);
haarkon 1:81896d606b4e 40 if (x > 500) {
haarkon 1:81896d606b4e 41 etatmvt = 1;
haarkon 1:81896d606b4e 42 pc.printf("\n");
haarkon 1:81896d606b4e 43 }
haarkon 0:86c5a1f6e21d 44 break;
haarkon 0:86c5a1f6e21d 45 case 1 :
haarkon 0:86c5a1f6e21d 46 // On tourne à droite de 90° (sens antitrigo => négatif) => Angle = -PI/2
haarkon 1:81896d606b4e 47 motor.setSpeed (150,-150);
haarkon 1:81896d606b4e 48 if (theta < (-PI/2.0)) {
haarkon 1:81896d606b4e 49 etatmvt = 2;
haarkon 1:81896d606b4e 50 pc.printf("\n");
haarkon 1:81896d606b4e 51 }
haarkon 0:86c5a1f6e21d 52 break;
haarkon 0:86c5a1f6e21d 53 case 2 :
haarkon 0:86c5a1f6e21d 54 // On avance de 50cm (coordonnés X = 500, Y = -500)
haarkon 1:81896d606b4e 55 motor.setSpeed (300,300);
haarkon 1:81896d606b4e 56 if (y < -500) {
haarkon 1:81896d606b4e 57 etatmvt = 3;
haarkon 1:81896d606b4e 58 pc.printf("\n");
haarkon 1:81896d606b4e 59 }
haarkon 0:86c5a1f6e21d 60 break;
haarkon 0:86c5a1f6e21d 61 case 3 :
haarkon 0:86c5a1f6e21d 62 // On tourne à droite de 90° (sens antitrigo => négatif) => Angle = -PI (Attention comme -PI = +PI, on teste le rebouclage)
haarkon 1:81896d606b4e 63 motor.setSpeed (150,-150);
haarkon 1:81896d606b4e 64 if (theta > 0) {
haarkon 1:81896d606b4e 65 etatmvt = 4;
haarkon 1:81896d606b4e 66 pc.printf("\n");
haarkon 1:81896d606b4e 67 }
haarkon 0:86c5a1f6e21d 68 break;
haarkon 0:86c5a1f6e21d 69 case 4 :
haarkon 0:86c5a1f6e21d 70 // On avance de 50cm (coordonnés X = 0, Y = -500)
haarkon 1:81896d606b4e 71 motor.setSpeed (300,300);
haarkon 1:81896d606b4e 72 if (x < 0) {
haarkon 1:81896d606b4e 73 etatmvt = 5;
haarkon 1:81896d606b4e 74 pc.printf("\n");
haarkon 1:81896d606b4e 75 }
haarkon 0:86c5a1f6e21d 76 break;
haarkon 0:86c5a1f6e21d 77 case 5 :
haarkon 0:86c5a1f6e21d 78 // On tourne à droite de 90° (sens antitrigo => négatif) => Angle = PI/2
haarkon 1:81896d606b4e 79 motor.setSpeed (200,-200);
haarkon 1:81896d606b4e 80 if (theta < (PI/2.0)) {
haarkon 1:81896d606b4e 81 etatmvt = 6;
haarkon 1:81896d606b4e 82 pc.printf("\n");
haarkon 1:81896d606b4e 83 }
haarkon 0:86c5a1f6e21d 84 break;
haarkon 0:86c5a1f6e21d 85 case 6 :
haarkon 0:86c5a1f6e21d 86 // On avance de 50cm (coordonnés X = 0, Y = 0)
haarkon 1:81896d606b4e 87 motor.setSpeed (300,300);
haarkon 1:81896d606b4e 88 if (y > 0) {
haarkon 1:81896d606b4e 89 etatmvt = 7;
haarkon 1:81896d606b4e 90 pc.printf("\n");
haarkon 1:81896d606b4e 91 }
haarkon 0:86c5a1f6e21d 92 break;
haarkon 0:86c5a1f6e21d 93 case 7 :
haarkon 0:86c5a1f6e21d 94 // On tourne à droite de 90° (sens antitrigo => négatif) => Angle = 0
haarkon 1:81896d606b4e 95 motor.setSpeed (200,-200);
haarkon 1:81896d606b4e 96 if (theta < 0) {
haarkon 1:81896d606b4e 97 etatmvt = 8;
haarkon 1:81896d606b4e 98 pc.printf("\n");
haarkon 1:81896d606b4e 99 }
haarkon 1:81896d606b4e 100 break;
haarkon 1:81896d606b4e 101 case 8 :
haarkon 1:81896d606b4e 102 // On avance de 50cm (coordonnés X = 500, Y = 0)
haarkon 1:81896d606b4e 103 motor.setSpeed (300,300);
haarkon 1:81896d606b4e 104 if (x > 500) {
haarkon 1:81896d606b4e 105 etatmvt = 9;
haarkon 1:81896d606b4e 106 pc.printf("\n");
haarkon 1:81896d606b4e 107 }
haarkon 1:81896d606b4e 108 break;
haarkon 1:81896d606b4e 109 case 9 :
haarkon 1:81896d606b4e 110 // On tourne à droite de 90° (sens antitrigo => négatif) => Angle = -PI/2
haarkon 1:81896d606b4e 111 motor.setSpeed (-150,150);
haarkon 1:81896d606b4e 112 if (theta > (PI/2.0)) {
haarkon 1:81896d606b4e 113 etatmvt = 10;
haarkon 1:81896d606b4e 114 pc.printf("\n");
haarkon 1:81896d606b4e 115 }
haarkon 1:81896d606b4e 116 break;
haarkon 1:81896d606b4e 117 case 10 :
haarkon 1:81896d606b4e 118 // On avance de 50cm (coordonnés X = 500, Y = -500)
haarkon 1:81896d606b4e 119 motor.setSpeed (300,300);
haarkon 1:81896d606b4e 120 if (y > 500) {
haarkon 1:81896d606b4e 121 etatmvt = 11;
haarkon 1:81896d606b4e 122 pc.printf("\n");
haarkon 1:81896d606b4e 123 }
haarkon 1:81896d606b4e 124 break;
haarkon 1:81896d606b4e 125 case 11 :
haarkon 1:81896d606b4e 126 // On tourne à droite de 90° (sens antitrigo => négatif) => Angle = -PI (Attention comme -PI = +PI, on teste le rebouclage)
haarkon 1:81896d606b4e 127 motor.setSpeed (-150,150);
haarkon 1:81896d606b4e 128 if (theta < 0) {
haarkon 1:81896d606b4e 129 etatmvt = 12;
haarkon 1:81896d606b4e 130 pc.printf("\n");
haarkon 1:81896d606b4e 131 }
haarkon 1:81896d606b4e 132 break;
haarkon 1:81896d606b4e 133 case 12 :
haarkon 1:81896d606b4e 134 // On avance de 50cm (coordonnés X = 0, Y = -500)
haarkon 1:81896d606b4e 135 motor.setSpeed (300,300);
haarkon 1:81896d606b4e 136 if (x < 0) {
haarkon 1:81896d606b4e 137 etatmvt = 13;
haarkon 1:81896d606b4e 138 pc.printf("\n");
haarkon 1:81896d606b4e 139 }
haarkon 1:81896d606b4e 140 break;
haarkon 1:81896d606b4e 141 case 13 :
haarkon 1:81896d606b4e 142 // On tourne à droite de 90° (sens antitrigo => négatif) => Angle = PI/2
haarkon 1:81896d606b4e 143 motor.setSpeed (-200,200);
haarkon 1:81896d606b4e 144 if (theta > (-PI/2.0)) {
haarkon 1:81896d606b4e 145 etatmvt = 14;
haarkon 1:81896d606b4e 146 pc.printf("\n");
haarkon 1:81896d606b4e 147 }
haarkon 1:81896d606b4e 148 break;
haarkon 1:81896d606b4e 149 case 14 :
haarkon 1:81896d606b4e 150 // On avance de 50cm (coordonnés X = 0, Y = 0)
haarkon 1:81896d606b4e 151 motor.setSpeed (300,300);
haarkon 1:81896d606b4e 152 if (y < 0) {
haarkon 1:81896d606b4e 153 etatmvt = 15;
haarkon 1:81896d606b4e 154 pc.printf("\n");
haarkon 1:81896d606b4e 155 }
haarkon 1:81896d606b4e 156 break;
haarkon 1:81896d606b4e 157 case 15 :
haarkon 1:81896d606b4e 158 // On tourne à droite de 90° (sens antitrigo => négatif) => Angle = 0
haarkon 1:81896d606b4e 159 motor.setSpeed (-200,200);
haarkon 1:81896d606b4e 160 if (theta > 0) {
haarkon 1:81896d606b4e 161 etatmvt = 0;
haarkon 1:81896d606b4e 162 pc.printf("\n");
haarkon 1:81896d606b4e 163 }
haarkon 0:86c5a1f6e21d 164 break;
haarkon 0:86c5a1f6e21d 165 }
haarkon 1:81896d606b4e 166 led1 = !led1;
haarkon 0:86c5a1f6e21d 167 led2 = !led2;
haarkon 1:81896d606b4e 168 wait (0.005);
haarkon 0:86c5a1f6e21d 169 }
haarkon 0:86c5a1f6e21d 170 }