AresENSEA-CDF2020 / Mbed 2 deprecated AresCDFMainCode_us2

Test

Dependencies:   mbed DRV8825

odo_asserv.cpp@17:176a1b4a2fa8, 2020-10-06 (annotated)

Committer:
Nanaud
Date:
Tue Oct 06 11:58:00 2020 +0000
Revision:
17:176a1b4a2fa8
Parent:
16:ae65ce77b1f9
Child:
18:48246daf0c06
Acc

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 17:176a1b4a2fa8 4 #define VMAX 50
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 17:176a1b4a2fa8 108
Nanaud 16:ae65ce77b1f9 109 double distanceCible = 0;
Nanaud 17:176a1b4a2fa8 110 double xC = 0, yC = 0; // x = xR et y = yR
Nanaud 16:ae65ce77b1f9 111 double consigneOrientation = 0;
Nanaud 16:ae65ce77b1f9 112 //double consigneOrientation = (90*3.1415)/180;
Nanaud 16:ae65ce77b1f9 113 int signe = 1;
Nanaud 16:ae65ce77b1f9 114 int cmdD = 0, cmdG = 0;
Nanaud 16:ae65ce77b1f9 115 double erreurAngle = 0;
Nanaud 16:ae65ce77b1f9 116 double erreurPre = 0;
Nanaud 16:ae65ce77b1f9 117 double deltaErreur = 0;
Nanaud 17:176a1b4a2fa8 118 const double coeffPro = 25.0; // 5.0 de base
Nanaud 17:176a1b4a2fa8 119 const double coeffDer = 50.0; // 3.0 de base
Nanaud 17:176a1b4a2fa8 120
Nanaud 17:176a1b4a2fa8 121 // NEW NEW NEW NEW
Nanaud 17:176a1b4a2fa8 122 int etape = 1;
Nanaud 17:176a1b4a2fa8 123 int acc = 1;
Nanaud 16:ae65ce77b1f9 124
Nanaud 16:ae65ce77b1f9 125 void asserv()
Nanaud 16:ae65ce77b1f9 126 {
Nanaud 17:176a1b4a2fa8 127 // Odométrie
Nanaud 16:ae65ce77b1f9 128 odo2();
Nanaud 16:ae65ce77b1f9 129
Nanaud 17:176a1b4a2fa8 130 // Calcul de la cible
Nanaud 16:ae65ce77b1f9 131 distanceCible = sqrt((xC-x)*(xC-x)+(yC-y)*(yC-y));
Nanaud 16:ae65ce77b1f9 132
Nanaud 16:ae65ce77b1f9 133 if(y > yC) {
Nanaud 16:ae65ce77b1f9 134 signe = -1;
Nanaud 16:ae65ce77b1f9 135 } else {
Nanaud 16:ae65ce77b1f9 136 signe = 1;
Nanaud 16:ae65ce77b1f9 137 }
Nanaud 16:ae65ce77b1f9 138
Nanaud 16:ae65ce77b1f9 139 consigneOrientation = signe * acos((xC-x)/((xC-x)*(xC-x)*(yC-y)*(yC-y)));
Nanaud 16:ae65ce77b1f9 140
Nanaud 17:176a1b4a2fa8 141 // Switch de sélection de l'étape
Nanaud 16:ae65ce77b1f9 142
Nanaud 17:176a1b4a2fa8 143 switch (etape) {
Nanaud 17:176a1b4a2fa8 144 case 0: //
Nanaud 17:176a1b4a2fa8 145 break;
Nanaud 16:ae65ce77b1f9 146
Nanaud 17:176a1b4a2fa8 147 case 1: // Rotation
Nanaud 17:176a1b4a2fa8 148 // Asservissement en position angulaire
Nanaud 17:176a1b4a2fa8 149 erreurAngle = consigneOrientation - O;
Nanaud 16:ae65ce77b1f9 150
Nanaud 17:176a1b4a2fa8 151 deltaErreur = erreurAngle - erreurPre;
Nanaud 16:ae65ce77b1f9 152
Nanaud 17:176a1b4a2fa8 153 erreurPre = erreurAngle;
Nanaud 16:ae65ce77b1f9 154
Nanaud 17:176a1b4a2fa8 155 int deltaCommande = coeffPro * erreurAngle + coeffDer * deltaErreur;
Nanaud 16:ae65ce77b1f9 156
Nanaud 17:176a1b4a2fa8 157 if (deltaCommande < VMAX) {
Nanaud 17:176a1b4a2fa8 158 cmdG = deltaCommande;
Nanaud 17:176a1b4a2fa8 159 cmdD = cmdG;
Nanaud 17:176a1b4a2fa8 160 } else {
Nanaud 17:176a1b4a2fa8 161 cmdG = VMAX;
Nanaud 17:176a1b4a2fa8 162 cmdD = cmdG;
Nanaud 17:176a1b4a2fa8 163 }
Nanaud 16:ae65ce77b1f9 164
Nanaud 17:176a1b4a2fa8 165 if(acc && cmdG <VMAX) {
Nanaud 17:176a1b4a2fa8 166 cmdG+=1;
Nanaud 17:176a1b4a2fa8 167 cmdD = cmdG;
Nanaud 17:176a1b4a2fa8 168 }
Nanaud 17:176a1b4a2fa8 169 else {
Nanaud 17:176a1b4a2fa8 170 acc = 0;
Nanaud 17:176a1b4a2fa8 171 }
Nanaud 16:ae65ce77b1f9 172
Nanaud 17:176a1b4a2fa8 173 vitesseMotG(abs(cmdG));
Nanaud 17:176a1b4a2fa8 174 vitesseMotD(abs(cmdD));
Nanaud 16:ae65ce77b1f9 175
Nanaud 17:176a1b4a2fa8 176 if (O > (consigneOrientation - (2*0.0174533)) && O < (consigneOrientation + (2*0.0174533))) {
Nanaud 17:176a1b4a2fa8 177 mot_dis();
Nanaud 17:176a1b4a2fa8 178 //etape++;
Nanaud 17:176a1b4a2fa8 179 acc = 1;
Nanaud 17:176a1b4a2fa8 180 }
Nanaud 17:176a1b4a2fa8 181 break;
Nanaud 16:ae65ce77b1f9 182
Nanaud 17:176a1b4a2fa8 183 case 2: // Avancer
Nanaud 17:176a1b4a2fa8 184 cmdD = abs((int)distanceCible);
Nanaud 17:176a1b4a2fa8 185 if(cmdD>VMAX) {
Nanaud 17:176a1b4a2fa8 186 cmdD = VMAX;
Nanaud 17:176a1b4a2fa8 187 }
Nanaud 17:176a1b4a2fa8 188 cmdG = cmdD;
Nanaud 16:ae65ce77b1f9 189
Nanaud 17:176a1b4a2fa8 190 motGauche_fwd();
Nanaud 17:176a1b4a2fa8 191 motDroite_fwd();
Nanaud 17:176a1b4a2fa8 192 vitesseMotG(cmdG);
Nanaud 17:176a1b4a2fa8 193 vitesseMotD(cmdD);
Nanaud 17:176a1b4a2fa8 194
Nanaud 17:176a1b4a2fa8 195 if (abs((int)distanceCible) < 1) mot_dis();
Nanaud 17:176a1b4a2fa8 196 break;
Nanaud 17:176a1b4a2fa8 197
Nanaud 17:176a1b4a2fa8 198 case 3: // Reculer
Nanaud 17:176a1b4a2fa8 199 break;
Nanaud 17:176a1b4a2fa8 200
Nanaud 17:176a1b4a2fa8 201 default:
Nanaud 17:176a1b4a2fa8 202 mot_dis();
Nanaud 16:ae65ce77b1f9 203 }
Nanaud 16:ae65ce77b1f9 204 }