Diff: main.cpp

Revision:

0:0d257bbf5c05

Child:

1:b44f69eb07c4

diff -r 000000000000 -r 0d257bbf5c05 main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Thu Mar 11 08:00:49 2021 +0000
@@ -0,0 +1,112 @@
+#include "mbed.h"
+#include "rtos.h"
+#include "MPU6050.h"
+
+// déclaration des objets
+Serial pc(USBTX, USBRX);
+DigitalOut led(LED1);
+MPU6050 module(I2C_SDA, I2C_SCL);
+
+// valeur de Te
+float tems = 10;
+
+// varailble necessire à la mesure de la position angulaire du gyropode
+float accelero[3]= {0};
+float gyro[3] = {0};
+float tauFC = 1;        // constante de tyemps en seconde du filtre passe bas du filtre complémentaire
+float aFC, bFC;         // les coefficient du filter passe bas du filtre complémentaire
+float angleATG ,angleNF, angleGyro ,omegarGyro, angleGyrop;
+
+
+
+int nbInc=0;
+char flagInterupt=0;
+
+void reception(char ch)
+{
+    static  int i=0;                // inice du dernier caratère recu
+    static  char chaine[10];        // chaine de stockage des caratères recus
+    char commande[3];               // chaine contenant la commande
+    char valeur[6];                 // chaine contenant la valeur
+
+    if ((ch==13) or (ch==10)) {
+
+        chaine[i]='\0';
+
+        // séparation de la commande et de la valeur
+        strcpy(commande, strtok(chaine, " "));
+        strcpy(valeur,  strtok(NULL, " "));
+
+        // affichage de commande et valeur
+        pc.printf("Commande %s \n\r",commande);
+        pc.printf("Valeur   %s \n\r",valeur);
+
+        // reinitialisation de l indice de chaine
+        i = 0;
+
+    } else {
+        chaine[i]=ch;
+        i++;
+    }
+}
+
+void interupt()
+{
+    nbInc++;
+//lecture des données de l'accéléro et du gyro
+    module.getAccelero(accelero);
+    module.getGyro(gyro);  
+//  
+    angleATG = atan2(accelero[0],accelero[1]);
+    omegarGyro = gyro[3];
+    
+    angleNF = angleATG + tauFC * omegarGyro;
+    angleGyro = aFC*( angleNF + bFC * angleGyrop);
+    
+    
+    
+    
+// memorisation des valeurs precedante pour les filtres recursifs
+    angleGyrop = angleGyro ;
+    
+    
+    flagInterupt=1;
+}
+
+RtosTimer timer(mbed::callback(interupt),osTimerPeriodic);
+
+int main()
+{
+    pc.printf("Test unitaire mecatro \n\r");
+    
+
+
+
+// initialisation et test de connection du MPU6050
+    while (module.testConnection()==0) {
+        pc.printf("not connected to mpu\n\r");
+        wait(1);
+    }
+    //changement du l'echelle du module
+    module.setGyroRange(MPU6050_GYRO_RANGE_2000);
+    module.setAcceleroRange(MPU6050_ACCELERO_RANGE_2G);
+
+// calcul des coefficients des filtres
+    aFC=(float)1/(1+tauFC/(tems/1000));
+    bFC=tauFC/(tems/1000);
+
+
+    
+//   initialisation de la latache periodique du noyau multitache
+    timer.start(tems);
+
+    while(1) {
+        if (pc.readable()!=0) {
+            reception(pc.getc());
+        }
+        if (flagInterupt==1) {
+            pc.printf("Ax = %3.2f, Ay = %3.2f, Az = %3.2f Wx = %3.2f, wy = %3.2f, wz = %3.2f \n\r",accelero[0],accelero[1],accelero[2],gyro[0],gyro[1],gyro[2]);
+            flagInterupt=0;
+        }
+    }
+}
\ No newline at end of file