Avion Radio IUT / Mbed 2 deprecated MecatroPWM

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed

main.cpp@2:7de884ffc9d9, 2021-04-15 (annotated)

Committer:
qmaker
Date:
Thu Apr 15 07:24:07 2021 +0000
Revision:
2:7de884ffc9d9
Parent:
1:b44f69eb07c4
Child:
3:3bc2882232a6
asserv position avec derivateur

Who changed what in which revision?

UserRevisionLine numberNew contents of line
qmaker 0:0d257bbf5c05 1 #include "mbed.h"
qmaker 0:0d257bbf5c05 2 #include "rtos.h"
qmaker 0:0d257bbf5c05 3 #include "MPU6050.h"
qmaker 1:b44f69eb07c4 4 #include "FastPWM.h"
qmaker 1:b44f69eb07c4 5 #include "codeurs.h"
qmaker 1:b44f69eb07c4 6
qmaker 1:b44f69eb07c4 7 int dato=0;
qmaker 0:0d257bbf5c05 8
qmaker 0:0d257bbf5c05 9 // déclaration des objets
qmaker 1:b44f69eb07c4 10 Serial pc(USBTX, USBRX,115200);
qmaker 1:b44f69eb07c4 11 Serial bluetooth(PA_9, PA_10,38400); //PINS TO CONECT. PA_9 WITH RX PIN HC-06
qmaker 0:0d257bbf5c05 12 DigitalOut led(LED1);
qmaker 0:0d257bbf5c05 13 MPU6050 module(I2C_SDA, I2C_SCL);
qmaker 0:0d257bbf5c05 14
qmaker 0:0d257bbf5c05 15 // valeur de Te
qmaker 0:0d257bbf5c05 16 float tems = 10;
qmaker 0:0d257bbf5c05 17
qmaker 2:7de884ffc9d9 18 // variable asserv position
qmaker 2:7de884ffc9d9 19 float erreurPosition, erreurPosition_p, consignePosition = 0;
qmaker 2:7de884ffc9d9 20 float KPP = 0.001;
qmaker 2:7de884ffc9d9 21
qmaker 0:0d257bbf5c05 22 // varailble necessire à la mesure de la position angulaire du gyropode
qmaker 0:0d257bbf5c05 23 float accelero[3]= {0};
qmaker 0:0d257bbf5c05 24 float gyro[3] = {0};
qmaker 1:b44f69eb07c4 25 float tauFC = 0.2; // constante de tyemps en seconde du filtre passe bas du filtre complémentaire
qmaker 1:b44f69eb07c4 26 float aFC, bFC; // les coefficient du filter passe bas du filtre complémentaire
qmaker 1:b44f69eb07c4 27 float angleATG,angleNF, angleGyro,omegarGyro, angleGyrop,angleGyroConsigne;
qmaker 1:b44f69eb07c4 28 float consigneAngle = -0.001;
qmaker 1:b44f69eb07c4 29 float CommandeVitesse = 0;
qmaker 1:b44f69eb07c4 30
qmaker 1:b44f69eb07c4 31 // varailble necessire à l'asserv angulaire et vitesse
qmaker 1:b44f69eb07c4 32 float offsetConsigneAngle = 0;
qmaker 1:b44f69eb07c4 33 float KPV = 0.026;
qmaker 1:b44f69eb07c4 34 float KDA = 0.08;
qmaker 1:b44f69eb07c4 35 float KPA =2.3;
qmaker 1:b44f69eb07c4 36 float KDV = 0.01;
qmaker 1:b44f69eb07c4 37 float erreurVitesse, offsetConsigneAngle_p;
qmaker 0:0d257bbf5c05 38
qmaker 1:b44f69eb07c4 39 // valeur du filtre passe bas
qmaker 1:b44f69eb07c4 40 float ec_f, ec_fp;
qmaker 1:b44f69eb07c4 41 float Te = 0.0002;
qmaker 1:b44f69eb07c4 42 float ec,ecCorrige; // grandeur de commande
qmaker 1:b44f69eb07c4 43 float ecVir;
qmaker 0:0d257bbf5c05 44
qmaker 1:b44f69eb07c4 45 // Codeurs
qmaker 1:b44f69eb07c4 46 Codeurs codeurs;
qmaker 1:b44f69eb07c4 47 int g, d, vitesseCodeur1, g_p, vitesseCodeur2, d_p;
qmaker 1:b44f69eb07c4 48 float moyVitesse_p, moyVitesse;
qmaker 1:b44f69eb07c4 49
qmaker 1:b44f69eb07c4 50 //sorties moteur
qmaker 1:b44f69eb07c4 51 FastPWM M1_1(D9);
qmaker 1:b44f69eb07c4 52 FastPWM M1_2(D10);
qmaker 1:b44f69eb07c4 53 FastPWM M2_1(D11);
qmaker 1:b44f69eb07c4 54 FastPWM M2_2(D12);
qmaker 0:0d257bbf5c05 55
qmaker 0:0d257bbf5c05 56 int nbInc=0;
qmaker 0:0d257bbf5c05 57 char flagInterupt=0;
qmaker 0:0d257bbf5c05 58
qmaker 1:b44f69eb07c4 59
qmaker 1:b44f69eb07c4 60 //reception
qmaker 1:b44f69eb07c4 61
qmaker 0:0d257bbf5c05 62 void reception(char ch)
qmaker 0:0d257bbf5c05 63 {
qmaker 0:0d257bbf5c05 64 static int i=0; // inice du dernier caratère recu
qmaker 0:0d257bbf5c05 65 static char chaine[10]; // chaine de stockage des caratères recus
qmaker 0:0d257bbf5c05 66 char commande[3]; // chaine contenant la commande
qmaker 0:0d257bbf5c05 67 char valeur[6]; // chaine contenant la valeur
qmaker 0:0d257bbf5c05 68
qmaker 0:0d257bbf5c05 69 if ((ch==13) or (ch==10)) {
qmaker 0:0d257bbf5c05 70
qmaker 0:0d257bbf5c05 71 chaine[i]='\0';
qmaker 0:0d257bbf5c05 72
qmaker 0:0d257bbf5c05 73 // séparation de la commande et de la valeur
qmaker 0:0d257bbf5c05 74 strcpy(commande, strtok(chaine, " "));
qmaker 0:0d257bbf5c05 75 strcpy(valeur, strtok(NULL, " "));
qmaker 0:0d257bbf5c05 76
qmaker 1:b44f69eb07c4 77
qmaker 1:b44f69eb07c4 78 if (strcmp( commande, "KPV" ) == 0) {
qmaker 1:b44f69eb07c4 79 KPV=atof(valeur);
qmaker 1:b44f69eb07c4 80 }
qmaker 1:b44f69eb07c4 81 if (strcmp( commande, "CA" ) == 0) {
qmaker 1:b44f69eb07c4 82 consigneAngle=atof(valeur);
qmaker 1:b44f69eb07c4 83 }
qmaker 1:b44f69eb07c4 84 if (strcmp( commande, "KDA" ) == 0) {
qmaker 1:b44f69eb07c4 85 KDA=atof(valeur);
qmaker 1:b44f69eb07c4 86 }
qmaker 1:b44f69eb07c4 87 if (strcmp( commande, "KPA" ) == 0) {
qmaker 1:b44f69eb07c4 88 KPA=atof(valeur);
qmaker 1:b44f69eb07c4 89 }
qmaker 1:b44f69eb07c4 90 if (strcmp( commande, "CV" ) == 0) {
qmaker 1:b44f69eb07c4 91 CommandeVitesse=atof(valeur);
qmaker 1:b44f69eb07c4 92 }
qmaker 1:b44f69eb07c4 93 if (strcmp( commande, "KDV" ) == 0) {
qmaker 1:b44f69eb07c4 94 KDV=atof(valeur);
qmaker 1:b44f69eb07c4 95 }
qmaker 0:0d257bbf5c05 96
qmaker 0:0d257bbf5c05 97 // reinitialisation de l indice de chaine
qmaker 0:0d257bbf5c05 98 i = 0;
qmaker 0:0d257bbf5c05 99
qmaker 0:0d257bbf5c05 100 } else {
qmaker 0:0d257bbf5c05 101 chaine[i]=ch;
qmaker 0:0d257bbf5c05 102 i++;
qmaker 0:0d257bbf5c05 103 }
qmaker 0:0d257bbf5c05 104 }
qmaker 0:0d257bbf5c05 105
qmaker 0:0d257bbf5c05 106 void interupt()
qmaker 0:0d257bbf5c05 107 {
qmaker 1:b44f69eb07c4 108 codeurs.read(g, d);
qmaker 1:b44f69eb07c4 109
qmaker 0:0d257bbf5c05 110 nbInc++;
qmaker 1:b44f69eb07c4 111
qmaker 1:b44f69eb07c4 112
qmaker 1:b44f69eb07c4 113
qmaker 1:b44f69eb07c4 114
qmaker 1:b44f69eb07c4 115 /**********************************************************************************************/
qmaker 1:b44f69eb07c4 116 /* Estimation position angulaire */
qmaker 1:b44f69eb07c4 117 /**********************************************************************************************/
qmaker 1:b44f69eb07c4 118
qmaker 1:b44f69eb07c4 119 // Lecture des données de l'accéléro et du gyro
qmaker 0:0d257bbf5c05 120 module.getAccelero(accelero);
qmaker 1:b44f69eb07c4 121 module.getGyro(gyro);
qmaker 1:b44f69eb07c4 122
qmaker 1:b44f69eb07c4 123 // calcul pos angulaire du gyropode à partir de l'arc tangeante des accelerations
qmaker 1:b44f69eb07c4 124 angleATG = atan2(accelero[1],accelero[0]);
qmaker 1:b44f69eb07c4 125
qmaker 1:b44f69eb07c4 126
qmaker 1:b44f69eb07c4 127 // Calcul de la sortie du filtre complementaire
qmaker 1:b44f69eb07c4 128 omegarGyro = -gyro[2];
qmaker 0:0d257bbf5c05 129 angleNF = angleATG + tauFC * omegarGyro;
qmaker 1:b44f69eb07c4 130 angleGyro = aFC*( angleNF + bFC * angleGyrop);
qmaker 1:b44f69eb07c4 131
qmaker 1:b44f69eb07c4 132 // Memorisation des valeurs precedante pour les filtres recursifs
qmaker 1:b44f69eb07c4 133 angleGyrop = angleGyro;
qmaker 1:b44f69eb07c4 134
qmaker 0:0d257bbf5c05 135
qmaker 1:b44f69eb07c4 136 /**********************************************************************************************/
qmaker 1:b44f69eb07c4 137 /* Asserv postion angulaire gyropode */
qmaker 1:b44f69eb07c4 138 /**********************************************************************************************/
qmaker 0:0d257bbf5c05 139
qmaker 1:b44f69eb07c4 140
qmaker 1:b44f69eb07c4 141 // Definir la consigne de l'asservissement de la pos angulaire
qmaker 1:b44f69eb07c4 142 angleGyroConsigne = consigneAngle+offsetConsigneAngle; // consigneAngle est la grandeur de commande issue de l'asservissement de l image de la vitesse
qmaker 1:b44f69eb07c4 143
qmaker 1:b44f69eb07c4 144
qmaker 1:b44f69eb07c4 145 // Calcul de la grandeur de commande de l'asservissement des pos angulaire
qmaker 1:b44f69eb07c4 146 ec= KPA*(angleGyroConsigne-angleGyro)-KDA*omegarGyro;
qmaker 1:b44f69eb07c4 147
qmaker 0:0d257bbf5c05 148
qmaker 1:b44f69eb07c4 149
qmaker 1:b44f69eb07c4 150 /**********************************************************************************************/
qmaker 2:7de884ffc9d9 151 /* Asserv Position */
qmaker 2:7de884ffc9d9 152 /**********************************************************************************************/
qmaker 2:7de884ffc9d9 153
qmaker 2:7de884ffc9d9 154 erreurPosition = consignePosition - (g-d);
qmaker 2:7de884ffc9d9 155 CommandeVitesse = KPP * (erreurPosition-erreurPosition_p);
qmaker 2:7de884ffc9d9 156 erreurPosition_p = erreurPosition;
qmaker 2:7de884ffc9d9 157
qmaker 2:7de884ffc9d9 158 if (CommandeVitesse>3) CommandeVitesse = 3;
qmaker 2:7de884ffc9d9 159 else if (CommandeVitesse < -3) CommandeVitesse = -3;
qmaker 2:7de884ffc9d9 160
qmaker 2:7de884ffc9d9 161
qmaker 2:7de884ffc9d9 162
qmaker 2:7de884ffc9d9 163 /**********************************************************************************************/
qmaker 1:b44f69eb07c4 164 /* Asserv Vitesse */
qmaker 1:b44f69eb07c4 165 /**********************************************************************************************/
qmaker 1:b44f69eb07c4 166
qmaker 1:b44f69eb07c4 167
qmaker 1:b44f69eb07c4 168 // Correcteur proportionel KPV
qmaker 1:b44f69eb07c4 169 erreurVitesse = CommandeVitesse-moyVitesse;
qmaker 1:b44f69eb07c4 170
qmaker 1:b44f69eb07c4 171 // Asservissement de vitesse
qmaker 1:b44f69eb07c4 172 offsetConsigneAngle = (KPV*erreurVitesse)+(KDV*(moyVitesse - moyVitesse_p));
qmaker 1:b44f69eb07c4 173 moyVitesse_p = moyVitesse;
qmaker 1:b44f69eb07c4 174
qmaker 1:b44f69eb07c4 175 // Calcul vitesse codeur
qmaker 1:b44f69eb07c4 176 vitesseCodeur1 = -g_p+g;
qmaker 1:b44f69eb07c4 177 g_p = g;
qmaker 1:b44f69eb07c4 178 vitesseCodeur2 = d_p-d;
qmaker 1:b44f69eb07c4 179 d_p = d;
qmaker 1:b44f69eb07c4 180 moyVitesse = (vitesseCodeur1+vitesseCodeur2)/2;
qmaker 1:b44f69eb07c4 181 moyVitesse = moyVitesse/10;
qmaker 1:b44f69eb07c4 182
qmaker 1:b44f69eb07c4 183
qmaker 1:b44f69eb07c4 184 /**********************************************************************************************/
qmaker 1:b44f69eb07c4 185 /* Commande Moteurs */
qmaker 1:b44f69eb07c4 186 /**********************************************************************************************/
qmaker 1:b44f69eb07c4 187
qmaker 1:b44f69eb07c4 188
qmaker 1:b44f69eb07c4 189 // Compenssation frottement sec
qmaker 1:b44f69eb07c4 190 if (ec<0)ecCorrige=ec-0.15;
qmaker 1:b44f69eb07c4 191 else if (ec>0)ecCorrige=ec+0.15;
qmaker 1:b44f69eb07c4 192 else ecCorrige = 0;
qmaker 1:b44f69eb07c4 193
qmaker 1:b44f69eb07c4 194 // Saturation
qmaker 1:b44f69eb07c4 195 if (ecCorrige<-0.5)ecCorrige=-0.5;
qmaker 1:b44f69eb07c4 196 if (ecCorrige>0.5)ecCorrige=0.5;
qmaker 1:b44f69eb07c4 197
qmaker 1:b44f69eb07c4 198 // Sorties PWM Moteurs
qmaker 1:b44f69eb07c4 199 M1_1.write(0.5+(ecCorrige+ecVir));
qmaker 1:b44f69eb07c4 200 M1_2.write(0.5-(ecCorrige+ecVir));
qmaker 1:b44f69eb07c4 201 M2_1.write(0.5-(ecCorrige-ecVir));
qmaker 1:b44f69eb07c4 202 M2_2.write(0.5+(ecCorrige-ecVir));
qmaker 1:b44f69eb07c4 203
qmaker 0:0d257bbf5c05 204 flagInterupt=1;
qmaker 0:0d257bbf5c05 205 }
qmaker 0:0d257bbf5c05 206
qmaker 0:0d257bbf5c05 207 RtosTimer timer(mbed::callback(interupt),osTimerPeriodic);
qmaker 0:0d257bbf5c05 208
qmaker 0:0d257bbf5c05 209 int main()
qmaker 0:0d257bbf5c05 210 {
qmaker 1:b44f69eb07c4 211
qmaker 0:0d257bbf5c05 212 pc.printf("Test unitaire mecatro \n\r");
qmaker 1:b44f69eb07c4 213
qmaker 1:b44f69eb07c4 214 M1_1.period_us(50);
qmaker 1:b44f69eb07c4 215 M1_2.period_us(50);
qmaker 1:b44f69eb07c4 216 M2_1.period_us(50);
qmaker 1:b44f69eb07c4 217 M2_2.period_us(50);
qmaker 1:b44f69eb07c4 218 M1_1.write(0.5);
qmaker 1:b44f69eb07c4 219 M1_2.write(0.5);
qmaker 1:b44f69eb07c4 220 M2_1.write(0.5);
qmaker 1:b44f69eb07c4 221 M2_2.write(0.5);
qmaker 1:b44f69eb07c4 222
qmaker 1:b44f69eb07c4 223 // Test connection codeurs
qmaker 1:b44f69eb07c4 224 pc.printf("Test des codeurs\r\n");
qmaker 1:b44f69eb07c4 225 while (!codeurs.test()) {
qmaker 1:b44f69eb07c4 226 pc.printf("Codeurs non connectes\r\n");
qmaker 1:b44f69eb07c4 227 wait(1);
qmaker 1:b44f69eb07c4 228 }
qmaker 1:b44f69eb07c4 229
qmaker 1:b44f69eb07c4 230 pc.printf("Codeurs ok\r\n");
qmaker 1:b44f69eb07c4 231 codeurs.reset();
qmaker 0:0d257bbf5c05 232
qmaker 0:0d257bbf5c05 233
qmaker 1:b44f69eb07c4 234 // initialisation et test de connection du MPU6050
qmaker 0:0d257bbf5c05 235 while (module.testConnection()==0) {
qmaker 0:0d257bbf5c05 236 pc.printf("not connected to mpu\n\r");
qmaker 0:0d257bbf5c05 237 wait(1);
qmaker 0:0d257bbf5c05 238 }
qmaker 0:0d257bbf5c05 239 //changement du l'echelle du module
qmaker 0:0d257bbf5c05 240 module.setGyroRange(MPU6050_GYRO_RANGE_2000);
qmaker 0:0d257bbf5c05 241 module.setAcceleroRange(MPU6050_ACCELERO_RANGE_2G);
qmaker 0:0d257bbf5c05 242
qmaker 1:b44f69eb07c4 243 // calcul des coefficients des filtres
qmaker 0:0d257bbf5c05 244 aFC=(float)1/(1+tauFC/(tems/1000));
qmaker 0:0d257bbf5c05 245 bFC=tauFC/(tems/1000);
qmaker 0:0d257bbf5c05 246
qmaker 1:b44f69eb07c4 247 // initialisation de la latache periodique du noyau multitache
qmaker 0:0d257bbf5c05 248 timer.start(tems);
qmaker 0:0d257bbf5c05 249
qmaker 0:0d257bbf5c05 250 while(1) {
qmaker 1:b44f69eb07c4 251
qmaker 0:0d257bbf5c05 252 if (flagInterupt==1) {
qmaker 1:b44f69eb07c4 253 pc.printf("%4.3f %4.3f %4.3f %4.3f \n\r",angleGyro,moyVitesse,offsetConsigneAngle,ec);
qmaker 0:0d257bbf5c05 254 flagInterupt=0;
qmaker 0:0d257bbf5c05 255 }
qmaker 1:b44f69eb07c4 256
qmaker 1:b44f69eb07c4 257 if(bluetooth.readable()) {
qmaker 1:b44f69eb07c4 258 dato=bluetooth.getc();
qmaker 1:b44f69eb07c4 259 pc.putc(dato);
qmaker 1:b44f69eb07c4 260 }
qmaker 1:b44f69eb07c4 261 if(pc.readable()) {
qmaker 1:b44f69eb07c4 262 dato=pc.getc();
qmaker 1:b44f69eb07c4 263 bluetooth.putc(dato);
qmaker 1:b44f69eb07c4 264 }
qmaker 1:b44f69eb07c4 265
qmaker 1:b44f69eb07c4 266 // Controle valeur de la commande via HC06
qmaker 1:b44f69eb07c4 267 if(dato == 'A') {
qmaker 1:b44f69eb07c4 268 CommandeVitesse = 3;
qmaker 1:b44f69eb07c4 269 }
qmaker 1:b44f69eb07c4 270 else if(dato == 'R') {
qmaker 1:b44f69eb07c4 271 CommandeVitesse = -3;
qmaker 1:b44f69eb07c4 272 }
qmaker 1:b44f69eb07c4 273 else if((dato == 'a') || (dato == 'r')) {
qmaker 1:b44f69eb07c4 274 CommandeVitesse = 0;
qmaker 1:b44f69eb07c4 275 }
qmaker 1:b44f69eb07c4 276
qmaker 1:b44f69eb07c4 277 else if((dato == 'g') || (dato == 'd')) {
qmaker 1:b44f69eb07c4 278 ecVir = 0;
qmaker 1:b44f69eb07c4 279 }
qmaker 1:b44f69eb07c4 280 else if(dato == 'G') {
qmaker 1:b44f69eb07c4 281 ecVir = -0.1;
qmaker 1:b44f69eb07c4 282 }
qmaker 1:b44f69eb07c4 283 else if(dato == 'D') {
qmaker 1:b44f69eb07c4 284 ecVir = 0.1;
qmaker 1:b44f69eb07c4 285 }
qmaker 1:b44f69eb07c4 286 else if(dato == 'o') {
qmaker 1:b44f69eb07c4 287 KPV =0;
qmaker 1:b44f69eb07c4 288 KDA =0;
qmaker 1:b44f69eb07c4 289 KPA =0;
qmaker 1:b44f69eb07c4 290 KDV =0;
qmaker 1:b44f69eb07c4 291 consigneAngle =0;
qmaker 1:b44f69eb07c4 292 }
qmaker 1:b44f69eb07c4 293 else if(dato == 'O') {
qmaker 1:b44f69eb07c4 294 KPV = 0.026;
qmaker 1:b44f69eb07c4 295 KDA = 0.08;
qmaker 1:b44f69eb07c4 296 KPA =2.3;
qmaker 1:b44f69eb07c4 297 KDV = 0.01;
qmaker 1:b44f69eb07c4 298 consigneAngle = -0.001;
qmaker 1:b44f69eb07c4 299 }
qmaker 1:b44f69eb07c4 300
qmaker 0:0d257bbf5c05 301 }
qmaker 0:0d257bbf5c05 302 }