File content as of revision 3:3ba377aafdfd:

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

#define capt 1000 // Nb d'impulsions
#define ecart 245 // Distance en mm entre les deux roues codeuses
#define diametreRoueCodeuse 51.450 // Diamètre de la roue codeuse en mm
#define perimetreRoueCodeuse (diametreRoueCodeuse * Pi)

// 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) / capt; // 1000 est le nombre d'impulsions par tour de la roue codeuse 
    float distD = (cpt_cddA * perimetreRoueCodeuse) / capt; // 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 = (ecart /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);
}