cebri alyac
Projet Gyro
Dependencies: mbed MPU6050 mbed-rtos FastPWM
Revision 0:b2da78b9e916, committed 2022-05-08
- Comitter:
- braichi13
- Date:
- Sun May 08 14:44:31 2022 +0000
- Commit message:
- Projet Gyro
Changed in this revision
diff -r 000000000000 -r b2da78b9e916 FastPWM.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/FastPWM.lib Sun May 08 14:44:31 2022 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/braichi13/code/FastPWM/#c60399891edd
diff -r 000000000000 -r b2da78b9e916 MPU6050.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/MPU6050.lib Sun May 08 14:44:31 2022 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/braichi13/code/MPU6050/#2b5072b2ee48
diff -r 000000000000 -r b2da78b9e916 main.cpp
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Sun May 08 14:44:31 2022 +0000
@@ -0,0 +1,244 @@
+#include "mbed.h"
+#include "MPU6050.h"
+#include "FastPWM.h"
+#include "rtos.h"
+#include "math.h"
+
+//definition des sorties PWM
+FastPWM PWM1Droit(D11); //IN1 mot Droit
+FastPWM PWM2Droit(D12); //IN2 mot Droit
+FastPWM PWM1Gauche(D10); //IN1 mot Gauche
+FastPWM PWM2Gauche(D9); //IN2 mot Gauche
+
+
+char flag;
+
+
+//definition des connexion serie
+Serial pc(USBTX, USBRX,115200);
+
+DigitalOut EnableGauche(D6);
+DigitalOut EnableDroit(D7);
+
+//acquisition position angulaire
+float accelero[3]= {0}; //case : 0=x; 1=y; z=2
+float gyro[3] = {0}; //case : 0=x; 1=y; z=2
+
+// filtre ec
+float Te = 15e-3,Tems=Te*1000; // Definition de la période d'échantillonage
+
+float TauFC=0.25, // constante de temps des filtres
+ AFC = 1/(1+TauFC/Te), // Filtre passe-bas pour l'accéléromètre
+ BFC = TauFC/Te/(1+TauFC/Te), // Filtre passe-haut pour le gyroscope
+ APB, // Constante de filtre pour le moteur
+ BPB; //Constante de filtre pour le moteur
+
+
+float angle_nf, //angle non filté rad
+ angle_f, //angle filtré rad teta_gyro
+ angle_deg_nf, //angle degré non filtré
+ angle_deg_f, //angle degré filtré
+ angle_w, //teta_gyro
+ angle_tot_nf,
+ angle_tot=0; //angle de rotation
+
+float c_Umot=0,
+ epsilon_mot,
+ Kp_mot=0.426, //coefficient proportionnel moteur
+ Kd_mot=1, //coefficient derive moteur
+ teta_offset = 0.0166, //coefficient pour ajuster le centre d'équilibre du gyro
+ erreur,
+ delta_teta,
+ teta_c = 0,
+ teta_c_D=0,
+ epsilon,
+ E1,
+ Kp = 3.26, //coefficient proportionnel
+ Kd =0.188, //coefficient derive
+ delta_alpha,
+ delta_offset=0.075, //coefficient pour vaincre les frottements secs
+ Umot,
+ Umot_f,
+ TauPB=0.155, // constante du filtre PB-tension
+ erreur_mot,
+ commande;
+
+MPU6050 module(I2C_SDA, I2C_SCL); // dénition de la connexion I2C
+
+
+void asservissement()
+{
+ float teta_obs = angle_tot - teta_offset; // calcule de la partie observation
+ float erreur = teta_c - teta_obs; // calcule de l'erreur angulaire
+ commande = Kp * erreur + Kd * gyro[2]; // calcule de la commande ( rapport cyclique)
+
+
+// limitation de la commande
+ if(commande>0.5) {
+ commande=0.5;
+ } else if (commande<-0.5) {
+ commande=-0.5;
+ }
+
+
+ Umot=commande*(1/(1+TauPB/Te))+Umot*(TauPB/Te/(1+TauPB/Te)); // filtrage de l'image de la tension moteur
+
+
+ erreur_mot = c_Umot - Umot; // calcule de l'erreur de tension moteur
+ teta_c = -(Kp_mot * erreur_mot + Kd_mot*(erreur_mot - E1)); // calcule de la consigne angulaire
+ E1 = erreur_mot; // stockage de la précédente valeur de l'erreur
+
+//compensation des frottements secs
+ if(commande>0) {
+ commande += delta_offset;
+ } else if (commande<0) {
+ commande -= delta_offset;
+ }
+ PWM1Droit.write(0.5-commande); // marche avant droit
+ PWM2Droit.write(0.5+commande); // marche arrière droit
+ PWM1Gauche.write(0.5-commande); // marche avant droit
+ PWM2Gauche.write(0.5+commande); //marche arrière gauche
+}
+
+void acquisition() // Acquisition Accéléro_gyro
+{
+
+ //récupération des données
+ module.getAccelero(accelero);
+ module.getGyro(gyro);
+
+ angle_nf = atan2(accelero[1],accelero[0]); //calcule de l'angle en rad non filtré
+
+ angle_tot_nf=-TauFC*gyro[2]+angle_nf; // Association gyro accéléromètre avant le filtre complémentaire !!! c'est -gyro[2] pour avoir le bon signe!!!
+
+ angle_tot=angle_tot_nf*AFC + angle_tot*BFC; // application du filtre complémentaire
+
+ asservissement();
+ flag = 1; // Flag de fin d'acquisition
+}
+
+
+
+// declaration d'un objet RtosTimmer
+RtosTimer timer(mbed::callback(acquisition),osTimerPeriodic);
+
+
+
+void reception(char ch) // reception des trames envoyées depuis le terminale
+{
+ static int i=0;
+ static char chaine[10]; // Chaine d'envoie de l'information
+ char commande[3]; //
+ char valeur[6]; //
+
+
+ if ((ch==13) or (ch==10)) {
+
+ chaine[i]='\0'; //fin du stockage de caractère
+
+
+ strcpy(commande, strtok(chaine, " "));
+ strcpy(valeur, strtok(NULL, " "));
+
+
+
+
+// pc.printf("commande *%s*\r\n",commande);
+// pc.printf("valeur *%s*\r\n",valeur);
+
+ if (strcmp(commande,"TC")==0) {
+ TauFC= atof(valeur); // constante de temps des filtres
+ AFC = 1/(1+TauFC/Te), // Filtre passe-bas pour l'accéléromètre
+ BFC = TauFC/Te/(1+TauFC/Te); // Filtre passe-haut pour le gyroscope
+ flag = 1;
+ }
+
+ else if (strcmp(commande,"Kd")==0) {
+ Kd= atof(valeur); // constante de dérivtaion
+ }
+
+
+ else if (strcmp(commande,"Kp")==0) {
+ Kp= atof(valeur); // constante de dérivtaion
+ }
+
+ else if (strcmp(commande,"Kpm")==0) {
+ Kp_mot= atof(valeur); // constante de dérivtaion
+ APB=1/(1+TauPB/Te),
+ BFC = TauPB/Te/(1+TauPB/Te);
+
+ }
+
+ else if (strcmp(commande,"Kdm")==0) {
+ Kd_mot= atof(valeur); // constante de dérivtaion
+ }
+
+ else if (strcmp(commande,"Oof")==0) {
+ teta_offset= atof(valeur); // constante de dérivtaion
+ }
+
+
+
+
+
+
+ else {
+ pc.printf("commande inconnue \r\n");
+ }
+ i = 0;
+
+
+ } else {
+ chaine[i]=ch;
+ i++;
+ }
+}
+
+
+int main()
+{
+ PWM1Droit.period_us(50);
+ PWM2Droit.period_us(50);
+ PWM1Gauche.period_us(50);
+ PWM2Gauche.period_us(50);
+
+
+
+ EnableGauche=0;
+ EnableDroit=0;
+
+ pc.printf("Gyro \n\r");
+ while (!module.testConnection()) {
+ pc.printf("not connected to mpu\n\r"); // MPU non connecté ( ! à l'adresse I2C)
+ wait(1);
+ }
+
+ pc.printf("connected to MPU\n\r");
+ //changement du l'echelle du module
+ module.setGyroRange(MPU6050_GYRO_RANGE_2000);
+ module.setAcceleroRange(MPU6050_ACCELERO_RANGE_2G);
+
+ EnableGauche=1;
+ EnableDroit=1;
+
+ timer.start((int)Tems);
+ pc.printf("TimerStart %d \n\r",(int)Tems);
+
+ while(1) {
+
+ if (pc.readable()!=0) {
+ reception(pc.getc());
+ }
+
+ if (flag==1) {
+ angle_deg_nf=angle_nf*(180/3.14159265359); //calcule de l'angle en deg non filtré
+ angle_deg_f=angle_f*(180/3.14159265359); //calcule de l'angle en deg filtré
+ angle_w=TauFC*(-gyro[2])*(180/3.14159265359); // calcule angle depuis gyro (tauFC= integrateur) !!! c'est -gyro[2] pour avoir le bon signe!!!
+ pc.printf("%.2f %.2f %.4f %.2f \n",angle_tot, teta_offset, Kp, Kd); // affichage des données sur le terminale
+ flag = 0;
+ }
+
+ }
+}
+
+
diff -r 000000000000 -r b2da78b9e916 mbed-rtos.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed-rtos.lib Sun May 08 14:44:31 2022 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/braichi13/code/mbed-rtos/#77205fc699b9
diff -r 000000000000 -r b2da78b9e916 mbed.bld --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Sun May 08 14:44:31 2022 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/mbed_official/code/mbed/builds/235179ab3f27 \ No newline at end of file