AresENSEA-CDF2020 / Mbed 2 deprecated AresCDFMainCode_us2

Test

Dependencies:   mbed DRV8825

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?

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 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 }