AresENSEA-CDF2020
/
AresCDFMainCode_capteur_US
Capteur_US
odo_asserv.cpp
- Committer:
- Nanaud
- Date:
- 2020-09-11
- Revision:
- 13:a72b0752aa6f
- Parent:
- 12:2c312916a621
- Child:
- 14:dd3c756c6d48
File content as of revision 13:a72b0752aa6f:
//Nom du fichier : odo_asserv.cpp #include "pins.h" ///// 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 long comptG = 0, comptD = 0; // nb de tics comptés pour chaque codeur ///// INTERRUPTIONS CODEURS void cdgaRise() { if(cdgB) comptG++; else comptG--; } void cddaRise() { if(cddB) comptD++; else comptD--; } ///// ODOMÉTRIE // 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; double distG = 0, distD = 0; // Distance parcourue par chaque roue // Approximation par segment de droite void odometrie() { x0 = x; y0 = y; phi0 = phi; 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; comptG = 0; comptD = 0; }