Skip to content

Commit

Permalink
pid controller rewritten
Browse files Browse the repository at this point in the history
  • Loading branch information
@conquat
conquat committed Jun 12, 2024
1 parent 412ac7f commit 87c0672
Show file tree
Hide file tree
Showing 2 changed files with 115 additions and 510 deletions.
239 changes: 70 additions & 169 deletions pid/pid.c
View file
Original file line number Diff line number Diff line change
@@ -1,181 +1,82 @@
/**
* @file pid.c
* @author Daniele Facinelli [[email protected]]
* @author Filippo Faccini [[email protected]]
* @author Simone Ruffini [[email protected]]
* @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
}
Loading

0 comments on commit 87c0672

Please sign in to comment.