File content as of revision 7:961c1acdf753:

#ifndef ODOMETRY_H
#define ODOMETRY_H

#include "mbed.h"
#include "RoboClaw.h"

#define PI 3.1415926535897932384626433832795
#define C 1.1538461538461538461538461538462

/*
*   Author : Benjamin Bertelone, reworked by Simon Emarre
*/

extern Serial pc;

class Odometry
{
    public:
        /** Constructeur standart
        * @param diameter_right Definit le diametre de la roue droite
        * @param diameter_left Definit le diametre de la roue gauche
        * @param v Definit l'entraxe du robot
        * @param rc Definit une reference vers l'objet permettant l'asserv des moteurs
            @note Cet objet doit etre initialise en amont
        */
        Odometry(double diameter_right, double diameter_left, double v, RoboClaw &rc);
        
        /** Les fonctions suivantes permettent de reinitialiser les valeurs de position de l'odometrie. 
        */
        void setPos(double x, double y, double theta);
        void setX(double x);
        void setY(double y);
        void setTheta(double theta);
        
        /** Les fonctions suivantes permettent de deplacer le robot vers une position ou un angle voulu
        */ 
        void GotoXYT(double x_goal, double y_goal, double theta_goal);
        void GotoThet(double theta_);
        void GotoDist(double distance);
        
        double getX() {return x;}
        double getY() {return y;}
        double getTheta() {return theta;} // ]-PI;PI]
        double getTheta_(double x, double y);
        
        double abs_d(double x){
            if(x<0) return -x;
            else return x;
        }
        
        /** Les fonction suivantes sont actuellement inutilisables, elles pourraient buguer en cas d'utilisation.
        * @note pour que les fonctions retournent une valeur correcte, il faut actualiser l'odometrie avec update_odo auparavant
        */
        double getVitLeft() {return m_vitLeft;}
        double getVitRight() {return m_vitRight;}
        
        double getDistLeft() {return m_distLeft;}
        double getDistRight() {return m_distRight;}
        
        void setDistLeft(double dist) {m_distLeft = dist;}
        void setDistRight(double dist) {m_distRight = dist;}

        double calcul_distance(double x, double y, double theta_goal);
        
        int32_t getPulsesLeft(void) {return m_pulses_left;}
        int32_t getPulsesRight(void) {return m_pulses_right;}
        double carre(double a) {return a*a;}
        
        /** La fonction retourne vraie quand le robot atteint l'angle voulu avec une marge d'erreur definie par la fonction
        * @param theta_ valeur de l'angle devant etre atteint
        */
        bool isArrivedRot(double theta_);    
        /** Permet de mettre à jour les valeurs de l'odometrie 
        */    
        void update_odo(void);
    
    private:
        RoboClaw &roboclaw;
        int32_t m_pulses_left;
        int32_t m_pulses_right;
        
        double x, y, theta;
        double m_vitLeft, m_vitRight;
        double m_distLeft, m_distRight;
        
        double m_distPerTick_left, m_distPerTick_right, m_v;
        
        double erreur_ang;
};

#endif