DifferentialController.h

#ifndef __DIFFERENTIALCONTROLLER_H__
#define __DIFFERENTIALCONTROLLER_H__
 
#include <memory>
 
#include "../lib/PeriodicProcess/PeriodicProcess.h"
#include "AbstractMotors.h"
#include "PID.h"

class DifferentialController : public PeriodicProcess
{
public:
    DifferentialController() : m_linSetpoint(0), m_angSetpoint(0), m_axleTrack(1){}

    void setInputs   (float linInput,    float angInput)   {m_linInput    = linInput;    m_angInput    = angInput;}
    void setSetpoints(float linSetpoint, float angSetpoint){m_linSetpoint = linSetpoint; m_angSetpoint = angSetpoint;}
    void setAxleTrack(float axleTrack);

    void setWheels(AbstractMotor& leftWheel, AbstractMotor& rightWheel){m_leftWheel = &leftWheel; m_rightWheel = &rightWheel;}
    void setPID(std::unique_ptr<PID> linPID, std::unique_ptr<PID> angPID){m_linPID = std::move(linPID); m_angPID = std::move(angPID);}
 
 

    float getLinSetpoint() const {return m_linSetpoint;}
    float getAngSetpoint() const {return m_angSetpoint;}

    float getLinOutput() const {return m_linVelOutput;}
    float getAngOutput() const {return m_angVelOutput;}

    float getAxleTrack() const {return m_axleTrack;}
 
    std::unique_ptr<PID> m_linPID;
    std::unique_ptr<PID> m_angPID;
protected:
    virtual void process(float timestep);
    virtual void onProcessEnabling();
    virtual void onProcessDisabling();
 
    float m_linInput; 
    float m_angInput; 
    float m_linSetpoint; 
    float m_angSetpoint; 
    float m_axleTrack; 
 
    float m_linVelOutput;
    float m_angVelOutput;
 
    AbstractMotor* m_leftWheel;
    AbstractMotor* m_rightWheel;
};
 
#endif // __DIFFERENTIALCONTROLLER_H__