File content as of revision 2:094c09903a9c:

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

#define ecart 120 // Distance en mm entre les deux roues motrices
#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
int posX, posY; // Position et orientation 
float theta; 

void odometrie(){

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