AresENSEA-CDF2020
/
AresCDFMainCode_us2
Test
Diff: odo_asserv.cpp
- Revision:
- 18:48246daf0c06
- Parent:
- 17:176a1b4a2fa8
- Child:
- 19:c419033c0967
diff -r 176a1b4a2fa8 -r 48246daf0c06 odo_asserv.cpp --- a/odo_asserv.cpp Tue Oct 06 11:58:00 2020 +0000 +++ b/odo_asserv.cpp Wed Oct 07 20:00:45 2020 +0000 @@ -1,7 +1,7 @@ //Nom du fichier : odo_asserv.cpp #include "pins.h" -#define VMAX 50 +#define VMAX 0.020 ///// VARIABLES Ticker ticker_odo; @@ -60,14 +60,8 @@ double dDist = 0, dAngl = 0; // Distance moyenne du robot et orientation double x = 0, y = 0, O = 0; -double vitG = 0, vitD = 0; -#define tpsTicker 0.020f - void odo2() { - vitG = (double) ((comptG * coeffG) / tpsTicker); - vitD = (double) ((comptD * coeffD) / tpsTicker); - dDist = (double) ((comptG * coeffG) + (comptD * coeffD)) / 2.0f; dAngl = (double) ((comptD * coeffD) - (comptG * coeffG)) / entraxe; @@ -75,9 +69,11 @@ 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; } @@ -111,16 +107,16 @@ double consigneOrientation = 0; //double consigneOrientation = (90*3.1415)/180; int signe = 1; -int cmdD = 0, cmdG = 0; +double cmdD = 0, cmdG = 0; double erreurAngle = 0; double erreurPre = 0; double deltaErreur = 0; -const double coeffPro = 25.0; // 5.0 de base -const double coeffDer = 50.0; // 3.0 de base +const double coeffPro = 0.06; // 0.023 de base +const double coeffDer = 0.06; // 0.023 de base // NEW NEW NEW NEW -int etape = 1; -int acc = 1; +int fnc = 0; +bool acc = 1; void asserv() { @@ -128,7 +124,8 @@ odo2(); // Calcul de la cible - distanceCible = sqrt((xC-x)*(xC-x)+(yC-y)*(yC-y)); + + distanceCible = sqrt(((xC-x)*(xC-x))+((yC-y)*(yC-y))); if(y > yC) { signe = -1; @@ -136,15 +133,30 @@ signe = 1; } - consigneOrientation = signe * acos((xC-x)/((xC-x)*(xC-x)*(yC-y)*(yC-y))); + 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 (etape) { - case 0: // + switch (fnc) { + case 0: // Stand-by + mot_dis(); break; case 1: // Rotation + mot_en(); + + if (consigneOrientation > 0) { + motGauche_bck(); + motDroite_fwd(); + } else { + motGauche_fwd(); + motDroite_bck(); + } + // Asservissement en position angulaire erreurAngle = consigneOrientation - O; @@ -152,36 +164,39 @@ erreurPre = erreurAngle; - int deltaCommande = coeffPro * erreurAngle + coeffDer * deltaErreur; + double deltaCommande = (abs(coeffPro * erreurAngle) + abs(coeffDer * deltaErreur)); - if (deltaCommande < VMAX) { - cmdG = deltaCommande; - cmdD = cmdG; - } else { - cmdG = VMAX; + if(acc) { + cmdG = cmdG + 0.0005; cmdD = cmdG; - } + + if (cmdG >= VMAX) acc = 0; + } else { + //acc = 0; - if(acc && cmdG <VMAX) { - cmdG+=1; - cmdD = cmdG; - } - else { - acc = 0; + if (deltaCommande < VMAX) { + cmdG = deltaCommande; + cmdD = cmdG; + } else { + cmdG = VMAX; + cmdD = cmdG; + } } vitesseMotG(abs(cmdG)); vitesseMotD(abs(cmdD)); if (O > (consigneOrientation - (2*0.0174533)) && O < (consigneOrientation + (2*0.0174533))) { - mot_dis(); - //etape++; + //mot_dis(); + fnc++; acc = 1; } break; case 2: // Avancer - cmdD = abs((int)distanceCible); + mot_en(); + + cmdD = abs((int)distanceCible)*0.0005; if(cmdD>VMAX) { cmdD = VMAX; } @@ -192,7 +207,10 @@ vitesseMotG(cmdG); vitesseMotD(cmdD); - if (abs((int)distanceCible) < 1) mot_dis(); + if (abs((int)distanceCible) < 3) { + //mot_dis(); + fnc=0; + } break; case 3: // Reculer