Diff: odo_asserv.cpp

Revision:

12:2c312916a621

Parent:

11:e62133022f88

Child:

13:a72b0752aa6f

--- a/odo_asserv.cpp	Fri Sep 11 11:11:56 2020 +0000
+++ b/odo_asserv.cpp	Fri Sep 11 13:47:56 2020 +0000
@@ -3,6 +3,8 @@
 
 ///// VARIABLES
 
+Ticker ticker_odo;
+
 // Coeff à définir empiriquement
 const double coeffGLong = 5.956, coeffDLong = -5.956; // constantes permettant la transformation tic/millimètre
 //const double coeffGAngl = 12.4516203705, coeffDAngl = 12.725; // constantes permettant la transformation tic/degré
@@ -31,6 +33,7 @@
 #define entraxe 245
 
 float x = 0, y = 0, phi = 0;
+float phi_deg;
 float x0 = 0, y0 = 0, phi0 = 0;
 float dDist = 0, dAngl = 0;
 float distG = 0, distD = 0; // Distance parcourue par chaque roue
@@ -41,12 +44,17 @@
     y0 = y;
     phi0 = phi;
 
-    dDist = ((comptG * coeffGLong) + (comptD * coeffDLong)) / 2;
-    dAngl = (comptD * coeffDAngl) - (comptG * coeffGAngl);
+    dDist = ((comptG / coeffGLong) + (comptD / coeffDLong)) / 2;
+    dAngl = ((comptD / coeffDAngl) - (comptG / coeffGAngl)) / entraxe;
 
-    x = x0 + dDist * cos(dAngl);
-    y = y0 + dDist * sin(dAngl);
+    x = x0 + dDist * cos(phi0);
+    y = y0 + dDist * sin(phi0);
     phi = phi0 + dAngl;
+    phi_deg = phi*180/Pi;
+    
+    
+    comptG = 0;
+    comptD = 0;
 }