Diff: main.cpp

Revision:

5:efd14a490f49

Parent:

4:80f612396136

Child:

7:09004b460bd1

--- a/main.cpp	Thu Feb 21 12:43:44 2019 +0000
+++ b/main.cpp	Thu Feb 21 14:23:12 2019 +0000
@@ -33,8 +33,8 @@
 #define PERIODE_ASSERV 50 //ms
 
 // Constantes Asservissement
-#define GAIN_POS 0.0001
-#define GAIN_ANG 0.0001
+#define GAIN_POS 1
+#define GAIN_ANG 0
 
 Serial pc(USBTX,USBRX);
 Timer timer;
@@ -112,13 +112,14 @@
     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 = -commandeXYZ.x*cos(position.z+4*PI/3)+commandeXYZ.y*sin(position.z+4*PI/3)+commandeXYZ.z;
+    commande123.z = 0;
     return commande123;
 }
 
 void moveBot(Vect3 commande123){
-    moteur1.write(commande123.x); 
-    moteur2.write(commande123.y);
+    moteur1.write(commande123.y); 
+    moteur2.write(commande123.x);
     moteur3.write(commande123.z);  
 }
 
@@ -138,7 +139,7 @@
     moteur3.write(0.7);
     
     timer.start();
-    pc.printf("SETUP effectue");
+    pc.printf("SETUP effectue\n\r");
     
     //variables
     Vect3 position    = initVect3();
@@ -146,9 +147,9 @@
     Vect3 commandeXYZ = initVect3();
     Vect3 commande123 = initVect3();
     Vect3 consigne    = initVect3();
-    consigne.x = 0.10;
-    consigne.y = 0.05;
-    consigne.z = PI/2;
+    consigne.x = 0.30;
+    consigne.y = 0.30;
+    consigne.z = PI/6;
     uint32_t seuilAffichage = PERIODE_AFF;
     uint32_t seuilAsserv = PERIODE_ASSERV;
     
@@ -157,9 +158,12 @@
         
         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());
             //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());
+            
             myled = !myled;
         }
         if (timer.read_ms() > seuilAsserv){