Avion Radio IUT / Mbed 2 deprecated MecatroPWM

Dependencies:   mbed

Files at this revision

API Documentation at this revision

Comitter:
qmaker
Date:
Thu Apr 15 08:43:24 2021 +0000
Parent:
2:7de884ffc9d9
Commit message:
en cours de dev

Changed in this revision

main.cpp Show annotated file Show diff for this revision Revisions of this file
--- a/main.cpp	Thu Apr 15 07:24:07 2021 +0000
+++ b/main.cpp	Thu Apr 15 08:43:24 2021 +0000
@@ -16,8 +16,14 @@
 float tems = 10;
 
 // variable asserv position
-float erreurPosition, erreurPosition_p, consignePosition = 0;
-float KPP = 0.001;
+float erreurPosition, erreurPosition_p;
+float consignePosition = 0;
+float KPP = 0.1, KDP = 0.1;
+
+// Variable asserv position Rotation
+float erreurRota, erreurRota_p;
+float consigneRota = 0;
+float KPR = 0.001, KDR = 0.01;
 
 // varailble necessire à la mesure de la position angulaire du gyropode
 float accelero[3]= {0};
@@ -93,6 +99,12 @@
         if (strcmp( commande, "KDV" ) == 0) {
             KDV=atof(valeur);
         }
+        if (strcmp( commande, "KDP" ) == 0) {
+            KDP=atof(valeur);
+        }
+        if (strcmp( commande, "KPP" ) == 0) {
+            KPP=atof(valeur);
+        }
 
         // reinitialisation de l indice de chaine
         i = 0;
@@ -152,11 +164,16 @@
     /**********************************************************************************************/
     
     erreurPosition = consignePosition - (g-d);
-    CommandeVitesse = KPP * (erreurPosition-erreurPosition_p);
+    CommandeVitesse = KPP * erreurPosition + KDP * (erreurPosition-erreurPosition_p);
     erreurPosition_p = erreurPosition;
-    
     if (CommandeVitesse>3) CommandeVitesse = 3;
     else if (CommandeVitesse < -3) CommandeVitesse = -3;
+ 
+    erreurRota = consigneRota - (g-d);
+    ecVir = KPR * erreurRota + KDR * (erreurRota-erreurRota_p);
+    erreurRota_p = erreurRota;    
+    if (ecVir>0.1) ecVir = 0.1;
+    else if (ecVir < -0.1) ecVir = -0.1;