AresENSEA-CDF2020
/
AresCDFMainCode_us2
Test
Diff: odo_asserv.cpp
- Revision:
- 12:2c312916a621
- Parent:
- 11:e62133022f88
- Child:
- 13:a72b0752aa6f
diff -r e62133022f88 -r 2c312916a621 odo_asserv.cpp --- 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; }