AresENSEA-CDF2020
/
AresCDFMainCode_us2
Test
odo_asserv.cpp@16:ae65ce77b1f9, 2020-10-02 (annotated)
- Committer:
- Nanaud
- Date:
- Fri Oct 02 21:09:45 2020 +0000
- Revision:
- 16:ae65ce77b1f9
- Parent:
- 14:dd3c756c6d48
- Child:
- 17:176a1b4a2fa8
02/10/2020
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 | 16:ae65ce77b1f9 | 4 | #define VMAX 20 |
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 | double vitG = 0, vitD = 0; |
Nanaud | 16:ae65ce77b1f9 | 64 | #define tpsTicker 0.020f |
Nanaud | 6:ea6b30c4bb01 | 65 | |
Nanaud | 16:ae65ce77b1f9 | 66 | void odo2() |
Nanaud | 10:0714feaaaee1 | 67 | { |
Nanaud | 16:ae65ce77b1f9 | 68 | vitG = (double) ((comptG * coeffG) / tpsTicker); |
Nanaud | 16:ae65ce77b1f9 | 69 | vitD = (double) ((comptD * coeffD) / tpsTicker); |
Nanaud | 16:ae65ce77b1f9 | 70 | |
Nanaud | 16:ae65ce77b1f9 | 71 | dDist = (double) ((comptG * coeffG) + (comptD * coeffD)) / 2.0f; |
Nanaud | 16:ae65ce77b1f9 | 72 | dAngl = (double) ((comptD * coeffD) - (comptG * coeffG)) / entraxe; |
Nanaud | 6:ea6b30c4bb01 | 73 | |
Nanaud | 16:ae65ce77b1f9 | 74 | x += (double) dDist * cos(O); |
Nanaud | 16:ae65ce77b1f9 | 75 | y += (double) dDist * sin(O); |
Nanaud | 16:ae65ce77b1f9 | 76 | O += (double) dAngl; |
Nanaud | 10:0714feaaaee1 | 77 | |
Nanaud | 16:ae65ce77b1f9 | 78 | if (O > 3.1415) O = O - (2.0f * 3.1415f); |
Nanaud | 16:ae65ce77b1f9 | 79 | if (O < -3.1415) O = O + (2.0f * 3.1415f); |
Nanaud | 14:dd3c756c6d48 | 80 | |
plmir | 12:2c312916a621 | 81 | comptG = 0; |
plmir | 12:2c312916a621 | 82 | comptD = 0; |
Nanaud | 14:dd3c756c6d48 | 83 | } |
Nanaud | 14:dd3c756c6d48 | 84 | //*/ |
Nanaud | 14:dd3c756c6d48 | 85 | |
Nanaud | 14:dd3c756c6d48 | 86 | /* |
Nanaud | 16:ae65ce77b1f9 | 87 | // odo3() |
Nanaud | 16:ae65ce77b1f9 | 88 | #define diametre 51.45 |
Nanaud | 16:ae65ce77b1f9 | 89 | #define N 1000 |
Nanaud | 16:ae65ce77b1f9 | 90 | #define entraxe 255 |
Nanaud | 16:ae65ce77b1f9 | 91 | const double coeffG = 1/5.956; |
Nanaud | 16:ae65ce77b1f9 | 92 | const double coeffD = 1/5.956; |
Nanaud | 14:dd3c756c6d48 | 93 | |
Nanaud | 16:ae65ce77b1f9 | 94 | void odo3() |
Nanaud | 16:ae65ce77b1f9 | 95 | { |
Nanaud | 16:ae65ce77b1f9 | 96 | dDist = (double) ((comptG * coeffG) + (comptD * coeffD)) / 2.0f; |
Nanaud | 16:ae65ce77b1f9 | 97 | dAngl = (double) ((comptD * coeffD) - (comptG * coeffG)) / entraxe; |
Nanaud | 14:dd3c756c6d48 | 98 | |
Nanaud | 16:ae65ce77b1f9 | 99 | x += (double) dDist * cos(O); |
Nanaud | 16:ae65ce77b1f9 | 100 | y += (double) dDist * sin(O); |
Nanaud | 16:ae65ce77b1f9 | 101 | O += (double) dAngl; |
Nanaud | 14:dd3c756c6d48 | 102 | |
Nanaud | 14:dd3c756c6d48 | 103 | comptG = 0; |
Nanaud | 14:dd3c756c6d48 | 104 | comptD = 0; |
Nanaud | 14:dd3c756c6d48 | 105 | } |
Nanaud | 14:dd3c756c6d48 | 106 | */ |
Nanaud | 16:ae65ce77b1f9 | 107 | |
Nanaud | 16:ae65ce77b1f9 | 108 | double distanceCible = 0; |
Nanaud | 16:ae65ce77b1f9 | 109 | double xC = 0, yC = -100; // x = xR et y = yR |
Nanaud | 16:ae65ce77b1f9 | 110 | double consigneOrientation = 0; |
Nanaud | 16:ae65ce77b1f9 | 111 | //double consigneOrientation = (90*3.1415)/180; |
Nanaud | 16:ae65ce77b1f9 | 112 | int signe = 1; |
Nanaud | 16:ae65ce77b1f9 | 113 | int cmdD = 0, cmdG = 0; |
Nanaud | 16:ae65ce77b1f9 | 114 | double erreurAngle = 0; |
Nanaud | 16:ae65ce77b1f9 | 115 | double erreurPre = 0; |
Nanaud | 16:ae65ce77b1f9 | 116 | double deltaErreur = 0; |
Nanaud | 16:ae65ce77b1f9 | 117 | const double coeffPro = 15.0; // 5.0 de base |
Nanaud | 16:ae65ce77b1f9 | 118 | const double coeffDer = 3.0; |
Nanaud | 16:ae65ce77b1f9 | 119 | bool phase_acc = 1; |
Nanaud | 16:ae65ce77b1f9 | 120 | |
Nanaud | 16:ae65ce77b1f9 | 121 | void asserv() |
Nanaud | 16:ae65ce77b1f9 | 122 | { |
Nanaud | 16:ae65ce77b1f9 | 123 | odo2(); |
Nanaud | 16:ae65ce77b1f9 | 124 | |
Nanaud | 16:ae65ce77b1f9 | 125 | |
Nanaud | 16:ae65ce77b1f9 | 126 | distanceCible = sqrt((xC-x)*(xC-x)+(yC-y)*(yC-y)); |
Nanaud | 16:ae65ce77b1f9 | 127 | |
Nanaud | 16:ae65ce77b1f9 | 128 | /* |
Nanaud | 16:ae65ce77b1f9 | 129 | if(y > yC) { |
Nanaud | 16:ae65ce77b1f9 | 130 | signe = -1; |
Nanaud | 16:ae65ce77b1f9 | 131 | } else { |
Nanaud | 16:ae65ce77b1f9 | 132 | signe = 1; |
Nanaud | 16:ae65ce77b1f9 | 133 | } |
Nanaud | 16:ae65ce77b1f9 | 134 | |
Nanaud | 16:ae65ce77b1f9 | 135 | consigneOrientation = signe * acos((xC-x)/((xC-x)*(xC-x)*(yC-y)*(yC-y))); |
Nanaud | 16:ae65ce77b1f9 | 136 | */ |
Nanaud | 16:ae65ce77b1f9 | 137 | |
Nanaud | 16:ae65ce77b1f9 | 138 | |
Nanaud | 16:ae65ce77b1f9 | 139 | cmdD = abs((int)distanceCible); |
Nanaud | 16:ae65ce77b1f9 | 140 | if(cmdD>VMAX) { |
Nanaud | 16:ae65ce77b1f9 | 141 | cmdD = VMAX; |
Nanaud | 16:ae65ce77b1f9 | 142 | } |
Nanaud | 16:ae65ce77b1f9 | 143 | cmdG = cmdD; |
Nanaud | 16:ae65ce77b1f9 | 144 | |
Nanaud | 16:ae65ce77b1f9 | 145 | |
Nanaud | 16:ae65ce77b1f9 | 146 | /* |
Nanaud | 16:ae65ce77b1f9 | 147 | motGauche_fwd(); |
Nanaud | 16:ae65ce77b1f9 | 148 | motDroite_fwd(); |
Nanaud | 16:ae65ce77b1f9 | 149 | vitesseMotG(cmdG); |
Nanaud | 16:ae65ce77b1f9 | 150 | vitesseMotD(cmdD); |
Nanaud | 16:ae65ce77b1f9 | 151 | */ |
Nanaud | 16:ae65ce77b1f9 | 152 | |
Nanaud | 16:ae65ce77b1f9 | 153 | erreurAngle = consigneOrientation - O; |
Nanaud | 16:ae65ce77b1f9 | 154 | |
Nanaud | 16:ae65ce77b1f9 | 155 | deltaErreur = erreurAngle - erreurPre; |
Nanaud | 16:ae65ce77b1f9 | 156 | |
Nanaud | 16:ae65ce77b1f9 | 157 | erreurPre = erreurAngle; |
Nanaud | 16:ae65ce77b1f9 | 158 | |
Nanaud | 16:ae65ce77b1f9 | 159 | int deltaCommande = coeffPro * erreurAngle + coeffDer * deltaErreur; |
Nanaud | 16:ae65ce77b1f9 | 160 | |
Nanaud | 16:ae65ce77b1f9 | 161 | motGauche_bck(); |
Nanaud | 16:ae65ce77b1f9 | 162 | motDroite_fwd(); |
Nanaud | 16:ae65ce77b1f9 | 163 | |
Nanaud | 16:ae65ce77b1f9 | 164 | cmdG = 0; |
Nanaud | 16:ae65ce77b1f9 | 165 | cmdD = 0; |
Nanaud | 16:ae65ce77b1f9 | 166 | |
Nanaud | 16:ae65ce77b1f9 | 167 | cmdG += deltaCommande; |
Nanaud | 16:ae65ce77b1f9 | 168 | cmdD -= deltaCommande; |
Nanaud | 16:ae65ce77b1f9 | 169 | |
Nanaud | 16:ae65ce77b1f9 | 170 | if(cmdD>VMAX) { |
Nanaud | 16:ae65ce77b1f9 | 171 | cmdD =VMAX; |
Nanaud | 16:ae65ce77b1f9 | 172 | } else if(cmdD < -VMAX) { |
Nanaud | 16:ae65ce77b1f9 | 173 | cmdD = -VMAX; |
Nanaud | 16:ae65ce77b1f9 | 174 | } |
Nanaud | 16:ae65ce77b1f9 | 175 | |
Nanaud | 16:ae65ce77b1f9 | 176 | if(cmdG>VMAX) { |
Nanaud | 16:ae65ce77b1f9 | 177 | cmdG =VMAX; |
Nanaud | 16:ae65ce77b1f9 | 178 | } else if(cmdG < -VMAX) { |
Nanaud | 16:ae65ce77b1f9 | 179 | cmdG = -VMAX; |
Nanaud | 16:ae65ce77b1f9 | 180 | } |
Nanaud | 16:ae65ce77b1f9 | 181 | |
Nanaud | 16:ae65ce77b1f9 | 182 | /* |
Nanaud | 16:ae65ce77b1f9 | 183 | if (cmdD > 0) { |
Nanaud | 16:ae65ce77b1f9 | 184 | //motDroite_fwd(); |
Nanaud | 16:ae65ce77b1f9 | 185 | motDroite_bck(); |
Nanaud | 16:ae65ce77b1f9 | 186 | } else { |
Nanaud | 16:ae65ce77b1f9 | 187 | //motDroite_bck(); |
Nanaud | 16:ae65ce77b1f9 | 188 | motDroite_fwd(); |
Nanaud | 16:ae65ce77b1f9 | 189 | } |
Nanaud | 16:ae65ce77b1f9 | 190 | |
Nanaud | 16:ae65ce77b1f9 | 191 | if (cmdG > 0) { |
Nanaud | 16:ae65ce77b1f9 | 192 | motGauche_fwd(); |
Nanaud | 16:ae65ce77b1f9 | 193 | //motGauche_bck(); |
Nanaud | 16:ae65ce77b1f9 | 194 | } else { |
Nanaud | 16:ae65ce77b1f9 | 195 | motGauche_bck(); |
Nanaud | 16:ae65ce77b1f9 | 196 | //motGauche_fwd(); |
Nanaud | 16:ae65ce77b1f9 | 197 | } |
Nanaud | 16:ae65ce77b1f9 | 198 | */ |
Nanaud | 16:ae65ce77b1f9 | 199 | |
Nanaud | 16:ae65ce77b1f9 | 200 | /* |
Nanaud | 16:ae65ce77b1f9 | 201 | if(cmdD>150) { |
Nanaud | 16:ae65ce77b1f9 | 202 | cmdD =150; |
Nanaud | 16:ae65ce77b1f9 | 203 | } else if(cmdD < 0) { |
Nanaud | 16:ae65ce77b1f9 | 204 | cmdD = 0; |
Nanaud | 16:ae65ce77b1f9 | 205 | } |
Nanaud | 16:ae65ce77b1f9 | 206 | |
Nanaud | 16:ae65ce77b1f9 | 207 | if(cmdG>150) { |
Nanaud | 16:ae65ce77b1f9 | 208 | cmdG = 150; |
Nanaud | 16:ae65ce77b1f9 | 209 | } else if(cmdG < 0) { |
Nanaud | 16:ae65ce77b1f9 | 210 | cmdG = 0; |
Nanaud | 16:ae65ce77b1f9 | 211 | } |
Nanaud | 16:ae65ce77b1f9 | 212 | */ |
Nanaud | 16:ae65ce77b1f9 | 213 | |
Nanaud | 16:ae65ce77b1f9 | 214 | /* |
Nanaud | 16:ae65ce77b1f9 | 215 | if (consigneOrientation - O < 3.1415) { |
Nanaud | 16:ae65ce77b1f9 | 216 | motGauche_bck(); |
Nanaud | 16:ae65ce77b1f9 | 217 | motDroite_fwd(); |
Nanaud | 16:ae65ce77b1f9 | 218 | } else { |
Nanaud | 16:ae65ce77b1f9 | 219 | motGauche_fwd(); |
Nanaud | 16:ae65ce77b1f9 | 220 | motDroite_bck(); |
Nanaud | 16:ae65ce77b1f9 | 221 | } |
Nanaud | 16:ae65ce77b1f9 | 222 | */ |
Nanaud | 16:ae65ce77b1f9 | 223 | |
Nanaud | 16:ae65ce77b1f9 | 224 | if (phase_acc) { |
Nanaud | 16:ae65ce77b1f9 | 225 | cmdG += 1; |
Nanaud | 16:ae65ce77b1f9 | 226 | cmdD = cmdG; |
Nanaud | 16:ae65ce77b1f9 | 227 | |
Nanaud | 16:ae65ce77b1f9 | 228 | if (cmdG >= VMAX) phase_acc = 0; |
Nanaud | 16:ae65ce77b1f9 | 229 | } |
Nanaud | 16:ae65ce77b1f9 | 230 | |
Nanaud | 16:ae65ce77b1f9 | 231 | vitesseMotG(abs(cmdG)); |
Nanaud | 16:ae65ce77b1f9 | 232 | vitesseMotD(abs(cmdD)); |
Nanaud | 16:ae65ce77b1f9 | 233 | |
Nanaud | 16:ae65ce77b1f9 | 234 | if (O > (consigneOrientation - (2*0.0174533)) && O < (consigneOrientation + (2*0.0174533))) mot_dis(); |
Nanaud | 16:ae65ce77b1f9 | 235 | } |