File content as of revision 16:4c0b1647e8ae:

//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;
}
*/