AresENSEA-CDF2020 / Mbed 2 deprecated AresCDFMainCode_us2

Test

Dependencies:   mbed DRV8825

odo_asserv.cpp@26:4670968fbc63, 2020-10-28 (annotated)

Committer:
g0dd4
Date:
Wed Oct 28 23:05:19 2020 +0000
Revision:
26:4670968fbc63
Parent:
24:be2b2be6907b 
Ajout OFFSET;

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 24:be2b2be6907b 5 #define VMAXROT 0.020
Nanaud 24:be2b2be6907b 6 #define VMAXLIN 0.040
Nanaud 22:f891c2bce091 7 #define entraxe 253 // (Valeur théorique = 255)
Nanaud 22:f891c2bce091 8 const double coeffGLong = 5.956, coeffDLong = 5.956; // tics/millimètre
Nanaud 20:7d206773f39e 9
Nanaud 22:f891c2bce091 10 /* Stratégie */
Nanaud 19:c419033c0967 11 int indice = 0;
Nanaud 22:f891c2bce091 12 /*
Nanaud 22:f891c2bce091 13 int objEtape[NbObj] = {0,1,1,1,1,1,1,1,1,1,1,1,1}; // Stratégie côté bleu
Nanaud 22:f891c2bce091 14 double objX[NbObj] = {110,645,645,468,645,336,189,200,645,468,645,371,215};
Nanaud 22:f891c2bce091 15 double objY[NbObj] = {1085,1200,1490,1490,1490,1788,1032,1200,920,920,920,519,780};
Nanaud 22:f891c2bce091 16 int objRecule[NbObj]= {0,0,0,0,1,0,0,1,0,0,1,0,0};
Nanaud 22:f891c2bce091 17 */
Nanaud 2:094c09903a9c 18
Nanaud 24:be2b2be6907b 19 /*
Nanaud 22:f891c2bce091 20 int objEtape[NbObj] = {0,1,1,1,1,1,1,1,1,1,1,1,1}; // Stratégie côté bleu
Nanaud 22:f891c2bce091 21 double objX[NbObj] = {110,645,645,200,645,336,189,200,645,468,645,371,215};
Nanaud 22:f891c2bce091 22 double objY[NbObj] = {1085,1200,1400,1400,1400,1788,1032,1200,920,920,920,519,780};
Nanaud 22:f891c2bce091 23 int objRecule[NbObj]= {0,0,0,0,1,0,0,1,0,0,1,0,0};
Nanaud 24:be2b2be6907b 24 */
Nanaud 24:be2b2be6907b 25
Nanaud 24:be2b2be6907b 26
Nanaud 24:be2b2be6907b 27 //int objEtape[NbObj] = {0,1,1,1}; // Stratégie côté bleu
Nanaud 24:be2b2be6907b 28 int objEtape [4] = {0,1,1,1};
Nanaud 24:be2b2be6907b 29 double objX[4] = {0,660, 660, 210};
Nanaud 24:be2b2be6907b 30 double objY[4] = {0,1070,1650,1300};
Nanaud 24:be2b2be6907b 31 int objRecule[NbObj]= {0,0,0,0};
Nanaud 24:be2b2be6907b 32
plmir 12:2c312916a621 33
Nanaud 22:f891c2bce091 34 /* Variable globale */
Nanaud 22:f891c2bce091 35 Ticker Ticker_asserv;
Nanaud 6:ea6b30c4bb01 36
Nanaud 6:ea6b30c4bb01 37 long comptG = 0, comptD = 0; // nb de tics comptés pour chaque codeur
Nanaud 6:ea6b30c4bb01 38
Nanaud 6:ea6b30c4bb01 39 ///// INTERRUPTIONS CODEURS
Nanaud 6:ea6b30c4bb01 40
Nanaud 6:ea6b30c4bb01 41 void cdgaRise()
Nanaud 6:ea6b30c4bb01 42 {
Nanaud 16:ae65ce77b1f9 43 if(cdgB) comptG--;
Nanaud 16:ae65ce77b1f9 44 else comptG++;
Nanaud 6:ea6b30c4bb01 45 }
Nanaud 2:094c09903a9c 46
Nanaud 6:ea6b30c4bb01 47 void cddaRise()
Nanaud 6:ea6b30c4bb01 48 {
Nanaud 16:ae65ce77b1f9 49 if(cddB) comptD--;
Nanaud 16:ae65ce77b1f9 50 else comptD++;
Nanaud 6:ea6b30c4bb01 51 }
Nanaud 6:ea6b30c4bb01 52
Nanaud 22:f891c2bce091 53
Nanaud 16:ae65ce77b1f9 54 const double coeffG = 0.16008537;
Nanaud 16:ae65ce77b1f9 55 const double coeffD = 0.16059957;
Nanaud 16:ae65ce77b1f9 56 double dDist = 0, dAngl = 0; // Distance moyenne du robot et orientation
Nanaud 19:c419033c0967 57 double x = 110, y = 1070, O = 0;
Nanaud 2:094c09903a9c 58
Nanaud 16:ae65ce77b1f9 59 void odo2()
Nanaud 10:0714feaaaee1 60 {
Nanaud 16:ae65ce77b1f9 61 dDist = (double) ((comptG * coeffG) + (comptD * coeffD)) / 2.0f;
Nanaud 16:ae65ce77b1f9 62 dAngl = (double) ((comptD * coeffD) - (comptG * coeffG)) / entraxe;
Nanaud 6:ea6b30c4bb01 63
Nanaud 16:ae65ce77b1f9 64 x += (double) dDist * cos(O);
Nanaud 16:ae65ce77b1f9 65 y += (double) dDist * sin(O);
Nanaud 16:ae65ce77b1f9 66 O += (double) dAngl;
Nanaud 10:0714feaaaee1 67
Nanaud 16:ae65ce77b1f9 68 if (O > 3.1415) O = O - (2.0f * 3.1415f);
Nanaud 16:ae65ce77b1f9 69 if (O < -3.1415) O = O + (2.0f * 3.1415f);
Nanaud 14:dd3c756c6d48 70
plmir 12:2c312916a621 71 comptG = 0;
plmir 12:2c312916a621 72 comptD = 0;
Nanaud 14:dd3c756c6d48 73 }
Nanaud 14:dd3c756c6d48 74 //*/
Nanaud 14:dd3c756c6d48 75
Nanaud 17:176a1b4a2fa8 76
Nanaud 16:ae65ce77b1f9 77 double distanceCible = 0;
Nanaud 17:176a1b4a2fa8 78 double xC = 0, yC = 0; // x = xR et y = yR
Nanaud 16:ae65ce77b1f9 79 double consigneOrientation = 0;
Nanaud 16:ae65ce77b1f9 80 int signe = 1;
Nanaud 18:48246daf0c06 81 double cmdD = 0, cmdG = 0;
Nanaud 16:ae65ce77b1f9 82 double erreurAngle = 0;
Nanaud 16:ae65ce77b1f9 83 double erreurPre = 0;
Nanaud 16:ae65ce77b1f9 84 double deltaErreur = 0;
Nanaud 24:be2b2be6907b 85 const double coeffPro = 0.020; // 0.023 de base
Nanaud 24:be2b2be6907b 86 const double coeffDer = 0.010; // 0.023 de base
Nanaud 17:176a1b4a2fa8 87
Nanaud 19:c419033c0967 88 // Ligne droite
Nanaud 19:c419033c0967 89 double erreurPreDist = 0;
Nanaud 19:c419033c0967 90 double deltaErreurDist = 0;
Nanaud 24:be2b2be6907b 91 const double coeffProDist = 0.0008; // 0.010 de base
Nanaud 24:be2b2be6907b 92 const double coeffDerDist = 0.0008; // 0.010 de base
Nanaud 19:c419033c0967 93
Nanaud 17:176a1b4a2fa8 94 // NEW NEW NEW NEW
Nanaud 18:48246daf0c06 95 int fnc = 0;
Nanaud 18:48246daf0c06 96 bool acc = 1;
Nanaud 19:c419033c0967 97 double distancePrecedente = 0;
Nanaud 20:7d206773f39e 98 bool stt = 0; // Dépassement du point
Nanaud 20:7d206773f39e 99
Nanaud 22:f891c2bce091 100 double OrientationMem = 0;
Nanaud 22:f891c2bce091 101 double OrPlusPi2 = 0, OrMoinsPi2 = 0;
Nanaud 22:f891c2bce091 102
Nanaud 22:f891c2bce091 103 // Controle dépassement cible
Nanaud 22:f891c2bce091 104 double distanceMem = 0;
Nanaud 22:f891c2bce091 105 double distancePlus = 0;
Nanaud 16:ae65ce77b1f9 106
Nanaud 16:ae65ce77b1f9 107 void asserv()
Nanaud 16:ae65ce77b1f9 108 {
Nanaud 17:176a1b4a2fa8 109 // Odométrie
Nanaud 16:ae65ce77b1f9 110 odo2();
Nanaud 16:ae65ce77b1f9 111
Nanaud 17:176a1b4a2fa8 112 // Calcul de la cible
Nanaud 18:48246daf0c06 113
Nanaud 18:48246daf0c06 114 distanceCible = sqrt(((xC-x)*(xC-x))+((yC-y)*(yC-y)));
Nanaud 16:ae65ce77b1f9 115
Nanaud 16:ae65ce77b1f9 116 if(y > yC) {
Nanaud 16:ae65ce77b1f9 117 signe = -1;
Nanaud 16:ae65ce77b1f9 118 } else {
Nanaud 16:ae65ce77b1f9 119 signe = 1;
Nanaud 16:ae65ce77b1f9 120 }
Nanaud 16:ae65ce77b1f9 121
Nanaud 18:48246daf0c06 122 if (xC >= x)
Nanaud 18:48246daf0c06 123 consigneOrientation = signe * acos((abs(xC-x))/distanceCible);
Nanaud 18:48246daf0c06 124
Nanaud 18:48246daf0c06 125 else
Nanaud 18:48246daf0c06 126 consigneOrientation = signe * (3.1415 - acos((abs(xC-x))/distanceCible));
Nanaud 16:ae65ce77b1f9 127
Nanaud 17:176a1b4a2fa8 128 // Switch de sélection de l'étape
Nanaud 16:ae65ce77b1f9 129
Nanaud 18:48246daf0c06 130 switch (fnc) {
Nanaud 18:48246daf0c06 131 case 0: // Stand-by
Nanaud 18:48246daf0c06 132 mot_dis();
Nanaud 17:176a1b4a2fa8 133 break;
Nanaud 16:ae65ce77b1f9 134
Nanaud 17:176a1b4a2fa8 135 case 1: // Rotation
Nanaud 18:48246daf0c06 136 mot_en();
Nanaud 18:48246daf0c06 137
Nanaud 20:7d206773f39e 138 // Si on doit reculer
Nanaud 20:7d206773f39e 139 if (objRecule[indice]==1) {
Nanaud 20:7d206773f39e 140 consigneOrientation += 3.1415;
Nanaud 20:7d206773f39e 141
Nanaud 20:7d206773f39e 142 if(consigneOrientation>3.1415) consigneOrientation-=2*3.1415;
Nanaud 20:7d206773f39e 143 if(consigneOrientation<-3.1415) consigneOrientation+=2*3.1415;
Nanaud 20:7d206773f39e 144 }
Nanaud 20:7d206773f39e 145
Nanaud 19:c419033c0967 146 // Choix du sens de rotation
Nanaud 19:c419033c0967 147
Nanaud 20:7d206773f39e 148 double Osens = 0;
Nanaud 20:7d206773f39e 149 if (O<0) Osens = O + (2.0f*3.1415f);
Nanaud 20:7d206773f39e 150 else Osens = O;
Nanaud 20:7d206773f39e 151
Nanaud 20:7d206773f39e 152 double consigneSens = 0;
Nanaud 20:7d206773f39e 153 if (consigneOrientation<0) consigneSens = consigneOrientation + (2.0f*3.1415f);
Nanaud 20:7d206773f39e 154 else consigneSens = consigneOrientation;
Nanaud 19:c419033c0967 155
Nanaud 20:7d206773f39e 156 double choixSens = consigneSens - Osens;
Nanaud 20:7d206773f39e 157
Nanaud 20:7d206773f39e 158 if ((choixSens > 0) && (choixSens <= 3.1415) || (choixSens<(-3.1415))) {
Nanaud 18:48246daf0c06 159 motGauche_bck();
Nanaud 18:48246daf0c06 160 motDroite_fwd();
Nanaud 18:48246daf0c06 161 } else {
Nanaud 18:48246daf0c06 162 motGauche_fwd();
Nanaud 18:48246daf0c06 163 motDroite_bck();
Nanaud 18:48246daf0c06 164 }
Nanaud 18:48246daf0c06 165
Nanaud 17:176a1b4a2fa8 166 // Asservissement en position angulaire
Nanaud 17:176a1b4a2fa8 167 erreurAngle = consigneOrientation - O;
Nanaud 16:ae65ce77b1f9 168
Nanaud 17:176a1b4a2fa8 169 deltaErreur = erreurAngle - erreurPre;
Nanaud 16:ae65ce77b1f9 170
Nanaud 17:176a1b4a2fa8 171 erreurPre = erreurAngle;
Nanaud 16:ae65ce77b1f9 172
Nanaud 18:48246daf0c06 173 double deltaCommande = (abs(coeffPro * erreurAngle) + abs(coeffDer * deltaErreur));
Nanaud 16:ae65ce77b1f9 174
Nanaud 18:48246daf0c06 175 if(acc) {
Nanaud 24:be2b2be6907b 176 cmdG = cmdG + 0.0008; // +0.0008
Nanaud 17:176a1b4a2fa8 177 cmdD = cmdG;
Nanaud 18:48246daf0c06 178
Nanaud 19:c419033c0967 179 if (cmdG >= VMAXROT) acc = 0;
Nanaud 18:48246daf0c06 180 } else {
Nanaud 18:48246daf0c06 181 //acc = 0;
Nanaud 16:ae65ce77b1f9 182
Nanaud 19:c419033c0967 183 if (deltaCommande < VMAXROT) {
Nanaud 18:48246daf0c06 184 cmdG = deltaCommande;
Nanaud 18:48246daf0c06 185 cmdD = cmdG;
Nanaud 18:48246daf0c06 186 } else {
Nanaud 19:c419033c0967 187 cmdG = VMAXROT;
Nanaud 18:48246daf0c06 188 cmdD = cmdG;
Nanaud 18:48246daf0c06 189 }
Nanaud 17:176a1b4a2fa8 190 }
Nanaud 16:ae65ce77b1f9 191
Nanaud 17:176a1b4a2fa8 192 vitesseMotG(abs(cmdG));
Nanaud 17:176a1b4a2fa8 193 vitesseMotD(abs(cmdD));
Nanaud 16:ae65ce77b1f9 194
Nanaud 19:c419033c0967 195 if (O > (consigneOrientation - (1*0.0174533)) && O < (consigneOrientation + (1*0.0174533))) {
Nanaud 18:48246daf0c06 196 fnc++;
Nanaud 20:7d206773f39e 197 rebooted = 1;
Nanaud 17:176a1b4a2fa8 198 acc = 1;
Nanaud 22:f891c2bce091 199 //azerty();
Nanaud 22:f891c2bce091 200 distanceMem = distanceCible;
Nanaud 22:f891c2bce091 201 distancePlus = 0;
Nanaud 20:7d206773f39e 202 break;
Nanaud 17:176a1b4a2fa8 203 }
Nanaud 20:7d206773f39e 204
Nanaud 17:176a1b4a2fa8 205 break;
Nanaud 16:ae65ce77b1f9 206
Nanaud 17:176a1b4a2fa8 207 case 2: // Avancer
Nanaud 22:f891c2bce091 208
Nanaud 20:7d206773f39e 209 if (rebooted == 1 && wtt==1) {
Nanaud 20:7d206773f39e 210 cmdG = 0;
Nanaud 20:7d206773f39e 211 cmdD = 0;
Nanaud 20:7d206773f39e 212 rebooted = 0;
Nanaud 20:7d206773f39e 213 acc=1;
Nanaud 20:7d206773f39e 214 mot_en();
Nanaud 20:7d206773f39e 215 }
Nanaud 20:7d206773f39e 216
Nanaud 19:c419033c0967 217 deltaErreurDist = distanceCible - erreurPreDist;
Nanaud 19:c419033c0967 218
Nanaud 19:c419033c0967 219 erreurPreDist = distanceCible;
Nanaud 19:c419033c0967 220
Nanaud 19:c419033c0967 221 double deltaCommande2 = (abs(coeffProDist * distanceCible) + abs(coeffDerDist * deltaErreurDist));
Nanaud 19:c419033c0967 222
Nanaud 19:c419033c0967 223 if(acc) {
Nanaud 24:be2b2be6907b 224 cmdG = cmdG + 0.0006; // +0.0008
Nanaud 19:c419033c0967 225 cmdD = cmdG;
Nanaud 19:c419033c0967 226
Nanaud 19:c419033c0967 227 if (cmdG >= VMAXLIN) {
Nanaud 19:c419033c0967 228 acc = 0;
Nanaud 19:c419033c0967 229 }
Nanaud 19:c419033c0967 230 } else {
Nanaud 19:c419033c0967 231
Nanaud 19:c419033c0967 232 if (deltaCommande2 < VMAXLIN) {
Nanaud 19:c419033c0967 233 cmdG = deltaCommande2;
Nanaud 19:c419033c0967 234 cmdD = cmdG;
Nanaud 19:c419033c0967 235 } else {
Nanaud 19:c419033c0967 236 cmdG = VMAXLIN;
Nanaud 19:c419033c0967 237 cmdD = cmdG;
Nanaud 19:c419033c0967 238 }
Nanaud 19:c419033c0967 239 }
Nanaud 19:c419033c0967 240
Nanaud 22:f891c2bce091 241 distancePlus += abs(dDist);
Nanaud 19:c419033c0967 242
Nanaud 22:f891c2bce091 243 if ((distanceCible < 10) || (distancePlus >= distanceMem)) {
Nanaud 19:c419033c0967 244 acc = 1;
Nanaud 19:c419033c0967 245 indice++;
Nanaud 20:7d206773f39e 246
Nanaud 20:7d206773f39e 247 if (indice>=(int)NbObj) {
Nanaud 20:7d206773f39e 248 fnc = 0;
Nanaud 20:7d206773f39e 249 } else {
Nanaud 20:7d206773f39e 250 fnc = objEtape[indice];
Nanaud 20:7d206773f39e 251 xC = objX[indice];
Nanaud 20:7d206773f39e 252 yC = objY[indice];
Nanaud 20:7d206773f39e 253 }
Nanaud 20:7d206773f39e 254
Nanaud 20:7d206773f39e 255 distancePrecedente = 0;
Nanaud 20:7d206773f39e 256 break;
Nanaud 20:7d206773f39e 257
Nanaud 19:c419033c0967 258 }
Nanaud 16:ae65ce77b1f9 259
Nanaud 20:7d206773f39e 260 if (objRecule[indice] == 0) {
Nanaud 20:7d206773f39e 261 motGauche_fwd();
Nanaud 20:7d206773f39e 262 motDroite_fwd();
Nanaud 20:7d206773f39e 263 } else {
Nanaud 20:7d206773f39e 264 motGauche_bck();
Nanaud 20:7d206773f39e 265 motDroite_bck();
Nanaud 20:7d206773f39e 266 }
Nanaud 20:7d206773f39e 267
Nanaud 17:176a1b4a2fa8 268 vitesseMotG(cmdG);
Nanaud 17:176a1b4a2fa8 269 vitesseMotD(cmdD);
Nanaud 17:176a1b4a2fa8 270
Nanaud 19:c419033c0967 271 distancePrecedente = distanceCible;
Nanaud 17:176a1b4a2fa8 272 break;
Nanaud 17:176a1b4a2fa8 273
Nanaud 17:176a1b4a2fa8 274
Nanaud 17:176a1b4a2fa8 275 default:
Nanaud 17:176a1b4a2fa8 276 mot_dis();
Nanaud 16:ae65ce77b1f9 277 }
Nanaud 16:ae65ce77b1f9 278 }