File content as of revision 9:e39b218ab20d:

#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;

/** Permet la gestion de l'odometrie du robot **/
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