diff --git a/pid/pid.c b/pid/pid.c index 900b449..4c4a385 100644 --- a/pid/pid.c +++ b/pid/pid.c @@ -1,181 +1,82 @@ -/** - * @file pid.c - * @author Daniele Facinelli [daniele.facinelli@studenti.unitn.it] - * @author Filippo Faccini [filippo.faccini@studenti.unitn.it] - * @author Simone Ruffini [simone.ruffini@tutanota.com] - * @date 2018-04-27 - * @updated 2021-05-11 Simone Ruffini: refactoring - * @ingroup - * @prefix PID - * - * @brief PID library - * - */ - -/* Includes ------------------------------------------------------------------*/ - #include "pid.h" -/* Private typedef -----------------------------------------------------------*/ -/* Private define ------------------------------------------------------------*/ - -/* Private macro -------------------------------------------------------------*/ - -#define CONSTRAIN(x, lower, upper) ((x) < (lower) ? (lower) : ((x) > (upper) ? (upper) : (x))) - -/* Private variables ---------------------------------------------------------*/ -/* Private function prototypes -----------------------------------------------*/ -/* Exported functions --------------------------------------------------------*/ - -void PIDInit( - PIDControl *pid, - float kp, - float ki, - float kd, - float sampleTimeSeconds, - float minOutput, - float maxOutput, - PID_ModeTypeDef mode, - PID_ControlActionTypeDef controlAction) { - pid->controlAction = controlAction; - pid->mode = mode; - pid->iTerm = 0.0f; - pid->input = 0.0f; - pid->lastInput = 0.0f; - pid->output = 0.0f; - pid->setpoint = 0.0f; - - if (sampleTimeSeconds > 0.0f) { - pid->sampleTime = sampleTimeSeconds; - } else { - // If the passed parameter was incorrect, set to 1 second - pid->sampleTime = 1.0f; +#ifdef PID_ERRORS_VECTOR +void pid_init(PidController_t pid_controller, + float kp, + float ki, + float kd, + float sample_time, + float anti_windUp, + float *prev_errors, + uint8_t n_prev_errors) { + pid_controller.kp = kp; + pid_controller.ki = ki; + pid_controller.kd = kd; + pid_controller.integrator = 0.0; + pid_controller.n_prev_errors = n_prev_errors; + pid_controller.prev_error_index = pid_controller.n_prev_errors - 1; + for (int i = 0; i < pid_controller.n_prev_errors; ++i) { + pid_controller.prev_errors[i] = 0.0; } - - PIDOutputLimitsSet(pid, minOutput, maxOutput); - PIDTuningsSet(pid, kp, ki, kd); + pid_controller.error = 0.0; + pid_controller.set_point = 0.0; + pid_controller.sample_time = sample_time; + pid_controller.anti_windUp = anti_windUp; } - -bool PIDCompute(PIDControl *pid) { - float error, dInput; - - if (pid->mode == PID_MODE_MANUAL) { - return false; - } - - // The classic PID error term - error = (pid->setpoint) - (pid->input); - - // Compute the integral term separately ahead of time - pid->iTerm += (pid->alteredKi) * error; - - // Constrain the integrator to make sure it does not exceed output bounds - pid->iTerm = CONSTRAIN((pid->iTerm), (pid->outMin), (pid->outMax)); - - // Take the "derivative on measurement" instead of "derivative on error" - dInput = (pid->input) - (pid->lastInput); - - // Run all the terms together to get the overall output - pid->output = (pid->alteredKp) * error + (pid->iTerm) - (pid->alteredKd) * dInput; - - // Bound the output - pid->output = CONSTRAIN((pid->output), (pid->outMin), (pid->outMax)); - - // Make the current input the former input - pid->lastInput = pid->input; - - return true; +#else +void pid_init(PidController_t pid_controller, + float kp, + float ki, + float kd, + float sample_time, + float anti_windUp) { + pid_controller.kp = kp; + pid_controller.ki = ki; + pid_controller.kd = kd; + pid_controller.integrator = 0.0; + pid_controller.prev_error = 0.0; + pid_controller.error = 0.0; + pid_controller.set_point = 0.0; + pid_controller.sample_time = sample_time; + pid_controller.anti_windUp = anti_windUp; } - -void PIDModeSet(PIDControl *pid, PID_ModeTypeDef mode) { - // If the mode changed from PID_MODE_MANUAL to PID_MODE_AUTOMATIC - if (pid->mode != mode && mode == PID_MODE_AUTOMATIC) { - // Initialize a few PID parameters to new values - pid->iTerm = pid->output; - pid->lastInput = pid->input; - - // Constrain the integrator to make sure it does not exceed output bounds - pid->iTerm = CONSTRAIN((pid->iTerm), (pid->outMin), (pid->outMax)); - } - - pid->mode = mode; +#endif + +void pid_update(PidController_t pid_controller, float status) { + #ifdef PID_ERRORS_VECTOR + pid_controller.prev_error_index = (pid_controller.prev_error_index + 1) % pid_controller.n_prev_errors; + pid_controller.prev_errors[pid_controller.prev_error_index] = pid_controller.error; + #else + pid_controller.prev_error = pid_controller.prev_error; + #endif + pid_controller.error = pid_controller.set_point - status; + pid_controller.integrator += pid_controller.error * pid_controller.sample_time; } -void PIDOutputLimitsSet(PIDControl *pid, float min, float max) { - // Check if the parameters are valid - if (min >= max) { - return; +float pid_compute(PidController_t pid_controller) { + #ifdef PID_ERRORS_VECTOR + uint8_t index = (pid_controller.prev_error_index + 1) % pid_controller.n_prev_errors; + float derivative = (pid_controller.error - pid_controller.prev_errors[index]) / (pid_controller.sample_time * pid_controller.n_prev_errors); + #else + float derivative = (pid_controller.error - pid_controller.prev_error) / pid_controller.sample_time; + #endif + float integral = pid_controller.ki * pid_controller.integrator; + if (integral > pid_controller.anti_windUp) { + integral = pid_controller.anti_windUp; + } else if (integral < -pid_controller.anti_windUp) { + integral = -pid_controller.anti_windUp; } - - // Save the parameters - pid->outMin = min; - pid->outMax = max; - - // If in automatic, apply the new constraints - if (pid->mode == PID_MODE_AUTOMATIC) { - pid->output = CONSTRAIN(pid->output, min, max); - pid->iTerm = CONSTRAIN(pid->iTerm, min, max); - } -} - -void PIDTuningsSet(PIDControl *pid, float kp, float ki, float kd) { - // Check if the parameters are valid - if (kp < 0.0f || ki < 0.0f || kd < 0.0f) { - return; - } - - // Save the parameters for displaying purposes - pid->dispKp = kp; - pid->dispKi = ki; - pid->dispKd = kd; - - // Alter the parameters for PID - pid->alteredKp = kp; - pid->alteredKi = ki * pid->sampleTime; - pid->alteredKd = kd / pid->sampleTime; - - // Apply reverse direction to the altered values if necessary - if (pid->controlAction == PID_CONTROL_ACTION_REVERSE) { - pid->alteredKp = -(pid->alteredKp); - pid->alteredKi = -(pid->alteredKi); - pid->alteredKd = -(pid->alteredKd); - } -} - -void PIDTuningKpSet(PIDControl *pid, float kp) { - PIDTuningsSet(pid, kp, pid->dispKi, pid->dispKd); -} - -void PIDTuningKiSet(PIDControl *pid, float ki) { - PIDTuningsSet(pid, pid->dispKp, ki, pid->dispKd); + float value = pid_controller.kp * pid_controller.error + integral + pid_controller.kd * derivative; + return value; } -void PIDTuningKdSet(PIDControl *pid, float kd) { - PIDTuningsSet(pid, pid->dispKp, pid->dispKi, kd); -} - -void PIDcontrolActionSet(PIDControl *pid, PID_ControlActionTypeDef controlAction) { - // If in automatic mode and the controller's sense of direction is reversed - if (pid->mode == PID_MODE_AUTOMATIC && controlAction == PID_CONTROL_ACTION_REVERSE) { - // Reverse sense of direction of PID gain constants - pid->alteredKp = -(pid->alteredKp); - pid->alteredKi = -(pid->alteredKi); - pid->alteredKd = -(pid->alteredKd); - } - - pid->controlAction = controlAction; -} - -void PIDSampleTimeSet(PIDControl *pid, float sampleTimeSeconds) { - float ratio; - - if (sampleTimeSeconds > 0.0f) { - // Find the ratio of change and apply to the altered values - ratio = sampleTimeSeconds / pid->sampleTime; - pid->alteredKi *= ratio; - pid->alteredKd /= ratio; - - // Save the new sampling time - pid->sampleTime = sampleTimeSeconds; +void pid_reset(PidController_t pid_controller) { + pid_controller.integrator = 0.0; + #ifdef PID_ERRORS_VECTOR + for (int i = 0; i < pid_controller.n_prev_errors; ++i) { + pid_controller.prev_errors[i] = 0.0; } + #else + pid_controller.prev_error = 0.0; + #endif } diff --git a/pid/pid.h b/pid/pid.h index cd5a183..b744014 100644 --- a/pid/pid.h +++ b/pid/pid.h @@ -1,345 +1,49 @@ -/** - * @file pid.c - * @author Daniele Facinelli [daniele.facinelli@studenti.unitn.it] - * @author Filippo Faccini [filippo.faccini@studenti.unitn.it] - * @author Simone Ruffini [simone.ruffini@tutanota.com] - * @date 2018-04-27 - * @updated 2021-05-11 Simone Ruffini: refactoring - * @ingroup - * @prefix PID - * - * @brief PID library - * - */ - -/* Define to prevent recursive inclusion -------------------------------------*/ -#ifndef _PID_CONTROLLER_H_ -#define _PID_CONTROLLER_H_ - -/* Includes ------------------------------------------------------------------*/ +#ifndef _PID_H_ +#define _PID_H_ #include #include -/* Exported types ------------------------------------------------------------*/ -typedef enum { - PID_MODE_MANUAL, /*!< PID controller is off, manual output control */ - PID_MODE_AUTOMATIC /*!< PID controller is on, automaitc output control */ -} PID_ModeTypeDef; - -typedef enum { - PID_CONTROL_ACTION_DIRECT, /*!< positive error gives positive control output correction */ - PID_CONTROL_ACTION_REVERSE /*!< positive error gives negative control output correction */ -} PID_ControlActionTypeDef; - -typedef struct { - float input; /*!< Input to the PID Controller */ - float lastInput; /*!< Previous input to the PID Controller */ - float output; /*!< Output of the PID Controller */ - float dispKp; /*!< User given product constant, used for display purpose */ - float dispKi; /*!< User given integral constant, used for display purpose */ - float dispKd; /*!< User given derivative constant, used for display purpose */ - float alteredKp; /*!< PID controller product gain */ - float alteredKi; /*!< PID controller integration gain */ - float alteredKd; /*!< PID controller derivative gain */ - float iTerm; /*!< Integral term */ - float sampleTime; /*!< PID controller sampling time (seconds) */ - float outMin; /*!< Output value min constraint */ - float outMax; /*!< Output value max constraint */ - float setpoint; /*!< User defined operating point */ - PID_ControlActionTypeDef controlAction; /*!< Method of correction action use by the controller: - The "verse" of output contribution with resepect to input */ - PID_ModeTypeDef mode; /*!< PID active switch*/ -} PIDControl; -/* Exported constants --------------------------------------------------------*/ -/* Exported macros -----------------------------------------------------------*/ -/* Exported functions --------------------------------------------------------*/ -/* Private types -------------------------------------------------------------*/ -/* Private variables ---------------------------------------------------------*/ -/* Private constants ---------------------------------------------------------*/ -/* Private Macros -----------------------------------------------------------*/ - -//********************************************************************************* -// Macros and Globals -//********************************************************************************* - -//********************************************************************************* -// Prototypes -//********************************************************************************* - -// -// PID Initialize -// Description: -// Initializes a PIDControl instantiation. This should be called at least once -// before any other PID functions are called. -// Parameters: -// pid - The address of a PIDControl instantiation. -// kp - Positive P gain constant value. -// ki - Positive I gain constant value. -// kd - Positive D gain constant value. -// sampleTimeSeconds - Interval in seconds on which PIDCompute will be called. -// minOutput - Constrain PID output to this minimum value. -// maxOutput - Constrain PID output to this maximum value. -// mode - Tells how the controller should respond if the user has taken over -// manual control or not. -// MANUAL: PID controller is off. User can manually control the -// output. -// AUTOMATIC: PID controller is on. PID controller controls the output. -// controlAction - The sense of direction of the controller -// DIRECT: A positive setpoint gives a positive output. -// REVERSE: A positive setpoint gives a negative output. -// Returns: -// Nothing. -// -extern void PIDInit( - PIDControl *pid, - float kp, - float ki, - float kd, - float sampleTimeSeconds, - float minOutput, - float maxOutput, - PID_ModeTypeDef mode, - PID_ControlActionTypeDef controlAction); - -// -// PID Compute -// Description: -// Should be called on a regular interval defined by sampleTimeSeconds. -// Typically, PIDSetpointSet and PIDInputSet should be called immediately -// before PIDCompute. -// Parameters: -// pid - The address of a PIDControl instantiation. -// Returns: -// True if in AUTOMATIC. False if in MANUAL. -// -extern bool PIDCompute(PIDControl *pid); - -// -// PID Mode Set -// Description: -// Sets the PID controller to a new mode. -// Parameters: -// pid - The address of a PIDControl instantiation. -// mode - Tells how the controller should respond if the user has taken over -// manual control or not. -// MANUAL: PID controller is off. User can manually control the -// output. -// AUTOMATIC: PID controller is on. PID controller controls the output. -// Returns: -// Nothing. -// -extern void PIDModeSet(PIDControl *pid, PID_ModeTypeDef mode); - -// -// PID Output Limits Set -// Description: -// Sets the new output limits. The new limits are applied to the PID -// immediately. -// Parameters: -// pid - The address of a PIDControl instantiation. -// min - Constrain PID output to this minimum value. -// max - Constrain PID output to this maximum value. -// Returns: -// Nothing. -// -extern void PIDOutputLimitsSet(PIDControl *pid, float min, float max); - -// -// PID Tunings Set -// Description: -// Sets the new gain constant values. -// Parameters: -// pid - The address of a PIDControl instantiation. -// kp - Positive P gain constant value. -// ki - Positive I gain constant value. -// kd - Positive D gain constant value. -// Returns: -// Nothing. -// -extern void PIDTuningsSet(PIDControl *pid, float kp, float ki, float kd); - -// -// PID Tuning Gain Constant P Set -// Description: -// Sets the proportional gain constant value. -// Parameters: -// pid - The address of a PIDControl instantiation. -// kp - Positive P gain constant value. -// Returns: -// Nothing. -// -extern void PIDTuningKpSet(PIDControl *pid, float kp); - -// -// PID Tuning Gain Constant I Set -// Description: -// Sets the proportional gain constant value. -// Parameters: -// pid - The address of a PIDControl instantiation. -// ki - Positive I gain constant value. -// Returns: -// Nothing. -// -extern void PIDTuningKiSet(PIDControl *pid, float ki); - -// -// PID Tuning Gain Constant D Set -// Description: -// Sets the proportional gain constant value. -// Parameters: -// pid - The address of a PIDControl instantiation. -// kd - Positive D gain constant value. -// Returns: -// Nothing. -// -extern void PIDTuningKdSet(PIDControl *pid, float kd); - -// -// PID Controller Direction Set -// Description: -// Sets the new controller direction. -// Parameters: -// pid - The address of a PIDControl instantiation. -// controlAction - The sense of direction of the controller -// DIRECT: A positive setpoint gives a positive output -// REVERSE: A positive setpoint gives a negative output -// Returns: -// Nothing. -// -extern void PIDcontrolActionSet(PIDControl *pid, PID_ControlActionTypeDef controlAction); - -// -// PID Sample Time Set -// Description: -// Sets the new sampling time (in seconds). -// Parameters: -// pid - The address of a PIDControl instantiation. -// sampleTimeSeconds - Interval in seconds on which PIDCompute will be called. -// Returns: -// Nothing. -// -extern void PIDSampleTimeSet(PIDControl *pid, float sampleTimeSeconds); - -// -// Basic Set and Get Functions for PID Parameters -// - -// -// PID Setpoint Set -// Description: -// Alters the setpoint the PID controller will try to achieve. -// Parameters: -// pid - The address of a PIDControl instantiation. -// setpoint - The desired setpoint the PID controller will try to obtain. -// Returns: -// Nothing. -// -inline void PIDSetpointSet(PIDControl *pid, float setpoint) { - pid->setpoint = setpoint; -} - -// -// PID Input Set -// Description: -// Should be called before calling PIDCompute so the PID controller will -// have an updated input value to work with. -// Parameters: -// pid - The address of a PIDControl instantiation. -// input - The value the controller will work with. -// Returns: -// Nothing. -// -inline void PIDInputSet(PIDControl *pid, float input) { - pid->input = input; -} - -// -// PID Output Get -// Description: -// Typically, this function is called after PIDCompute in order to -// retrieve the output of the controller. -// Parameters: -// pid - The address of a PIDControl instantiation. -// Returns: -// The output of the specific PID controller. -// -inline float PIDOutputGet(PIDControl *pid) { - return pid->output; -} - -// -// PID Proportional Gain Constant Get -// Description: -// Returns the proportional gain constant value the particular -// controller is set to. -// Parameters: -// pid - The address of a PIDControl instantiation. -// Returns: -// The proportional gain constant. -// -inline float PIDKpGet(PIDControl *pid) { - return pid->dispKp; -} - -// -// PID Integral Gain Constant Get -// Description: -// Returns the integral gain constant value the particular -// controller is set to. -// Parameters: -// pid - The address of a PIDControl instantiation. -// Returns: -// The integral gain constant. -// -inline float PIDKiGet(PIDControl *pid) { - return pid->dispKi; -} - -// -// PID Derivative Gain Constant Get -// Description: -// Returns the derivative gain constant value the particular -// controller is set to. -// Parameters: -// pid - The address of a PIDControl instantiation. -// Returns: -// The derivative gain constant. -// -inline float PIDKdGet(PIDControl *pid) { - return pid->dispKd; -} - -// -// PID Mode Get -// Description: -// Returns the mode the particular controller is set to. -// Parameters: -// pid - The address of a PIDControl instantiation. -// Returns: -// MANUAL or AUTOMATIC depending on what the user set the -// controller to. -// -inline PID_ModeTypeDef PIDModeGet(PIDControl *pid) { - return pid->mode; -} - -// -// PID Direction Get -// Description: -// Returns the direction the particular controller is set to. -// Parameters: -// pid - The address of a PIDControl instantiation. -// Returns: -// DIRECT or REVERSE depending on what the user set the -// controller to. -// -inline PID_ControlActionTypeDef PIDcontrolActionGet(PIDControl *pid) { - return pid->controlAction; -} - -inline float PIDInputGet(PIDControl *pid) { - return pid->input; -} - -#endif // PID_CONTROLLER_H +// #define PID_ERRORS_VECTOR + +typedef struct pidController_t{ + float kp; + float ki; + float kd; + float integrator; + float error; + float sample_time; + float set_point; + float anti_windUp; + #ifdef PID_ERRORS_VECTOR + uint8_t n_prev_errors; + int prev_error_index; + float *prev_errors; + #else + float prev_error; + #endif +} PidController_t; + +#ifdef PID_ERRORS_VECTOR +void pid_init(PidController_t pid_controller, + float kp, + float ki, + float kd, + float sample_time, + float anti_windUp, + float *prev_errors, + uint8_t n_prev_errors); +#else +void pid_init(PidController_t pid_controller, + float kp, + float ki, + float kd, + float sample_time, + float anti_windUp); +#endif + +void pid_update(PidController_t pid_controller, float status); +float pid_compute(PidController_t pid_controller); +void pid_reset(PidController_t pid_controller); + +#endif