florent ollivier / Mbed 2 deprecated Gyro

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:b435eadf76b4, 2022-03-28 (annotated)

Committer:
flo__
Date:
Mon Mar 28 15:54:19 2022 +0000
Revision:
0:b435eadf76b4
Child:
1:111167e39dfa
28/03/2022

Who changed what in which revision?

UserRevisionLine numberNew contents of line
flo__ 0:b435eadf76b4 1 #include "mbed.h"
flo__ 0:b435eadf76b4 2 #include "MPU6050.h"
flo__ 0:b435eadf76b4 3 #include "FastPWM.h"
flo__ 0:b435eadf76b4 4 #include "rtos.h"
flo__ 0:b435eadf76b4 5 #include "math.h"
flo__ 0:b435eadf76b4 6
flo__ 0:b435eadf76b4 7 //definition des sorties PWM
flo__ 0:b435eadf76b4 8 FastPWM PWM1Droit(D11); //IN1 mot Droit
flo__ 0:b435eadf76b4 9 FastPWM PWM2Droit(D12); //IN2 mot Droit
flo__ 0:b435eadf76b4 10 FastPWM PWM1Gauche(D9); //IN1 mot Gauche
flo__ 0:b435eadf76b4 11 FastPWM PWM2Gauche(D10); //IN2 mot Gauche
flo__ 0:b435eadf76b4 12
flo__ 0:b435eadf76b4 13 // on déclare un "flag" en globale
flo__ 0:b435eadf76b4 14 char flag;
flo__ 0:b435eadf76b4 15
flo__ 0:b435eadf76b4 16
flo__ 0:b435eadf76b4 17 //definition des connexion serie (RS232 en 115200 bauds)
flo__ 0:b435eadf76b4 18 Serial pc(USBTX, USBRX,115200);
flo__ 0:b435eadf76b4 19
flo__ 0:b435eadf76b4 20 DigitalOut EnableGauche(D6);
flo__ 0:b435eadf76b4 21 DigitalOut EnableDroite(D7);
flo__ 0:b435eadf76b4 22
flo__ 0:b435eadf76b4 23
flo__ 0:b435eadf76b4 24 //création des tableaux qui contiennent les valeurs sur x,y et z de l'accéléromètre et du gyroscope
flo__ 0:b435eadf76b4 25 float accelero[3]= {0};
flo__ 0:b435eadf76b4 26 float gyro[3] = {0};
flo__ 0:b435eadf76b4 27
flo__ 0:b435eadf76b4 28 // création de la variable "angleMesureFiltre" qui contient l'inclinaison filtré du gyropode
flo__ 0:b435eadf76b4 29 float angleMesureFiltre;
flo__ 0:b435eadf76b4 30
flo__ 0:b435eadf76b4 31 //création de variable
flo__ 0:b435eadf76b4 32 double angleErreur,alpha;
flo__ 0:b435eadf76b4 33
flo__ 0:b435eadf76b4 34 double erreurUmot, Umot=0;
flo__ 0:b435eadf76b4 35
flo__ 0:b435eadf76b4 36 //création de la constante d'échantillonage "Te" en [s] et Tems en [ms]
flo__ 0:b435eadf76b4 37 float Te = 20e-3,Tems=Te*1000;
flo__ 0:b435eadf76b4 38
flo__ 0:b435eadf76b4 39 //création de la constante de temps "TauFC" de nos filtre
flo__ 0:b435eadf76b4 40 float TauFC=0.25;
flo__ 0:b435eadf76b4 41
flo__ 0:b435eadf76b4 42 //caractéristique de la boucle d'asservissement
flo__ 0:b435eadf76b4 43 double kp=2, kd=0, k2=0, offset=0;
flo__ 0:b435eadf76b4 44
flo__ 0:b435eadf76b4 45 //création du coef "AFC" et "BFC" que l'on applique respectivement sur l'entrée et la sortie
flo__ 0:b435eadf76b4 46 float AFC = 1/(1+TauFC/Te),
flo__ 0:b435eadf76b4 47 BFC = TauFC/Te/(1+TauFC/Te);
flo__ 0:b435eadf76b4 48
flo__ 0:b435eadf76b4 49 //création de la vaiable qui stocke la valeur de l'angle non filtré grâce et fitré à l'accéléromètre
flo__ 0:b435eadf76b4 50 float angle_nf=0;
flo__ 0:b435eadf76b4 51 float angle_f=0;
flo__ 0:b435eadf76b4 52
flo__ 0:b435eadf76b4 53 //Cr&ation de la variable qui stocke la valeurs de l'angle filtré en grâce au gyroscope (angle filtré haute fréquence)
flo__ 0:b435eadf76b4 54 float angle_f_HF=0;
flo__ 0:b435eadf76b4 55
flo__ 0:b435eadf76b4 56 //definition de la connnection en "I2C" entre le nucleo et le MPU6050
flo__ 0:b435eadf76b4 57 MPU6050 module(I2C_SDA, I2C_SCL);
flo__ 0:b435eadf76b4 58
flo__ 0:b435eadf76b4 59
flo__ 0:b435eadf76b4 60 //Création de la fonction permettant l'acquisition des valeurs de l'accéléro et du gyro
flo__ 0:b435eadf76b4 61 void acquisition()
flo__ 0:b435eadf76b4 62 {
flo__ 0:b435eadf76b4 63
flo__ 0:b435eadf76b4 64 //récupération des données
flo__ 0:b435eadf76b4 65 module.getAccelero(accelero);
flo__ 0:b435eadf76b4 66 module.getGyro(gyro);
flo__ 0:b435eadf76b4 67
flo__ 0:b435eadf76b4 68 //calcul de l'angle non filtré avec l'accéléro
flo__ 0:b435eadf76b4 69 angle_nf = atan2(accelero[1],accelero[0]); //arctan(y/x)
flo__ 0:b435eadf76b4 70
flo__ 0:b435eadf76b4 71 //filtrage de l'angle non filtré avec l'accéléro
flo__ 0:b435eadf76b4 72 angle_f = AFC * angle_nf + BFC * angle_f;
flo__ 0:b435eadf76b4 73
flo__ 0:b435eadf76b4 74 //calcul et filtrage de l'angle mesuré avec le gyroscope
flo__ 0:b435eadf76b4 75 angle_f_HF = AFC *TauFC*(-gyro[2]) + BFC * angle_f_HF;
flo__ 0:b435eadf76b4 76
flo__ 0:b435eadf76b4 77 //calcul finale de l'angle d'inclinaison du gyropode
flo__ 0:b435eadf76b4 78 angleMesureFiltre =angle_f+(angle_f_HF);
flo__ 0:b435eadf76b4 79
flo__ 0:b435eadf76b4 80
flo__ 0:b435eadf76b4 81
flo__ 0:b435eadf76b4 82 //asservissment angle
flo__ 0:b435eadf76b4 83 angleErreur= 0 - (angleMesureFiltre + offset);
flo__ 0:b435eadf76b4 84 alpha = kp*angleErreur + kd*gyro[2] +k2*erreurUmot; //gyro[2] = vitesse angulaire y
flo__ 0:b435eadf76b4 85
flo__ 0:b435eadf76b4 86 if(alpha>0.5) alpha=0.5;
flo__ 0:b435eadf76b4 87 if(alpha<-0.5) alpha=-0.5;
flo__ 0:b435eadf76b4 88
flo__ 0:b435eadf76b4 89 PWM1Droit.write(0.5-alpha);
flo__ 0:b435eadf76b4 90 PWM2Droit.write(0.5+alpha); //motDroit
flo__ 0:b435eadf76b4 91
flo__ 0:b435eadf76b4 92 PWM1Gauche.write(0.5-alpha); //motGauche
flo__ 0:b435eadf76b4 93 PWM2Gauche.write(0.5+alpha);
flo__ 0:b435eadf76b4 94
flo__ 0:b435eadf76b4 95 //calcul fltrage vitesse Umot(attention fréquence de coupure à modifier)
flo__ 0:b435eadf76b4 96 Umot = AFC * alpha + BFC * Umot;
flo__ 0:b435eadf76b4 97
flo__ 0:b435eadf76b4 98 //asservssment en translation
flo__ 0:b435eadf76b4 99 erreurUmot = 0 - Umot;
flo__ 0:b435eadf76b4 100
flo__ 0:b435eadf76b4 101 //on leve le drapeau
flo__ 0:b435eadf76b4 102 flag = 1;
flo__ 0:b435eadf76b4 103 }
flo__ 0:b435eadf76b4 104
flo__ 0:b435eadf76b4 105 //declaration d'un objet RtosTimmer
flo__ 0:b435eadf76b4 106 RtosTimer timer(mbed::callback(acquisition),osTimerPeriodic);
flo__ 0:b435eadf76b4 107
flo__ 0:b435eadf76b4 108
flo__ 0:b435eadf76b4 109 //Création de la fonction qui permet la réception de commande
flo__ 0:b435eadf76b4 110 void reception(char ch)
flo__ 0:b435eadf76b4 111 {
flo__ 0:b435eadf76b4 112 static int i=0;
flo__ 0:b435eadf76b4 113 static char chaine[10];
flo__ 0:b435eadf76b4 114 char commande[3];
flo__ 0:b435eadf76b4 115 char valeur[6];
flo__ 0:b435eadf76b4 116
flo__ 0:b435eadf76b4 117 //Scrutation des caractères "Carriage Return" et "Line Feed"
flo__ 0:b435eadf76b4 118 if ((ch==13) or (ch==10)) {
flo__ 0:b435eadf76b4 119
flo__ 0:b435eadf76b4 120 //on vide la chaine de caractère
flo__ 0:b435eadf76b4 121 chaine[i]='\0';
flo__ 0:b435eadf76b4 122
flo__ 0:b435eadf76b4 123 //on sépare la chaine de caractère en une variable commande et valeur
flo__ 0:b435eadf76b4 124 strcpy(commande, strtok(chaine, " "));
flo__ 0:b435eadf76b4 125 strcpy(valeur, strtok(NULL, " "));
flo__ 0:b435eadf76b4 126
flo__ 0:b435eadf76b4 127 //pc.printf("commande *%s*\r\n",commande);
flo__ 0:b435eadf76b4 128 //pc.printf("valeur *%s*\r\n",valeur);
flo__ 0:b435eadf76b4 129
flo__ 0:b435eadf76b4 130 //Si la commande "TC" est reconnu alors on change la valeur de la constante de temps "TauFC" et on met à jour la valeur des coef AFC et BFC
flo__ 0:b435eadf76b4 131 if (strcmp(commande,"TC")==0) {
flo__ 0:b435eadf76b4 132 TauFC= atof(valeur);
flo__ 0:b435eadf76b4 133 AFC = 1/(1+TauFC/Te),
flo__ 0:b435eadf76b4 134 BFC = TauFC/Te/(1+TauFC/Te);
flo__ 0:b435eadf76b4 135 flag = 1;
flo__ 0:b435eadf76b4 136 }
flo__ 0:b435eadf76b4 137
flo__ 0:b435eadf76b4 138 if (strcmp(commande,"of")==0) {
flo__ 0:b435eadf76b4 139 offset = atof(valeur);
flo__ 0:b435eadf76b4 140 }
flo__ 0:b435eadf76b4 141 if (strcmp(commande,"kd")==0) {
flo__ 0:b435eadf76b4 142 kd = atof(valeur);
flo__ 0:b435eadf76b4 143 }
flo__ 0:b435eadf76b4 144 if (strcmp(commande,"kp")==0) {
flo__ 0:b435eadf76b4 145 kp = atof(valeur);
flo__ 0:b435eadf76b4 146 }
flo__ 0:b435eadf76b4 147 if (strcmp(commande,"k2")==0) {
flo__ 0:b435eadf76b4 148 k2 = atof(valeur);
flo__ 0:b435eadf76b4 149 }else {
flo__ 0:b435eadf76b4 150 //la commande n'est pas reconnu
flo__ 0:b435eadf76b4 151 pc.printf("commande inconnue \r\n");
flo__ 0:b435eadf76b4 152 }
flo__ 0:b435eadf76b4 153 i = 0;
flo__ 0:b435eadf76b4 154
flo__ 0:b435eadf76b4 155
flo__ 0:b435eadf76b4 156 } else {
flo__ 0:b435eadf76b4 157
flo__ 0:b435eadf76b4 158 //on incrémente la chaine de caractère si "Carriage Return" ou "Line Feed" n'est pas détecté
flo__ 0:b435eadf76b4 159 chaine[i]=ch;
flo__ 0:b435eadf76b4 160 i++;
flo__ 0:b435eadf76b4 161 }
flo__ 0:b435eadf76b4 162 }
flo__ 0:b435eadf76b4 163
flo__ 0:b435eadf76b4 164 int main()
flo__ 0:b435eadf76b4 165 {
flo__ 0:b435eadf76b4 166
flo__ 0:b435eadf76b4 167
flo__ 0:b435eadf76b4 168
flo__ 0:b435eadf76b4 169 pc.printf("Le Gyro est allume\n\r");
flo__ 0:b435eadf76b4 170
flo__ 0:b435eadf76b4 171 //vérification de la communication entre le MPU6050 et le nucleo
flo__ 0:b435eadf76b4 172 while (!module.testConnection()) {
flo__ 0:b435eadf76b4 173 pc.printf("not connected to mpu\n\r");
flo__ 0:b435eadf76b4 174 wait(1);
flo__ 0:b435eadf76b4 175 }
flo__ 0:b435eadf76b4 176
flo__ 0:b435eadf76b4 177 pc.printf("connected to MPU\n\r");
flo__ 0:b435eadf76b4 178
flo__ 0:b435eadf76b4 179 //Initialisation PWM
flo__ 0:b435eadf76b4 180 PWM1Droit.period_us(50);
flo__ 0:b435eadf76b4 181 PWM2Droit.period_us(50);
flo__ 0:b435eadf76b4 182 PWM1Gauche.period_us(50);
flo__ 0:b435eadf76b4 183 PWM2Gauche.period_us(50);
flo__ 0:b435eadf76b4 184
flo__ 0:b435eadf76b4 185 /*Initialisation rapport cyclique
flo__ 0:b435eadf76b4 186 PWM1Droit.write(0.3);
flo__ 0:b435eadf76b4 187 PWM2Droit.write(0.7); //motDroit
flo__ 0:b435eadf76b4 188
flo__ 0:b435eadf76b4 189 PWM1Gauche.write(0.3); //motGauche
flo__ 0:b435eadf76b4 190 PWM2Gauche.write(0.7); */
flo__ 0:b435eadf76b4 191
flo__ 0:b435eadf76b4 192 //Mise e marche des moteurs
flo__ 0:b435eadf76b4 193 EnableGauche = 1;
flo__ 0:b435eadf76b4 194 EnableDroite = 1;
flo__ 0:b435eadf76b4 195
flo__ 0:b435eadf76b4 196 //Initialisation de la période d'échantillonnage "Tems"[s] (soit 20ms)
flo__ 0:b435eadf76b4 197 timer.start((int)Tems);
flo__ 0:b435eadf76b4 198 pc.printf("TimerStart %d \n\r",(int)Tems);
flo__ 0:b435eadf76b4 199
flo__ 0:b435eadf76b4 200 //changement de l'echelle du module
flo__ 0:b435eadf76b4 201 module.setGyroRange(MPU6050_GYRO_RANGE_2000);
flo__ 0:b435eadf76b4 202 module.setAcceleroRange(MPU6050_ACCELERO_RANGE_2G);
flo__ 0:b435eadf76b4 203
flo__ 0:b435eadf76b4 204 while(1) {
flo__ 0:b435eadf76b4 205 if (pc.readable()!=0) {
flo__ 0:b435eadf76b4 206 reception(pc.getc());
flo__ 0:b435eadf76b4 207 }
flo__ 0:b435eadf76b4 208
flo__ 0:b435eadf76b4 209 if (flag==1) {
flo__ 0:b435eadf76b4 210
flo__ 0:b435eadf76b4 211 //pc.printf("%3.2f %3.2f %3.2f %3.2f\n\r",accelero[0],accelero[1],accelero[2],TauFC);
flo__ 0:b435eadf76b4 212 //pc.printf("%3.2f %3.2f \n\r",angle_f,angle_nf);
flo__ 0:b435eadf76b4 213 pc.printf("%3.2f %3.2f %3.2f\n\r",angleMesureFiltre,alpha,offset); //gyro[2] = vitesse angulaire y
flo__ 0:b435eadf76b4 214 //pc.printf("%3.2f %3.2f %3.2f\n\r",gyro[0],gyro[1],gyro[2]);
flo__ 0:b435eadf76b4 215
flo__ 0:b435eadf76b4 216 flag = 0;
flo__ 0:b435eadf76b4 217 }
flo__ 0:b435eadf76b4 218
flo__ 0:b435eadf76b4 219 }
flo__ 0:b435eadf76b4 220 }
flo__ 0:b435eadf76b4 221
flo__ 0:b435eadf76b4 222
flo__ 0:b435eadf76b4 223
flo__ 0:b435eadf76b4 224