Diff: main.cpp

Revision:

7:09004b460bd1

Parent:

5:efd14a490f49

Parent:

6:85ade96c99b1

Child:

8:91eb7435c3e0

--- a/main.cpp	Thu Feb 21 14:23:12 2019 +0000
+++ b/main.cpp	Tue Mar 12 11:36:23 2019 +0000
@@ -35,6 +35,10 @@
 // Constantes Asservissement
 #define GAIN_POS 1
 #define GAIN_ANG 0
+#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;
@@ -69,7 +73,6 @@
 }
 
 Vect3 calculatePosition(){
-    
     static Vect3 theta = initVect3();
     static Vect3 position = initVect3();
     Vect3 dTheta;
@@ -101,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;
 }
 
@@ -112,8 +127,7 @@
     Vect3 commande123;
     commande123.x = -commandeXYZ.x*cos(position.z+0*PI/3)+commandeXYZ.y*sin(position.z+0*PI/3)+commandeXYZ.z;
     commande123.y = -commandeXYZ.x*cos(position.z+2*PI/3)+commandeXYZ.y*sin(position.z+2*PI/3)+commandeXYZ.z;
-    //commande123.z = -commandeXYZ.x*cos(position.z+4*PI/3)+commandeXYZ.y*sin(position.z+4*PI/3)+commandeXYZ.z;
-    commande123.z = 0;
+    commande123.z = -commandeXYZ.x*cos(position.z+4*PI/3)+commandeXYZ.y*sin(position.z+4*PI/3)+commandeXYZ.z;
     return commande123;
 }
 
@@ -134,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\n\r");
     
@@ -150,6 +160,7 @@
     consigne.x = 0.30;
     consigne.y = 0.30;
     consigne.z = PI/6;
+
     uint32_t seuilAffichage = PERIODE_AFF;
     uint32_t seuilAsserv = PERIODE_ASSERV;
     
@@ -173,7 +184,6 @@
             commandeXYZ = calcCommandeXYZ(erreur);
             commande123 = calcCommande123(commandeXYZ, position);
             moveBot(commande123);
-        }
-        
+        }        
     }
 }
\ No newline at end of file