AresENSEA-CDF2020 / Mbed 2 deprecated AresCDFMainCode_us2

Test

Dependencies:   mbed DRV8825

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?

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