AresENSEA-CDF2020
/
AresCDFMainCode_us2
Test
odo_asserv.cpp
- Committer:
- g0dd4
- Date:
- 2020-10-28
- Revision:
- 26:4670968fbc63
- Parent:
- 24:be2b2be6907b
File content as of revision 26:4670968fbc63:
/* #include */ #include "pins.h" /* #define & constantes */ #define VMAXROT 0.020 #define VMAXLIN 0.040 #define entraxe 253 // (Valeur théorique = 255) const double coeffGLong = 5.956, coeffDLong = 5.956; // tics/millimètre /* Stratégie */ int indice = 0; /* int objEtape[NbObj] = {0,1,1,1,1,1,1,1,1,1,1,1,1}; // Stratégie côté bleu double objX[NbObj] = {110,645,645,468,645,336,189,200,645,468,645,371,215}; double objY[NbObj] = {1085,1200,1490,1490,1490,1788,1032,1200,920,920,920,519,780}; int objRecule[NbObj]= {0,0,0,0,1,0,0,1,0,0,1,0,0}; */ /* int objEtape[NbObj] = {0,1,1,1,1,1,1,1,1,1,1,1,1}; // Stratégie côté bleu double objX[NbObj] = {110,645,645,200,645,336,189,200,645,468,645,371,215}; double objY[NbObj] = {1085,1200,1400,1400,1400,1788,1032,1200,920,920,920,519,780}; int objRecule[NbObj]= {0,0,0,0,1,0,0,1,0,0,1,0,0}; */ //int objEtape[NbObj] = {0,1,1,1}; // Stratégie côté bleu int objEtape [4] = {0,1,1,1}; double objX[4] = {0,660, 660, 210}; double objY[4] = {0,1070,1650,1300}; int objRecule[NbObj]= {0,0,0,0}; /* Variable globale */ Ticker Ticker_asserv; long comptG = 0, comptD = 0; // nb de tics comptés pour chaque codeur ///// INTERRUPTIONS CODEURS void cdgaRise() { if(cdgB) comptG--; else comptG++; } void cddaRise() { if(cddB) comptD--; else comptD++; } const double coeffG = 0.16008537; const double coeffD = 0.16059957; double dDist = 0, dAngl = 0; // Distance moyenne du robot et orientation double x = 110, y = 1070, O = 0; void odo2() { dDist = (double) ((comptG * coeffG) + (comptD * coeffD)) / 2.0f; dAngl = (double) ((comptD * coeffD) - (comptG * coeffG)) / entraxe; x += (double) dDist * cos(O); y += (double) dDist * sin(O); O += (double) dAngl; if (O > 3.1415) O = O - (2.0f * 3.1415f); if (O < -3.1415) O = O + (2.0f * 3.1415f); comptG = 0; comptD = 0; } //*/ double distanceCible = 0; double xC = 0, yC = 0; // x = xR et y = yR double consigneOrientation = 0; int signe = 1; double cmdD = 0, cmdG = 0; double erreurAngle = 0; double erreurPre = 0; double deltaErreur = 0; const double coeffPro = 0.020; // 0.023 de base const double coeffDer = 0.010; // 0.023 de base // Ligne droite double erreurPreDist = 0; double deltaErreurDist = 0; const double coeffProDist = 0.0008; // 0.010 de base const double coeffDerDist = 0.0008; // 0.010 de base // NEW NEW NEW NEW int fnc = 0; bool acc = 1; double distancePrecedente = 0; bool stt = 0; // Dépassement du point double OrientationMem = 0; double OrPlusPi2 = 0, OrMoinsPi2 = 0; // Controle dépassement cible double distanceMem = 0; double distancePlus = 0; void asserv() { // Odométrie odo2(); // Calcul de la cible distanceCible = sqrt(((xC-x)*(xC-x))+((yC-y)*(yC-y))); if(y > yC) { signe = -1; } else { signe = 1; } if (xC >= x) consigneOrientation = signe * acos((abs(xC-x))/distanceCible); else consigneOrientation = signe * (3.1415 - acos((abs(xC-x))/distanceCible)); // Switch de sélection de l'étape switch (fnc) { case 0: // Stand-by mot_dis(); break; case 1: // Rotation mot_en(); // Si on doit reculer if (objRecule[indice]==1) { consigneOrientation += 3.1415; if(consigneOrientation>3.1415) consigneOrientation-=2*3.1415; if(consigneOrientation<-3.1415) consigneOrientation+=2*3.1415; } // Choix du sens de rotation double Osens = 0; if (O<0) Osens = O + (2.0f*3.1415f); else Osens = O; double consigneSens = 0; if (consigneOrientation<0) consigneSens = consigneOrientation + (2.0f*3.1415f); else consigneSens = consigneOrientation; double choixSens = consigneSens - Osens; if ((choixSens > 0) && (choixSens <= 3.1415) || (choixSens<(-3.1415))) { motGauche_bck(); motDroite_fwd(); } else { motGauche_fwd(); motDroite_bck(); } // Asservissement en position angulaire erreurAngle = consigneOrientation - O; deltaErreur = erreurAngle - erreurPre; erreurPre = erreurAngle; double deltaCommande = (abs(coeffPro * erreurAngle) + abs(coeffDer * deltaErreur)); if(acc) { cmdG = cmdG + 0.0008; // +0.0008 cmdD = cmdG; if (cmdG >= VMAXROT) acc = 0; } else { //acc = 0; if (deltaCommande < VMAXROT) { cmdG = deltaCommande; cmdD = cmdG; } else { cmdG = VMAXROT; cmdD = cmdG; } } vitesseMotG(abs(cmdG)); vitesseMotD(abs(cmdD)); if (O > (consigneOrientation - (1*0.0174533)) && O < (consigneOrientation + (1*0.0174533))) { fnc++; rebooted = 1; acc = 1; //azerty(); distanceMem = distanceCible; distancePlus = 0; break; } break; case 2: // Avancer if (rebooted == 1 && wtt==1) { cmdG = 0; cmdD = 0; rebooted = 0; acc=1; mot_en(); } deltaErreurDist = distanceCible - erreurPreDist; erreurPreDist = distanceCible; double deltaCommande2 = (abs(coeffProDist * distanceCible) + abs(coeffDerDist * deltaErreurDist)); if(acc) { cmdG = cmdG + 0.0006; // +0.0008 cmdD = cmdG; if (cmdG >= VMAXLIN) { acc = 0; } } else { if (deltaCommande2 < VMAXLIN) { cmdG = deltaCommande2; cmdD = cmdG; } else { cmdG = VMAXLIN; cmdD = cmdG; } } distancePlus += abs(dDist); if ((distanceCible < 10) || (distancePlus >= distanceMem)) { acc = 1; indice++; if (indice>=(int)NbObj) { fnc = 0; } else { fnc = objEtape[indice]; xC = objX[indice]; yC = objY[indice]; } distancePrecedente = 0; break; } if (objRecule[indice] == 0) { motGauche_fwd(); motDroite_fwd(); } else { motGauche_bck(); motDroite_bck(); } vitesseMotG(cmdG); vitesseMotD(cmdD); distancePrecedente = distanceCible; break; default: mot_dis(); } }