pidff.h

#pragma once
#include "core/utils/controls/feedback_base.h"
#include "core/utils/controls/feedforward.h"
#include "core/utils/controls/pid.h"
 
class PIDFF : public Feedback {
  public:
    PIDFF(PID::pid_config_t &pid_cfg, FeedForward::ff_config_t &ff_cfg);

    void init(double start_pt, double set_pt) override;

    void set_target(double set_pt);
 
    double get_target() const;
    double get_sensor_val() const;
    double update(double val) override;

    double update(double val, double vel_setpt, double a_setpt = 0);

    double get() override;

    void set_limits(double lower, double upper) override;

    bool is_on_target() override;
 
    void reset();
 
    PID pid;
 
  private:
    FeedForward::ff_config_t &ff_cfg;
 
    FeedForward ff;
 
    double out;
    double lower_lim, upper_lim;
};