![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
Test
odo_asserv.cpp
- Committer:
- Nanaud
- Date:
- 2020-10-12
- Revision:
- 19:c419033c0967
- Parent:
- 18:48246daf0c06
- Child:
- 20:7d206773f39e
File content as of revision 19:c419033c0967:
//Nom du fichier : odo_asserv.cpp #include "pins.h" #define VMAXROT 0.060 // 0.030 #define VMAXLIN 0.100 // 0.050 // Objectifs #define NbObj 4 int indice = 0; int objEtape [4] = {0,1,1,1}; double objX[4] = {0,660, 660, 210}; double objY[4] = {0,1070,1650,1300}; ///// VARIABLES Ticker ticker_odo; Ticker ticker_asserv; // Coeff à définir empiriquement const double coeffGLong = 5.956, coeffDLong = 5.956; // constantes permettant la transformation tic/millimètre const double coeffGAngl = 737.447, coeffDAngl = 748.057; // constantes permettant la transformation tic/radian 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++; } ///* // odo2() //#define diametre 51.45 // 51.45 théorique //#define N 1000 // 1000 théorique #define entraxe 253 // 255 théorique //const double coeffG = ((double)(diametre/2)/(double)N)*2.0f*3.1415f; //const double coeffD = ((double)(diametre/2)/(double)N)*2.0f*3.1415f; 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; //double consigneOrientation = (90*3.1415)/180; int signe = 1; double cmdD = 0, cmdG = 0; double erreurAngle = 0; double erreurPre = 0; double deltaErreur = 0; const double coeffPro = 0.08; // 0.023 de base const double coeffDer = 0.06; // 0.023 de base // Ligne droite //double erreurDist = 0; double erreurPreDist = 0; double deltaErreurDist = 0; const double coeffProDist = 0.10; // 0.023 de base const double coeffDerDist = 0.10; // 0.023 de base // NEW NEW NEW NEW int fnc = 0; bool acc = 1; double distancePrecedente = 0; bool stt = 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(); // Choix du sens de rotation double choixSens = consigneOrientation - O; if (choixSens > 3.1415) choixSens -= (2.0f * 3.1415f); else if (choixSens < 3.1415) choixSens += (2.0f * 3.1415f); if (choixSens > 0) { 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.0005; 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))) { //mot_dis(); fnc++; acc = 1; } break; case 2: // Avancer mot_en(); /* cmdD = abs((int)distanceCible)*0.0001; // *0.005 if(cmdD>VMAXLIN) { cmdD = VMAXLIN; } cmdG = cmdD; */ // Asservissement distance à parcourir //erreurDist = consigneOrientation - O; deltaErreurDist = distanceCible - erreurPreDist; erreurPreDist = distanceCible; double deltaCommande2 = (abs(coeffProDist * distanceCible) + abs(coeffDerDist * deltaErreurDist)); if(acc) { cmdG = cmdG + 0.0005; cmdD = cmdG; if (cmdG >= VMAXLIN) { acc = 0; stt = 1; } } else { //acc = 0; if (deltaCommande2 < VMAXLIN) { cmdG = deltaCommande2; cmdD = cmdG; } else { cmdG = VMAXLIN; cmdD = cmdG; } } vitesseMotG(abs(cmdG)); vitesseMotD(abs(cmdD)); //if (distanceCible < 15)[ //if ((x < xC+10) && (x > xC-10) && (y < yC+10) && (y > yC-10)) { if ((distanceCible < 10) || (stt==1 && (distancePrecedente < distanceCible))) { //mot_dis(); //fnc=0; acc = 1; stt = 0; indice++; fnc = objEtape[indice]; xC = objX[indice]; yC = objY[indice]; } motGauche_fwd(); motDroite_fwd(); vitesseMotG(cmdG); vitesseMotD(cmdD); distancePrecedente = distanceCible; break; case 3: // Reculer break; default: mot_dis(); } }