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@1:111167e39dfa, 2022-03-31 (annotated)

Committer:
flo__
Date:
Thu Mar 31 10:44:47 2022 +0000
Revision:
1:111167e39dfa
Parent:
0:b435eadf76b4 
31/03/22

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