Diff: main.cpp

Revision:

6:85ade96c99b1

Parent:

3:737ac9c24ca5

Child:

7:09004b460bd1

--- a/main.cpp	Wed Feb 13 18:12:28 2019 +0000
+++ b/main.cpp	Tue Mar 12 11:11:47 2019 +0000
@@ -34,6 +34,10 @@
 // Constantes Asservissement
 #define GAIN_POS 0.0001
 #define GAIN_ANG 0.0001
+#define GAIN_POS_INT 0.1
+#define GAIN_ANG_INT 0.1
+#define ERREUR_POS 0.5
+#define ERREUR_ANG 0.5
 
 Serial pc(USBTX,USBRX);
 Timer timer;
@@ -100,10 +104,22 @@
 }
 
 Vect3 calcCommandeXYZ(Vect3 erreur){
-    Vect3 commande;   
-    commande.x = GAIN_POS*erreur.x;
-    commande.y = GAIN_POS*erreur.y;
-    commande.z = GAIN_ANG*erreur.z;
+    static Vect3 commande;
+    if (erreur.x > ERREUR_POS){
+        commande.x = GAIN_POS*erreur.x;
+    } else {
+        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);
+    }
+    if (erreur.z > ERREUR_ANG){
+        commande.z = GAIN_ANG*erreur.z;
+    } else {
+        commande.z = GAIN_ANG*(erreur.z + GAIN_ANG_INT*commande.z);
+    }
     return commande;
 }
 
@@ -132,10 +148,6 @@
     moteur2.setup(DECOUP_HACH);
     moteur3.setup(DECOUP_HACH);
     
-    moteur1.write(-0.4);
-    moteur2.write(-0.7);
-    moteur3.write(0.7);
-    
     timer.start();
     pc.printf("SETUP effectue");
     
@@ -145,8 +157,8 @@
     Vect3 commandeXYZ = initVect3();
     Vect3 commande123 = initVect3();
     Vect3 consigne    = initVect3();
-    consigne.x = 0.10;
-    consigne.y = 0.05;
+    consigne.x = 10;
+    consigne.y = 5;
     consigne.z = PI/2;
     uint32_t seuilAffichage = PERIODE_AFF;
     uint32_t seuilAsserv = PERIODE_ASSERV;