![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
Capteur_US
odo_asserv.cpp
- Committer:
- plmir
- Date:
- 2020-09-11
- Revision:
- 12:2c312916a621
- Parent:
- 11:e62133022f88
- Child:
- 13:a72b0752aa6f
File content as of revision 12:2c312916a621:
//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 = 12*2*Pi*53791, coeffDAngl = 12*2*Pi*54972; // 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 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 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; phi_deg = phi*180/Pi; comptG = 0; comptD = 0; } ///// CONSIGNE ANGLE void angle() { }