VelocityController.h

#ifndef __VELOCITYCONTROLLER_H__
#define __VELOCITYCONTROLLER_H__
 
#include "DifferentialController.h"
 
 
#define ENABLE_VELOCITYCONTROLLER_LOGS 0        // for debug purposes
#define VELOCITYCONTROLLER_LOGS_TIMESTEP 20e-3 // mm
#if ENABLE_VELOCITYCONTROLLER_LOGS
#include "Teleplot.h"
#include <math.h>
#endif

class VelocityController : public DifferentialController
{
public:
    VelocityController() : m_rampLinVelSetpoint(0), m_rampAngVelSetpoint(0), m_maxLinAcc(INFINITY), m_maxLinDec(INFINITY), m_maxAngAcc(INFINITY), m_maxAngDec(INFINITY), m_linSpinGoal(0.0), m_angSpinGoal(0.0), m_spinShutdown(true) {}
 
    void setMaxAngAcc(float maxAngAcc);
 
    void setMaxLinAcc(float maxLinAcc);

 
    void setMaxLinDec(float maxLinDec);
 
    void setMaxAngDec(float maxAngDec);

    void setSpinShutdown(bool spinShutdown);
    float getMaxLinAcc() const { return m_maxLinAcc; }
    float getMaxAngAcc() const { return m_maxAngAcc; }
    float getMaxLinDec() const { return m_maxLinDec; }
    float getMaxAngDec() const { return m_maxAngDec; }
    float getLinSpinGoal() const { return m_linSpinGoal; }
    float getAngSpinGoal() const { return m_angSpinGoal; }
    bool getSpinShutdown() const { return m_spinShutdown; }
 
    void set_stop(bool stop);
 
protected:
    float genRampSetpoint(float stepSetpoint, float input, float rampSetpoint, float maxAcc, float maxDec, float timestep);
    virtual void process(float timestep);
    virtual void onProcessEnabling();
 
    float m_rampLinVelSetpoint;       // in mm/s (no longer w/e unit)
    float m_rampAngVelSetpoint;     // in rad/s (no longer w/e unit)
    float m_maxLinAcc;   // always positive, in mm/s^2
    float m_maxLinDec;  // always positive, in mm/s^2
    float m_maxAngAcc;   // always positive, in mm/s^2
    float m_maxAngDec;  // always positive, in mm/s^2
    bool m_spinShutdown;                                                              
    float m_linSpinGoal;                                                              // Velocity wanted before the spin alarm
    float m_angSpinGoal;                                                              // Velocity wanted before the spin alarm
    bool m_sensors_stop=false;
    bool m_sensors_restart=false;
    bool m_sensors_stoped=false;
    float m_last_linSetpoint;
    float m_last_angSetpoint;
 
#if ENABLE_VELOCITYCONTROLLER_LOGS
    friend class VelocityControllerLogs;
#endif // ENABLE_VELOCITYCONTROLLER_LOGS
};
 
#if ENABLE_VELOCITYCONTROLLER_LOGS
class VelocityControllerLogs : public PeriodicProcess
{
public:
    void setController(const VelocityController &controller) { m_controller = &controller; }
 
protected:
    virtual void process(float timestep);
 
    const VelocityController *m_controller;
};
#endif // ENABLE_VELOCITYCONTROLLER_LOGS
 
#endif // __VELOCITYCONTROLLER_H__