odo_asserv.cpp

00001 /* #include */
00002 #include "pins.h"
00003 
00004 /* #define & constantes */
00005 #define VMAXROT 0.020
00006 #define VMAXLIN 0.040
00007 #define entraxe 253 // (Valeur théorique = 255)
00008 const double coeffGLong = 5.956, coeffDLong = 5.956; // tics/millimètre
00009 
00010 /* Stratégie */
00011 int indice = 0;
00012 /*
00013 int objEtape[NbObj] = {0,1,1,1,1,1,1,1,1,1,1,1,1}; // Stratégie côté bleu
00014 double objX[NbObj] =  {110,645,645,468,645,336,189,200,645,468,645,371,215};
00015 double objY[NbObj] =  {1085,1200,1490,1490,1490,1788,1032,1200,920,920,920,519,780};
00016 int objRecule[NbObj]= {0,0,0,0,1,0,0,1,0,0,1,0,0};
00017 */
00018 
00019 /*
00020 int objEtape[NbObj] = {0,1,1,1,1,1,1,1,1,1,1,1,1}; // Stratégie côté bleu
00021 double objX[NbObj] =  {110,645,645,200,645,336,189,200,645,468,645,371,215};
00022 double objY[NbObj] =  {1085,1200,1400,1400,1400,1788,1032,1200,920,920,920,519,780};
00023 int objRecule[NbObj]= {0,0,0,0,1,0,0,1,0,0,1,0,0};
00024 */
00025 
00026 
00027 //int objEtape[NbObj] = {0,1,1,1}; // Stratégie côté bleu
00028 int objEtape [4] = {0,1,1,1};
00029 double objX[4] = {0,660, 660, 210};
00030 double objY[4] = {0,1070,1650,1300};
00031 int objRecule[NbObj]= {0,0,0,0};
00032 
00033 
00034 /* Variable globale */
00035 Ticker Ticker_asserv;
00036 
00037 long comptG = 0, comptD = 0; // nb de tics comptés pour chaque codeur
00038 
00039 ///// INTERRUPTIONS CODEURS
00040 
00041 void cdgaRise()
00042 {
00043     if(cdgB) comptG--;
00044     else comptG++;
00045 }
00046 
00047 void cddaRise()
00048 {
00049     if(cddB) comptD--;
00050     else comptD++;
00051 }
00052 
00053 
00054 const double coeffG = 0.16008537;
00055 const double coeffD = 0.16059957;
00056 double dDist = 0, dAngl = 0; // Distance moyenne du robot et orientation
00057 double x = 110, y = 1070, O = 0;
00058 
00059 void odo2()
00060 {
00061     dDist = (double) ((comptG * coeffG) + (comptD * coeffD)) / 2.0f;
00062     dAngl = (double) ((comptD * coeffD) - (comptG * coeffG)) / entraxe;
00063 
00064     x += (double) dDist * cos(O);
00065     y += (double) dDist * sin(O);
00066     O += (double) dAngl;
00067 
00068     if (O > 3.1415) O = O - (2.0f * 3.1415f);
00069     if (O < -3.1415) O = O + (2.0f * 3.1415f);
00070 
00071     comptG = 0;
00072     comptD = 0;
00073 }
00074 //*/
00075 
00076 
00077 double distanceCible = 0;
00078 double xC = 0, yC = 0; // x = xR et y = yR
00079 double consigneOrientation = 0;
00080 int signe = 1;
00081 double cmdD = 0, cmdG = 0;
00082 double erreurAngle = 0;
00083 double erreurPre = 0;
00084 double deltaErreur = 0;
00085 const double coeffPro = 0.020; // 0.023 de base
00086 const double coeffDer = 0.010; // 0.023 de base
00087 
00088 // Ligne droite
00089 double erreurPreDist = 0;
00090 double deltaErreurDist = 0;
00091 const double coeffProDist = 0.0008; // 0.010 de base
00092 const double coeffDerDist = 0.0008; // 0.010 de base
00093 
00094 // NEW NEW NEW NEW
00095 int fnc = 0;
00096 bool acc = 1;
00097 double distancePrecedente = 0;
00098 bool stt = 0; // Dépassement du point
00099 
00100 double OrientationMem = 0;
00101 double OrPlusPi2 = 0, OrMoinsPi2 = 0;
00102 
00103 // Controle dépassement cible
00104 double distanceMem = 0;
00105 double distancePlus = 0;
00106 
00107 void asserv()
00108 {
00109     // Odométrie
00110     odo2();
00111 
00112     // Calcul de la cible
00113 
00114     distanceCible = sqrt(((xC-x)*(xC-x))+((yC-y)*(yC-y)));
00115 
00116     if(y > yC) {
00117         signe = -1;
00118     } else {
00119         signe = 1;
00120     }
00121 
00122     if (xC >= x)
00123         consigneOrientation = signe * acos((abs(xC-x))/distanceCible);
00124 
00125     else
00126         consigneOrientation = signe * (3.1415 - acos((abs(xC-x))/distanceCible));
00127 
00128     // Switch de sélection de l'étape
00129 
00130     switch (fnc) {
00131         case 0: // Stand-by
00132             mot_dis();
00133             break;
00134 
00135         case 1: // Rotation
00136             mot_en();
00137 
00138             // Si on doit reculer
00139             if (objRecule[indice]==1) {
00140                 consigneOrientation += 3.1415;
00141 
00142                 if(consigneOrientation>3.1415) consigneOrientation-=2*3.1415;
00143                 if(consigneOrientation<-3.1415) consigneOrientation+=2*3.1415;
00144             }
00145 
00146             // Choix du sens de rotation
00147 
00148             double Osens = 0;
00149             if (O<0) Osens = O + (2.0f*3.1415f);
00150             else Osens = O;
00151 
00152             double consigneSens = 0;
00153             if (consigneOrientation<0) consigneSens = consigneOrientation + (2.0f*3.1415f);
00154             else consigneSens = consigneOrientation;
00155 
00156             double choixSens = consigneSens - Osens;
00157 
00158             if ((choixSens > 0) && (choixSens <= 3.1415) || (choixSens<(-3.1415))) {
00159                 motGauche_bck();
00160                 motDroite_fwd();
00161             } else {
00162                 motGauche_fwd();
00163                 motDroite_bck();
00164             }
00165 
00166             // Asservissement en position angulaire
00167             erreurAngle =  consigneOrientation - O;
00168 
00169             deltaErreur = erreurAngle - erreurPre;
00170 
00171             erreurPre  = erreurAngle;
00172 
00173             double deltaCommande = (abs(coeffPro * erreurAngle) + abs(coeffDer * deltaErreur));
00174 
00175             if(acc) {
00176                 cmdG = cmdG + 0.0008; // +0.0008
00177                 cmdD = cmdG;
00178 
00179                 if (cmdG >= VMAXROT) acc = 0;
00180             } else {
00181                 //acc = 0;
00182 
00183                 if (deltaCommande < VMAXROT) {
00184                     cmdG = deltaCommande;
00185                     cmdD = cmdG;
00186                 } else {
00187                     cmdG = VMAXROT;
00188                     cmdD = cmdG;
00189                 }
00190             }
00191 
00192             vitesseMotG(abs(cmdG));
00193             vitesseMotD(abs(cmdD));
00194 
00195             if (O > (consigneOrientation - (1*0.0174533)) && O < (consigneOrientation + (1*0.0174533))) {
00196                 fnc++;
00197                 rebooted = 1;
00198                 acc = 1;
00199                 //azerty();
00200                 distanceMem = distanceCible;
00201                 distancePlus = 0;
00202                 break;
00203             }
00204 
00205             break;
00206 
00207         case 2: // Avancer
00208 
00209             if (rebooted == 1 && wtt==1) {
00210                 cmdG = 0;
00211                 cmdD = 0;
00212                 rebooted = 0;
00213                 acc=1;
00214                 mot_en();
00215             }
00216 
00217             deltaErreurDist = distanceCible - erreurPreDist;
00218 
00219             erreurPreDist  = distanceCible;
00220 
00221             double deltaCommande2 = (abs(coeffProDist * distanceCible) + abs(coeffDerDist * deltaErreurDist));
00222 
00223             if(acc) {
00224                 cmdG = cmdG + 0.0006; // +0.0008
00225                 cmdD = cmdG;
00226 
00227                 if (cmdG >= VMAXLIN) {
00228                     acc = 0;
00229                 }
00230             } else {
00231 
00232                 if (deltaCommande2 < VMAXLIN) {
00233                     cmdG = deltaCommande2;
00234                     cmdD = cmdG;
00235                 } else {
00236                     cmdG = VMAXLIN;
00237                     cmdD = cmdG;
00238                 }
00239             }
00240 
00241             distancePlus += abs(dDist);
00242 
00243             if ((distanceCible < 10) || (distancePlus >= distanceMem)) {
00244                 acc = 1;
00245                 indice++;
00246 
00247                 if (indice>=(int)NbObj) {
00248                     fnc = 0;
00249                 } else {
00250                     fnc = objEtape[indice];
00251                     xC = objX[indice];
00252                     yC = objY[indice];
00253                 }
00254 
00255                 distancePrecedente = 0;
00256                 break;
00257 
00258             }
00259 
00260             if (objRecule[indice] == 0) {
00261                 motGauche_fwd();
00262                 motDroite_fwd();
00263             } else {
00264                 motGauche_bck();
00265                 motDroite_bck();
00266             }
00267 
00268             vitesseMotG(cmdG);
00269             vitesseMotD(cmdD);
00270 
00271             distancePrecedente = distanceCible;
00272             break;
00273 
00274 
00275         default:
00276             mot_dis();
00277     }
00278 }