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?
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 | 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 | } |