desp koval
/
Mecatro_Filtre
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Dependencies: mbed MPU6050 mbed-rtos McatroAntoineAndrii
Embed:
(wiki syntax)
Show/hide line numbers
mecafiltre.cpp
00001 #include "mbed.h" 00002 #include "rtos.h" 00003 #include "MPU6050.h" 00004 #include "FastPWM.h" 00005 00006 //****************************************************************************************** 00007 // déclaration des objets 00008 //****************************************************************************************** 00009 00010 Serial pc(USBTX, USBRX,115200); 00011 Serial hc(D1, D0,115200); 00012 DigitalOut led(LED1); 00013 MPU6050 module(D4, D5); 00014 AnalogIn tensBat(A4); 00015 00016 //****************************************************************************************** 00017 // initialisation des broche PWM en sortie 00018 //****************************************************************************************** 00019 PwmOut PWMGP(D11); // moteur gauche 00020 PwmOut PWMGM(D12); // moteur gauche 00021 PwmOut PWMDP(D10); // moteur Droit 00022 PwmOut PWMDM(D9); // moteur Droit 00023 00024 // valeur de Te 00025 float tems = 10; 00026 00027 //******************************************************************************************\ 00028 // variable necessaire à la mesure de la position angulaire du gyropode \ 00029 //******************************************************************************************/ 00030 00031 float accelero[3]= {0}; 00032 float gyro[3] = {0}; 00033 float tauFC = 0.285; // constante de temps en secondes du filtre passe bas du filtre complémentaire 00034 float aFC, bFC; // les coefficient du filter passe bas du filtre complémentaire 00035 float angleATG,angleNF, angleGyropodeF,omegarGyropode, angleGyropodeFp; 00036 float angleOffset = 0.07; 00037 00038 //******************************************************************************************\ 00039 // variable correcteur dérivé et proportionelle \ 00040 //******************************************************************************************/ 00041 00042 float K=3.8, alpha,alphaR; // correcteur et consigne 00043 float KD=0.06 ;//correcteur dérivé 00044 00045 //******************************************************************************************\ 00046 // Variable asservi de vitesse \ 00047 //******************************************************************************************/ 00048 00049 float PBv,Pbfc, Tfv=0.345; //coeff passe bas asserv vitesse 00050 float Err,PCsAng=0,PCsAngF; // erreur de vitesse 00051 float alphaCons = 0; // initialisation à 0 de la consigne 00052 float Kv=2.23; // initialisation du ceoff propotionelle dérivé 00053 float alphaF, alphaP; // angle alpha Filtrée et précédent pour filtre récursif 00054 float Kdv=0.035; 00055 float Edrrp; 00056 float deriv ; 00057 char receptionHC; 00058 00059 int nbInc=0; 00060 volatile int flagInterupt=0; 00061 00062 void reception(char ch) 00063 { 00064 static int i=0; // indice du dernier caratère recu 00065 static char chaine[10]; // chaine de stockage des caratères recus 00066 char commande[3]; // chaine contenant la commande 00067 char valeur[6]; // chaine contenant la valeur 00068 00069 if ((ch==13) or (ch==10)) { 00070 00071 chaine[i]='\0'; 00072 00073 // séparation de la commande et de la valeur 00074 strcpy(commande, strtok(chaine, " ")); 00075 strcpy(valeur, strtok(NULL, " ")); 00076 00077 00078 if (strcmp( commande, "TFC" ) == 0) { // commande modification de constante du filtre 00079 tauFC = atof(valeur); 00080 aFC=(float)1/(1+tauFC/(tems/1000)); 00081 bFC=tauFC/(tems/1000); 00082 } 00083 if (strcmp( commande, "K" ) == 0) { // commande modification du correcteur proportionelle 00084 K = atof(valeur); 00085 } 00086 00087 if (strcmp( commande, "KD" ) == 0) { // commande modification du correcteur Dérivé 00088 KD = atof(valeur); 00089 } 00090 if (strcmp( commande, "Kv" ) == 0) { // commande modification du correcteur de vitesse 00091 Kv = atof(valeur); 00092 } 00093 if (strcmp( commande, "Tfv" ) == 0) { // commande modification de constante du filtre 00094 Tfv = atof(valeur); 00095 PBv = (float)1/(1+Tfv/(tems/1000)); 00096 Pbfc=Tfv/(tems/1000); 00097 } 00098 if (strcmp( commande, "Kdv" ) == 0) { // commande modification de constante du filtre 00099 Kdv= atof(valeur); 00100 } 00101 00102 // reinitialisation de l indice de chaine 00103 i = 0; 00104 00105 } else { 00106 chaine[i]=ch; 00107 i++; 00108 } 00109 } 00110 00111 void interupt() 00112 { 00113 nbInc++; 00114 00115 //lecture des données de l'accéléro et du gyro 00116 module.getAccelero(accelero); 00117 module.getGyro(gyro); 00118 00119 //******************************************************************************************\ 00120 // Mesure des positions angulaire \ 00121 //******************************************************************************************/ 00122 00123 angleATG = -atan2(accelero[1],accelero[0]); // angle du gyropode calcule par la tengente 00124 omegarGyropode = gyro[2]; 00125 angleNF = (angleATG + tauFC * omegarGyropode)-angleOffset; // Angle non filtré 00126 angleGyropodeF = aFC*( angleNF + bFC * angleGyropodeFp); // Angle filtré 00127 angleGyropodeFp = angleGyropodeF; // memorisation des valeurs précedante pour les filtres recursifs de l'angle gyropode 00128 00129 00130 00131 00132 //******************************************************************************************\ 00133 // Asservissement de vitesse \ 00134 //******************************************************************************************/ 00135 00136 Err= alphaCons - alphaF; // calcule de l'erreur 00137 PCsAng = -Kv*Err ; // calcul de l'angle de consigne avec err de vitesse 00138 deriv =((Err - Edrrp)/(tems/1000)); // calcule de la dérivé afin de stabiliser 00139 PCsAngF = PCsAng + Kdv* deriv; // calcul de l'angle Filtre avec action dériver sur entrée 00140 Edrrp = Err; // Valeur précédente de l'erreur 00141 00142 00143 // calcul de la grandeur de commande 00144 alpha = K*(PCsAngF- angleGyropodeF )- KD*(omegarGyropode); // calcul de la sortie prop dérivé 00145 00146 00147 //Mise en place de la saturation 00148 if (alpha>0.5) alpha=0.5; 00149 if (alpha<-0.5) alpha=-0.5; 00150 00151 00152 //******************************************************************************************\ 00153 // filtre récursif angle \ 00154 //******************************************************************************************/ 00155 00156 alphaF = PBv*(alpha + Pbfc * alphaP ) ;// calcul filtre récursif 00157 alphaP=alphaF; // valeurs précédente de l'angle alpha filtré 00158 00159 00160 //******************************************************************************************\ 00161 // correction couple résistant \ 00162 /******************************************************************************************/ 00163 00164 if (alpha>0) alphaR=alpha+0.15; 00165 if (alpha<0) alphaR=alpha-0.15; 00166 00167 00168 //******************************************************************************************\ 00169 // Sortie PWM moteur \ 00170 //******************************************************************************************/ 00171 PWMGP.write(0.5-alphaR); 00172 PWMGM.write(0.5+alphaR); 00173 PWMDP.write(0.5-alphaR); 00174 PWMDM.write(0.5+alphaR); 00175 00176 flagInterupt=1; 00177 } 00178 00179 RtosTimer timer(mbed::callback(interupt),osTimerPeriodic); 00180 00181 int main(){ 00182 00183 //******************************************************************************************\ 00184 // initialisation des période PWM \ 00185 //******************************************************************************************/ 00186 PWMDM.period_us(50); 00187 PWMDP.period_us(50); 00188 PWMGM.period_us(50); 00189 PWMGP.period_us(50); 00190 00191 // initialisation et test de connection du MPU6050 00192 int test; 00193 while ((test = module.testConnection())!=0x98) { 00194 pc.printf("not connected to mpu %02X\n\r", test); 00195 wait(1); 00196 } 00197 pc.printf("connected to mpu %02X\n\r", test); 00198 00199 //changement du l'echelle du module 00200 module.setGyroRange(MPU6050_GYRO_RANGE_2000); 00201 module.setAcceleroRange(MPU6050_ACCELERO_RANGE_2G); 00202 00203 00204 //******************************************************************************************\ 00205 // calcul des coefficients des filtres \ 00206 //******************************************************************************************/ 00207 00208 aFC=(float)1/(1+tauFC/(tems/1000)); // coeff correcteur dérivé premier 00209 bFC=tauFC/(tems/1000); 00210 PBv = (float)1/(1+Tfv/(tems/1000)); // Coeff coorrecteur dérivé 00211 Pbfc=Tfv/(tems/1000); 00212 00213 //******************************************************************************************\ 00214 // initialisation de la la tache periodique du noyau multitache \ 00215 //******************************************************************************************/ 00216 00217 timer.start(tems); 00218 00219 while(1) { 00220 00221 //******************************************************************************************\ 00222 // Contrôle du gyropode via le bluetooth \ 00223 //******************************************************************************************/ 00224 00225 if (hc.readable()!=0) { 00226 receptionHC=hc.getc(); 00227 00228 if(receptionHC=='A'){ 00229 pc.printf("Avancer"); 00230 alphaCons=0.11; 00231 } 00232 if(receptionHC=='R'){ 00233 pc.printf("Reculer"); 00234 alphaCons=-0.11; 00235 } 00236 else if (receptionHC=='S'){ 00237 pc.printf("Stable"); 00238 alphaCons=0; 00239 PWMGP.write(0.5-alphaR); 00240 PWMGM.write(0.5+alphaR); 00241 PWMDP.write(0.5-alphaR); 00242 PWMDM.write(0.5+alphaR); 00243 } 00244 else if (receptionHC=='D'){ 00245 pc.printf("Droite"); 00246 PWMGP.write(0.5+alphaR+0.15); 00247 PWMGM.write(0.5-alphaR-0.15); 00248 PWMDP.write(0.5-alphaR-0.15); 00249 PWMDM.write(0.5+alphaR+0.15); 00250 } 00251 else if (receptionHC=='G'){ 00252 pc.printf("Gauche"); 00253 PWMGP.write(0.5-alphaR-0.15); 00254 PWMGM.write(0.5+alphaR+0.15); 00255 PWMDP.write(0.5+alphaR+0.15); 00256 PWMDM.write(0.5-alphaR-0.15); 00257 } 00258 } 00259 00260 if (pc.readable()!=0) { 00261 reception(pc.getc()); 00262 } 00263 if (flagInterupt==1) { 00264 // codeurs.read(g,d);} 00265 pc.printf("%d %d %6.3f %6.3f \r\n",0,0,0,tensBat.read()); 00266 flagInterupt=0; 00267 00268 } 00269 } 00270 } 00271 00272 00273 00274 00275
Generated on Fri Aug 12 2022 02:56:50 by
1.7.2