Revision 1:111167e39dfa, committed 2022-03-31

Comitter:

flo__

Date:

Thu Mar 31 10:44:47 2022 +0000

Parent:

0:b435eadf76b4

Commit message:

31/03/22

Changed in this revision

--- a/main.cpp	Mon Mar 28 15:54:19 2022 +0000
+++ b/main.cpp	Thu Mar 31 10:44:47 2022 +0000
@@ -30,21 +30,27 @@
 
 //création de variable
 double angleErreur,alpha;
+double angleConsigne;
 
-double erreurUmot, Umot=0;
+double erreurUmot=0, erreurUmotPrec=0, Umot=0, Uc=0;
 
 //création de la constante d'échantillonage "Te" en [s] et Tems en [ms]
 float Te = 20e-3,Tems=Te*1000;
 
 //création de la constante de temps "TauFC" de nos filtre
-float TauFC=0.25;
+float TauFC=0.25,
+      TauFC2=0.25;
 
 //caractéristique de la boucle d'asservissement
-double kp=2, kd=0, k2=0, offset=0;
+double kp=4, kd=0.136, kp2=0.576, kd2=0.561, offset=0;
 
 //création du coef "AFC" et "BFC" que l'on applique respectivement sur l'entrée et la sortie
 float AFC = 1/(1+TauFC/Te),
       BFC = TauFC/Te/(1+TauFC/Te);
+      
+//création du coef "AFC2" et "BFC2" que l'on applique respectivement sur l'entrée et la sortie
+float AFC2 = 1/(1+TauFC2/Te),
+      BFC2 = TauFC2/Te/(1+TauFC2/Te);
 
 //création de la vaiable qui stocke la valeur de l'angle non filtré grâce et fitré à l'accéléromètre
 float angle_nf=0;
@@ -76,12 +82,14 @@
 
     //calcul finale de l'angle d'inclinaison du gyropode
     angleMesureFiltre =angle_f+(angle_f_HF);
-    
-    
-    
+
+    //asservssment en translation
+    erreurUmot = -(Uc - Umot);
+
     //asservissment angle
-    angleErreur= 0 - (angleMesureFiltre + offset);
-    alpha = kp*angleErreur + kd*gyro[2] +k2*erreurUmot;    //gyro[2] = vitesse angulaire y
+    angleConsigne = kp2*erreurUmot + (erreurUmot-erreurUmotPrec)*kd2;
+    angleErreur= angleConsigne - (angleMesureFiltre + offset);
+    alpha = kp*angleErreur + kd*gyro[2] ;    //gyro[2] = vitesse angulaire y
 
     if(alpha>0.5) alpha=0.5;
     if(alpha<-0.5) alpha=-0.5;
@@ -93,10 +101,10 @@
     PWM2Gauche.write(0.5+alpha);
 
     //calcul fltrage vitesse Umot(attention fréquence de coupure à modifier)
-    Umot = AFC * alpha + BFC * Umot;
+    Umot = AFC2 * alpha + BFC2 * Umot;
+
     
-    //asservssment en translation
-    erreurUmot = 0 - Umot;
+    erreurUmotPrec=erreurUmot;
     
     //on leve le drapeau
     flag = 1;
@@ -143,10 +151,13 @@
         }
         if (strcmp(commande,"kp")==0) {
             kp = atof(valeur);
-        } 
-        if (strcmp(commande,"k2")==0) {
-            k2 = atof(valeur);
-        }else {
+        }
+        if (strcmp(commande,"kp2")==0) {
+            kp2 = atof(valeur);
+        }
+        if (strcmp(commande,"kd2")==0) {
+            kd2 = atof(valeur);
+        } else {
             //la commande n'est pas reconnu
             pc.printf("commande inconnue \r\n");
         }
@@ -164,7 +175,7 @@
 int main()
 {
 
-    
+
 
     pc.printf("Le Gyro est allume\n\r");