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@0:0d257bbf5c05, 2021-03-11 (annotated)

Committer:
qmaker
Date:
Thu Mar 11 08:00:49 2021 +0000
Revision:
0:0d257bbf5c05
Child:
1:b44f69eb07c4
mpu6050 corrige

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 0:0d257bbf5c05 4
qmaker 0:0d257bbf5c05 5 // déclaration des objets
qmaker 0:0d257bbf5c05 6 Serial pc(USBTX, USBRX);
qmaker 0:0d257bbf5c05 7 DigitalOut led(LED1);
qmaker 0:0d257bbf5c05 8 MPU6050 module(I2C_SDA, I2C_SCL);
qmaker 0:0d257bbf5c05 9
qmaker 0:0d257bbf5c05 10 // valeur de Te
qmaker 0:0d257bbf5c05 11 float tems = 10;
qmaker 0:0d257bbf5c05 12
qmaker 0:0d257bbf5c05 13 // varailble necessire à la mesure de la position angulaire du gyropode
qmaker 0:0d257bbf5c05 14 float accelero[3]= {0};
qmaker 0:0d257bbf5c05 15 float gyro[3] = {0};
qmaker 0:0d257bbf5c05 16 float tauFC = 1; // constante de tyemps en seconde du filtre passe bas du filtre complémentaire
qmaker 0:0d257bbf5c05 17 float aFC, bFC; // les coefficient du filter passe bas du filtre complémentaire
qmaker 0:0d257bbf5c05 18 float angleATG ,angleNF, angleGyro ,omegarGyro, angleGyrop;
qmaker 0:0d257bbf5c05 19
qmaker 0:0d257bbf5c05 20
qmaker 0:0d257bbf5c05 21
qmaker 0:0d257bbf5c05 22 int nbInc=0;
qmaker 0:0d257bbf5c05 23 char flagInterupt=0;
qmaker 0:0d257bbf5c05 24
qmaker 0:0d257bbf5c05 25 void reception(char ch)
qmaker 0:0d257bbf5c05 26 {
qmaker 0:0d257bbf5c05 27 static int i=0; // inice du dernier caratère recu
qmaker 0:0d257bbf5c05 28 static char chaine[10]; // chaine de stockage des caratères recus
qmaker 0:0d257bbf5c05 29 char commande[3]; // chaine contenant la commande
qmaker 0:0d257bbf5c05 30 char valeur[6]; // chaine contenant la valeur
qmaker 0:0d257bbf5c05 31
qmaker 0:0d257bbf5c05 32 if ((ch==13) or (ch==10)) {
qmaker 0:0d257bbf5c05 33
qmaker 0:0d257bbf5c05 34 chaine[i]='\0';
qmaker 0:0d257bbf5c05 35
qmaker 0:0d257bbf5c05 36 // séparation de la commande et de la valeur
qmaker 0:0d257bbf5c05 37 strcpy(commande, strtok(chaine, " "));
qmaker 0:0d257bbf5c05 38 strcpy(valeur, strtok(NULL, " "));
qmaker 0:0d257bbf5c05 39
qmaker 0:0d257bbf5c05 40 // affichage de commande et valeur
qmaker 0:0d257bbf5c05 41 pc.printf("Commande %s \n\r",commande);
qmaker 0:0d257bbf5c05 42 pc.printf("Valeur %s \n\r",valeur);
qmaker 0:0d257bbf5c05 43
qmaker 0:0d257bbf5c05 44 // reinitialisation de l indice de chaine
qmaker 0:0d257bbf5c05 45 i = 0;
qmaker 0:0d257bbf5c05 46
qmaker 0:0d257bbf5c05 47 } else {
qmaker 0:0d257bbf5c05 48 chaine[i]=ch;
qmaker 0:0d257bbf5c05 49 i++;
qmaker 0:0d257bbf5c05 50 }
qmaker 0:0d257bbf5c05 51 }
qmaker 0:0d257bbf5c05 52
qmaker 0:0d257bbf5c05 53 void interupt()
qmaker 0:0d257bbf5c05 54 {
qmaker 0:0d257bbf5c05 55 nbInc++;
qmaker 0:0d257bbf5c05 56 //lecture des données de l'accéléro et du gyro
qmaker 0:0d257bbf5c05 57 module.getAccelero(accelero);
qmaker 0:0d257bbf5c05 58 module.getGyro(gyro);
qmaker 0:0d257bbf5c05 59 //
qmaker 0:0d257bbf5c05 60 angleATG = atan2(accelero[0],accelero[1]);
qmaker 0:0d257bbf5c05 61 omegarGyro = gyro[3];
qmaker 0:0d257bbf5c05 62
qmaker 0:0d257bbf5c05 63 angleNF = angleATG + tauFC * omegarGyro;
qmaker 0:0d257bbf5c05 64 angleGyro = aFC*( angleNF + bFC * angleGyrop);
qmaker 0:0d257bbf5c05 65
qmaker 0:0d257bbf5c05 66
qmaker 0:0d257bbf5c05 67
qmaker 0:0d257bbf5c05 68
qmaker 0:0d257bbf5c05 69 // memorisation des valeurs precedante pour les filtres recursifs
qmaker 0:0d257bbf5c05 70 angleGyrop = angleGyro ;
qmaker 0:0d257bbf5c05 71
qmaker 0:0d257bbf5c05 72
qmaker 0:0d257bbf5c05 73 flagInterupt=1;
qmaker 0:0d257bbf5c05 74 }
qmaker 0:0d257bbf5c05 75
qmaker 0:0d257bbf5c05 76 RtosTimer timer(mbed::callback(interupt),osTimerPeriodic);
qmaker 0:0d257bbf5c05 77
qmaker 0:0d257bbf5c05 78 int main()
qmaker 0:0d257bbf5c05 79 {
qmaker 0:0d257bbf5c05 80 pc.printf("Test unitaire mecatro \n\r");
qmaker 0:0d257bbf5c05 81
qmaker 0:0d257bbf5c05 82
qmaker 0:0d257bbf5c05 83
qmaker 0:0d257bbf5c05 84
qmaker 0:0d257bbf5c05 85 // initialisation et test de connection du MPU6050
qmaker 0:0d257bbf5c05 86 while (module.testConnection()==0) {
qmaker 0:0d257bbf5c05 87 pc.printf("not connected to mpu\n\r");
qmaker 0:0d257bbf5c05 88 wait(1);
qmaker 0:0d257bbf5c05 89 }
qmaker 0:0d257bbf5c05 90 //changement du l'echelle du module
qmaker 0:0d257bbf5c05 91 module.setGyroRange(MPU6050_GYRO_RANGE_2000);
qmaker 0:0d257bbf5c05 92 module.setAcceleroRange(MPU6050_ACCELERO_RANGE_2G);
qmaker 0:0d257bbf5c05 93
qmaker 0:0d257bbf5c05 94 // calcul des coefficients des filtres
qmaker 0:0d257bbf5c05 95 aFC=(float)1/(1+tauFC/(tems/1000));
qmaker 0:0d257bbf5c05 96 bFC=tauFC/(tems/1000);
qmaker 0:0d257bbf5c05 97
qmaker 0:0d257bbf5c05 98
qmaker 0:0d257bbf5c05 99
qmaker 0:0d257bbf5c05 100 // initialisation de la latache periodique du noyau multitache
qmaker 0:0d257bbf5c05 101 timer.start(tems);
qmaker 0:0d257bbf5c05 102
qmaker 0:0d257bbf5c05 103 while(1) {
qmaker 0:0d257bbf5c05 104 if (pc.readable()!=0) {
qmaker 0:0d257bbf5c05 105 reception(pc.getc());
qmaker 0:0d257bbf5c05 106 }
qmaker 0:0d257bbf5c05 107 if (flagInterupt==1) {
qmaker 0:0d257bbf5c05 108 pc.printf("Ax = %3.2f, Ay = %3.2f, Az = %3.2f Wx = %3.2f, wy = %3.2f, wz = %3.2f \n\r",accelero[0],accelero[1],accelero[2],gyro[0],gyro[1],gyro[2]);
qmaker 0:0d257bbf5c05 109 flagInterupt=0;
qmaker 0:0d257bbf5c05 110 }
qmaker 0:0d257bbf5c05 111 }
qmaker 0:0d257bbf5c05 112 }