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_t step_size, real_t gain_kp, real_t gain_ki, real_t gain_kd, real_t filter_tf,
        PidType type = PidType::TRAPEZOIDAL);
 
    void reset(real_t command, real_t measured);
 
    real_t evaluate(real_t command, real_t measured);
 
private:
    real_t m_kp = 0.0;
    real_t m_ki = 0.0;
    real_t m_kd = 0.0;
    real_t m_tf = 0.0;
    real_t m_step_size = 0.0;
    PidType m_type = PidType::TRAPEZOIDAL;
 
    real_t m_e1 = 0.0;
    real_t m_e2 = 0.0;
    real_t m_u1 = 0.0;
    real_t m_u2 = 0.0;
};
 
} // namespace fsb
 
#endif