AresENSEA-CDF2020
/
AresCDFMainCode_us2
Test
odo_asserv.cpp@19:c419033c0967, 2020-10-12 (annotated)
- Committer:
- Nanaud
- Date:
- Mon Oct 12 19:17:40 2020 +0000
- Revision:
- 19:c419033c0967
- Parent:
- 18:48246daf0c06
- Child:
- 20:7d206773f39e
Dernieres modifs
Who changed what in which revision?
User | Revision | Line number | New 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 | 19:c419033c0967 | 4 | #define VMAXROT 0.060 // 0.030 |
Nanaud | 19:c419033c0967 | 5 | #define VMAXLIN 0.100 // 0.050 |
Nanaud | 19:c419033c0967 | 6 | |
Nanaud | 19:c419033c0967 | 7 | // Objectifs |
Nanaud | 19:c419033c0967 | 8 | #define NbObj 4 |
Nanaud | 19:c419033c0967 | 9 | int indice = 0; |
Nanaud | 19:c419033c0967 | 10 | int objEtape [4] = {0,1,1,1}; |
Nanaud | 19:c419033c0967 | 11 | double objX[4] = {0,660, 660, 210}; |
Nanaud | 19:c419033c0967 | 12 | double objY[4] = {0,1070,1650,1300}; |
Nanaud | 2:094c09903a9c | 13 | |
Nanaud | 6:ea6b30c4bb01 | 14 | ///// VARIABLES |
plmir | 12:2c312916a621 | 15 | Ticker ticker_odo; |
Nanaud | 16:ae65ce77b1f9 | 16 | Ticker ticker_asserv; |
plmir | 12:2c312916a621 | 17 | |
Nanaud | 10:0714feaaaee1 | 18 | // Coeff à définir empiriquement |
Nanaud | 16:ae65ce77b1f9 | 19 | const double coeffGLong = 5.956, coeffDLong = 5.956; // constantes permettant la transformation tic/millimètre |
Nanaud | 16:ae65ce77b1f9 | 20 | const double coeffGAngl = 737.447, coeffDAngl = 748.057; // constantes permettant la transformation tic/radian |
Nanaud | 6:ea6b30c4bb01 | 21 | |
Nanaud | 6:ea6b30c4bb01 | 22 | long comptG = 0, comptD = 0; // nb de tics comptés pour chaque codeur |
Nanaud | 6:ea6b30c4bb01 | 23 | |
Nanaud | 14:dd3c756c6d48 | 24 | |
Nanaud | 6:ea6b30c4bb01 | 25 | ///// INTERRUPTIONS CODEURS |
Nanaud | 6:ea6b30c4bb01 | 26 | |
Nanaud | 6:ea6b30c4bb01 | 27 | void cdgaRise() |
Nanaud | 6:ea6b30c4bb01 | 28 | { |
Nanaud | 16:ae65ce77b1f9 | 29 | if(cdgB) comptG--; |
Nanaud | 16:ae65ce77b1f9 | 30 | else comptG++; |
Nanaud | 6:ea6b30c4bb01 | 31 | } |
Nanaud | 2:094c09903a9c | 32 | |
Nanaud | 6:ea6b30c4bb01 | 33 | void cddaRise() |
Nanaud | 6:ea6b30c4bb01 | 34 | { |
Nanaud | 16:ae65ce77b1f9 | 35 | if(cddB) comptD--; |
Nanaud | 16:ae65ce77b1f9 | 36 | else comptD++; |
Nanaud | 6:ea6b30c4bb01 | 37 | } |
Nanaud | 6:ea6b30c4bb01 | 38 | |
Nanaud | 16:ae65ce77b1f9 | 39 | |
Nanaud | 16:ae65ce77b1f9 | 40 | |
Nanaud | 14:dd3c756c6d48 | 41 | ///* |
Nanaud | 16:ae65ce77b1f9 | 42 | // odo2() |
Nanaud | 16:ae65ce77b1f9 | 43 | //#define diametre 51.45 // 51.45 théorique |
Nanaud | 16:ae65ce77b1f9 | 44 | //#define N 1000 // 1000 théorique |
Nanaud | 16:ae65ce77b1f9 | 45 | #define entraxe 253 // 255 théorique |
Nanaud | 16:ae65ce77b1f9 | 46 | //const double coeffG = ((double)(diametre/2)/(double)N)*2.0f*3.1415f; |
Nanaud | 16:ae65ce77b1f9 | 47 | //const double coeffD = ((double)(diametre/2)/(double)N)*2.0f*3.1415f; |
Nanaud | 16:ae65ce77b1f9 | 48 | const double coeffG = 0.16008537; |
Nanaud | 16:ae65ce77b1f9 | 49 | const double coeffD = 0.16059957; |
Nanaud | 16:ae65ce77b1f9 | 50 | double dDist = 0, dAngl = 0; // Distance moyenne du robot et orientation |
Nanaud | 19:c419033c0967 | 51 | double x = 110, y = 1070, O = 0; |
Nanaud | 2:094c09903a9c | 52 | |
Nanaud | 16:ae65ce77b1f9 | 53 | void odo2() |
Nanaud | 10:0714feaaaee1 | 54 | { |
Nanaud | 16:ae65ce77b1f9 | 55 | dDist = (double) ((comptG * coeffG) + (comptD * coeffD)) / 2.0f; |
Nanaud | 16:ae65ce77b1f9 | 56 | dAngl = (double) ((comptD * coeffD) - (comptG * coeffG)) / entraxe; |
Nanaud | 6:ea6b30c4bb01 | 57 | |
Nanaud | 16:ae65ce77b1f9 | 58 | x += (double) dDist * cos(O); |
Nanaud | 16:ae65ce77b1f9 | 59 | y += (double) dDist * sin(O); |
Nanaud | 16:ae65ce77b1f9 | 60 | O += (double) dAngl; |
Nanaud | 10:0714feaaaee1 | 61 | |
Nanaud | 18:48246daf0c06 | 62 | |
Nanaud | 16:ae65ce77b1f9 | 63 | if (O > 3.1415) O = O - (2.0f * 3.1415f); |
Nanaud | 16:ae65ce77b1f9 | 64 | if (O < -3.1415) O = O + (2.0f * 3.1415f); |
Nanaud | 14:dd3c756c6d48 | 65 | |
Nanaud | 18:48246daf0c06 | 66 | |
plmir | 12:2c312916a621 | 67 | comptG = 0; |
plmir | 12:2c312916a621 | 68 | comptD = 0; |
Nanaud | 14:dd3c756c6d48 | 69 | } |
Nanaud | 14:dd3c756c6d48 | 70 | //*/ |
Nanaud | 14:dd3c756c6d48 | 71 | |
Nanaud | 17:176a1b4a2fa8 | 72 | |
Nanaud | 16:ae65ce77b1f9 | 73 | double distanceCible = 0; |
Nanaud | 17:176a1b4a2fa8 | 74 | double xC = 0, yC = 0; // x = xR et y = yR |
Nanaud | 16:ae65ce77b1f9 | 75 | double consigneOrientation = 0; |
Nanaud | 16:ae65ce77b1f9 | 76 | //double consigneOrientation = (90*3.1415)/180; |
Nanaud | 16:ae65ce77b1f9 | 77 | int signe = 1; |
Nanaud | 18:48246daf0c06 | 78 | double cmdD = 0, cmdG = 0; |
Nanaud | 16:ae65ce77b1f9 | 79 | double erreurAngle = 0; |
Nanaud | 16:ae65ce77b1f9 | 80 | double erreurPre = 0; |
Nanaud | 16:ae65ce77b1f9 | 81 | double deltaErreur = 0; |
Nanaud | 19:c419033c0967 | 82 | const double coeffPro = 0.08; // 0.023 de base |
Nanaud | 18:48246daf0c06 | 83 | const double coeffDer = 0.06; // 0.023 de base |
Nanaud | 17:176a1b4a2fa8 | 84 | |
Nanaud | 19:c419033c0967 | 85 | // Ligne droite |
Nanaud | 19:c419033c0967 | 86 | //double erreurDist = 0; |
Nanaud | 19:c419033c0967 | 87 | double erreurPreDist = 0; |
Nanaud | 19:c419033c0967 | 88 | double deltaErreurDist = 0; |
Nanaud | 19:c419033c0967 | 89 | const double coeffProDist = 0.10; // 0.023 de base |
Nanaud | 19:c419033c0967 | 90 | const double coeffDerDist = 0.10; // 0.023 de base |
Nanaud | 19:c419033c0967 | 91 | |
Nanaud | 17:176a1b4a2fa8 | 92 | // NEW NEW NEW NEW |
Nanaud | 18:48246daf0c06 | 93 | int fnc = 0; |
Nanaud | 18:48246daf0c06 | 94 | bool acc = 1; |
Nanaud | 19:c419033c0967 | 95 | double distancePrecedente = 0; |
Nanaud | 19:c419033c0967 | 96 | bool stt = 0; |
Nanaud | 16:ae65ce77b1f9 | 97 | |
Nanaud | 16:ae65ce77b1f9 | 98 | void asserv() |
Nanaud | 16:ae65ce77b1f9 | 99 | { |
Nanaud | 17:176a1b4a2fa8 | 100 | // Odométrie |
Nanaud | 16:ae65ce77b1f9 | 101 | odo2(); |
Nanaud | 16:ae65ce77b1f9 | 102 | |
Nanaud | 17:176a1b4a2fa8 | 103 | // Calcul de la cible |
Nanaud | 18:48246daf0c06 | 104 | |
Nanaud | 18:48246daf0c06 | 105 | distanceCible = sqrt(((xC-x)*(xC-x))+((yC-y)*(yC-y))); |
Nanaud | 16:ae65ce77b1f9 | 106 | |
Nanaud | 16:ae65ce77b1f9 | 107 | if(y > yC) { |
Nanaud | 16:ae65ce77b1f9 | 108 | signe = -1; |
Nanaud | 16:ae65ce77b1f9 | 109 | } else { |
Nanaud | 16:ae65ce77b1f9 | 110 | signe = 1; |
Nanaud | 16:ae65ce77b1f9 | 111 | } |
Nanaud | 16:ae65ce77b1f9 | 112 | |
Nanaud | 18:48246daf0c06 | 113 | if (xC >= x) |
Nanaud | 18:48246daf0c06 | 114 | consigneOrientation = signe * acos((abs(xC-x))/distanceCible); |
Nanaud | 18:48246daf0c06 | 115 | |
Nanaud | 18:48246daf0c06 | 116 | else |
Nanaud | 18:48246daf0c06 | 117 | consigneOrientation = signe * (3.1415 - acos((abs(xC-x))/distanceCible)); |
Nanaud | 16:ae65ce77b1f9 | 118 | |
Nanaud | 17:176a1b4a2fa8 | 119 | // Switch de sélection de l'étape |
Nanaud | 16:ae65ce77b1f9 | 120 | |
Nanaud | 18:48246daf0c06 | 121 | switch (fnc) { |
Nanaud | 18:48246daf0c06 | 122 | case 0: // Stand-by |
Nanaud | 18:48246daf0c06 | 123 | mot_dis(); |
Nanaud | 17:176a1b4a2fa8 | 124 | break; |
Nanaud | 16:ae65ce77b1f9 | 125 | |
Nanaud | 17:176a1b4a2fa8 | 126 | case 1: // Rotation |
Nanaud | 18:48246daf0c06 | 127 | mot_en(); |
Nanaud | 18:48246daf0c06 | 128 | |
Nanaud | 19:c419033c0967 | 129 | // Choix du sens de rotation |
Nanaud | 19:c419033c0967 | 130 | double choixSens = consigneOrientation - O; |
Nanaud | 19:c419033c0967 | 131 | |
Nanaud | 19:c419033c0967 | 132 | if (choixSens > 3.1415) choixSens -= (2.0f * 3.1415f); |
Nanaud | 19:c419033c0967 | 133 | else if (choixSens < 3.1415) choixSens += (2.0f * 3.1415f); |
Nanaud | 19:c419033c0967 | 134 | |
Nanaud | 19:c419033c0967 | 135 | if (choixSens > 0) { |
Nanaud | 18:48246daf0c06 | 136 | motGauche_bck(); |
Nanaud | 18:48246daf0c06 | 137 | motDroite_fwd(); |
Nanaud | 18:48246daf0c06 | 138 | } else { |
Nanaud | 18:48246daf0c06 | 139 | motGauche_fwd(); |
Nanaud | 18:48246daf0c06 | 140 | motDroite_bck(); |
Nanaud | 18:48246daf0c06 | 141 | } |
Nanaud | 18:48246daf0c06 | 142 | |
Nanaud | 17:176a1b4a2fa8 | 143 | // Asservissement en position angulaire |
Nanaud | 17:176a1b4a2fa8 | 144 | erreurAngle = consigneOrientation - O; |
Nanaud | 16:ae65ce77b1f9 | 145 | |
Nanaud | 17:176a1b4a2fa8 | 146 | deltaErreur = erreurAngle - erreurPre; |
Nanaud | 16:ae65ce77b1f9 | 147 | |
Nanaud | 17:176a1b4a2fa8 | 148 | erreurPre = erreurAngle; |
Nanaud | 16:ae65ce77b1f9 | 149 | |
Nanaud | 18:48246daf0c06 | 150 | double deltaCommande = (abs(coeffPro * erreurAngle) + abs(coeffDer * deltaErreur)); |
Nanaud | 16:ae65ce77b1f9 | 151 | |
Nanaud | 18:48246daf0c06 | 152 | if(acc) { |
Nanaud | 18:48246daf0c06 | 153 | cmdG = cmdG + 0.0005; |
Nanaud | 17:176a1b4a2fa8 | 154 | cmdD = cmdG; |
Nanaud | 18:48246daf0c06 | 155 | |
Nanaud | 19:c419033c0967 | 156 | if (cmdG >= VMAXROT) acc = 0; |
Nanaud | 18:48246daf0c06 | 157 | } else { |
Nanaud | 18:48246daf0c06 | 158 | //acc = 0; |
Nanaud | 16:ae65ce77b1f9 | 159 | |
Nanaud | 19:c419033c0967 | 160 | if (deltaCommande < VMAXROT) { |
Nanaud | 18:48246daf0c06 | 161 | cmdG = deltaCommande; |
Nanaud | 18:48246daf0c06 | 162 | cmdD = cmdG; |
Nanaud | 18:48246daf0c06 | 163 | } else { |
Nanaud | 19:c419033c0967 | 164 | cmdG = VMAXROT; |
Nanaud | 18:48246daf0c06 | 165 | cmdD = cmdG; |
Nanaud | 18:48246daf0c06 | 166 | } |
Nanaud | 17:176a1b4a2fa8 | 167 | } |
Nanaud | 16:ae65ce77b1f9 | 168 | |
Nanaud | 17:176a1b4a2fa8 | 169 | vitesseMotG(abs(cmdG)); |
Nanaud | 17:176a1b4a2fa8 | 170 | vitesseMotD(abs(cmdD)); |
Nanaud | 16:ae65ce77b1f9 | 171 | |
Nanaud | 19:c419033c0967 | 172 | if (O > (consigneOrientation - (1*0.0174533)) && O < (consigneOrientation + (1*0.0174533))) { |
Nanaud | 18:48246daf0c06 | 173 | //mot_dis(); |
Nanaud | 18:48246daf0c06 | 174 | fnc++; |
Nanaud | 17:176a1b4a2fa8 | 175 | acc = 1; |
Nanaud | 17:176a1b4a2fa8 | 176 | } |
Nanaud | 17:176a1b4a2fa8 | 177 | break; |
Nanaud | 16:ae65ce77b1f9 | 178 | |
Nanaud | 17:176a1b4a2fa8 | 179 | case 2: // Avancer |
Nanaud | 18:48246daf0c06 | 180 | mot_en(); |
Nanaud | 18:48246daf0c06 | 181 | |
Nanaud | 19:c419033c0967 | 182 | /* |
Nanaud | 19:c419033c0967 | 183 | cmdD = abs((int)distanceCible)*0.0001; // *0.005 |
Nanaud | 19:c419033c0967 | 184 | if(cmdD>VMAXLIN) { |
Nanaud | 19:c419033c0967 | 185 | cmdD = VMAXLIN; |
Nanaud | 17:176a1b4a2fa8 | 186 | } |
Nanaud | 17:176a1b4a2fa8 | 187 | cmdG = cmdD; |
Nanaud | 19:c419033c0967 | 188 | */ |
Nanaud | 19:c419033c0967 | 189 | |
Nanaud | 19:c419033c0967 | 190 | // Asservissement distance à parcourir |
Nanaud | 19:c419033c0967 | 191 | //erreurDist = consigneOrientation - O; |
Nanaud | 19:c419033c0967 | 192 | |
Nanaud | 19:c419033c0967 | 193 | deltaErreurDist = distanceCible - erreurPreDist; |
Nanaud | 19:c419033c0967 | 194 | |
Nanaud | 19:c419033c0967 | 195 | erreurPreDist = distanceCible; |
Nanaud | 19:c419033c0967 | 196 | |
Nanaud | 19:c419033c0967 | 197 | double deltaCommande2 = (abs(coeffProDist * distanceCible) + abs(coeffDerDist * deltaErreurDist)); |
Nanaud | 19:c419033c0967 | 198 | |
Nanaud | 19:c419033c0967 | 199 | if(acc) { |
Nanaud | 19:c419033c0967 | 200 | cmdG = cmdG + 0.0005; |
Nanaud | 19:c419033c0967 | 201 | cmdD = cmdG; |
Nanaud | 19:c419033c0967 | 202 | |
Nanaud | 19:c419033c0967 | 203 | if (cmdG >= VMAXLIN) { |
Nanaud | 19:c419033c0967 | 204 | acc = 0; |
Nanaud | 19:c419033c0967 | 205 | stt = 1; |
Nanaud | 19:c419033c0967 | 206 | } |
Nanaud | 19:c419033c0967 | 207 | } else { |
Nanaud | 19:c419033c0967 | 208 | //acc = 0; |
Nanaud | 19:c419033c0967 | 209 | |
Nanaud | 19:c419033c0967 | 210 | if (deltaCommande2 < VMAXLIN) { |
Nanaud | 19:c419033c0967 | 211 | cmdG = deltaCommande2; |
Nanaud | 19:c419033c0967 | 212 | cmdD = cmdG; |
Nanaud | 19:c419033c0967 | 213 | } else { |
Nanaud | 19:c419033c0967 | 214 | cmdG = VMAXLIN; |
Nanaud | 19:c419033c0967 | 215 | cmdD = cmdG; |
Nanaud | 19:c419033c0967 | 216 | } |
Nanaud | 19:c419033c0967 | 217 | } |
Nanaud | 19:c419033c0967 | 218 | |
Nanaud | 19:c419033c0967 | 219 | vitesseMotG(abs(cmdG)); |
Nanaud | 19:c419033c0967 | 220 | vitesseMotD(abs(cmdD)); |
Nanaud | 19:c419033c0967 | 221 | |
Nanaud | 19:c419033c0967 | 222 | //if (distanceCible < 15)[ |
Nanaud | 19:c419033c0967 | 223 | //if ((x < xC+10) && (x > xC-10) && (y < yC+10) && (y > yC-10)) { |
Nanaud | 19:c419033c0967 | 224 | if ((distanceCible < 10) || (stt==1 && (distancePrecedente < distanceCible))) { |
Nanaud | 19:c419033c0967 | 225 | //mot_dis(); |
Nanaud | 19:c419033c0967 | 226 | //fnc=0; |
Nanaud | 19:c419033c0967 | 227 | acc = 1; |
Nanaud | 19:c419033c0967 | 228 | stt = 0; |
Nanaud | 19:c419033c0967 | 229 | indice++; |
Nanaud | 19:c419033c0967 | 230 | fnc = objEtape[indice]; |
Nanaud | 19:c419033c0967 | 231 | xC = objX[indice]; |
Nanaud | 19:c419033c0967 | 232 | yC = objY[indice]; |
Nanaud | 19:c419033c0967 | 233 | } |
Nanaud | 16:ae65ce77b1f9 | 234 | |
Nanaud | 17:176a1b4a2fa8 | 235 | motGauche_fwd(); |
Nanaud | 17:176a1b4a2fa8 | 236 | motDroite_fwd(); |
Nanaud | 17:176a1b4a2fa8 | 237 | vitesseMotG(cmdG); |
Nanaud | 17:176a1b4a2fa8 | 238 | vitesseMotD(cmdD); |
Nanaud | 17:176a1b4a2fa8 | 239 | |
Nanaud | 19:c419033c0967 | 240 | distancePrecedente = distanceCible; |
Nanaud | 19:c419033c0967 | 241 | |
Nanaud | 17:176a1b4a2fa8 | 242 | break; |
Nanaud | 17:176a1b4a2fa8 | 243 | |
Nanaud | 17:176a1b4a2fa8 | 244 | case 3: // Reculer |
Nanaud | 17:176a1b4a2fa8 | 245 | break; |
Nanaud | 17:176a1b4a2fa8 | 246 | |
Nanaud | 17:176a1b4a2fa8 | 247 | default: |
Nanaud | 17:176a1b4a2fa8 | 248 | mot_dis(); |
Nanaud | 16:ae65ce77b1f9 | 249 | } |
Nanaud | 16:ae65ce77b1f9 | 250 | } |