AresENSEA-CDF2020 / Mbed 2 deprecated AresCDFMainCode

Dependencies:   mbed DRV8825

odo_asserv.cpp@26:bb2b778bd351, 2020-11-03 (annotated)

Committer:
Nanaud
Date:
Tue Nov 03 16:29:13 2020 +0000
Revision:
26:bb2b778bd351
Parent:
25:869b1c1f51a7 
UI

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Nanaud 22:f891c2bce091 1 /* #include */
Nanaud 2:094c09903a9c 2 #include "pins.h"
Nanaud 16:ae65ce77b1f9 3
Nanaud 22:f891c2bce091 4 /* #define & constantes */
Nanaud 26:bb2b778bd351 5 #define VMAXROT 0.050 // 0.020
Nanaud 26:bb2b778bd351 6 #define VMAXLIN 0.110 // 0.060
Nanaud 20:7d206773f39e 7
Nanaud 26:bb2b778bd351 8 /* Pointeurs */
Nanaud 26:bb2b778bd351 9 //double *strategie_X_bleu = NULL;
Nanaud 26:bb2b778bd351 10 //double *strategie_X_jaune = NULL;
Nanaud 26:bb2b778bd351 11 double *stratX_select = NULL;
Nanaud 26:bb2b778bd351 12 const double x_init_bleu = 113;
Nanaud 26:bb2b778bd351 13 const double x_init_jaune = 2887;
Nanaud 26:bb2b778bd351 14 const double y_init = 1071;
Nanaud 26:bb2b778bd351 15 const double O_init_bleu = 0;
Nanaud 26:bb2b778bd351 16 const double O_init_jaune = 3.1414;
Nanaud 24:be2b2be6907b 17
Nanaud 25:869b1c1f51a7 18 #define entraxe 253 // (Valeur théorique = 255)
Nanaud 25:869b1c1f51a7 19 const double coeffG = 0.16008537; // mm/tic
Nanaud 25:869b1c1f51a7 20 const double coeffD = 0.16059957; // mm/tic
Nanaud 25:869b1c1f51a7 21 /* COEFF ROTATION */
Nanaud 26:bb2b778bd351 22 const double coeffPro = 0.050; // 0.020
Nanaud 26:bb2b778bd351 23 const double coeffDer = 0.040; // 0.010
Nanaud 25:869b1c1f51a7 24 /* COEFF LIGNE DROITE */
Nanaud 26:bb2b778bd351 25 const double coeffProDist = 0.002; // 0.0008
Nanaud 25:869b1c1f51a7 26 const double coeffDerDist = 0.003; // 0.0008
Nanaud 25:869b1c1f51a7 27
Nanaud 26:bb2b778bd351 28 #define ACC_ROT 0.003
Nanaud 25:869b1c1f51a7 29 #define ACC_LIN 0.002
Nanaud 25:869b1c1f51a7 30
Nanaud 25:869b1c1f51a7 31 /* Stratégie */
Nanaud 25:869b1c1f51a7 32 int indiceStrategie = 0;
Nanaud 25:869b1c1f51a7 33
Nanaud 25:869b1c1f51a7 34 /* // Stratégie ultra simple
Nanaud 25:869b1c1f51a7 35 int objEtape[NbObj] = {0,1,1,1};
Nanaud 25:869b1c1f51a7 36 double objX[NbObj] = {0,660, 660, 210};
Nanaud 25:869b1c1f51a7 37 double objY[NbObj] = {0,1070,1650,1300};
Nanaud 25:869b1c1f51a7 38 int objRecule[NbObj]= {0,0,0,0};*/
Nanaud 24:be2b2be6907b 39
Nanaud 26:bb2b778bd351 40 // Côté bleu
Nanaud 26:bb2b778bd351 41 /*
Nanaud 26:bb2b778bd351 42 #define X_INIT 110
Nanaud 26:bb2b778bd351 43 #define Y_INIT 1070
Nanaud 26:bb2b778bd351 44 #define O_INIT 0
Nanaud 26:bb2b778bd351 45 */
plmir 12:2c312916a621 46
Nanaud 26:bb2b778bd351 47 /*
Nanaud 26:bb2b778bd351 48 // Côté jaune
Nanaud 26:bb2b778bd351 49 #define X_INIT 2890
Nanaud 26:bb2b778bd351 50 #define Y_INIT 1070
Nanaud 26:bb2b778bd351 51 #define O_INIT 3.1414
Nanaud 26:bb2b778bd351 52 */
Nanaud 26:bb2b778bd351 53
Nanaud 26:bb2b778bd351 54 /* // Stratégie Lucas Blue
Nanaud 25:869b1c1f51a7 55 int objEtape[NbObj] = {0,1,1,1,1,1,1,1,1,1};
Nanaud 25:869b1c1f51a7 56 double objX[NbObj] = {110,200,660,627,450,235,698,300,300,230};
Nanaud 25:869b1c1f51a7 57 double objY[NbObj] = {1070,1070,1200,743,920,1124,672,460,1100,750};
Nanaud 25:869b1c1f51a7 58 int objRecule[NbObj]= {0,0,0,0,0,0,1,0,0,1};
Nanaud 26:bb2b778bd351 59 */
Nanaud 6:ea6b30c4bb01 60
Nanaud 26:bb2b778bd351 61 /* // Stratégie Lucas Yellow
Nanaud 25:869b1c1f51a7 62 int objEtape[NbObj] = {0,1,1,1,1,1,1,1,1,1};
Nanaud 25:869b1c1f51a7 63 double objX[NbObj] = {2890,2800,2340,2373,2550,2765,2302,2700,2700,2800};
Nanaud 25:869b1c1f51a7 64 double objY[NbObj] = {1070,1070,1200,743,920,1124,672,460,1100,750};
Nanaud 25:869b1c1f51a7 65 int objRecule[NbObj]= {0,0,0,0,0,0,1,0,0,1};
Nanaud 25:869b1c1f51a7 66 */
Nanaud 6:ea6b30c4bb01 67
Nanaud 26:bb2b778bd351 68 // Stratégie Pharoah
Nanaud 26:bb2b778bd351 69
Nanaud 26:bb2b778bd351 70 int objEtape[NbObj] = {0,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1};
Nanaud 26:bb2b778bd351 71 double objX_bleu[NbObj] = {110,253,793,671,529,134,598,868,868,474,353,262,262,262,950,250,350};
Nanaud 26:bb2b778bd351 72 double objX_jaune[NbObj] = {2887,2747,2207,2329,2471,2866,2402,2132,2132,2526,2647,2738,2738,2738,2050,2750,2650};
Nanaud 26:bb2b778bd351 73 double objY[NbObj] = {1070,1070,1347,1490,1490,1490,1490,1226,589,589,738,817,1411,1290,920,920,920};
Nanaud 26:bb2b778bd351 74 int objRecule[NbObj]= {0,0,0,0,0,0,1,0,0,0,0,0,0,1,0,0,1};
Nanaud 26:bb2b778bd351 75
Nanaud 26:bb2b778bd351 76
Nanaud 25:869b1c1f51a7 77 /* Variable globale */
Nanaud 25:869b1c1f51a7 78 Ticker Ticker_asserv; // Ticker pour l'asservissement en position
Nanaud 25:869b1c1f51a7 79
Nanaud 25:869b1c1f51a7 80 int action = 0; // Actions possibles : Rotation & Avancer/Reculer
Nanaud 25:869b1c1f51a7 81 long comptG = 0, comptD = 0; // Nb de tics compté par roue
Nanaud 25:869b1c1f51a7 82 double dDist = 0, dAngl = 0; // Distance moyenne du robot et orientation
Nanaud 26:bb2b778bd351 83 double x = 0, y = 0, O = 0;
Nanaud 25:869b1c1f51a7 84
Nanaud 25:869b1c1f51a7 85 /* Interruptions codeurs */
Nanaud 6:ea6b30c4bb01 86
Nanaud 6:ea6b30c4bb01 87 void cdgaRise()
Nanaud 6:ea6b30c4bb01 88 {
Nanaud 16:ae65ce77b1f9 89 if(cdgB) comptG--;
Nanaud 16:ae65ce77b1f9 90 else comptG++;
Nanaud 6:ea6b30c4bb01 91 }
Nanaud 2:094c09903a9c 92
Nanaud 6:ea6b30c4bb01 93 void cddaRise()
Nanaud 6:ea6b30c4bb01 94 {
Nanaud 16:ae65ce77b1f9 95 if(cddB) comptD--;
Nanaud 16:ae65ce77b1f9 96 else comptD++;
Nanaud 6:ea6b30c4bb01 97 }
Nanaud 6:ea6b30c4bb01 98
Nanaud 16:ae65ce77b1f9 99 void odo2()
Nanaud 10:0714feaaaee1 100 {
Nanaud 16:ae65ce77b1f9 101 dDist = (double) ((comptG * coeffG) + (comptD * coeffD)) / 2.0f;
Nanaud 16:ae65ce77b1f9 102 dAngl = (double) ((comptD * coeffD) - (comptG * coeffG)) / entraxe;
Nanaud 6:ea6b30c4bb01 103
Nanaud 16:ae65ce77b1f9 104 x += (double) dDist * cos(O);
Nanaud 16:ae65ce77b1f9 105 y += (double) dDist * sin(O);
Nanaud 16:ae65ce77b1f9 106 O += (double) dAngl;
Nanaud 10:0714feaaaee1 107
Nanaud 16:ae65ce77b1f9 108 if (O > 3.1415) O = O - (2.0f * 3.1415f);
Nanaud 16:ae65ce77b1f9 109 if (O < -3.1415) O = O + (2.0f * 3.1415f);
Nanaud 14:dd3c756c6d48 110
plmir 12:2c312916a621 111 comptG = 0;
plmir 12:2c312916a621 112 comptD = 0;
Nanaud 14:dd3c756c6d48 113 }
Nanaud 14:dd3c756c6d48 114 //*/
Nanaud 14:dd3c756c6d48 115
Nanaud 17:176a1b4a2fa8 116
Nanaud 16:ae65ce77b1f9 117 double distanceCible = 0;
Nanaud 17:176a1b4a2fa8 118 double xC = 0, yC = 0; // x = xR et y = yR
Nanaud 16:ae65ce77b1f9 119 double consigneOrientation = 0;
Nanaud 16:ae65ce77b1f9 120 int signe = 1;
Nanaud 18:48246daf0c06 121 double cmdD = 0, cmdG = 0;
Nanaud 16:ae65ce77b1f9 122 double erreurAngle = 0;
Nanaud 16:ae65ce77b1f9 123 double erreurPre = 0;
Nanaud 16:ae65ce77b1f9 124 double deltaErreur = 0;
Nanaud 25:869b1c1f51a7 125
Nanaud 17:176a1b4a2fa8 126
Nanaud 19:c419033c0967 127 // Ligne droite
Nanaud 19:c419033c0967 128 double erreurPreDist = 0;
Nanaud 19:c419033c0967 129 double deltaErreurDist = 0;
Nanaud 25:869b1c1f51a7 130
Nanaud 19:c419033c0967 131
Nanaud 17:176a1b4a2fa8 132 // NEW NEW NEW NEW
Nanaud 25:869b1c1f51a7 133
Nanaud 18:48246daf0c06 134 bool acc = 1;
Nanaud 19:c419033c0967 135 double distancePrecedente = 0;
Nanaud 20:7d206773f39e 136 bool stt = 0; // Dépassement du point
Nanaud 20:7d206773f39e 137
Nanaud 22:f891c2bce091 138 double OrientationMem = 0;
Nanaud 22:f891c2bce091 139 double OrPlusPi2 = 0, OrMoinsPi2 = 0;
Nanaud 22:f891c2bce091 140
Nanaud 22:f891c2bce091 141 // Controle dépassement cible
Nanaud 22:f891c2bce091 142 double distanceMem = 0;
Nanaud 22:f891c2bce091 143 double distancePlus = 0;
Nanaud 16:ae65ce77b1f9 144
Nanaud 16:ae65ce77b1f9 145 void asserv()
Nanaud 16:ae65ce77b1f9 146 {
Nanaud 17:176a1b4a2fa8 147 // Odométrie
Nanaud 16:ae65ce77b1f9 148 odo2();
Nanaud 16:ae65ce77b1f9 149
Nanaud 17:176a1b4a2fa8 150 // Calcul de la cible
Nanaud 18:48246daf0c06 151
Nanaud 18:48246daf0c06 152 distanceCible = sqrt(((xC-x)*(xC-x))+((yC-y)*(yC-y)));
Nanaud 16:ae65ce77b1f9 153
Nanaud 16:ae65ce77b1f9 154 if(y > yC) {
Nanaud 16:ae65ce77b1f9 155 signe = -1;
Nanaud 16:ae65ce77b1f9 156 } else {
Nanaud 16:ae65ce77b1f9 157 signe = 1;
Nanaud 16:ae65ce77b1f9 158 }
Nanaud 16:ae65ce77b1f9 159
Nanaud 18:48246daf0c06 160 if (xC >= x)
Nanaud 18:48246daf0c06 161 consigneOrientation = signe * acos((abs(xC-x))/distanceCible);
Nanaud 18:48246daf0c06 162
Nanaud 18:48246daf0c06 163 else
Nanaud 18:48246daf0c06 164 consigneOrientation = signe * (3.1415 - acos((abs(xC-x))/distanceCible));
Nanaud 16:ae65ce77b1f9 165
Nanaud 17:176a1b4a2fa8 166 // Switch de sélection de l'étape
Nanaud 16:ae65ce77b1f9 167
Nanaud 25:869b1c1f51a7 168 switch (action) {
Nanaud 18:48246daf0c06 169 case 0: // Stand-by
Nanaud 18:48246daf0c06 170 mot_dis();
Nanaud 17:176a1b4a2fa8 171 break;
Nanaud 16:ae65ce77b1f9 172
Nanaud 17:176a1b4a2fa8 173 case 1: // Rotation
Nanaud 18:48246daf0c06 174 mot_en();
Nanaud 18:48246daf0c06 175
Nanaud 20:7d206773f39e 176 // Si on doit reculer
Nanaud 25:869b1c1f51a7 177 if (objRecule[indiceStrategie]==1) {
Nanaud 20:7d206773f39e 178 consigneOrientation += 3.1415;
Nanaud 20:7d206773f39e 179
Nanaud 20:7d206773f39e 180 if(consigneOrientation>3.1415) consigneOrientation-=2*3.1415;
Nanaud 20:7d206773f39e 181 if(consigneOrientation<-3.1415) consigneOrientation+=2*3.1415;
Nanaud 20:7d206773f39e 182 }
Nanaud 20:7d206773f39e 183
Nanaud 19:c419033c0967 184 // Choix du sens de rotation
Nanaud 19:c419033c0967 185
Nanaud 20:7d206773f39e 186 double Osens = 0;
Nanaud 20:7d206773f39e 187 if (O<0) Osens = O + (2.0f*3.1415f);
Nanaud 20:7d206773f39e 188 else Osens = O;
Nanaud 20:7d206773f39e 189
Nanaud 20:7d206773f39e 190 double consigneSens = 0;
Nanaud 20:7d206773f39e 191 if (consigneOrientation<0) consigneSens = consigneOrientation + (2.0f*3.1415f);
Nanaud 20:7d206773f39e 192 else consigneSens = consigneOrientation;
Nanaud 19:c419033c0967 193
Nanaud 20:7d206773f39e 194 double choixSens = consigneSens - Osens;
Nanaud 20:7d206773f39e 195
Nanaud 20:7d206773f39e 196 if ((choixSens > 0) && (choixSens <= 3.1415) || (choixSens<(-3.1415))) {
Nanaud 18:48246daf0c06 197 motGauche_bck();
Nanaud 18:48246daf0c06 198 motDroite_fwd();
Nanaud 18:48246daf0c06 199 } else {
Nanaud 18:48246daf0c06 200 motGauche_fwd();
Nanaud 18:48246daf0c06 201 motDroite_bck();
Nanaud 18:48246daf0c06 202 }
Nanaud 18:48246daf0c06 203
Nanaud 17:176a1b4a2fa8 204 // Asservissement en position angulaire
Nanaud 17:176a1b4a2fa8 205 erreurAngle = consigneOrientation - O;
Nanaud 16:ae65ce77b1f9 206
Nanaud 17:176a1b4a2fa8 207 deltaErreur = erreurAngle - erreurPre;
Nanaud 16:ae65ce77b1f9 208
Nanaud 17:176a1b4a2fa8 209 erreurPre = erreurAngle;
Nanaud 16:ae65ce77b1f9 210
Nanaud 18:48246daf0c06 211 double deltaCommande = (abs(coeffPro * erreurAngle) + abs(coeffDer * deltaErreur));
Nanaud 16:ae65ce77b1f9 212
Nanaud 18:48246daf0c06 213 if(acc) {
Nanaud 25:869b1c1f51a7 214 cmdG = cmdG + ACC_ROT; // +0.0008
Nanaud 17:176a1b4a2fa8 215 cmdD = cmdG;
Nanaud 18:48246daf0c06 216
Nanaud 19:c419033c0967 217 if (cmdG >= VMAXROT) acc = 0;
Nanaud 18:48246daf0c06 218 } else {
Nanaud 18:48246daf0c06 219 //acc = 0;
Nanaud 16:ae65ce77b1f9 220
Nanaud 19:c419033c0967 221 if (deltaCommande < VMAXROT) {
Nanaud 18:48246daf0c06 222 cmdG = deltaCommande;
Nanaud 18:48246daf0c06 223 cmdD = cmdG;
Nanaud 18:48246daf0c06 224 } else {
Nanaud 19:c419033c0967 225 cmdG = VMAXROT;
Nanaud 18:48246daf0c06 226 cmdD = cmdG;
Nanaud 18:48246daf0c06 227 }
Nanaud 17:176a1b4a2fa8 228 }
Nanaud 16:ae65ce77b1f9 229
Nanaud 17:176a1b4a2fa8 230 vitesseMotG(abs(cmdG));
Nanaud 17:176a1b4a2fa8 231 vitesseMotD(abs(cmdD));
Nanaud 16:ae65ce77b1f9 232
Nanaud 19:c419033c0967 233 if (O > (consigneOrientation - (1*0.0174533)) && O < (consigneOrientation + (1*0.0174533))) {
Nanaud 25:869b1c1f51a7 234 action++;
Nanaud 25:869b1c1f51a7 235 us_rbt_restart = 1;
Nanaud 17:176a1b4a2fa8 236 acc = 1;
Nanaud 22:f891c2bce091 237 //azerty();
Nanaud 22:f891c2bce091 238 distanceMem = distanceCible;
Nanaud 22:f891c2bce091 239 distancePlus = 0;
Nanaud 20:7d206773f39e 240 break;
Nanaud 17:176a1b4a2fa8 241 }
Nanaud 20:7d206773f39e 242
Nanaud 17:176a1b4a2fa8 243 break;
Nanaud 16:ae65ce77b1f9 244
Nanaud 17:176a1b4a2fa8 245 case 2: // Avancer
Nanaud 22:f891c2bce091 246
Nanaud 25:869b1c1f51a7 247 if (us_rbt_restart == 1 && us_libre == 1) {
Nanaud 20:7d206773f39e 248 cmdG = 0;
Nanaud 20:7d206773f39e 249 cmdD = 0;
Nanaud 25:869b1c1f51a7 250 us_rbt_restart = 0;
Nanaud 20:7d206773f39e 251 acc=1;
Nanaud 20:7d206773f39e 252 mot_en();
Nanaud 20:7d206773f39e 253 }
Nanaud 20:7d206773f39e 254
Nanaud 19:c419033c0967 255 deltaErreurDist = distanceCible - erreurPreDist;
Nanaud 19:c419033c0967 256
Nanaud 19:c419033c0967 257 erreurPreDist = distanceCible;
Nanaud 19:c419033c0967 258
Nanaud 19:c419033c0967 259 double deltaCommande2 = (abs(coeffProDist * distanceCible) + abs(coeffDerDist * deltaErreurDist));
Nanaud 19:c419033c0967 260
Nanaud 19:c419033c0967 261 if(acc) {
Nanaud 25:869b1c1f51a7 262 cmdG = cmdG + ACC_LIN; // +0.0006
Nanaud 19:c419033c0967 263 cmdD = cmdG;
Nanaud 19:c419033c0967 264
Nanaud 19:c419033c0967 265 if (cmdG >= VMAXLIN) {
Nanaud 19:c419033c0967 266 acc = 0;
Nanaud 19:c419033c0967 267 }
Nanaud 19:c419033c0967 268 } else {
Nanaud 19:c419033c0967 269
Nanaud 19:c419033c0967 270 if (deltaCommande2 < VMAXLIN) {
Nanaud 19:c419033c0967 271 cmdG = deltaCommande2;
Nanaud 19:c419033c0967 272 cmdD = cmdG;
Nanaud 19:c419033c0967 273 } else {
Nanaud 19:c419033c0967 274 cmdG = VMAXLIN;
Nanaud 19:c419033c0967 275 cmdD = cmdG;
Nanaud 19:c419033c0967 276 }
Nanaud 19:c419033c0967 277 }
Nanaud 19:c419033c0967 278
Nanaud 22:f891c2bce091 279 distancePlus += abs(dDist);
Nanaud 19:c419033c0967 280
Nanaud 22:f891c2bce091 281 if ((distanceCible < 10) || (distancePlus >= distanceMem)) {
Nanaud 19:c419033c0967 282 acc = 1;
Nanaud 25:869b1c1f51a7 283 indiceStrategie++;
Nanaud 20:7d206773f39e 284
Nanaud 25:869b1c1f51a7 285 if (indiceStrategie >= (int)NbObj) {
Nanaud 25:869b1c1f51a7 286 action = 0;
Nanaud 20:7d206773f39e 287 } else {
Nanaud 25:869b1c1f51a7 288 action = objEtape[indiceStrategie];
Nanaud 26:bb2b778bd351 289 xC = stratX_select[indiceStrategie];
Nanaud 25:869b1c1f51a7 290 yC = objY[indiceStrategie];
Nanaud 20:7d206773f39e 291 }
Nanaud 20:7d206773f39e 292
Nanaud 20:7d206773f39e 293 distancePrecedente = 0;
Nanaud 20:7d206773f39e 294 break;
Nanaud 19:c419033c0967 295 }
Nanaud 16:ae65ce77b1f9 296
Nanaud 25:869b1c1f51a7 297 if (objRecule[indiceStrategie] == 0) {
Nanaud 20:7d206773f39e 298 motGauche_fwd();
Nanaud 20:7d206773f39e 299 motDroite_fwd();
Nanaud 20:7d206773f39e 300 } else {
Nanaud 20:7d206773f39e 301 motGauche_bck();
Nanaud 20:7d206773f39e 302 motDroite_bck();
Nanaud 20:7d206773f39e 303 }
Nanaud 20:7d206773f39e 304
Nanaud 17:176a1b4a2fa8 305 vitesseMotG(cmdG);
Nanaud 17:176a1b4a2fa8 306 vitesseMotD(cmdD);
Nanaud 17:176a1b4a2fa8 307
Nanaud 19:c419033c0967 308 distancePrecedente = distanceCible;
Nanaud 17:176a1b4a2fa8 309 break;
Nanaud 17:176a1b4a2fa8 310
Nanaud 17:176a1b4a2fa8 311
Nanaud 17:176a1b4a2fa8 312 default:
Nanaud 17:176a1b4a2fa8 313 mot_dis();
Nanaud 16:ae65ce77b1f9 314 }
Nanaud 16:ae65ce77b1f9 315 }