Diff: main.cpp

Revision:

1:b44f69eb07c4

Parent:

0:0d257bbf5c05

Child:

2:7de884ffc9d9

--- a/main.cpp	Thu Mar 11 08:00:49 2021 +0000
+++ b/main.cpp	Thu Apr 15 06:44:26 2021 +0000
@@ -1,9 +1,14 @@
 #include "mbed.h"
 #include "rtos.h"
 #include "MPU6050.h"
+#include "FastPWM.h"
+#include "codeurs.h"
+
+int dato=0;
 
 // déclaration des objets
-Serial pc(USBTX, USBRX);
+Serial pc(USBTX, USBRX,115200);
+Serial bluetooth(PA_9, PA_10,38400); //PINS TO CONECT. PA_9 WITH RX PIN HC-06
 DigitalOut led(LED1);
 MPU6050 module(I2C_SDA, I2C_SCL);
 
@@ -13,15 +18,43 @@
 // 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;
+float tauFC = 0.2;                 // 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,angleGyroConsigne;
+float consigneAngle = -0.001;
+float CommandeVitesse = 0;
+
+// varailble necessire à l'asserv angulaire et vitesse
+float offsetConsigneAngle = 0;
+float KPV = 0.026;
+float KDA = 0.08;
+float KPA =2.3;
+float KDV = 0.01;
+float erreurVitesse, offsetConsigneAngle_p;
 
+// valeur du filtre passe bas
+float ec_f, ec_fp;
+float Te = 0.0002;
+float ec,ecCorrige;                       // grandeur de commande
+float ecVir;
 
+// Codeurs
+Codeurs codeurs;
+int g, d, vitesseCodeur1, g_p, vitesseCodeur2, d_p;
+float  moyVitesse_p, moyVitesse;
+
+//sorties moteur
+FastPWM M1_1(D9);
+FastPWM M1_2(D10);
+FastPWM M2_1(D11);
+FastPWM M2_2(D12);
 
 int nbInc=0;
 char flagInterupt=0;
 
+
+//reception
+
 void reception(char ch)
 {
     static  int i=0;                // inice du dernier caratère recu
@@ -37,9 +70,25 @@
         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);
+        
+        if (strcmp( commande, "KPV" ) == 0) {
+            KPV=atof(valeur);
+        }
+        if (strcmp( commande, "CA" ) == 0) {
+            consigneAngle=atof(valeur);
+        }
+        if (strcmp( commande, "KDA" ) == 0) {
+            KDA=atof(valeur);
+        }
+        if (strcmp( commande, "KPA" ) == 0) {
+            KPA=atof(valeur);
+        }
+        if (strcmp( commande, "CV" ) == 0) {
+            CommandeVitesse=atof(valeur);
+        }
+        if (strcmp( commande, "KDV" ) == 0) {
+            KDV=atof(valeur);
+        }
 
         // reinitialisation de l indice de chaine
         i = 0;
@@ -52,24 +101,89 @@
 
 void interupt()
 {
+    codeurs.read(g, d);
+
     nbInc++;
-//lecture des données de l'accéléro et du gyro
+
+
+
+
+    /**********************************************************************************************/
+    /*                     Estimation position angulaire                                          */
+    /**********************************************************************************************/
+
+    // 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];
-    
+    module.getGyro(gyro);
+
+    // calcul pos angulaire du gyropode à partir de l'arc tangeante des accelerations
+    angleATG = atan2(accelero[1],accelero[0]);
+
+
+    // Calcul de la sortie du filtre complementaire
+    omegarGyro = -gyro[2];
     angleNF = angleATG + tauFC * omegarGyro;
-    angleGyro = aFC*( angleNF + bFC * angleGyrop);
+    angleGyro =  aFC*( angleNF + bFC * angleGyrop);
+
+    // Memorisation des valeurs precedante pour les filtres recursifs
+    angleGyrop = angleGyro;
+
     
+    /**********************************************************************************************/
+    /*                     Asserv postion angulaire gyropode                                      */
+    /**********************************************************************************************/
     
-    
+
+    // Definir la consigne de l'asservissement de la pos angulaire
+    angleGyroConsigne = consigneAngle+offsetConsigneAngle;              // consigneAngle est la grandeur de commande issue de l'asservissement de l image de la vitesse
+
+ 
+    // Calcul de la grandeur de commande de l'asservissement des pos angulaire
+    ec= KPA*(angleGyroConsigne-angleGyro)-KDA*omegarGyro;
+
     
-// memorisation des valeurs precedante pour les filtres recursifs
-    angleGyrop = angleGyro ;
-    
-    
+
+    /**********************************************************************************************/
+    /*                     Asserv Vitesse                                                         */
+    /**********************************************************************************************/
+
+
+    // Correcteur proportionel KPV
+    erreurVitesse = CommandeVitesse-moyVitesse;
+
+    // Asservissement de vitesse
+    offsetConsigneAngle = (KPV*erreurVitesse)+(KDV*(moyVitesse - moyVitesse_p));
+    moyVitesse_p = moyVitesse;
+
+    // Calcul vitesse codeur
+    vitesseCodeur1 = -g_p+g;
+    g_p = g;
+    vitesseCodeur2 = d_p-d;
+    d_p = d;
+    moyVitesse = (vitesseCodeur1+vitesseCodeur2)/2;
+    moyVitesse = moyVitesse/10;
+
+
+    /**********************************************************************************************/
+    /*                     Commande Moteurs                                                       */
+    /**********************************************************************************************/
+
+
+    // Compenssation frottement sec
+    if (ec<0)ecCorrige=ec-0.15;
+    else if (ec>0)ecCorrige=ec+0.15;
+    else ecCorrige = 0;
+
+    // Saturation
+    if (ecCorrige<-0.5)ecCorrige=-0.5;
+    if (ecCorrige>0.5)ecCorrige=0.5;
+
+    // Sorties PWM Moteurs
+    M1_1.write(0.5+(ecCorrige+ecVir));
+    M1_2.write(0.5-(ecCorrige+ecVir));
+    M2_1.write(0.5-(ecCorrige-ecVir));
+    M2_2.write(0.5+(ecCorrige-ecVir));
+
     flagInterupt=1;
 }
 
@@ -77,12 +191,30 @@
 
 int main()
 {
+
     pc.printf("Test unitaire mecatro \n\r");
-    
+
+    M1_1.period_us(50);
+    M1_2.period_us(50);
+    M2_1.period_us(50);
+    M2_2.period_us(50);
+    M1_1.write(0.5);
+    M1_2.write(0.5);
+    M2_1.write(0.5);
+    M2_2.write(0.5);
+
+    // Test connection codeurs
+    pc.printf("Test des codeurs\r\n");
+    while (!codeurs.test()) {
+        pc.printf("Codeurs non connectes\r\n");
+        wait(1);
+    }
+
+    pc.printf("Codeurs ok\r\n");
+    codeurs.reset();
 
 
-
-// initialisation et test de connection du MPU6050
+    // initialisation et test de connection du MPU6050
     while (module.testConnection()==0) {
         pc.printf("not connected to mpu\n\r");
         wait(1);
@@ -91,22 +223,63 @@
     module.setGyroRange(MPU6050_GYRO_RANGE_2000);
     module.setAcceleroRange(MPU6050_ACCELERO_RANGE_2G);
 
-// calcul des coefficients des filtres
+    // calcul des coefficients des filtres 
     aFC=(float)1/(1+tauFC/(tems/1000));
     bFC=tauFC/(tems/1000);
 
-
-    
-//   initialisation de la latache periodique du noyau multitache
+    //  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]);
+            pc.printf("%4.3f %4.3f %4.3f %4.3f \n\r",angleGyro,moyVitesse,offsetConsigneAngle,ec);
             flagInterupt=0;
         }
+        
+        if(bluetooth.readable()) {
+            dato=bluetooth.getc();
+            pc.putc(dato);
+        }
+        if(pc.readable()) {
+            dato=pc.getc();
+            bluetooth.putc(dato);
+        }
+        
+        // Controle valeur de la commande via HC06
+        if(dato == 'A') {
+            CommandeVitesse = 3;
+        }
+        else if(dato == 'R') {
+            CommandeVitesse = -3;
+        }
+        else if((dato == 'a') || (dato == 'r')) {
+            CommandeVitesse = 0;
+        }
+        
+        else if((dato == 'g') || (dato == 'd')) {
+            ecVir = 0;
+        }
+        else if(dato == 'G') {
+            ecVir = -0.1;
+        }
+        else if(dato == 'D') {
+            ecVir = 0.1;
+        }
+        else if(dato == 'o') {
+            KPV =0;
+            KDA =0;
+            KPA =0;
+            KDV =0;
+            consigneAngle =0;
+        }
+        else if(dato == 'O') {
+            KPV = 0.026;
+            KDA = 0.08;
+            KPA =2.3;
+            KDV = 0.01;
+            consigneAngle = -0.001;
+        }
+
     }
 }
\ No newline at end of file