File content as of revision 4:3e6e78d6d3d9:

#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:
        Odometry(double diameter_right, double diameter_left, double v, RoboClaw &rc);
        
        void setPos(double x, double y, double theta);
        void setX(double x);
        void setY(double y);
        void setTheta(double theta);
        
        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;
        }
        
        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;}
        
        void update_odo(void);
        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;}
        
        bool isArrivedRot(double theta_);
    
    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