Thomas Hamain / Mbed 2 deprecated asservissement_robot2

asservissement robot 2, C++, doc fournie

Dependencies:   mbed QEI L298 Asservissement

main.cpp@2:41e56a06c580, 2019-03-20 (annotated)

Committer:
hamaint
Date:
Wed Mar 20 00:28:46 2019 +0000
Revision:
2:41e56a06c580
Parent:
1:11cbd2bf65d7 
version shlag mais c++ en mode bg;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
hamaint 0:a6cbfa280472 1 #include "mbed.h"
hamaint 1:11cbd2bf65d7 2 #include "QEI.h"
hamaint 2:41e56a06c580 3 #include "L298.h"
hamaint 1:11cbd2bf65d7 4 #include "math.h"
hamaint 0:a6cbfa280472 5
hamaint 0:a6cbfa280472 6 Serial pc(SERIAL_TX,SERIAL_RX);
hamaint 0:a6cbfa280472 7
hamaint 1:11cbd2bf65d7 8 QEI encodeur_a (D2, D3, NC, 1200, QEI::X4_ENCODING);
hamaint 1:11cbd2bf65d7 9 QEI encodeur_b (D4, D5, NC, 1200, QEI::X4_ENCODING);
hamaint 1:11cbd2bf65d7 10
hamaint 1:11cbd2bf65d7 11
hamaint 1:11cbd2bf65d7 12 /*pour 8v*/
hamaint 1:11cbd2bf65d7 13 static float Pl = 0.01; //0.03
hamaint 1:11cbd2bf65d7 14 static float Il = 0.0001; //0.001
hamaint 1:11cbd2bf65d7 15 static float Dl = 0;
hamaint 1:11cbd2bf65d7 16
hamaint 1:11cbd2bf65d7 17 static float Pa = 0.01; //0.01
hamaint 1:11cbd2bf65d7 18 static float Ia = 0.0001; //0.0001
hamaint 1:11cbd2bf65d7 19 static float Da = 0;
hamaint 1:11cbd2bf65d7 20
hamaint 1:11cbd2bf65d7 21 #define LINEIQUE 0
hamaint 1:11cbd2bf65d7 22 #define ANGULAIRE 1
hamaint 1:11cbd2bf65d7 23
hamaint 1:11cbd2bf65d7 24 #define FREQ_CORRECTION 0.01
hamaint 1:11cbd2bf65d7 25 #define ERR_STATIQUE 100
hamaint 1:11cbd2bf65d7 26 #define LIMITE_TEMPS 1/FREQ_CORRECTION /*3 secondes*/
hamaint 1:11cbd2bf65d7 27
hamaint 1:11cbd2bf65d7 28 #define DISTANCE_ENTRE_ROUES 270 /*en mm*/
hamaint 1:11cbd2bf65d7 29 #define DIMENSION_ROUE_A 70 /*en mm*/
hamaint 1:11cbd2bf65d7 30 #define DIMENSION_ROUE_B 70 /*en mm*/
hamaint 1:11cbd2bf65d7 31 #define TIC_PAR_TOUR_A 1200
hamaint 1:11cbd2bf65d7 32 #define TIC_PAR_TOUR_B 1200
hamaint 1:11cbd2bf65d7 33
hamaint 1:11cbd2bf65d7 34 #define LIMITE_VITESSE 0.5
hamaint 1:11cbd2bf65d7 35
hamaint 1:11cbd2bf65d7 36 void pos(int commande_a, int commande_b);
hamaint 1:11cbd2bf65d7 37 void pos_a(int commande_a);
hamaint 1:11cbd2bf65d7 38 void pos_b(int commande_b);
hamaint 1:11cbd2bf65d7 39 void deplacement(int theta, int r);
hamaint 1:11cbd2bf65d7 40 float PID(float erreur,float type);
hamaint 1:11cbd2bf65d7 41
hamaint 0:a6cbfa280472 42 int main() {
hamaint 2:41e56a06c580 43
hamaint 2:41e56a06c580 44 L298 pontH();
hamaint 1:11cbd2bf65d7 45
hamaint 1:11cbd2bf65d7 46 float distance = 0;
hamaint 1:11cbd2bf65d7 47 float orientation_deg = 90;
hamaint 0:a6cbfa280472 48
hamaint 1:11cbd2bf65d7 49 float pulse_a;
hamaint 1:11cbd2bf65d7 50 float pulse_b;
hamaint 1:11cbd2bf65d7 51
hamaint 1:11cbd2bf65d7 52 float pid_a;
hamaint 1:11cbd2bf65d7 53 float pid_b;
hamaint 1:11cbd2bf65d7 54
hamaint 1:11cbd2bf65d7 55 float pid_dist;
hamaint 1:11cbd2bf65d7 56 float pid_ori;
hamaint 1:11cbd2bf65d7 57
hamaint 1:11cbd2bf65d7 58 float err_dist;
hamaint 1:11cbd2bf65d7 59 float err_ori;
hamaint 1:11cbd2bf65d7 60
hamaint 1:11cbd2bf65d7 61 float n_commande_a;
hamaint 1:11cbd2bf65d7 62 float n_commande_b;
hamaint 1:11cbd2bf65d7 63
hamaint 1:11cbd2bf65d7 64 float res_a = (float)DIMENSION_ROUE_A*3.14/(float)TIC_PAR_TOUR_A;
hamaint 1:11cbd2bf65d7 65 float res_b = (float)DIMENSION_ROUE_B*3.14/(float)TIC_PAR_TOUR_B;
hamaint 1:11cbd2bf65d7 66
hamaint 1:11cbd2bf65d7 67 float orientation_dist = orientation_deg * (float)DISTANCE_ENTRE_ROUES * 3.14 / 360;
hamaint 0:a6cbfa280472 68
hamaint 2:41e56a06c580 69 //moteur_init();
hamaint 0:a6cbfa280472 70
hamaint 1:11cbd2bf65d7 71 while(1)
hamaint 1:11cbd2bf65d7 72 {
hamaint 1:11cbd2bf65d7 73 pulse_a = (float)encodeur_a.getPulses() * res_a;
hamaint 1:11cbd2bf65d7 74 pulse_b = (float)encodeur_b.getPulses()* res_b;
hamaint 1:11cbd2bf65d7 75
hamaint 1:11cbd2bf65d7 76 err_dist = distance - (pulse_a + pulse_b) /2;
hamaint 1:11cbd2bf65d7 77 err_ori = orientation_dist - (pulse_a - pulse_b) /2;
hamaint 1:11cbd2bf65d7 78
hamaint 1:11cbd2bf65d7 79 printf("%f\t%f\t%f\t%f\n\r",err_dist,err_ori,pulse_a,pulse_b);
hamaint 1:11cbd2bf65d7 80
hamaint 1:11cbd2bf65d7 81 pid_dist = PID(err_dist,LINEIQUE);
hamaint 1:11cbd2bf65d7 82 pid_ori = PID(err_ori,ANGULAIRE);
hamaint 1:11cbd2bf65d7 83
hamaint 1:11cbd2bf65d7 84 pid_a = - pid_dist + pid_ori;
hamaint 1:11cbd2bf65d7 85 pid_b = - pid_dist - pid_ori;
hamaint 0:a6cbfa280472 86
hamaint 0:a6cbfa280472 87
hamaint 1:11cbd2bf65d7 88 //printf("%f\n\r",pulse_a);
hamaint 1:11cbd2bf65d7 89
hamaint 1:11cbd2bf65d7 90 n_commande_a = pid_a;
hamaint 1:11cbd2bf65d7 91 n_commande_b = pid_b;
hamaint 1:11cbd2bf65d7 92
hamaint 1:11cbd2bf65d7 93
hamaint 1:11cbd2bf65d7 94 if (n_commande_a>1)
hamaint 1:11cbd2bf65d7 95 n_commande_a=LIMITE_VITESSE;
hamaint 1:11cbd2bf65d7 96 else if (n_commande_a<-LIMITE_VITESSE)
hamaint 1:11cbd2bf65d7 97 n_commande_a = -LIMITE_VITESSE;
hamaint 1:11cbd2bf65d7 98
hamaint 1:11cbd2bf65d7 99 if (n_commande_b>LIMITE_VITESSE)
hamaint 1:11cbd2bf65d7 100 n_commande_b=LIMITE_VITESSE;
hamaint 1:11cbd2bf65d7 101 else if (n_commande_b<-LIMITE_VITESSE)
hamaint 1:11cbd2bf65d7 102 n_commande_b = -LIMITE_VITESSE;
hamaint 0:a6cbfa280472 103
hamaint 1:11cbd2bf65d7 104
hamaint 2:41e56a06c580 105 //moteur_a(n_commande_a);
hamaint 2:41e56a06c580 106 //moteur_b(n_commande_b);
hamaint 1:11cbd2bf65d7 107 wait(FREQ_CORRECTION);
hamaint 1:11cbd2bf65d7 108
hamaint 1:11cbd2bf65d7 109 }
hamaint 0:a6cbfa280472 110
hamaint 1:11cbd2bf65d7 111 return 0;
hamaint 0:a6cbfa280472 112 }
hamaint 0:a6cbfa280472 113
hamaint 0:a6cbfa280472 114
hamaint 1:11cbd2bf65d7 115
hamaint 1:11cbd2bf65d7 116
hamaint 1:11cbd2bf65d7 117 float PID(float erreur,float type)
hamaint 1:11cbd2bf65d7 118 {
hamaint 1:11cbd2bf65d7 119 static float errPrev = 0;
hamaint 1:11cbd2bf65d7 120 static float errSum = 0;
hamaint 1:11cbd2bf65d7 121 static float errDif = 0;
hamaint 1:11cbd2bf65d7 122
hamaint 1:11cbd2bf65d7 123 float P,I,D;
hamaint 1:11cbd2bf65d7 124
hamaint 1:11cbd2bf65d7 125 errSum += erreur;
hamaint 1:11cbd2bf65d7 126
hamaint 1:11cbd2bf65d7 127 errDif = erreur - errPrev;
hamaint 1:11cbd2bf65d7 128 errPrev = erreur;
hamaint 1:11cbd2bf65d7 129
hamaint 1:11cbd2bf65d7 130 if (type){
hamaint 1:11cbd2bf65d7 131 P = erreur * Pl;
hamaint 1:11cbd2bf65d7 132 I = errSum * Il;
hamaint 1:11cbd2bf65d7 133 D = errDif * Dl;
hamaint 1:11cbd2bf65d7 134 } else {
hamaint 1:11cbd2bf65d7 135 P = erreur * Pa;
hamaint 1:11cbd2bf65d7 136 I = errSum * Ia;
hamaint 1:11cbd2bf65d7 137 D = errDif * Da;
hamaint 0:a6cbfa280472 138 }
hamaint 1:11cbd2bf65d7 139
hamaint 1:11cbd2bf65d7 140 return P + I + D; //Le résultat est la somme des trois
hamaint 1:11cbd2bf65d7 141 //composantes calculées précédemment
hamaint 1:11cbd2bf65d7 142 }
hamaint 0:a6cbfa280472 143
hamaint 1:11cbd2bf65d7 144
hamaint 1:11cbd2bf65d7 145
hamaint 1:11cbd2bf65d7 146
hamaint 1:11cbd2bf65d7 147 /*asservissement des deux roues individuellement*/
hamaint 1:11cbd2bf65d7 148 /*pas très utile pour le déplacement mais intéressant*/
hamaint 1:11cbd2bf65d7 149 /*de garder comme un exemple*/
hamaint 1:11cbd2bf65d7 150
hamaint 1:11cbd2bf65d7 151 /*
hamaint 1:11cbd2bf65d7 152
hamaint 1:11cbd2bf65d7 153 void pos(int commande_a, int commande_b)
hamaint 1:11cbd2bf65d7 154 {
hamaint 1:11cbd2bf65d7 155
hamaint 1:11cbd2bf65d7 156
hamaint 1:11cbd2bf65d7 157
hamaint 1:11cbd2bf65d7 158 int erreur_a = 0;
hamaint 1:11cbd2bf65d7 159 int somme_erreur_a = 0;
hamaint 1:11cbd2bf65d7 160
hamaint 1:11cbd2bf65d7 161 int derniere_erreur_a = 0;
hamaint 1:11cbd2bf65d7 162 int derivee_a = 0;
hamaint 1:11cbd2bf65d7 163
hamaint 1:11cbd2bf65d7 164 float n_commande_a;
hamaint 1:11cbd2bf65d7 165
hamaint 1:11cbd2bf65d7 166 int validation_position_a = 0;
hamaint 1:11cbd2bf65d7 167
hamaint 1:11cbd2bf65d7 168 int erreur_b = 0;
hamaint 1:11cbd2bf65d7 169 int somme_erreur_b = 0;
hamaint 1:11cbd2bf65d7 170
hamaint 1:11cbd2bf65d7 171 int derniere_erreur_b = 0;
hamaint 1:11cbd2bf65d7 172 int derivee_b = 0;
hamaint 0:a6cbfa280472 173
hamaint 1:11cbd2bf65d7 174 float n_commande_b;
hamaint 1:11cbd2bf65d7 175
hamaint 1:11cbd2bf65d7 176 int validation_position_b = 0;
hamaint 1:11cbd2bf65d7 177
hamaint 1:11cbd2bf65d7 178 do
hamaint 1:11cbd2bf65d7 179 {
hamaint 1:11cbd2bf65d7 180
hamaint 1:11cbd2bf65d7 181 erreur_a = encodeur_a.getPulses()-commande_a;
hamaint 1:11cbd2bf65d7 182 erreur_b = encodeur_b.getPulses()-commande_b;
hamaint 1:11cbd2bf65d7 183
hamaint 1:11cbd2bf65d7 184 somme_erreur_a = somme_erreur_a + erreur_a;
hamaint 1:11cbd2bf65d7 185 somme_erreur_b = somme_erreur_b + erreur_b;
hamaint 0:a6cbfa280472 186
hamaint 1:11cbd2bf65d7 187 derivee_a = erreur_a - derniere_erreur_a;
hamaint 1:11cbd2bf65d7 188 derivee_b = erreur_b - derniere_erreur_b;
hamaint 1:11cbd2bf65d7 189
hamaint 1:11cbd2bf65d7 190 n_commande_a = erreur_a * P + somme_erreur_a * I + derivee_a * D;
hamaint 1:11cbd2bf65d7 191 n_commande_b = erreur_b * P + somme_erreur_b * I + derivee_b * D;
hamaint 1:11cbd2bf65d7 192
hamaint 1:11cbd2bf65d7 193 derniere_erreur_a = erreur_a;
hamaint 1:11cbd2bf65d7 194 derniere_erreur_b = erreur_b;
hamaint 1:11cbd2bf65d7 195
hamaint 1:11cbd2bf65d7 196 if (n_commande_a>1)
hamaint 1:11cbd2bf65d7 197 n_commande_a=1;
hamaint 1:11cbd2bf65d7 198 else if (n_commande_a<-1)
hamaint 1:11cbd2bf65d7 199 n_commande_a = -1;
hamaint 0:a6cbfa280472 200
hamaint 1:11cbd2bf65d7 201 if (n_commande_b>1)
hamaint 1:11cbd2bf65d7 202 n_commande_b=1;
hamaint 1:11cbd2bf65d7 203 else if (n_commande_b<-1)
hamaint 1:11cbd2bf65d7 204 n_commande_b = -1;
hamaint 1:11cbd2bf65d7 205
hamaint 1:11cbd2bf65d7 206 moteur_a(-n_commande_a);
hamaint 1:11cbd2bf65d7 207 moteur_b(-n_commande_b);
hamaint 1:11cbd2bf65d7 208
hamaint 1:11cbd2bf65d7 209 if (abs(erreur_a)<ERR_STATIQUE)
hamaint 1:11cbd2bf65d7 210 validation_position_a++;
hamaint 1:11cbd2bf65d7 211 else
hamaint 1:11cbd2bf65d7 212 validation_position_a = 0;
hamaint 1:11cbd2bf65d7 213
hamaint 1:11cbd2bf65d7 214 if (abs(erreur_b)<ERR_STATIQUE)
hamaint 1:11cbd2bf65d7 215 validation_position_b++;
hamaint 1:11cbd2bf65d7 216 else
hamaint 1:11cbd2bf65d7 217 validation_position_b = 0;
hamaint 1:11cbd2bf65d7 218
hamaint 1:11cbd2bf65d7 219 wait(FREQ_CORRECTION);
hamaint 1:11cbd2bf65d7 220
hamaint 1:11cbd2bf65d7 221 } while (validation_position_a<LIMITE_TEMPS || validation_position_b<LIMITE_TEMPS);
hamaint 1:11cbd2bf65d7 222 moteur_a(0);
hamaint 1:11cbd2bf65d7 223 moteur_b(0);
hamaint 0:a6cbfa280472 224
hamaint 1:11cbd2bf65d7 225 encodeur_a.reset();
hamaint 1:11cbd2bf65d7 226 encodeur_b.reset();
hamaint 1:11cbd2bf65d7 227 }
hamaint 0:a6cbfa280472 228
hamaint 1:11cbd2bf65d7 229
hamaint 1:11cbd2bf65d7 230
hamaint 0:a6cbfa280472 231
hamaint 1:11cbd2bf65d7 232
hamaint 1:11cbd2bf65d7 233 */