Revision 9:eb3f9744ae5d, committed 2019-03-16

Comitter:

natienza

Date:

Sat Mar 16 08:45:31 2019 +0000

Parent:

8:91eb7435c3e0

Commit message:

asserv test

Changed in this revision

--- a/TLE5206_lib.lib	Wed Mar 13 10:29:04 2019 +0000
+++ b/TLE5206_lib.lib	Sat Mar 16 08:45:31 2019 +0000
@@ -1,1 +1,1 @@
-https://os.mbed.com/users/gpongnot/code/TLE5206_lib/#03a724b7421c
+https://os.mbed.com/users/natienza/code/TLE5206_lib/#1117196d767b

--- a/main.cpp	Wed Mar 13 10:29:04 2019 +0000
+++ b/main.cpp	Sat Mar 16 08:45:31 2019 +0000
@@ -30,17 +30,17 @@
 
 #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
+#define PERIODE_ASSERV 25 //ms
+#define Te 25 //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 1.4
+#define GAIN_ANG 5
+#define Ti 5000000 //ms
 #define GAIN_POS_INT 0.1
 #define GAIN_ANG_INT 0.1
-#define ERREUR_POS 0.5
-#define ERREUR_ANG 0.5
+#define ERREUR_POS 0
+#define ERREUR_ANG 0
 
 Serial pc(USBTX,USBRX);
 Timer timer;
@@ -100,16 +100,18 @@
 Vect3 calcErreur(Vect3 consigne, Vect3 position){
     Vect3 erreur;
     erreur.x = position.x - consigne.x;
-    erreur.x = position.y - consigne.y;
-    erreur.x = position.z - consigne.z;
+    erreur.y = position.y - consigne.y;
+    erreur.z = position.z - consigne.z;
     return erreur;
 }
 
 Vect3 calcCommandeXYZ(Vect3 erreur){
+    /*
     static Vect3 commande;
     static Vect3 oldErreur;
     if (erreur.x > ERREUR_POS){
         commande.x = GAIN_POS*erreur.x;
+        oldErreur.x = 0;
     } else {
         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);
@@ -128,6 +130,32 @@
     }
     oldErreur = erreur;
     return commande;
+    */
+    static Vect3 integrateur = initVect3();
+    Vect3 commande;
+    if (abs(erreur.x) > ERREUR_POS){
+        integrateur.x = 0;
+    } else {
+        integrateur.x = integrateur.x + Te*erreur.x;
+    }
+    commande.x = GAIN_POS*(erreur.x + integrateur.x/Ti);
+    
+    if (abs(erreur.y) > ERREUR_POS){
+        integrateur.y = 0;
+    } else {
+        integrateur.y = integrateur.y + Te*erreur.y;
+    }
+    commande.y = GAIN_POS*(erreur.y + integrateur.y/Ti);
+    
+    if (abs(erreur.z) > ERREUR_ANG){
+        integrateur.z = 0;
+    } else {
+        integrateur.z = integrateur.z + Te*erreur.z;
+    }
+    commande.z = GAIN_ANG*(erreur.z + integrateur.z/Ti);
+    
+    return commande;
+    
 }
 
 Vect3 calcCommande123(Vect3 commandeXYZ, Vect3 position){
@@ -164,9 +192,9 @@
     Vect3 commandeXYZ = initVect3();
     Vect3 commande123 = initVect3();
     Vect3 consigne    = initVect3();
-    consigne.x = 0.30;
-    consigne.y = 0.30;
-    consigne.z = PI/6;
+    consigne.x = 0.50;
+    consigne.y = 0;
+    consigne.z = 0;
 
     uint32_t seuilAffichage = PERIODE_AFF;
     uint32_t seuilAsserv = PERIODE_ASSERV;
@@ -176,7 +204,7 @@
         
         if (timer.read_ms() > seuilAffichage){
             seuilAffichage += PERIODE_AFF;
-            //pc.printf("lacet : %f\n\rpositionX : %f\n\rpositionY: %f\n\n\r",360/(2*PI)*position.z, position.x, position.y);
+            pc.printf("lacet : %f\n\rpositionX : %f\n\rpositionY: %f\n\n\r",360/(2*PI)*position.z, position.x, position.y);
             pc.printf("erreur lacet : %f\n\rerreurX : %f\n\rereeurY: %f\n\n\r",erreur.z, erreur.x, erreur.y);
             pc.printf("commande lacet : %f\n\rcommandeX : %f\n\rcommandeY: %f\n\n\r",commande123.z, commande123.x, commande123.y);
             //pc.printf("compt3 : %f\n\rcompt1 : %f\n\rcompt2: %f\n\n\r",2*PI/RESOLUTION_ENCO*compt3.read_value(),2*PI/RESOLUTION_ENCO*compt1.read_value(), 2*PI/RESOLUTION_ENCO*compt2.read_value());