Nouvelle version de l'asservissement du robot 2 effectuée pour le projet de IESE4

Dependencies:   mbed QEI

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers main.cpp Source File

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 }