fsb_pid.h

#ifndef FSB_PID_H
#define FSB_PID_H
 
#include <cstdint>
#include "fsb_types.h"
 
namespace fsb
{
 
enum class PidType : uint8_t
{
    TRAPEZOIDAL = 0,
    BACKWARDS = 1
};
 
class PidController
{
public:
    PidController() = default;
 
    void initialize(
        Real step_size, Real gain_kp, Real gain_ki, Real gain_kd, Real filter_tf,
        PidType type = PidType::TRAPEZOIDAL);
 
    void reset(Real command, Real measured);
 
    Real evaluate(Real command, Real measured);
 
private:
    Real m_kp = 0.0;
    Real m_ki = 0.0;
    Real m_kd = 0.0;
    Real m_tf = 0.0;
    Real m_step_size = 0.0;
    PidType m_type = PidType::TRAPEZOIDAL;
 
    Real m_e1 = 0.0;
    Real m_e2 = 0.0;
    Real m_u1 = 0.0;
    Real m_u2 = 0.0;
};
 
} // namespace fsb
 
#endif