AresENSEA-CDF2020 / Mbed 2 deprecated AresCDFMainCode_us2

Test

Dependencies:   mbed DRV8825

odo_asserv.cpp@23:a74135a0271d, 2020-10-28 (annotated)

Committer:
Nanaud
Date:
Wed Oct 28 16:27:05 2020 +0000
Revision:
23:a74135a0271d
Parent:
22:f891c2bce091
Child:
24:be2b2be6907b
avant => double to int

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