AresENSEA-CDF2020
/
AresCDFMainCode_us2
Test
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?
User | Revision | Line number | New 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 | } |