Diff: main.cpp

Revision:

4:80f612396136

Parent:

3:737ac9c24ca5

Child:

5:efd14a490f49

diff -r 737ac9c24ca5 -r 80f612396136 main.cpp
--- a/main.cpp	Wed Feb 13 18:12:28 2019 +0000
+++ b/main.cpp	Thu Feb 21 12:43:44 2019 +0000
@@ -4,13 +4,14 @@
 #include "TLE5206_lib.h"
 
 // Caractéristiques de structure
-#define RAYON_ROUE 0.005 // [m]
-#define DIST1 0.015 // [m]
-#define DIST2 0.015 // [m]
-#define DIST3 0.015 // [m]
+#define RAYON_ROUE 0.025 // [m]
+#define DIST1 0.15 // [m]
+#define DIST2 0.15 // [m]
+#define DIST3 0.0075 // [m]
 #define PI 3.14159265359
-#define RESOLUTION_ENCO 14336*4 // = 14*1024*4 = rapport_reduction * nbr de top par tour
-
+#define RESOLUTION_ENCO 14336 // = 14*1024 = rapport_reduction * nbr de top par tour
+#define ETALONNAGE_LACET 0.88
+#define ETALONNAGE_XY 1.065
 // Liaison SPI avec les compteurs 
 #define SPI_SCLK PA_5 //A4
 #define SPI_MISO PA_6 //A5
@@ -81,12 +82,12 @@
     theta.y += dTheta.y;
     theta.z += dTheta.z;
     
-    dPsi = RAYON_ROUE * ( dTheta.x + dTheta.y + dTheta.z)/(DIST1 + DIST2 + DIST3);
+    dPsi = ETALONNAGE_LACET*RAYON_ROUE * ( dTheta.x + dTheta.y + dTheta.z)/(DIST1 + DIST2 + DIST3);
 
     position.z += dPsi;// Psi actuel
     
-    position.x += ((-RAYON_ROUE * dTheta.x + DIST1 * dPsi) *  cos(position.z) + (-RAYON_ROUE * dTheta.y + DIST1 * dPsi) *  cos(position.z + 2*PI/3) + (-RAYON_ROUE * dTheta.z + DIST1 * dPsi)*cos(position.z + 4*PI/3))*2/3;
-    position.y +=-((-RAYON_ROUE * dTheta.x + DIST1 * dPsi) *  sin(position.z) + (-RAYON_ROUE * dTheta.y + DIST1 * dPsi) *  sin(position.z + 2*PI/3) + (-RAYON_ROUE * dTheta.z + DIST1 * dPsi)*sin(position.z  + 4*PI/3))*2/3;
+    position.x += ETALONNAGE_XY*((-RAYON_ROUE * dTheta.x + DIST1 * dPsi) *  cos(position.z) + (-RAYON_ROUE * dTheta.y + DIST1 * dPsi) *  cos(position.z + 2*PI/3) + (-RAYON_ROUE * dTheta.z + DIST1 * dPsi)*cos(position.z + 4*PI/3))*2/3;
+    position.y +=-ETALONNAGE_XY*((-RAYON_ROUE * dTheta.x + DIST1 * dPsi) *  sin(position.z) + (-RAYON_ROUE * dTheta.y + DIST1 * dPsi) *  sin(position.z + 2*PI/3) + (-RAYON_ROUE * dTheta.z + DIST1 * dPsi)*sin(position.z  + 4*PI/3))*2/3;
     
     return position;
 }
@@ -156,7 +157,7 @@
         
         if (timer.read_ms() > seuilAffichage){
             seuilAffichage += PERIODE_AFF;
-            pc.printf("lacet : %f\n\rpositionX : %f\n\rpositionY: %f\n\n\r",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("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;