File content as of revision 12:2c312916a621:

//Nom du fichier : odo_asserv_2.cpp
#include "pins.h"

///// VARIABLES

Ticker ticker_odo_2;

// 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()
{
    
}