|
#include <modules/my_math/matrix/math.hpp>
|
#include <stdint.h>
|
#define AC_PID_Basic_FILT_E_HZ_DEFAULT 20.0f // default input filter frequency
|
#define AC_PID_Basic_FILT_D_HZ_DEFAULT 10.0f // default input filter frequency
|
#define M_2PI (M_PI * 2)
|
#define FLT_EPSILON 1.19209289550781250000000000000000000e-7F
|
inline float constrain_float(float amt, float low, float high)
|
{
|
// the check for NaN as a float prevents propagation of floating point
|
// errors through any function that uses constrain_value(). The normal
|
// float semantics already handle -Inf and +Inf
|
|
|
|
if (amt < low) {
|
return low;
|
}
|
|
if (amt > high) {
|
return high;
|
}
|
|
return amt;
|
}
|
|
inline bool is_zero(const float x) {
|
return fabs(x) < FLT_EPSILON;
|
}
|
inline bool is_positive(const float fVal1) {
|
return (fVal1 >= FLT_EPSILON);
|
}
|
|
/*
|
* @brief: Check whether a double is less than zero
|
*/
|
inline bool is_negative(const float fVal1) {
|
return (fVal1 <= (-1.0 * FLT_EPSILON));
|
}
|
|
|
class AC_PID_Basic {
|
public:
|
|
// Constructor for PID
|
AC_PID_Basic(float initial_p, float initial_i, float initial_d, float initial_ff, float initial_imax, float initial_filt_E_hz=AC_PID_Basic_FILT_E_HZ_DEFAULT, float initial_filt_D_hz=AC_PID_Basic_FILT_D_HZ_DEFAULT);
|
|
// set target and measured inputs to PID controller and calculate outputs
|
// target and error are filtered
|
// the derivative is then calculated and filtered
|
// the integral is then updated based on the setting of the limit flag
|
float update_all(float target, float measurement, float dt, bool limit = false) ;
|
float update_all(float target, float measurement, float dt, bool limit_neg, bool limit_pos) ;
|
|
// update the integral
|
// if the limit flags are set the integral is only allowed to shrink
|
void update_i(float dt, bool limit_neg, bool limit_pos);
|
|
// get results from pid controller
|
float get_p() const { return _error * _kp; }
|
float get_i() const { return _integrator; }
|
float get_d() const { return _derivative * _kd; }
|
float get_ff() const { return _target * _kff; }
|
float get_error() const { return _error; }
|
|
// reset the integrator
|
void reset_I();
|
|
// input and D term filter will be reset to the next value provided to set_input()
|
void reset_filter() { _reset_filter = true; }
|
|
|
// get accessors
|
float &kP() { return _kp; }
|
float &kI() { return _ki; }
|
float &kD() { return _kd; }
|
float &ff() { return _kff;}
|
float &filt_E_hz() { return _filt_E_hz; }
|
float &filt_D_hz() { return _filt_D_hz; }
|
float imax() const { return _kimax; }
|
float get_filt_E_alpha(float dt) ;
|
float get_filt_D_alpha(float dt) ;
|
|
// set accessors
|
void kP(float v) { _kp=v; }
|
void kI(float v) { _ki=v; }
|
void kD(float v) { _kd=v; }
|
void ff(float v) { _kff=v; }
|
void imax(float v) { _kimax=fabs(v); }
|
void filt_E_hz(float hz) { _filt_E_hz=fabs(hz); }
|
void filt_D_hz(float hz) { _filt_D_hz=fabs(hz); }
|
|
// integrator setting functions
|
void set_integrator(float target, float measurement, float i);
|
void set_integrator(float error, float i);
|
void set_integrator(float i);
|
float calc_lowpass_alpha_dt(float dt, float cutoff_freq);
|
|
// parameter var table
|
|
|
protected:
|
|
// parameters
|
float _kp;
|
float _ki;
|
float _kd;
|
float _kff;
|
float _kimax;
|
float _filt_E_hz; // PID error filter frequency in Hz
|
float _filt_D_hz; // PID derivative filter frequency in Hz
|
|
// internal variables
|
float _target; // target value to enable filtering
|
float _error; // error value to enable filtering
|
float _derivative; // last derivative for low-pass filter
|
float _integrator; // integrator value
|
bool _reset_filter; // true when input filter should be reset during next call to set_input
|
|
|
};
|
