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@1:b44f69eb07c4, 2021-04-15 (annotated)

Committer:
qmaker
Date:
Thu Apr 15 06:44:26 2021 +0000
Revision:
1:b44f69eb07c4
Parent:
0:0d257bbf5c05
Child:
2:7de884ffc9d9
Version asservissement avec codeurs et BT

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