Diff: main.cpp

Revision:

8:91eb7435c3e0

Parent:

7:09004b460bd1

Child:

9:eb3f9744ae5d

--- a/main.cpp	Tue Mar 12 11:36:23 2019 +0000
+++ b/main.cpp	Wed Mar 13 10:29:04 2019 +0000
@@ -31,10 +31,12 @@
 #define DECOUP_HACH 50 //us - 20 000 kHz pour les oreilles
 #define PERIODE_AFF 500 //ms
 #define PERIODE_ASSERV 50 //ms
+#define Te 50 //ms /!\ Te doit être égale à la période d'asservissement
 
 // Constantes Asservissement
 #define GAIN_POS 1
 #define GAIN_ANG 0
+#define Ti 200 //ms
 #define GAIN_POS_INT 0.1
 #define GAIN_ANG_INT 0.1
 #define ERREUR_POS 0.5
@@ -105,21 +107,26 @@
 
 Vect3 calcCommandeXYZ(Vect3 erreur){
     static Vect3 commande;
+    static Vect3 oldErreur;
     if (erreur.x > ERREUR_POS){
         commande.x = GAIN_POS*erreur.x;
     } else {
-        commande.x = GAIN_POS*(erreur.x + GAIN_POS_INT*commande.x);
+        commande.x = commande.x + GAIN_POS*(Ti+Te)/Te*erreur.x - GAIN_POS*oldErreur.x;
+        //commande.x = GAIN_POS*(erreur.x + GAIN_POS_INT*commande.x);
     }
     if (erreur.y > ERREUR_POS){
         commande.y = GAIN_POS*erreur.y;
     } else {
-        commande.y = GAIN_POS*(erreur.y + GAIN_POS_INT*commande.y);
+        commande.y = commande.y + GAIN_POS*(Ti+Te)/Te*erreur.y - GAIN_POS*oldErreur.y;
+        //commande.y = GAIN_POS*(erreur.y + GAIN_POS_INT*commande.y);
     }
     if (erreur.z > ERREUR_ANG){
         commande.z = GAIN_ANG*erreur.z;
     } else {
-        commande.z = GAIN_ANG*(erreur.z + GAIN_ANG_INT*commande.z);
+        commande.z = commande.z + GAIN_ANG*(Ti+Te)/Te*erreur.z - GAIN_ANG*oldErreur.z;
+        //commande.z = GAIN_ANG*(erreur.z + GAIN_ANG_INT*commande.z);
     }
+    oldErreur = erreur;
     return commande;
 }