AresENSEA-CDF2020
/
AresCDFMainCode_us2
Test
Diff: odo_asserv.cpp
- Revision:
- 14:dd3c756c6d48
- Parent:
- 13:a72b0752aa6f
- Child:
- 16:ae65ce77b1f9
diff -r a72b0752aa6f -r dd3c756c6d48 odo_asserv.cpp --- a/odo_asserv.cpp Fri Sep 11 14:15:57 2020 +0000 +++ b/odo_asserv.cpp Wed Sep 16 12:31:54 2020 +0000 @@ -1,17 +1,18 @@ //Nom du fichier : odo_asserv.cpp #include "pins.h" +//#define _PI_ 3.14159265359 ///// 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é -const double coeffGAngl = 53791/(12*2*Pi), coeffDAngl = 54972/(12*2*Pi); // constantes permettant la transformation tic/radian +//const double coeffGAngl = 53791/(12*2*_PI_), coeffDAngl = 54972/(12*2*_PI_); // constantes permettant la transformation tic/radian +const double coeffGAngl = 713.425, coeffDAngl = 729.089; // constantes permettant la transformation tic/radian long comptG = 0, comptD = 0; // nb de tics comptés pour chaque codeur + ///// INTERRUPTIONS CODEURS void cdgaRise() @@ -26,31 +27,70 @@ else comptD--; } -///// ODOMÉTRIE +///* +///// 1) ODOMÉTRIE Geonobot : Approximation par segment de droite -// Variables et constantes -//#define NbPulseCodeur 1000 -#define entraxe 245 - -double x = 0, y = 0, phi = 0; -double x0 = 0, y0 = 0, phi0 = 0; -double dDist = 0, dAngl = 0; +#define entraxe 245 // Distance entre les roues codeuses +double xB = 0, yB = 0, phiB = 0; // Nouvelles coordonnées +double xA = 0, yA = 0, phiA = 0; // Dernières coordonnées calculées +double dDist = 0, dAngl = 0; // Distance moyenne du robot et orientation double distG = 0, distD = 0; // Distance parcourue par chaque roue -// Approximation par segment de droite -void odometrie() +void odoGeonobot1() { - x0 = x; - y0 = y; - phi0 = phi; + xA = xB; + yA = yB; + phiA = phiB; dDist = ((comptG / coeffGLong) + (comptD / coeffDLong)) / 2; dAngl = ((comptD / coeffDAngl) - (comptG / coeffGAngl)) / entraxe; - x = x0 + dDist * cos(phi0); - y = y0 + dDist * sin(phi0); - phi = phi0 + dAngl; - + xB = xA + dDist * cos(phiA); + yB = yA + dDist * sin(phiA); + phiB = phiA + dAngl; + comptG = 0; comptD = 0; -} \ No newline at end of file +} +//*/ + +/* +///// 2) ODOMÉTRIE Geonobot : Approximation par arc de cercle + +#define entraxe 245 // Distance entre les roues codeuses +double xB = 0, yB = 0, phiB = 0; // Nouvelles coordonnées +double xA = 0, yA = 0, phiA = 0; // Dernières coordonnées calculées +double xO = 0, yO = 0; // Centre du cercle de rayon R +double dDist = 0, dAngl = 0; // Distance moyenne du robot et orientation +double distG = 0, distD = 0; // Distance parcourue par chaque roue +double rayon = 0; + +void odoGeonobot2() +{ + xA = xB; + yA = yB; + phiA = phiB; + + dDist = ((comptG / coeffGLong) + (comptD / coeffDLong)) / 2; + dAngl = ((comptD / coeffDAngl) - (comptG / coeffGAngl)) / entraxe; + rayon = dDist / dAngl; + + xO = xA - R*sin(phiA); + yO = yA + R*cos(phiA); + + phiB = phiA + dAngl; + + if (dAngl == 0) { + xB = xO + R*sin(phiB); + yB = yO + R*cos(phiB); + } + + else { + xB = xO + dDist*cos(phiB); + yB = yO + dDist*sin(phiB); + } + + comptG = 0; + comptD = 0; +} +*/