Odometry.h

#ifndef __ODOMETRY_H__
#define __ODOMETRY_H__
 
#include "../lib/PeriodicProcess/PeriodicProcess.h"

struct Position
{
    Position() : x(0), y(0), theta(0){}
    Position(float x, float y, float theta) : x(x), y(y), theta(theta){}
    static bool compare(const Position *one , const Position *two){return one->x==two->x && one->y==two->y && one->theta==two->theta;}
    float x;
    float y;
    float theta;
};

class AbstractCodewheel
{
public:
 
    virtual ~AbstractCodewheel() = default;

    virtual float getTraveledDistance() = 0;
    virtual float restart() = 0;
};

class Odometry : public PeriodicProcess
{
public:
    void setPosition(float x, float y, float theta){m_pos.x = x; m_pos.y = y; m_pos.theta = theta;}
    void setAxleTrack(float axleTrack);
    void setSlippage (float slippage);
    void setCodewheels(const AbstractCodewheel& leftCodewheel, const AbstractCodewheel& rightCodewheel){m_leftCodewheel = const_cast<AbstractCodewheel*>(&leftCodewheel), m_rightCodewheel = const_cast<AbstractCodewheel*>(&rightCodewheel);}
    const Position* getPosition() const {return &m_pos;}
    float getLinVel() const {return m_linVel;}
    float getAngVel() const {return m_angVel;}
    float getAxleTrack() const {return m_axleTrack;}
    float getSlippage () const {return m_slippage;}
 
protected:
    virtual void process(float timestep);
 
    Position m_pos; 
    float m_linVel; 
    float m_angVel; 
    float m_axleTrack;
    float m_slippage;
 
    AbstractCodewheel* m_leftCodewheel;
    AbstractCodewheel* m_rightCodewheel;
};
 
#endif // __ODOMETRY_H__