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