Diff: main.cpp

Revision:

0:a59a3d743804

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Thu Mar 31 07:43:50 2022 +0000
@@ -0,0 +1,169 @@
+#include "mbed.h"
+#include "MPU6050.h"
+#include "FastPWM.h"
+#include "rtos.h"
+#include "math.h"
+
+//definition des sorties PWM
+FastPWM PWM_avant_droit(D11);           //IN1 mot Droit
+FastPWM PWM_arriere_droit(D12);        //IN2 mot Droit
+FastPWM PWM_avant_gauche(D10);          //IN1 mot Gauche
+FastPWM PWM_arriere_gauche(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};
+float gyro[3] = {0};
+
+// filtre ec
+float Te = 20e-3,Tems=Te*1000;
+
+float TauFC=0.25,
+      AFC = 1/(1+TauFC/Te),
+      BFC = TauFC/Te/(1+TauFC/Te);
+
+float tetaG; // variable correspondant à l'angle non filtré nommé tetaG
+float tetaGf ; // angle sortie du filtre passe bas
+float tetaOmegaf ; // angle en sortie du filtre passe haut
+float tetaGyro; // c'est l'angle teta obtenu en sommant les deux variables obtenues après filtrage tetaomegaf et tetagf
+float tetaCons = 0 ; // position angulaire initialisé à 0
+float Kp =0;     // Kp correcteur proportionnel de la mesure d'angle du gyro
+float KpUmot; // correcteur proportionnel de la consigne en tension du moteur avec la tension moteur mesuré filtré
+float Kd = 0; // correcteur dérivé
+float ConsUmot; // consigne de tension moteur
+float EpsilonTeta; // erreur mesuré de la position angulaire du gyro
+double Umot;
+double tetaOffs= 0.15;
+
+MPU6050 module(I2C_SDA, I2C_SCL);
+
+void acquisition()
+{
+
+    //récupération des données
+    module.getAccelero(accelero);
+    module.getGyro(gyro);
+
+    tetaG = atan2(accelero[1],accelero[0]);   // calcul de l'angle teta_g correspondant à l'arctan de la position  ax et az
+
+    tetaGf = AFC*tetaG + BFC*tetaGf ;     //fct de transfert
+    tetaOmegaf= AFC*(-TauFC*gyro[2]) + BFC*tetaOmegaf ;
+    tetaGyro = tetaOmegaf +  tetaGf;
+    tetaGyro= - tetaGyro+0.15;
+
+
+
+// Correcteur proportionnel pour Umot en sortie
+    EpsilonTeta = tetaCons - tetaGyro;
+    Umot= EpsilonTeta*Kp + Kd*gyro[2];
+
+    PWM_avant_droit.write(0.5+Umot);
+    PWM_arriere_droit.write(0.5-Umot);
+
+    PWM_avant_gauche.write(0.5+Umot);
+    PWM_arriere_gauche.write(0.5-Umot);
+
+    flag = 1;
+}
+
+
+
+// declaration d'un objet RtosTimmer
+RtosTimer timer(mbed::callback(acquisition),osTimerPeriodic);
+
+
+
+void reception(char ch)
+{
+    static  int i=0;
+    static char chaine[10];
+    char commande[3];
+    char valeur[6];
+
+
+    if ((ch==13) or (ch==10)) {
+
+        chaine[i]='\0';
+
+
+        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);
+            AFC = 1/(1+TauFC/Te),
+            BFC = TauFC/Te/(1+TauFC/Te);
+            flag = 1;
+
+        } else if  (strcmp(commande,"Kp")==0){
+          Kp=atof(valeur);
+        
+        }else if  (strcmp(commande,"Kd")==0){
+          Kd=atof(valeur);
+        }
+        i = 0;
+
+
+    } else {
+        chaine[i]=ch;
+        i++;
+    }
+}
+
+int main()
+{
+    PWM_avant_droit.period_us(50);
+    PWM_arriere_droit.period_us(50);
+    PWM_avant_gauche.period_us(50);
+    PWM_arriere_gauche.period_us(50);
+
+
+
+
+
+    EnableGauche=1;
+    EnableDroit=1;
+
+    pc.printf("Gyro \n\r");
+    while (!module.testConnection()) {
+        pc.printf("not connected to mpu\n\r");
+        wait(1);
+    }
+
+    pc.printf("connected to MPU\n\r");
+
+    timer.start((int)Tems);
+    pc.printf("TimerStart %d \n\r",(int)Tems);
+
+    //changement du 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.4f %3.4f %3.4f %3.4f\t\n\r",tetaG,tetaGf, tetaOmegaf, tetaGyro);
+
+            flag = 0;
+        }
+    }
+}
+
+