File content as of revision 5:34ed652f8c31:

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

// Variables globales
// cpt_cdgA est le compteur d'impulsion du codeur de gauche
// cpt_cddA est le compteur d'impulsion du codeur de droite
float posX, posY; // Position et orientation 
float theta; 

void odometrie(){

    float distG = (cpt_cdgA * perimetreRoueCodeuse) / NbPulseCodeur; // 1000 est le nombre d'impulsions par tour de la roue codeuse 
    float distD = (cpt_cddA * perimetreRoueCodeuse) / NbPulseCodeur; // 1000 est le nombre d'impulsions par tour de la roue codeuse 
    
    float distRobot = (distG + distD) / 2; // Distance parcourue par le robot (en mm)
    float rayon = (ecartCodeuse /2) * ((distD + distG) / (distD - distD)); // Trajectoire s'apparente à un cercle
    
    float dTheta = distRobot / rayon; // Changement d'orientation => Commande
    float posX0 = posX - rayon*cos(theta); 
    float posY0 = posY - rayon*sin(theta);
    
    theta = theta + dTheta; // Mise à jour des coordonnées
    posX = posX0 + rayon*cos(theta);
    posY = posY0 + rayon*sin(theta);
}