AresENSEA-CDF2020 / Mbed 2 deprecated AresCDFMainCode_us2

Test

Dependencies:   mbed DRV8825

odo_asserv.cpp@20:7d206773f39e, 2020-10-20 (annotated)

Committer:
Nanaud
Date:
Tue Oct 20 17:53:32 2020 +0000
Revision:
20:7d206773f39e
Parent:
19:c419033c0967
Child:
22:f891c2bce091
Fonctionnel, detection a ameliorer

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Nanaud 2:094c09903a9c 1 //Nom du fichier : odo_asserv.cpp
Nanaud 2:094c09903a9c 2 #include "pins.h"
Nanaud 16:ae65ce77b1f9 3
Nanaud 20:7d206773f39e 4 #define VMAXROT 0.050 // 0.030
Nanaud 20:7d206773f39e 5 #define VMAXLIN 0.130 // 0.050
Nanaud 20:7d206773f39e 6
Nanaud 20:7d206773f39e 7 //const float DISTLIM = 150.0;
Nanaud 20:7d206773f39e 8
Nanaud 20:7d206773f39e 9 int cptErreur = 0;
Nanaud 19:c419033c0967 10
Nanaud 19:c419033c0967 11 // Objectifs
Nanaud 20:7d206773f39e 12 //#define NbObj 5
Nanaud 19:c419033c0967 13 int indice = 0;
Nanaud 20:7d206773f39e 14 int objEtape[NbObj] = {0,1,1,1,1,1,1,1};
Nanaud 20:7d206773f39e 15 double objX[NbObj] = {0,300,660,660,210,660,400,210};
Nanaud 20:7d206773f39e 16 double objY[NbObj] = {0,1070,1270,1650,1300,1650,1800,1300};
Nanaud 20:7d206773f39e 17 int objRecule[NbObj] = {0,0,0,0,0,1,0,0};
Nanaud 2:094c09903a9c 18
Nanaud 6:ea6b30c4bb01 19 ///// VARIABLES
plmir 12:2c312916a621 20 Ticker ticker_odo;
Nanaud 16:ae65ce77b1f9 21 Ticker ticker_asserv;
plmir 12:2c312916a621 22
Nanaud 10:0714feaaaee1 23 // Coeff à définir empiriquement
Nanaud 16:ae65ce77b1f9 24 const double coeffGLong = 5.956, coeffDLong = 5.956; // constantes permettant la transformation tic/millimètre
Nanaud 20:7d206773f39e 25 //const double coeffGAngl = 737.447, coeffDAngl = 748.057; // constantes permettant la transformation tic/radian
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 20:7d206773f39e 29 //bool objOnce = 0;
Nanaud 14:dd3c756c6d48 30
Nanaud 6:ea6b30c4bb01 31 ///// INTERRUPTIONS CODEURS
Nanaud 6:ea6b30c4bb01 32
Nanaud 6:ea6b30c4bb01 33 void cdgaRise()
Nanaud 6:ea6b30c4bb01 34 {
Nanaud 16:ae65ce77b1f9 35 if(cdgB) comptG--;
Nanaud 16:ae65ce77b1f9 36 else comptG++;
Nanaud 6:ea6b30c4bb01 37 }
Nanaud 2:094c09903a9c 38
Nanaud 6:ea6b30c4bb01 39 void cddaRise()
Nanaud 6:ea6b30c4bb01 40 {
Nanaud 16:ae65ce77b1f9 41 if(cddB) comptD--;
Nanaud 16:ae65ce77b1f9 42 else comptD++;
Nanaud 6:ea6b30c4bb01 43 }
Nanaud 6:ea6b30c4bb01 44
Nanaud 14:dd3c756c6d48 45 ///*
Nanaud 16:ae65ce77b1f9 46 // odo2()
Nanaud 16:ae65ce77b1f9 47 //#define diametre 51.45 // 51.45 théorique
Nanaud 16:ae65ce77b1f9 48 //#define N 1000 // 1000 théorique
Nanaud 16:ae65ce77b1f9 49 #define entraxe 253 // 255 théorique
Nanaud 16:ae65ce77b1f9 50 //const double coeffG = ((double)(diametre/2)/(double)N)*2.0f*3.1415f;
Nanaud 16:ae65ce77b1f9 51 //const double coeffD = ((double)(diametre/2)/(double)N)*2.0f*3.1415f;
Nanaud 16:ae65ce77b1f9 52 const double coeffG = 0.16008537;
Nanaud 16:ae65ce77b1f9 53 const double coeffD = 0.16059957;
Nanaud 16:ae65ce77b1f9 54 double dDist = 0, dAngl = 0; // Distance moyenne du robot et orientation
Nanaud 19:c419033c0967 55 double x = 110, y = 1070, O = 0;
Nanaud 2:094c09903a9c 56
Nanaud 16:ae65ce77b1f9 57 void odo2()
Nanaud 10:0714feaaaee1 58 {
Nanaud 16:ae65ce77b1f9 59 dDist = (double) ((comptG * coeffG) + (comptD * coeffD)) / 2.0f;
Nanaud 16:ae65ce77b1f9 60 dAngl = (double) ((comptD * coeffD) - (comptG * coeffG)) / entraxe;
Nanaud 6:ea6b30c4bb01 61
Nanaud 16:ae65ce77b1f9 62 x += (double) dDist * cos(O);
Nanaud 16:ae65ce77b1f9 63 y += (double) dDist * sin(O);
Nanaud 16:ae65ce77b1f9 64 O += (double) dAngl;
Nanaud 10:0714feaaaee1 65
Nanaud 16:ae65ce77b1f9 66 if (O > 3.1415) O = O - (2.0f * 3.1415f);
Nanaud 16:ae65ce77b1f9 67 if (O < -3.1415) O = O + (2.0f * 3.1415f);
Nanaud 14:dd3c756c6d48 68
plmir 12:2c312916a621 69 comptG = 0;
plmir 12:2c312916a621 70 comptD = 0;
Nanaud 14:dd3c756c6d48 71 }
Nanaud 14:dd3c756c6d48 72 //*/
Nanaud 14:dd3c756c6d48 73
Nanaud 17:176a1b4a2fa8 74
Nanaud 16:ae65ce77b1f9 75 double distanceCible = 0;
Nanaud 17:176a1b4a2fa8 76 double xC = 0, yC = 0; // x = xR et y = yR
Nanaud 16:ae65ce77b1f9 77 double consigneOrientation = 0;
Nanaud 16:ae65ce77b1f9 78 //double consigneOrientation = (90*3.1415)/180;
Nanaud 16:ae65ce77b1f9 79 int signe = 1;
Nanaud 18:48246daf0c06 80 double cmdD = 0, cmdG = 0;
Nanaud 16:ae65ce77b1f9 81 double erreurAngle = 0;
Nanaud 16:ae65ce77b1f9 82 double erreurPre = 0;
Nanaud 16:ae65ce77b1f9 83 double deltaErreur = 0;
Nanaud 20:7d206773f39e 84 const double coeffPro = 0.075; // 0.023 de base
Nanaud 20:7d206773f39e 85 const double coeffDer = 0.060; // 0.023 de base
Nanaud 17:176a1b4a2fa8 86
Nanaud 19:c419033c0967 87 // Ligne droite
Nanaud 19:c419033c0967 88 //double erreurDist = 0;
Nanaud 19:c419033c0967 89 double erreurPreDist = 0;
Nanaud 19:c419033c0967 90 double deltaErreurDist = 0;
Nanaud 20:7d206773f39e 91 const double coeffProDist = 0.0005; // 0.010 de base
Nanaud 20:7d206773f39e 92 const double coeffDerDist = 0.0005; // 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 20:7d206773f39e 100 // Reculer
Nanaud 20:7d206773f39e 101 double distanceFaite = 0;
Nanaud 16:ae65ce77b1f9 102
Nanaud 16:ae65ce77b1f9 103 void asserv()
Nanaud 16:ae65ce77b1f9 104 {
Nanaud 17:176a1b4a2fa8 105 // Odométrie
Nanaud 16:ae65ce77b1f9 106 odo2();
Nanaud 16:ae65ce77b1f9 107
Nanaud 17:176a1b4a2fa8 108 // Calcul de la cible
Nanaud 18:48246daf0c06 109
Nanaud 18:48246daf0c06 110 distanceCible = sqrt(((xC-x)*(xC-x))+((yC-y)*(yC-y)));
Nanaud 16:ae65ce77b1f9 111
Nanaud 16:ae65ce77b1f9 112 if(y > yC) {
Nanaud 16:ae65ce77b1f9 113 signe = -1;
Nanaud 16:ae65ce77b1f9 114 } else {
Nanaud 16:ae65ce77b1f9 115 signe = 1;
Nanaud 16:ae65ce77b1f9 116 }
Nanaud 16:ae65ce77b1f9 117
Nanaud 18:48246daf0c06 118 if (xC >= x)
Nanaud 18:48246daf0c06 119 consigneOrientation = signe * acos((abs(xC-x))/distanceCible);
Nanaud 18:48246daf0c06 120
Nanaud 18:48246daf0c06 121 else
Nanaud 18:48246daf0c06 122 consigneOrientation = signe * (3.1415 - acos((abs(xC-x))/distanceCible));
Nanaud 16:ae65ce77b1f9 123
Nanaud 17:176a1b4a2fa8 124 // Switch de sélection de l'étape
Nanaud 16:ae65ce77b1f9 125
Nanaud 18:48246daf0c06 126 switch (fnc) {
Nanaud 18:48246daf0c06 127 case 0: // Stand-by
Nanaud 18:48246daf0c06 128 mot_dis();
Nanaud 17:176a1b4a2fa8 129 break;
Nanaud 16:ae65ce77b1f9 130
Nanaud 17:176a1b4a2fa8 131 case 1: // Rotation
Nanaud 18:48246daf0c06 132 mot_en();
Nanaud 18:48246daf0c06 133
Nanaud 20:7d206773f39e 134 // Si on doit reculer
Nanaud 20:7d206773f39e 135 if (objRecule[indice]==1) {
Nanaud 20:7d206773f39e 136 consigneOrientation += 3.1415;
Nanaud 20:7d206773f39e 137
Nanaud 20:7d206773f39e 138 if(consigneOrientation>3.1415) consigneOrientation-=2*3.1415;
Nanaud 20:7d206773f39e 139 if(consigneOrientation<-3.1415) consigneOrientation+=2*3.1415;
Nanaud 20:7d206773f39e 140 }
Nanaud 20:7d206773f39e 141
Nanaud 19:c419033c0967 142 // Choix du sens de rotation
Nanaud 19:c419033c0967 143
Nanaud 20:7d206773f39e 144 double Osens = 0;
Nanaud 20:7d206773f39e 145 if (O<0) Osens = O + (2.0f*3.1415f);
Nanaud 20:7d206773f39e 146 else Osens = O;
Nanaud 20:7d206773f39e 147
Nanaud 20:7d206773f39e 148 double consigneSens = 0;
Nanaud 20:7d206773f39e 149 if (consigneOrientation<0) consigneSens = consigneOrientation + (2.0f*3.1415f);
Nanaud 20:7d206773f39e 150 else consigneSens = consigneOrientation;
Nanaud 19:c419033c0967 151
Nanaud 20:7d206773f39e 152 double choixSens = consigneSens - Osens;
Nanaud 20:7d206773f39e 153
Nanaud 20:7d206773f39e 154 //if(objRecule[indice] == 0) {
Nanaud 20:7d206773f39e 155 //if (choixSens > 3.1415) choixSens -= (2.0f * 3.1415f);
Nanaud 20:7d206773f39e 156 //else if (choixSens < 3.1415) choixSens += (2.0f * 3.1415f);
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 20:7d206773f39e 165 //}
Nanaud 20:7d206773f39e 166
Nanaud 20:7d206773f39e 167 /*
Nanaud 20:7d206773f39e 168 if(objRecule[indice] == 1) {
Nanaud 20:7d206773f39e 169 //if (choixSens > 3.1415) choixSens -= (2.0f * 3.1415f);
Nanaud 20:7d206773f39e 170 //else if (choixSens < 3.1415) choixSens += (2.0f * 3.1415f);
Nanaud 20:7d206773f39e 171
Nanaud 20:7d206773f39e 172 if (choixSens > 0) {
Nanaud 20:7d206773f39e 173 motGauche_fwd();
Nanaud 20:7d206773f39e 174 motDroite_bck();
Nanaud 20:7d206773f39e 175 } else {
Nanaud 20:7d206773f39e 176 motGauche_bck();
Nanaud 20:7d206773f39e 177 motDroite_fwd();
Nanaud 20:7d206773f39e 178 }
Nanaud 20:7d206773f39e 179 }
Nanaud 20:7d206773f39e 180 */
Nanaud 20:7d206773f39e 181
Nanaud 20:7d206773f39e 182 /*
Nanaud 20:7d206773f39e 183 double choixSens = consigneOrientation - O;
Nanaud 20:7d206773f39e 184
Nanaud 20:7d206773f39e 185 if(objRecule[indice] == 0) {
Nanaud 20:7d206773f39e 186 if (choixSens > 3.1415) choixSens -= (2.0f * 3.1415f);
Nanaud 20:7d206773f39e 187 else if (choixSens < 3.1415) choixSens += (2.0f * 3.1415f);
Nanaud 20:7d206773f39e 188
Nanaud 20:7d206773f39e 189 if (choixSens > 0) {
Nanaud 20:7d206773f39e 190 motGauche_bck();
Nanaud 20:7d206773f39e 191 motDroite_fwd();
Nanaud 20:7d206773f39e 192 } else {
Nanaud 20:7d206773f39e 193 motGauche_fwd();
Nanaud 20:7d206773f39e 194 motDroite_bck();
Nanaud 20:7d206773f39e 195 }
Nanaud 20:7d206773f39e 196 }
Nanaud 20:7d206773f39e 197
Nanaud 20:7d206773f39e 198 if(objRecule[indice] == 1) {
Nanaud 20:7d206773f39e 199 if (choixSens > 3.1415) choixSens -= (2.0f * 3.1415f);
Nanaud 20:7d206773f39e 200 else if (choixSens < 3.1415) choixSens += (2.0f * 3.1415f);
Nanaud 20:7d206773f39e 201
Nanaud 20:7d206773f39e 202 if (choixSens > 0) {
Nanaud 20:7d206773f39e 203 motGauche_fwd();
Nanaud 20:7d206773f39e 204 motDroite_bck();
Nanaud 20:7d206773f39e 205 } else {
Nanaud 20:7d206773f39e 206 motGauche_bck();
Nanaud 20:7d206773f39e 207 motDroite_fwd();
Nanaud 20:7d206773f39e 208 }
Nanaud 20:7d206773f39e 209 }
Nanaud 20:7d206773f39e 210 */
Nanaud 18:48246daf0c06 211
Nanaud 17:176a1b4a2fa8 212 // Asservissement en position angulaire
Nanaud 17:176a1b4a2fa8 213 erreurAngle = consigneOrientation - O;
Nanaud 16:ae65ce77b1f9 214
Nanaud 17:176a1b4a2fa8 215 deltaErreur = erreurAngle - erreurPre;
Nanaud 16:ae65ce77b1f9 216
Nanaud 17:176a1b4a2fa8 217 erreurPre = erreurAngle;
Nanaud 16:ae65ce77b1f9 218
Nanaud 18:48246daf0c06 219 double deltaCommande = (abs(coeffPro * erreurAngle) + abs(coeffDer * deltaErreur));
Nanaud 16:ae65ce77b1f9 220
Nanaud 18:48246daf0c06 221 if(acc) {
Nanaud 20:7d206773f39e 222 cmdG = cmdG + 0.0007; // +0.0008
Nanaud 17:176a1b4a2fa8 223 cmdD = cmdG;
Nanaud 18:48246daf0c06 224
Nanaud 19:c419033c0967 225 if (cmdG >= VMAXROT) acc = 0;
Nanaud 18:48246daf0c06 226 } else {
Nanaud 18:48246daf0c06 227 //acc = 0;
Nanaud 16:ae65ce77b1f9 228
Nanaud 19:c419033c0967 229 if (deltaCommande < VMAXROT) {
Nanaud 18:48246daf0c06 230 cmdG = deltaCommande;
Nanaud 18:48246daf0c06 231 cmdD = cmdG;
Nanaud 18:48246daf0c06 232 } else {
Nanaud 19:c419033c0967 233 cmdG = VMAXROT;
Nanaud 18:48246daf0c06 234 cmdD = cmdG;
Nanaud 18:48246daf0c06 235 }
Nanaud 17:176a1b4a2fa8 236 }
Nanaud 16:ae65ce77b1f9 237
Nanaud 17:176a1b4a2fa8 238 vitesseMotG(abs(cmdG));
Nanaud 17:176a1b4a2fa8 239 vitesseMotD(abs(cmdD));
Nanaud 16:ae65ce77b1f9 240
Nanaud 19:c419033c0967 241 if (O > (consigneOrientation - (1*0.0174533)) && O < (consigneOrientation + (1*0.0174533))) {
Nanaud 18:48246daf0c06 242 //mot_dis();
Nanaud 18:48246daf0c06 243 fnc++;
Nanaud 20:7d206773f39e 244 rebooted = 1;
Nanaud 17:176a1b4a2fa8 245 acc = 1;
Nanaud 20:7d206773f39e 246 //objOnce = 0;
Nanaud 20:7d206773f39e 247 break;
Nanaud 17:176a1b4a2fa8 248 }
Nanaud 20:7d206773f39e 249
Nanaud 17:176a1b4a2fa8 250 break;
Nanaud 16:ae65ce77b1f9 251
Nanaud 17:176a1b4a2fa8 252 case 2: // Avancer
Nanaud 20:7d206773f39e 253
Nanaud 20:7d206773f39e 254 if (rebooted == 1 && wtt==1) {
Nanaud 20:7d206773f39e 255 cmdG = 0;
Nanaud 20:7d206773f39e 256 cmdD = 0;
Nanaud 20:7d206773f39e 257 rebooted = 0;
Nanaud 20:7d206773f39e 258 acc=1;
Nanaud 20:7d206773f39e 259 mot_en();
Nanaud 20:7d206773f39e 260 }
Nanaud 20:7d206773f39e 261
Nanaud 20:7d206773f39e 262 /*
Nanaud 20:7d206773f39e 263 if(objRecule[indice]==0) {
Nanaud 20:7d206773f39e 264 if ((::distance[0] >= 120) && (::distance[1] >= 140) && (::distance[5] >= 140)) {
Nanaud 20:7d206773f39e 265 mot_en();
Nanaud 20:7d206773f39e 266 cptErreur=0;
Nanaud 20:7d206773f39e 267
Nanaud 20:7d206773f39e 268 } else {
Nanaud 20:7d206773f39e 269 cptErreur++;
Nanaud 20:7d206773f39e 270
Nanaud 20:7d206773f39e 271 if(cptErreur>=5) {
Nanaud 20:7d206773f39e 272 mot_dis();
Nanaud 20:7d206773f39e 273 acc = 1;
Nanaud 20:7d206773f39e 274 }
Nanaud 20:7d206773f39e 275
Nanaud 20:7d206773f39e 276 break;
Nanaud 20:7d206773f39e 277 }
Nanaud 20:7d206773f39e 278 }
Nanaud 20:7d206773f39e 279
Nanaud 20:7d206773f39e 280 else if(objRecule[indice]==1) {
Nanaud 20:7d206773f39e 281 if ((::distance[2] >= 140) && (::distance[3] >= 120) && (::distance[4] >= 140)) {
Nanaud 20:7d206773f39e 282 mot_en();
Nanaud 20:7d206773f39e 283 cptErreur=0;
Nanaud 20:7d206773f39e 284
Nanaud 20:7d206773f39e 285 } else {
Nanaud 20:7d206773f39e 286 cptErreur++;
Nanaud 20:7d206773f39e 287
Nanaud 20:7d206773f39e 288 if(cptErreur>=5) {
Nanaud 20:7d206773f39e 289 mot_dis();
Nanaud 20:7d206773f39e 290 acc = 1;
Nanaud 20:7d206773f39e 291 }
Nanaud 20:7d206773f39e 292
Nanaud 20:7d206773f39e 293 break;
Nanaud 20:7d206773f39e 294 }
Nanaud 20:7d206773f39e 295 }
Nanaud 20:7d206773f39e 296 */
Nanaud 18:48246daf0c06 297
Nanaud 19:c419033c0967 298 /*
Nanaud 19:c419033c0967 299 cmdD = abs((int)distanceCible)*0.0001; // *0.005
Nanaud 19:c419033c0967 300 if(cmdD>VMAXLIN) {
Nanaud 19:c419033c0967 301 cmdD = VMAXLIN;
Nanaud 17:176a1b4a2fa8 302 }
Nanaud 17:176a1b4a2fa8 303 cmdG = cmdD;
Nanaud 19:c419033c0967 304 */
Nanaud 19:c419033c0967 305
Nanaud 19:c419033c0967 306 // Asservissement distance à parcourir
Nanaud 19:c419033c0967 307 //erreurDist = consigneOrientation - O;
Nanaud 19:c419033c0967 308
Nanaud 19:c419033c0967 309 deltaErreurDist = distanceCible - erreurPreDist;
Nanaud 19:c419033c0967 310
Nanaud 19:c419033c0967 311 erreurPreDist = distanceCible;
Nanaud 19:c419033c0967 312
Nanaud 19:c419033c0967 313 double deltaCommande2 = (abs(coeffProDist * distanceCible) + abs(coeffDerDist * deltaErreurDist));
Nanaud 19:c419033c0967 314
Nanaud 19:c419033c0967 315 if(acc) {
Nanaud 20:7d206773f39e 316 cmdG = cmdG + 0.0012; // +0.0008
Nanaud 19:c419033c0967 317 cmdD = cmdG;
Nanaud 19:c419033c0967 318
Nanaud 19:c419033c0967 319 if (cmdG >= VMAXLIN) {
Nanaud 19:c419033c0967 320 acc = 0;
Nanaud 20:7d206773f39e 321 //stt = 1;
Nanaud 19:c419033c0967 322 }
Nanaud 19:c419033c0967 323 } else {
Nanaud 19:c419033c0967 324 //acc = 0;
Nanaud 19:c419033c0967 325
Nanaud 19:c419033c0967 326 if (deltaCommande2 < VMAXLIN) {
Nanaud 19:c419033c0967 327 cmdG = deltaCommande2;
Nanaud 19:c419033c0967 328 cmdD = cmdG;
Nanaud 19:c419033c0967 329 } else {
Nanaud 19:c419033c0967 330 cmdG = VMAXLIN;
Nanaud 19:c419033c0967 331 cmdD = cmdG;
Nanaud 19:c419033c0967 332 }
Nanaud 19:c419033c0967 333 }
Nanaud 19:c419033c0967 334
Nanaud 20:7d206773f39e 335 //vitesseMotG(abs(cmdG));
Nanaud 20:7d206773f39e 336 //vitesseMotD(abs(cmdD));
Nanaud 19:c419033c0967 337
Nanaud 19:c419033c0967 338 //if (distanceCible < 15)[
Nanaud 19:c419033c0967 339 //if ((x < xC+10) && (x > xC-10) && (y < yC+10) && (y > yC-10)) {
Nanaud 20:7d206773f39e 340 if ((distanceCible < 10) || (acc==0 && wtt==1 && rebooted==0 && (distancePrecedente < distanceCible))) {
Nanaud 19:c419033c0967 341 //mot_dis();
Nanaud 20:7d206773f39e 342 //fnc++;
Nanaud 19:c419033c0967 343 acc = 1;
Nanaud 20:7d206773f39e 344 //stt = 0;
Nanaud 20:7d206773f39e 345
Nanaud 20:7d206773f39e 346 //if (objOnce == 0) {
Nanaud 19:c419033c0967 347 indice++;
Nanaud 20:7d206773f39e 348 //objOnce=1;
Nanaud 20:7d206773f39e 349
Nanaud 20:7d206773f39e 350
Nanaud 20:7d206773f39e 351
Nanaud 20:7d206773f39e 352 //}
Nanaud 20:7d206773f39e 353
Nanaud 20:7d206773f39e 354 if (indice>=(int)NbObj) {
Nanaud 20:7d206773f39e 355 fnc = 0;
Nanaud 20:7d206773f39e 356 } else {
Nanaud 20:7d206773f39e 357 fnc = objEtape[indice];
Nanaud 20:7d206773f39e 358 xC = objX[indice];
Nanaud 20:7d206773f39e 359 yC = objY[indice];
Nanaud 20:7d206773f39e 360 }
Nanaud 20:7d206773f39e 361
Nanaud 20:7d206773f39e 362 distancePrecedente = 0;
Nanaud 20:7d206773f39e 363 break;
Nanaud 20:7d206773f39e 364
Nanaud 20:7d206773f39e 365 /*
Nanaud 20:7d206773f39e 366 if (indice>=NbObj) {
Nanaud 20:7d206773f39e 367 fnc = 0;
Nanaud 20:7d206773f39e 368 } else {
Nanaud 20:7d206773f39e 369 fnc = objEtape[indice];
Nanaud 20:7d206773f39e 370 xC = objX[indice];
Nanaud 20:7d206773f39e 371 yC = objY[indice];
Nanaud 20:7d206773f39e 372 }
Nanaud 20:7d206773f39e 373 */
Nanaud 19:c419033c0967 374 }
Nanaud 16:ae65ce77b1f9 375
Nanaud 20:7d206773f39e 376 if (objRecule[indice] == 0) {
Nanaud 20:7d206773f39e 377 motGauche_fwd();
Nanaud 20:7d206773f39e 378 motDroite_fwd();
Nanaud 20:7d206773f39e 379 } else {
Nanaud 20:7d206773f39e 380 motGauche_bck();
Nanaud 20:7d206773f39e 381 motDroite_bck();
Nanaud 20:7d206773f39e 382 }
Nanaud 20:7d206773f39e 383
Nanaud 17:176a1b4a2fa8 384 vitesseMotG(cmdG);
Nanaud 17:176a1b4a2fa8 385 vitesseMotD(cmdD);
Nanaud 17:176a1b4a2fa8 386
Nanaud 19:c419033c0967 387 distancePrecedente = distanceCible;
Nanaud 17:176a1b4a2fa8 388 break;
Nanaud 17:176a1b4a2fa8 389
Nanaud 20:7d206773f39e 390 /*
Nanaud 17:176a1b4a2fa8 391 case 3: // Reculer
Nanaud 20:7d206773f39e 392
Nanaud 20:7d206773f39e 393 if (objRecule[indice] == 1) {
Nanaud 20:7d206773f39e 394 distanceFaite += abs(dDist);
Nanaud 20:7d206773f39e 395
Nanaud 20:7d206773f39e 396 motGauche_bck();
Nanaud 20:7d206773f39e 397 motDroite_bck();
Nanaud 20:7d206773f39e 398 vitesseMotG(0.01);
Nanaud 20:7d206773f39e 399 vitesseMotD(0.01);
Nanaud 20:7d206773f39e 400 }
Nanaud 20:7d206773f39e 401
Nanaud 20:7d206773f39e 402 if ((distanceFaite >= 100) || (objRecule[indice] == 0)) {
Nanaud 20:7d206773f39e 403 indice++;
Nanaud 20:7d206773f39e 404 fnc = objEtape[indice];
Nanaud 20:7d206773f39e 405 xC = objX[indice];
Nanaud 20:7d206773f39e 406 yC = objY[indice];
Nanaud 20:7d206773f39e 407 distanceFaite = 0;
Nanaud 20:7d206773f39e 408 }
Nanaud 20:7d206773f39e 409
Nanaud 17:176a1b4a2fa8 410 break;
Nanaud 20:7d206773f39e 411 */
Nanaud 17:176a1b4a2fa8 412
Nanaud 17:176a1b4a2fa8 413 default:
Nanaud 17:176a1b4a2fa8 414 mot_dis();
Nanaud 16:ae65ce77b1f9 415 }
Nanaud 16:ae65ce77b1f9 416 }