Asservissement.cpp

00001 /**
00002  * Includes
00003  */
00004 #include "Asservissement.h"
00005 
00006 
00007 
00008 Asservissement::Asservissement(L298 pontH, QEI encodA, QEI encodB, double freq_asserv=100):_pontH(pontH), _encodA(encodA), _encodB(encodB), _freq_correction(freq_asserv)
00009 {
00010     _dist = 0;
00011     _or_deg = 0;
00012 }
00013 
00014 void Asservissement::move(AUTORISATION rep)
00015 {
00016     //uniquement appelé par le créateur de trajectoire
00017     //c'est à ca que ça sert le "autorisation"
00018     if (rep == OUI)
00019     {
00020         
00021     
00022     double or_dist = _or_deg * _dist_roues * M_PI / 360;
00023     double a,b,err_dist,err_ori,pid_dist,pid_ori,pid_a,pid_b;
00024     
00025     //trouver une condition à laquelle sortir de la boucle
00026     //IDEE on incremente un compteur si les deux err sont en dessous d'une
00027     // limite #double _err_statique sinon il retombe à 0 et si pendant 
00028     //LIMITE_TEMPS = 3/FREQ_CORRECTION on atteind compteur = limite_temps 
00029     //alors on sort de la boucle (ca avait bien marché quand j'avais fait des 
00030     // test en mode yolo
00031     //pourrait etre interessant de voir si le createur de trajectoire peut
00032     // pas y faire quelques chose lui aussi
00033     
00034     while(1) 
00035     {
00036         a = _encodA.getPulses() * _resA;
00037         b = _encodB.getPulses() * _resB;
00038         
00039         err_dist = _dist - (a + b) /2;
00040         err_ori = or_dist - (a - b) /2;
00041 
00042         pid_dist = calculPID(err_dist,LINEIQUE);
00043         pid_ori = calculPID(err_ori,ANGULAIRE);
00044 
00045         pid_a = - pid_dist + pid_ori;
00046         pid_b = - pid_dist - pid_ori;
00047         
00048         if (pid_a>_lim_vitesseA)
00049             pid_a = _lim_vitesseA;
00050         else if (pid_a<-_lim_vitesseA)
00051             pid_a = -_lim_vitesseA;
00052 
00053         if (pid_b>_lim_vitesseB)
00054             pid_b=_lim_vitesseB;
00055         else if (pid_b<-_lim_vitesseB)
00056             pid_b = -_lim_vitesseB;
00057 
00058 
00059         _pontH.A(pid_a);
00060         _pontH.B(pid_b);
00061         wait(_freq_correction);
00062         
00063     }
00064     }
00065     
00066 }
00067 
00068 void Asservissement::move(double dist, double or_deg)
00069 {
00070     
00071     double or_dist = or_deg * _dist_roues * M_PI / 360;
00072     double a,b,err_dist,err_ori,pid_dist,pid_ori,pid_a,pid_b;
00073     
00074     //trouver une condition à laquelle sortir de la boucle
00075     //IDEE on incremente un compteur si les deux err sont en dessous d'une
00076     // limite #double _err_statique sinon il retombe à 0 et si pendant 
00077     //LIMITE_TEMPS = 3/FREQ_CORRECTION on atteind compteur = limite_temps 
00078     //alors on sort de la boucle (ca avait bien marché quand j'avais fait des 
00079     // test en mode yolo
00080     //pourrait etre interessant de voir si le createur de trajectoire peut
00081     // pas y faire quelques chose lui aussi
00082     
00083     //peut être qu'il faut faire un truc en mode, le calculateur de trajectoire
00084     //defini en permanance une nouvelle trajectoire, vue que c'est dans la mm
00085     //class, on pourrait plutôt jouer sur le fait que move utilise des variables
00086     //privées _dist et _or_deg
00087     //c'est pour ca que j'ai fait une fonction move qui prend rien en parametre
00088     //elle sera appellée par le générateur de trajectoire
00089     while(1) 
00090     {
00091         a = _encodA.getPulses() * _resA;
00092         b = _encodB.getPulses() * _resB;
00093         
00094         err_dist = dist - (a + b) /2;
00095         err_ori = or_dist - (a - b) /2;
00096 
00097         pid_dist = calculPID(err_dist,LINEIQUE);
00098         pid_ori = calculPID(err_ori,ANGULAIRE);
00099 
00100         pid_a = - pid_dist + pid_ori;
00101         pid_b = - pid_dist - pid_ori;
00102         
00103         if (pid_a>_lim_vitesseA)
00104             pid_a = _lim_vitesseA;
00105         else if (pid_a<-_lim_vitesseA)
00106             pid_a = -_lim_vitesseA;
00107 
00108         if (pid_b>_lim_vitesseB)
00109             pid_b=_lim_vitesseB;
00110         else if (pid_b<-_lim_vitesseB)
00111             pid_b = -_lim_vitesseB;
00112 
00113 
00114         _pontH.A(pid_a);
00115         _pontH.B(pid_b);
00116         wait(_freq_correction);
00117         
00118     }
00119     /*
00120 
00121     float orientation_dist = orientation_deg * (float)DISTANCE_ENTRE_ROUES * 3.14 / 360;
00122 
00123     while(1)
00124     {
00125         pulse_a = (float)encodeur_a.getPulses() * res_a;
00126         pulse_b = (float)encodeur_b.getPulses()* res_b;
00127 
00128         err_dist = distance - (pulse_a + pulse_b) /2;
00129         err_ori = orientation_dist - (pulse_a - pulse_b) /2;
00130 
00131         printf("%f\t%f\t%f\t%f\n\r",err_dist,err_ori,pulse_a,pulse_b);
00132 
00133         pid_dist = PID(err_dist,LINEIQUE);
00134         pid_ori = PID(err_ori,ANGULAIRE);
00135 
00136         pid_a = - pid_dist + pid_ori;
00137         pid_b = - pid_dist - pid_ori;
00138 
00139 
00140         //printf("%f\n\r",pulse_a);
00141 
00142         n_commande_a = pid_a;
00143         n_commande_b = pid_b;
00144 
00145 
00146         if (n_commande_a>1)
00147             n_commande_a=LIMITE_VITESSE;
00148         else if (n_commande_a<-LIMITE_VITESSE)
00149             n_commande_a = -LIMITE_VITESSE;
00150 
00151         if (n_commande_b>LIMITE_VITESSE)
00152             n_commande_b=LIMITE_VITESSE;
00153         else if (n_commande_b<-LIMITE_VITESSE)
00154             n_commande_b = -LIMITE_VITESSE;
00155 
00156 
00157         //moteur_a(n_commande_a);
00158         //moteur_b(n_commande_b);
00159         wait(FREQ_CORRECTION);
00160 
00161     }
00162 
00163 
00164     */
00165 }
00166 
00167 //donne le coefficient à balancer dirrecteument
00168 double Asservissement::calculPID(double erreur,PID type)
00169 {
00170     static double errPrev = 0;
00171     static double errSum = 0;
00172     static double errDif = 0;
00173 
00174     double P,I,D;
00175 
00176     errSum += erreur;
00177 
00178     errDif = erreur - errPrev;
00179     errPrev = erreur;
00180 
00181     if (type == LINEIQUE) {
00182         P = erreur * _Pl;
00183         I = errSum * _Il;
00184         D = errDif * _Dl;
00185     } else if (type == ANGULAIRE) {
00186         P = erreur * _Pa;
00187         I = errSum * _Ia;
00188         D = errDif * _Da;
00189     }
00190 
00191     return P + I + D;
00192 }
00193 
00194 void Asservissement::setPID(PID type, double P, double I, double D)
00195 {
00196     if (type == LINEIQUE) {
00197         _Pl = P;
00198         _Il = I;
00199         _Dl = D;
00200     } else if (type == ANGULAIRE) {
00201         _Pa = P;
00202         _Ia = I;
00203         _Da = D;
00204     }
00205 }
00206 
00207 
00208 void Asservissement::setDimensions(int dist_roues, int dimA, int dimB, int ticA, int ticB)
00209 {
00210         _dist_roues = dist_roues;
00211         _dimA = dimA;
00212         _dimB = dimB;
00213         _ticA = ticA;
00214         _ticB = ticB;
00215         
00216         _resA = _dimA*3.14/_ticA;
00217         _resB = _dimB*3.14/_ticB;
00218 }
00219 
00220 
00221 void Asservissement::setDimensions(int dist_roues, int dim, int tic)
00222 {
00223         _dist_roues = dist_roues;
00224         _dimA = dim;
00225         _dimB = dim;
00226         _ticA = tic;
00227         _ticB = tic;
00228         
00229         _resA = _dimA*3.14/_ticA;
00230         _resB = _resB;
00231 }
00232 
00233 
00234 void Asservissement::setCalculateur(double lim_vitesseA, double lim_vitesseB, double accA, double accB, double deccA, double deccB)
00235 {
00236         _lim_vitesseA = lim_vitesseA;
00237         _lim_vitesseB = lim_vitesseB;
00238         _accA = accA;
00239         _accB = accB;
00240         _deccA = deccA;
00241         _deccB = deccB;
00242 }
00243 
00244 
00245 void Asservissement::setCalculateur(double lim_vitesse, double acc, double decc)
00246 {
00247         _lim_vitesseA = lim_vitesse;
00248         _lim_vitesseB = lim_vitesse;
00249         _accA = acc;
00250         _accB = acc;
00251         _deccA = decc;
00252         _deccB = decc;
00253 }
00254