AresENSEA-CDF2020 / Mbed 2 deprecated AresCDFMainCode_us2

Test

Dependencies:   mbed DRV8825

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?

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