AresENSEA-CDF2020
/
AresCDFMainCode_us2
Test
Diff: odo_asserv.cpp
- Revision:
- 24:be2b2be6907b
- Parent:
- 23:a74135a0271d
diff -r a74135a0271d -r be2b2be6907b odo_asserv.cpp --- a/odo_asserv.cpp Wed Oct 28 16:27:05 2020 +0000 +++ b/odo_asserv.cpp Wed Oct 28 20:08:39 2020 +0000 @@ -2,8 +2,8 @@ #include "pins.h" /* #define & constantes */ -#define VMAXROT 0.050 -#define VMAXLIN 0.080 +#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 @@ -16,10 +16,20 @@ 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; @@ -72,14 +82,14 @@ double erreurAngle = 0; double erreurPre = 0; double deltaErreur = 0; -const double coeffPro = 0.075; // 0.023 de base -const double coeffDer = 0.060; // 0.023 de base +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.0004; // 0.010 de base -const double coeffDerDist = 0.0004; // 0.010 de base +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; @@ -163,7 +173,7 @@ double deltaCommande = (abs(coeffPro * erreurAngle) + abs(coeffDer * deltaErreur)); if(acc) { - cmdG = cmdG + 0.0001; // +0.0008 + cmdG = cmdG + 0.0008; // +0.0008 cmdD = cmdG; if (cmdG >= VMAXROT) acc = 0; @@ -211,7 +221,7 @@ double deltaCommande2 = (abs(coeffProDist * distanceCible) + abs(coeffDerDist * deltaErreurDist)); if(acc) { - cmdG = cmdG + 0.0001; // +0.0008 + cmdG = cmdG + 0.0006; // +0.0008 cmdD = cmdG; if (cmdG >= VMAXLIN) {