AresENSEA-CDF2020
/
AresCDFMainCode_capteur_US
Capteur_US
odo_asserv.cpp
- Committer:
- Nanaud
- Date:
- 2020-09-16
- Revision:
- 14:dd3c756c6d48
- Parent:
- 13:a72b0752aa6f
File content as of revision 14:dd3c756c6d48:
//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 = 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() { if(cdgB) comptG++; else comptG--; } void cddaRise() { if(cddB) comptD++; else comptD--; } ///* ///// 1) ODOMÉTRIE Geonobot : Approximation par segment de droite #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 void odoGeonobot1() { xA = xB; yA = yB; phiA = phiB; dDist = ((comptG / coeffGLong) + (comptD / coeffDLong)) / 2; dAngl = ((comptD / coeffDAngl) - (comptG / coeffGAngl)) / entraxe; xB = xA + dDist * cos(phiA); yB = yA + dDist * sin(phiA); phiB = phiA + dAngl; comptG = 0; comptD = 0; } //*/ /* ///// 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; } */