Nouvelle version de l'asservissement du robot 2 effectuée pour le projet de IESE4
Embed:
(wiki syntax)
Show/hide line numbers
main.cpp
00001 #include "mbed.h" 00002 #include "QEI.h" 00003 #include "Moteur.h" 00004 #include "math.h" 00005 00006 #define PI 3.14159265359 00007 00008 #define DISTANCE_ENTRE_ROUES 197.0 /*en mm*/ 00009 #define DIAMETRE_ROUE_A 70.06 /*en mm*/ 00010 #define DIAMETRE_ROUE_B 70.02 /*en mm*/ 00011 #define TIC_PAR_TOUR_A 1200 /*Nombre de tics de l'encodeur*/ 00012 #define TIC_PAR_TOUR_B 1200 /*Nombre de tics de l'encodeur*/ 00013 #define DIST_PAR_TIC_A 0.183417 /*Distance parcourue entre 2 tics pour la roue A en mm*/ /*DIAMETRE_ROUE_A*PI/TIC_PAR_TOUR_A*/ 00014 #define DIST_PAR_TIC_B 0.183312 /*Distance parcourue entre 2 tics pour la roue B en mm*/ /*DIAMETRE_ROUE_B*PI/TIC_PAR_TOUR_B*/ 00015 00016 #define NIV_PRECI_MM 1 /*Précision des déplacements du robot*/ 00017 #define NIV_PRECI_DEG 1 /*Précision des rotations du robot*/ 00018 00019 // Liaison PC 00020 Serial pc(SERIAL_TX, SERIAL_RX, 115200); 00021 00022 // Initialisation encodeur 00023 QEI encodeur_a (D2, D3, NC, 1200, QEI::X4_ENCODING); 00024 QEI encodeur_b (D4, D5, NC, 1200, QEI::X4_ENCODING); 00025 00026 Ticker timer; 00027 DigitalIn way(PC_8,PullDown); 00028 DigitalIn languette(D12,PullDown); 00029 00030 // Prototypes des fonctions 00031 double* get_vitesses(); 00032 double* get_distances(); 00033 double get_angle(double dist_A, double dist_B); 00034 double get_dist_moy(double dist_A, double dist_B); 00035 double PID_vitA(double erreur_vitA); 00036 double PID_vitB(double erreur_vitB); 00037 double PID_distA(double erreur_distA); 00038 double PID_distB(double erreur_distB); 00039 double PID_angleA(double erreur_angleA); 00040 double PID_angleB(double erreur_angleB); 00041 double PID_distA2(double erreur_distA2); 00042 double PID_distB2(double erreur_distB2); 00043 double PID_angleA2(double erreur_angleA2); 00044 double PID_angleB2(double erreur_angleB2); 00045 00046 // Coefficients PID pour asservissement en vitesse 00047 const double KpvA = 0.00240; //0.00240 00048 const double TivA = 0.0000020; //0.000020 00049 const double TdvA = 0.0; 00050 00051 const double KpvB = 0.00245; // 0.00245 00052 const double TivB = 0.0000024; // 0.00024 00053 const double TdvB = 0.0; 00054 00055 // Coefficients PID pour asservissement en position 00056 const double KpdA = 5.5;//5.5 -> 20 cm; 00057 const double TidA = 0.007; //0.007 00058 const double TddA = 0.0; 00059 00060 const double KpdB = 5.5;//5.5 -> 20 cm; 00061 const double TidB = 0.007; //0.007 00062 const double TddB = 0.0; 00063 00064 // Coefficients PID pour asservissement en angle 00065 const double KpaA = 8; //8 00066 const double KiaA = 0.08;//0.08 00067 const double KdaA = 0.0; 00068 00069 const double KpaB = 8; //8 00070 const double KiaB = 0.08;//0.08 00071 const double KdaB = 0.0; 00072 00073 00074 // Coefficients PID pour asservissement en position2 00075 const double KpdA2 = 0;//5.5 -> 20 cm; 00076 const double TidA2 = 0.00; //0.007 00077 const double TddA2 = 0.0; 00078 00079 const double KpdB2 = 0;//5.5 -> 20 cm; 00080 const double TidB2 = 0.00; //0.007 00081 const double TddB2 = 0.0; 00082 00083 // Coefficients PID pour asservissement en angle2 00084 const double KpaA2 = 7; //8 00085 const double KiaA2 = 0.005;//0.08 00086 const double KdaA2 = 0.0; 00087 00088 const double KpaB2 = 6.8; //8 00089 const double KiaB2 = 0.005;//0.08 00090 const double KdaB2 = 0.0; 00091 00092 // Erreurs moteur A 00093 double errPrev_vitA = 0, errSum_vitA = 0, errDif_vitA = 0; 00094 double errPrev_distA = 0, errSum_distA = 0, errDif_distA = 0; 00095 00096 // Erreurs moteur B 00097 double errPrev_vitB = 0, errSum_vitB = 0, errDif_vitB = 0; 00098 double errPrev_distB = 0, errSum_distB = 0, errDif_distB = 0; 00099 00100 // Erreurs angle 00101 double errPrev_angleA = 0, errSum_angleA = 0, errDif_angleA = 0; 00102 double errPrev_angleB = 0, errSum_angleB = 0, errDif_angleB = 0; 00103 00104 // Erreurs distance2 00105 double errPrev_distA2 = 0, errSum_distA2 = 0, errDif_distA2 = 0; 00106 double errPrev_distB2 = 0, errSum_distB2 = 0, errDif_distB2 = 0; 00107 00108 // Erreurs angle2 00109 double errPrev_angleA2 = 0, errSum_angleA2 = 0, errDif_angleA2 = 0; 00110 double errPrev_angleB2 = 0, errSum_angleB2 = 0, errDif_angleB2 = 0; 00111 00112 // Tableau des vitesses, positions [0] moteur A / [1] moteur B et coord [0]--> X / [1]--> Y / [2] angle 00113 double T_vitesses[2], T_distances[2], T_coord[3]; 00114 const uint16_t L = 1000; //Longueur tableau d'affichage 00115 const uint8_t nbr_consigne = 1; 00116 const uint8_t nbr_result_afficher = 6; 00117 00118 int main(){ 00119 printf("\nbjr \r\n\n"); 00120 00121 moteur_init(); /*Initialisation des moteurs*/ 00122 00123 // Consigne 00124 double consigne_vit_A, consigne_vit_B; 00125 double consigne_dist; // = 200; 00126 double consigne_angle; // = 0; 00127 double T_consigne_dist[nbr_consigne] = {0.0}; 00128 double T_consigne_angle[nbr_consigne] = {10.0}; 00129 00130 //Initialisation des variables 00131 int32_t i = 0, j = 0, k = 0, l = 0, p = 0, r = 0; 00132 double dist_A, dist_B, dist, vit_A, vit_B; 00133 double pwm_PID_A, pwm_PID_B; 00134 double erreur_vit_A, erreur_vit_B; 00135 double erreur_dist;// erreur_dist_A, erreur_dist_B; 00136 double temps, temps0; 00137 double tmps_start; //dif_temps; 00138 double erreur_angle, angle; 00139 double pid_distA, pid_distB, pid_angleA, pid_angleB; 00140 00141 // Initionalise le tableau des vitesses avec que des zéros 00142 double T_aff[L][nbr_result_afficher]; 00143 for(k = 0 ; k < L ; k++){ 00144 for(l = 0 ; l < nbr_result_afficher ; l++){ 00145 T_aff[k][l] = 0; 00146 } 00147 } 00148 00149 //Mesure du temps au lancement du programme en µs 00150 temps0 = us_ticker_read(); 00151 for (p = 0; p < nbr_consigne; p++){ 00152 consigne_dist = T_consigne_dist[p]; 00153 consigne_angle = T_consigne_angle[p]; 00154 while(1) { 00155 // Retourne la vitesse du robot 00156 get_vitesses(); 00157 vit_A = T_vitesses[0]; 00158 vit_B = T_vitesses[1]; 00159 00160 // Retourne la distance parcourue par le robot 00161 get_distances(); 00162 dist_A = T_distances[0]; 00163 dist_B = T_distances[1]; 00164 00165 // Calcul la position du robot 00166 dist = get_dist_moy(dist_A, dist_B); 00167 00168 // Erreur angle en degré 00169 angle = get_angle(dist_A, dist_B); 00170 erreur_angle = consigne_angle - angle; 00171 00172 //Temps depuis le lancement du robot 00173 temps = us_ticker_read(); //Mesure de temps initiale en µs 00174 tmps_start = (temps - temps0)/1000000; 00175 00176 // Erreur de distance 00177 erreur_dist = consigne_dist - dist; 00178 00179 // Calcul de la consigne de vitesse à partir du PID en distance 00180 if(consigne_angle > 3){ 00181 pid_distA = PID_distA2(erreur_dist); 00182 pid_distB = PID_distB2(erreur_dist); 00183 pid_angleA = PID_angleA2(erreur_angle); 00184 pid_angleB = PID_angleB2(erreur_angle); 00185 } 00186 else{ 00187 pid_distA = PID_distA(erreur_dist); 00188 pid_distB = PID_distB(erreur_dist); 00189 pid_angleA = PID_angleA(erreur_angle); 00190 pid_angleB = PID_angleB(erreur_angle); 00191 } 00192 consigne_vit_A = pid_distA - pid_angleA; 00193 consigne_vit_B = pid_distB + pid_angleB; 00194 00195 // Erreur de vitesse 00196 erreur_vit_A = consigne_vit_A - vit_A; 00197 erreur_vit_B = consigne_vit_B - vit_B; 00198 00199 // Calcul de la consigne pour le moteur à partir du PID en position 00200 pwm_PID_A = PID_vitA(erreur_vit_A); 00201 pwm_PID_B = PID_vitB(erreur_vit_B); 00202 00203 // Limite la vitesse des moteurs à 50% 00204 if((pwm_PID_A) >= 0.556f){ 00205 pwm_PID_A = 0.556; 00206 } 00207 if((pwm_PID_B) >= 0.55f){ 00208 pwm_PID_B = 0.55; 00209 } 00210 00211 if((pwm_PID_A) <= -0.556f){ 00212 pwm_PID_A = -0.556; 00213 } 00214 if((pwm_PID_B) <= -0.55f){ 00215 pwm_PID_B = -0.55; 00216 } 00217 00218 // Envoie la nouvelle commande de vitesse 00219 moteur_b(pwm_PID_B); 00220 moteur_a(pwm_PID_A); 00221 00222 //Affichage des distances et des vitesses /!\ Couteux en temps, modifie le temps de réponse de l'asservissement /!\ Uniquement debug 00223 //printf(" dist_A = %.2f , dist_B = %.2f , vit_A = %.1f mm/s, vit_B = %.1f mm/s, temps = %.2f, i = %2d \n\r erreur_dist_A = %.1f mm/s, erreur_dist_B = %.1f mm/s erreur_vit_A = %.1f mm/s, erreur_vit_B = %.1f mm/s \n\n\r", dist_A, dist_B, vit_A, vit_B, tmps_start, i, erreur_dist_A, erreur_dist_B, erreur_vit_A, erreur_vit_B); 00224 //printf(" A = %.2f, B = %.2f, A = %.2f, B = %.2f\n\r", pid_distA, pid_distB, pid_angleA, pid_angleB); 00225 //printf("A = %.2f, B = %.2f\n\r", consigne_vit_A, consigne_vit_B); 00226 if(i < L){ 00227 T_aff[i][0] = dist; 00228 T_aff[i][1] = angle; 00229 T_aff[i][2] = pid_distA; 00230 T_aff[i][3] = pid_angleA; 00231 T_aff[i][4] = consigne_vit_A; 00232 T_aff[i][5] = consigne_vit_B; 00233 } 00234 i++; 00235 if(i == L){ 00236 printf("Tableau rempli\r\n"); 00237 } 00238 00239 // Le robot s'arrête à + ou - de NIV_PRECI_MM de la commande de distance 00240 if ((erreur_dist < NIV_PRECI_MM) && (erreur_dist > -NIV_PRECI_MM)) { 00241 if ((erreur_angle < NIV_PRECI_DEG && erreur_angle > -NIV_PRECI_DEG)) { 00242 moteur_a(0); 00243 moteur_b(0); 00244 break; 00245 } 00246 } 00247 00248 // Arrete le robot après 8 secondes 00249 if (tmps_start > 4){ 00250 moteur_a(0); 00251 moteur_b(0); 00252 break; 00253 } 00254 00255 // Arrête les moteurs après 700mm 00256 if (dist > 700 || dist < -700) { 00257 moteur_a(0); 00258 moteur_b(0); 00259 break; 00260 } 00261 // Arrête les moteurs après rotation de 200° 00262 if (angle > 200 || angle < -200) { 00263 moteur_a(0); 00264 moteur_b(0); 00265 break; 00266 } 00267 } 00268 00269 // Affiche les résultats après que le robot soit arreté 00270 printf("Consigne %d \n\r", p); 00271 for(j = 0; j < nbr_result_afficher; j++){ 00272 printf("\n\rValeur %d \n\r", j+1); 00273 for(r = 0; r < L; r+=L/400){ 00274 printf("%.2f, ", T_aff[r][j]); 00275 } 00276 } 00277 // Rempli le tableau d'affichage de 0 00278 for(k = 0 ; k < L ; k++){ 00279 for(l = 0 ; l < nbr_result_afficher ; l++){ 00280 T_aff[k][l] = 0; 00281 } 00282 } 00283 00284 // Recommence au début du tableau d'affichage 00285 i = 0; 00286 00287 temps0 = us_ticker_read(); 00288 } 00289 } 00290 00291 double* get_vitesses(){ 00292 int32_t tic_A, tic_B, dif_tic_A, dif_tic_B; 00293 double temps, dif_temps, dif_dist_A, dif_dist_B; 00294 double ddist, dangle, dx, dy, angle; 00295 00296 //Mesure du nombres de tic de chaque roue 00297 tic_A = encodeur_a.getPulses(); 00298 tic_B = encodeur_b.getPulses(); 00299 angle = get_angle(T_distances[0], T_distances[1])*(PI/180); 00300 00301 temps = us_ticker_read(); //Mesure de temps initiale en µs 00302 wait(0.005); //"Pause" pour ralentir le programme --> Effectue les mesures tous les 5ms(200Hz) /!\ Modifier cette valeur modifie le coef intégrateur des PID /!\ 00303 00304 //Mesure de la différence de tic et de temps entre avant et après la "pause" 00305 dif_tic_A = encodeur_a.getPulses() - tic_A; 00306 dif_tic_B = encodeur_b.getPulses() - tic_B; 00307 dif_temps = (us_ticker_read() - temps)/1000000; 00308 00309 //Distance parcourue pendant la "pause" 00310 dif_dist_A = dif_tic_A*DIST_PAR_TIC_A; 00311 dif_dist_B = dif_tic_B*DIST_PAR_TIC_B; 00312 00313 // Calcul des petits déplacements en coordonnées pendant la "pause" 00314 ddist = (dif_dist_A + dif_dist_B)/2; 00315 dangle = ((dif_dist_A - dif_dist_B)/DISTANCE_ENTRE_ROUES)*(180/PI); 00316 dx = ddist*cos(angle); 00317 dy = ddist*sin(angle); 00318 00319 //Tableau des coordonnées (x, y et angle) 00320 T_coord[0] = T_coord[0] + dx; 00321 T_coord[1] = T_coord[1] + dy; 00322 T_coord[2] = T_coord[2] + dangle; 00323 00324 //Vitesse moyenne pendant la "pause" 00325 T_vitesses[0] = dif_dist_A/dif_temps; 00326 T_vitesses[1] = dif_dist_B/dif_temps; 00327 00328 return T_vitesses; 00329 } 00330 00331 double* get_distances(){ 00332 int32_t tic_A, tic_B; 00333 00334 //Mesure du nombres de tic de chaque roue²i 00335 tic_A = encodeur_a.getPulses(); 00336 tic_B = encodeur_b.getPulses(); 00337 00338 //Distance parcourue depuis l'allumage du robot pour chaque roue 00339 T_distances[0] = tic_A*DIST_PAR_TIC_A; 00340 T_distances[1] = tic_B*DIST_PAR_TIC_B; 00341 00342 return T_distances; 00343 } 00344 00345 double get_angle(double dist_A, double dist_B){ 00346 //return atan((dist_A-dist_B)/DISTANCE_ENTRE_ROUES)*(92); 00347 //return atan2((double)dist_A-dist_B,DISTANCE_ENTRE_ROUES)*(114.592); 00348 return fmod(((dist_A-dist_B)/DISTANCE_ENTRE_ROUES)*(180/PI), 360); 00349 } 00350 00351 double get_dist_moy(double dist_A, double dist_B){ 00352 return (dist_A + dist_B)/2; 00353 } 00354 00355 double PID_vitA(double erreur_vit){ 00356 double P, I, D; 00357 errSum_vitA += erreur_vit; 00358 errDif_vitA = erreur_vit - errPrev_vitA; 00359 errPrev_vitA = erreur_vit; 00360 00361 P = erreur_vit * KpvA; 00362 I = errSum_vitA * TivA; 00363 D = errDif_vitA * TdvA; 00364 return P + I + D; 00365 } 00366 00367 double PID_vitB(double erreur_vit){ 00368 double P, I, D; 00369 errSum_vitB += erreur_vit; 00370 errDif_vitB = erreur_vit - errPrev_vitB; 00371 errPrev_vitB = erreur_vit; 00372 00373 P = erreur_vit * KpvB; 00374 I = errSum_vitB * TivB; 00375 D = errDif_vitB * TdvB; 00376 return P + I + D; 00377 } 00378 00379 //PID pour le déplacement en ligne droite seul 00380 double PID_distA(double erreur_dist){ 00381 double P, I, D; 00382 errSum_distA += erreur_dist; 00383 errDif_distA = erreur_dist - errPrev_distA; 00384 errPrev_distA = erreur_dist; 00385 00386 P = erreur_dist * KpdA; 00387 I = errSum_distA * TidA; 00388 D = errDif_distA * TddA; 00389 return P + I + D; 00390 } 00391 00392 double PID_distB(double erreur_dist){ 00393 double P, I, D; 00394 errSum_distB += erreur_dist; 00395 errDif_distB = erreur_dist - errPrev_distB; 00396 errPrev_distB = erreur_dist; 00397 00398 P = erreur_dist * KpdB; 00399 I = errSum_distB * TidB; 00400 D = errDif_distB * TddB; 00401 return P + I + D; 00402 } 00403 00404 double PID_angleA(double erreur_angle){ 00405 double P, I, D; 00406 errSum_angleA += erreur_angle; 00407 errDif_angleA = erreur_angle - errPrev_angleA; 00408 errPrev_angleA = erreur_angle; 00409 00410 P = erreur_angle * KpaA; 00411 I = errSum_angleA * KiaA; 00412 D = errDif_angleA * KdaA; 00413 return P + I + D; 00414 } 00415 00416 double PID_angleB(double erreur_angle){ 00417 00418 double P, I, D; 00419 errSum_angleB += erreur_angle; 00420 errDif_angleB = erreur_angle - errPrev_angleB; 00421 errPrev_angleB = erreur_angle; 00422 00423 P = erreur_angle * KpaB; 00424 I = errSum_angleB * KiaB; 00425 D = errDif_angleB * KdaB; 00426 return P + I + D; 00427 } 00428 00429 // PID pour la rotation seule 00430 double PID_distA2(double erreur_dist2){ 00431 double P, I, D; 00432 errSum_distA2 += erreur_dist2; 00433 errDif_distA2 = erreur_dist2 - errPrev_distA2; 00434 errPrev_distA2 = erreur_dist2; 00435 00436 P = erreur_dist2 * KpdA2; 00437 I = errSum_distA2 * TidA2; 00438 D = errDif_distA2 * TddA2; 00439 return P + I + D; 00440 } 00441 00442 double PID_distB2(double erreur_dist2){ 00443 double P, I, D; 00444 errSum_distB2 += erreur_dist2; 00445 errDif_distB2 = erreur_dist2 - errPrev_distB2; 00446 errPrev_distB2 = erreur_dist2; 00447 00448 P = erreur_dist2 * KpdB2; 00449 I = errSum_distB2 * TidB2; 00450 D = errDif_distB2 * TddB2; 00451 return P + I + D; 00452 } 00453 00454 double PID_angleA2(double erreur_angle2){ 00455 double P, I, D; 00456 errSum_angleA2 += erreur_angle2; 00457 errDif_angleA2 = erreur_angle2 - errPrev_angleA2; 00458 errPrev_angleA2 = erreur_angle2; 00459 00460 P = erreur_angle2 * KpaA2; 00461 I = errSum_angleA2 * KiaA2; 00462 D = errDif_angleA2 * KdaA2; 00463 return P + I + D; 00464 } 00465 00466 double PID_angleB2(double erreur_angle2){ 00467 double P, I, D; 00468 errSum_angleB2 += erreur_angle2; 00469 errDif_angleB2 = erreur_angle2 - errPrev_angleB2; 00470 errPrev_angleB2 = erreur_angle2; 00471 00472 P = erreur_angle2 * KpaB2; 00473 I = errSum_angleB2 * KiaB2; 00474 D = errDif_angleB2 * KdaB2; 00475 return P + I + D; 00476 }
Generated on Thu Aug 25 2022 06:33:45 by
1.7.2