main.cpp

00001 #include "mbed.h"
00002 #include "rtos.h"
00003 #include "MPU6050.h"
00004 #include "FastPWM.h"
00005 #include "codeurs.h"
00006 
00007 int dato=0;
00008 
00009 // déclaration des objets
00010 Serial pc(USBTX, USBRX,115200);
00011 Serial bluetooth(PA_9, PA_10,38400); //PINS TO CONECT. PA_9 WITH RX PIN HC-06
00012 DigitalOut led(LED1);
00013 MPU6050 module(I2C_SDA, I2C_SCL);
00014 
00015 // valeur de Te
00016 float tems = 10;
00017 
00018 // variable asserv position
00019 float erreurPosition, erreurPosition_p;
00020 float consignePosition = 0;
00021 float KPP = 0.1, KDP = 0.1;
00022 
00023 // Variable asserv position Rotation
00024 float erreurRota, erreurRota_p;
00025 float consigneRota = 0;
00026 float KPR = 0.001, KDR = 0.01;
00027 
00028 // varailble necessire à la mesure de la position angulaire du gyropode
00029 float accelero[3]= {0};
00030 float gyro[3] = {0};
00031 float tauFC = 0.2;                 // constante de tyemps en seconde du filtre passe bas du filtre complémentaire
00032 float aFC, bFC;                    // les coefficient du filter passe bas du filtre complémentaire
00033 float angleATG,angleNF, angleGyro,omegarGyro, angleGyrop,angleGyroConsigne;
00034 float consigneAngle = -0.001;
00035 float CommandeVitesse = 0;
00036 
00037 // varailble necessire à l'asserv angulaire et vitesse
00038 float offsetConsigneAngle = 0;
00039 float KPV = 0.026;
00040 float KDA = 0.08;
00041 float KPA =2.3;
00042 float KDV = 0.01;
00043 float erreurVitesse, offsetConsigneAngle_p;
00044 
00045 // valeur du filtre passe bas
00046 float ec_f, ec_fp;
00047 float Te = 0.0002;
00048 float ec,ecCorrige;                       // grandeur de commande
00049 float ecVir;
00050 
00051 // Codeurs
00052 Codeurs codeurs;
00053 int g, d, vitesseCodeur1, g_p, vitesseCodeur2, d_p;
00054 float  moyVitesse_p, moyVitesse;
00055 
00056 //sorties moteur
00057 FastPWM M1_1(D9);
00058 FastPWM M1_2(D10);
00059 FastPWM M2_1(D11);
00060 FastPWM M2_2(D12);
00061 
00062 int nbInc=0;
00063 char flagInterupt=0;
00064 
00065 
00066 //reception
00067 
00068 void reception(char ch)
00069 {
00070     static  int i=0;                // inice du dernier caratère recu
00071     static  char chaine[10];        // chaine de stockage des caratères recus
00072     char commande[3];               // chaine contenant la commande
00073     char valeur[6];                 // chaine contenant la valeur
00074 
00075     if ((ch==13) or (ch==10)) {
00076 
00077         chaine[i]='\0';
00078 
00079         // séparation de la commande et de la valeur
00080         strcpy(commande, strtok(chaine, " "));
00081         strcpy(valeur,  strtok(NULL, " "));
00082 
00083         
00084         if (strcmp( commande, "KPV" ) == 0) {
00085             KPV=atof(valeur);
00086         }
00087         if (strcmp( commande, "CA" ) == 0) {
00088             consigneAngle=atof(valeur);
00089         }
00090         if (strcmp( commande, "KDA" ) == 0) {
00091             KDA=atof(valeur);
00092         }
00093         if (strcmp( commande, "KPA" ) == 0) {
00094             KPA=atof(valeur);
00095         }
00096         if (strcmp( commande, "CV" ) == 0) {
00097             CommandeVitesse=atof(valeur);
00098         }
00099         if (strcmp( commande, "KDV" ) == 0) {
00100             KDV=atof(valeur);
00101         }
00102         if (strcmp( commande, "KDP" ) == 0) {
00103             KDP=atof(valeur);
00104         }
00105         if (strcmp( commande, "KPP" ) == 0) {
00106             KPP=atof(valeur);
00107         }
00108 
00109         // reinitialisation de l indice de chaine
00110         i = 0;
00111 
00112     } else {
00113         chaine[i]=ch;
00114         i++;
00115     }
00116 }
00117 
00118 void interupt()
00119 {
00120     codeurs.read(g, d);
00121 
00122     nbInc++;
00123 
00124 
00125 
00126 
00127     /**********************************************************************************************/
00128     /*                     Estimation position angulaire                                          */
00129     /**********************************************************************************************/
00130 
00131     // Lecture des données de l'accéléro et du gyro
00132     module.getAccelero(accelero);
00133     module.getGyro(gyro);
00134 
00135     // calcul pos angulaire du gyropode à partir de l'arc tangeante des accelerations
00136     angleATG = atan2(accelero[1],accelero[0]);
00137 
00138 
00139     // Calcul de la sortie du filtre complementaire
00140     omegarGyro = -gyro[2];
00141     angleNF = angleATG + tauFC * omegarGyro;
00142     angleGyro =  aFC*( angleNF + bFC * angleGyrop);
00143 
00144     // Memorisation des valeurs precedante pour les filtres recursifs
00145     angleGyrop = angleGyro;
00146 
00147     
00148     /**********************************************************************************************/
00149     /*                     Asserv postion angulaire gyropode                                      */
00150     /**********************************************************************************************/
00151     
00152 
00153     // Definir la consigne de l'asservissement de la pos angulaire
00154     angleGyroConsigne = consigneAngle+offsetConsigneAngle;              // consigneAngle est la grandeur de commande issue de l'asservissement de l image de la vitesse
00155 
00156  
00157     // Calcul de la grandeur de commande de l'asservissement des pos angulaire
00158     ec= KPA*(angleGyroConsigne-angleGyro)-KDA*omegarGyro;
00159 
00160     
00161 
00162     /**********************************************************************************************/
00163     /*                     Asserv Position                                                        */
00164     /**********************************************************************************************/
00165     
00166     erreurPosition = consignePosition - (g-d);
00167     CommandeVitesse = KPP * erreurPosition + KDP * (erreurPosition-erreurPosition_p);
00168     erreurPosition_p = erreurPosition;
00169     if (CommandeVitesse>3) CommandeVitesse = 3;
00170     else if (CommandeVitesse < -3) CommandeVitesse = -3;
00171  
00172     erreurRota = consigneRota - (g-d);
00173     ecVir = KPR * erreurRota + KDR * (erreurRota-erreurRota_p);
00174     erreurRota_p = erreurRota;    
00175     if (ecVir>0.1) ecVir = 0.1;
00176     else if (ecVir < -0.1) ecVir = -0.1;
00177     
00178     
00179     
00180     /**********************************************************************************************/
00181     /*                     Asserv Vitesse                                                         */
00182     /**********************************************************************************************/
00183 
00184 
00185     // Correcteur proportionel KPV
00186     erreurVitesse = CommandeVitesse-moyVitesse;
00187 
00188     // Asservissement de vitesse
00189     offsetConsigneAngle = (KPV*erreurVitesse)+(KDV*(moyVitesse - moyVitesse_p));
00190     moyVitesse_p = moyVitesse;
00191 
00192     // Calcul vitesse codeur
00193     vitesseCodeur1 = -g_p+g;
00194     g_p = g;
00195     vitesseCodeur2 = d_p-d;
00196     d_p = d;
00197     moyVitesse = (vitesseCodeur1+vitesseCodeur2)/2;
00198     moyVitesse = moyVitesse/10;
00199 
00200 
00201     /**********************************************************************************************/
00202     /*                     Commande Moteurs                                                       */
00203     /**********************************************************************************************/
00204 
00205 
00206     // Compenssation frottement sec
00207     if (ec<0)ecCorrige=ec-0.15;
00208     else if (ec>0)ecCorrige=ec+0.15;
00209     else ecCorrige = 0;
00210 
00211     // Saturation
00212     if (ecCorrige<-0.5)ecCorrige=-0.5;
00213     if (ecCorrige>0.5)ecCorrige=0.5;
00214 
00215     // Sorties PWM Moteurs
00216     M1_1.write(0.5+(ecCorrige+ecVir));
00217     M1_2.write(0.5-(ecCorrige+ecVir));
00218     M2_1.write(0.5-(ecCorrige-ecVir));
00219     M2_2.write(0.5+(ecCorrige-ecVir));
00220 
00221     flagInterupt=1;
00222 }
00223 
00224 RtosTimer timer(mbed::callback(interupt),osTimerPeriodic);
00225 
00226 int main()
00227 {
00228 
00229     pc.printf("Test unitaire mecatro \n\r");
00230 
00231     M1_1.period_us(50);
00232     M1_2.period_us(50);
00233     M2_1.period_us(50);
00234     M2_2.period_us(50);
00235     M1_1.write(0.5);
00236     M1_2.write(0.5);
00237     M2_1.write(0.5);
00238     M2_2.write(0.5);
00239 
00240     // Test connection codeurs
00241     pc.printf("Test des codeurs\r\n");
00242     while (!codeurs.test()) {
00243         pc.printf("Codeurs non connectes\r\n");
00244         wait(1);
00245     }
00246 
00247     pc.printf("Codeurs ok\r\n");
00248     codeurs.reset();
00249 
00250 
00251     // initialisation et test de connection du MPU6050
00252     while (module.testConnection()==0) {
00253         pc.printf("not connected to mpu\n\r");
00254         wait(1);
00255     }
00256     //changement du l'echelle du module
00257     module.setGyroRange(MPU6050_GYRO_RANGE_2000);
00258     module.setAcceleroRange(MPU6050_ACCELERO_RANGE_2G);
00259 
00260     // calcul des coefficients des filtres 
00261     aFC=(float)1/(1+tauFC/(tems/1000));
00262     bFC=tauFC/(tems/1000);
00263 
00264     //  initialisation de la latache periodique du noyau multitache
00265     timer.start(tems);
00266 
00267     while(1) {
00268 
00269         if (flagInterupt==1) {
00270             pc.printf("%4.3f %4.3f %4.3f %4.3f \n\r",angleGyro,moyVitesse,offsetConsigneAngle,ec);
00271             flagInterupt=0;
00272         }
00273         
00274         if(bluetooth.readable()) {
00275             dato=bluetooth.getc();
00276             pc.putc(dato);
00277         }
00278         if(pc.readable()) {
00279             dato=pc.getc();
00280             bluetooth.putc(dato);
00281         }
00282         
00283         // Controle valeur de la commande via HC06
00284         if(dato == 'A') {
00285             CommandeVitesse = 3;
00286         }
00287         else if(dato == 'R') {
00288             CommandeVitesse = -3;
00289         }
00290         else if((dato == 'a') || (dato == 'r')) {
00291             CommandeVitesse = 0;
00292         }
00293         
00294         else if((dato == 'g') || (dato == 'd')) {
00295             ecVir = 0;
00296         }
00297         else if(dato == 'G') {
00298             ecVir = -0.1;
00299         }
00300         else if(dato == 'D') {
00301             ecVir = 0.1;
00302         }
00303         else if(dato == 'o') {
00304             KPV =0;
00305             KDA =0;
00306             KPA =0;
00307             KDV =0;
00308             consigneAngle =0;
00309         }
00310         else if(dato == 'O') {
00311             KPV = 0.026;
00312             KDA = 0.08;
00313             KPA =2.3;
00314             KDV = 0.01;
00315             consigneAngle = -0.001;
00316         }
00317 
00318     }
00319 }