Diff: main.cpp

Revision:

0:b435eadf76b4

Child:

1:111167e39dfa

diff -r 000000000000 -r b435eadf76b4 main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Mon Mar 28 15:54:19 2022 +0000
@@ -0,0 +1,224 @@
+#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(D9);          //IN1 mot Gauche
+FastPWM PWM2Gauche(D10);         //IN2 mot Gauche
+
+// on déclare un "flag" en globale
+char flag;
+
+
+//definition des connexion serie (RS232 en 115200 bauds)
+Serial pc(USBTX, USBRX,115200);
+
+DigitalOut EnableGauche(D6);
+DigitalOut EnableDroite(D7);
+
+
+//création des tableaux qui contiennent les valeurs sur x,y et z de l'accéléromètre et du gyroscope
+float accelero[3]= {0};
+float gyro[3] = {0};
+
+// création de la variable "angleMesureFiltre" qui contient l'inclinaison filtré du gyropode
+float angleMesureFiltre;
+
+//création de variable
+double angleErreur,alpha;
+
+double erreurUmot, Umot=0;
+
+//création de la constante d'échantillonage "Te" en [s] et Tems en [ms]
+float Te = 20e-3,Tems=Te*1000;
+
+//création de la constante de temps "TauFC" de nos filtre
+float TauFC=0.25;
+
+//caractéristique de la boucle d'asservissement
+double kp=2, kd=0, k2=0, offset=0;
+
+//création du coef "AFC" et "BFC" que l'on applique respectivement sur l'entrée et la sortie
+float AFC = 1/(1+TauFC/Te),
+      BFC = TauFC/Te/(1+TauFC/Te);
+
+//création de la vaiable qui stocke la valeur de l'angle non filtré grâce et fitré à l'accéléromètre
+float angle_nf=0;
+float angle_f=0;
+
+//Cr&ation de la variable qui stocke la valeurs de l'angle filtré en grâce au gyroscope (angle filtré haute fréquence)
+float angle_f_HF=0;
+
+//definition de la connnection en "I2C" entre le nucleo et le MPU6050
+MPU6050 module(I2C_SDA, I2C_SCL);
+
+
+//Création de la fonction permettant l'acquisition des valeurs de l'accéléro et du gyro
+void acquisition()
+{
+
+    //récupération des données
+    module.getAccelero(accelero);
+    module.getGyro(gyro);
+
+    //calcul de l'angle non filtré avec l'accéléro
+    angle_nf = atan2(accelero[1],accelero[0]); //arctan(y/x)
+
+    //filtrage de l'angle non filtré avec l'accéléro
+    angle_f = AFC * angle_nf + BFC * angle_f;
+
+    //calcul et filtrage de l'angle mesuré avec le gyroscope
+    angle_f_HF = AFC *TauFC*(-gyro[2]) + BFC * angle_f_HF;
+
+    //calcul finale de l'angle d'inclinaison du gyropode
+    angleMesureFiltre =angle_f+(angle_f_HF);
+    
+    
+    
+    //asservissment angle
+    angleErreur= 0 - (angleMesureFiltre + offset);
+    alpha = kp*angleErreur + kd*gyro[2] +k2*erreurUmot;    //gyro[2] = vitesse angulaire y
+
+    if(alpha>0.5) alpha=0.5;
+    if(alpha<-0.5) alpha=-0.5;
+
+    PWM1Droit.write(0.5-alpha);
+    PWM2Droit.write(0.5+alpha);       //motDroit
+
+    PWM1Gauche.write(0.5-alpha);      //motGauche
+    PWM2Gauche.write(0.5+alpha);
+
+    //calcul fltrage vitesse Umot(attention fréquence de coupure à modifier)
+    Umot = AFC * alpha + BFC * Umot;
+    
+    //asservssment en translation
+    erreurUmot = 0 - Umot;
+    
+    //on leve le drapeau
+    flag = 1;
+}
+
+//declaration d'un objet RtosTimmer
+RtosTimer timer(mbed::callback(acquisition),osTimerPeriodic);
+
+
+//Création de la fonction qui permet la réception de commande
+void reception(char ch)
+{
+    static  int i=0;
+    static char chaine[10];
+    char commande[3];
+    char valeur[6];
+
+    //Scrutation des caractères "Carriage Return" et "Line Feed"
+    if ((ch==13) or (ch==10)) {
+
+        //on vide la chaine de caractère
+        chaine[i]='\0';
+
+        //on sépare la chaine de caractère en une variable commande et valeur
+        strcpy(commande, strtok(chaine, " "));
+        strcpy(valeur,  strtok(NULL, " "));
+
+        //pc.printf("commande *%s*\r\n",commande);
+        //pc.printf("valeur   *%s*\r\n",valeur);
+
+        //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
+        if (strcmp(commande,"TC")==0) {
+            TauFC= atof(valeur);
+            AFC = 1/(1+TauFC/Te),
+            BFC = TauFC/Te/(1+TauFC/Te);
+            flag = 1;
+        }
+
+        if (strcmp(commande,"of")==0) {
+            offset = atof(valeur);
+        }
+        if (strcmp(commande,"kd")==0) {
+            kd = atof(valeur);
+        }
+        if (strcmp(commande,"kp")==0) {
+            kp = atof(valeur);
+        } 
+        if (strcmp(commande,"k2")==0) {
+            k2 = atof(valeur);
+        }else {
+            //la commande n'est pas reconnu
+            pc.printf("commande inconnue \r\n");
+        }
+        i = 0;
+
+
+    } else {
+
+        //on incrémente la chaine de caractère si "Carriage Return" ou "Line Feed" n'est pas détecté
+        chaine[i]=ch;
+        i++;
+    }
+}
+
+int main()
+{
+
+    
+
+    pc.printf("Le Gyro est allume\n\r");
+
+    //vérification de la communication entre le MPU6050 et le nucleo
+    while (!module.testConnection()) {
+        pc.printf("not connected to mpu\n\r");
+        wait(1);
+    }
+
+    pc.printf("connected to MPU\n\r");
+
+    //Initialisation PWM
+    PWM1Droit.period_us(50);
+    PWM2Droit.period_us(50);
+    PWM1Gauche.period_us(50);
+    PWM2Gauche.period_us(50);
+
+    /*Initialisation rapport cyclique
+    PWM1Droit.write(0.3);
+    PWM2Droit.write(0.7);       //motDroit
+
+    PWM1Gauche.write(0.3);      //motGauche
+    PWM2Gauche.write(0.7); */
+
+    //Mise e marche des moteurs
+    EnableGauche = 1;
+    EnableDroite = 1;
+
+    //Initialisation de la période d'échantillonnage "Tems"[s] (soit 20ms)
+    timer.start((int)Tems);
+    pc.printf("TimerStart %d \n\r",(int)Tems);
+
+    //changement de l'echelle du module
+    module.setGyroRange(MPU6050_GYRO_RANGE_2000);
+    module.setAcceleroRange(MPU6050_ACCELERO_RANGE_2G);
+
+    while(1) {
+        if (pc.readable()!=0) {
+            reception(pc.getc());
+        }
+
+        if (flag==1) {
+
+            //pc.printf("%3.2f %3.2f %3.2f %3.2f\n\r",accelero[0],accelero[1],accelero[2],TauFC);
+            //pc.printf("%3.2f %3.2f \n\r",angle_f,angle_nf);
+            pc.printf("%3.2f %3.2f %3.2f\n\r",angleMesureFiltre,alpha,offset);  //gyro[2] = vitesse angulaire y
+            //pc.printf("%3.2f %3.2f %3.2f\n\r",gyro[0],gyro[1],gyro[2]);
+
+            flag = 0;
+        }
+
+    }
+}
+
+
+
+