update pid, work version
This commit is contained in:
Alekseev
2
.gitattributes
vendored
Normal file
2
.gitattributes
vendored
Normal file
@@ -0,0 +1,2 @@
|
||||
# Auto detect text files and perform LF normalization
|
||||
* text=auto
|
||||
BIN
PID Controller Implementation in Software.pdf
Normal file
BIN
PID Controller Implementation in Software.pdf
Normal file
Binary file not shown.
93
PID.c
93
PID.c
@@ -1,55 +1,38 @@
|
||||
#include "PID.h"
|
||||
|
||||
/*test push*/
|
||||
void PIDController_Init(PIDController *pid) {
|
||||
|
||||
void pidControllerInit(PidController *pid, float Kp, float Ki, float Kd, float tau, float limMin, float limMax,
|
||||
float limMinInt, float limMaxInt, float T) {
|
||||
pid->Kp = Kp;
|
||||
pid->Ki = Ki;
|
||||
pid->Kd = Kd;
|
||||
pid->KpDefault = Kp;
|
||||
pid->KiDefault = Ki;
|
||||
pid->KdDefault = Kd;
|
||||
/* Clear controller variables */
|
||||
pid->integrator = 0.0f;
|
||||
pid->prevError = 0.0f;
|
||||
|
||||
pid->tau = tau;
|
||||
pid->limMin = limMin;
|
||||
pid->limMax = limMax;
|
||||
pid->limMinInt = limMinInt;
|
||||
pid->limMaxInt = limMaxInt;
|
||||
pid->T = T;
|
||||
pid->differentiator = 0.0f;
|
||||
pid->prevMeasurement = 0.0f;
|
||||
|
||||
/* Clear controller variables */
|
||||
pid->integrator = 0.0f;
|
||||
pid->prevError = 0.0f;
|
||||
pid->out = 0.0f;
|
||||
|
||||
pid->differentiator = 0.0f;
|
||||
pid->prevMeasurement = 0.0f;
|
||||
|
||||
pid->out = 0.0f;
|
||||
|
||||
pid->enable = false;
|
||||
}
|
||||
|
||||
float pidControllerUpdate(PidController *pid, float setpoint, float measurement) {
|
||||
float PIDController_Update(PIDController *pid, float setpoint, float measurement) {
|
||||
|
||||
/*
|
||||
* Error signal
|
||||
*/
|
||||
/*
|
||||
* Error signal
|
||||
*/
|
||||
float error = setpoint - measurement;
|
||||
|
||||
|
||||
/*
|
||||
* Proportional
|
||||
*/
|
||||
/*
|
||||
* Proportional
|
||||
*/
|
||||
pid->proportional = pid->Kp * error;
|
||||
|
||||
|
||||
/*
|
||||
* Integral
|
||||
*/
|
||||
/*
|
||||
* Integral
|
||||
*/
|
||||
pid->integrator = pid->integrator + 0.5f * pid->Ki * pid->T * (error + pid->prevError);
|
||||
|
||||
/* Anti-wind-up via integrator clamping */
|
||||
/* Anti-wind-up via integrator clamping */
|
||||
if (pid->integrator > pid->limMaxInt) {
|
||||
|
||||
pid->integrator = pid->limMaxInt;
|
||||
@@ -61,30 +44,19 @@ float pidControllerUpdate(PidController *pid, float setpoint, float measurement)
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* Derivative (band-limited differentiator)
|
||||
*/
|
||||
|
||||
pid->differentiator = -(2.0f * pid->Kd * (measurement -
|
||||
pid->prevMeasurement) /* Note: derivative on measurement, therefore minus sign in front of equation! */
|
||||
+ (2.0f * pid->tau - pid->T) * pid->differentiator)
|
||||
/ (2.0f * pid->tau + pid->T);
|
||||
/*
|
||||
* Derivative (band-limited differentiator)
|
||||
*/
|
||||
|
||||
pid->differentiator = -(2.0f * pid->Kd * (measurement - pid->prevMeasurement) /* Note: derivative on measurement, therefore minus sign in front of equation! */
|
||||
+ (2.0f * pid->tau - pid->T) * pid->differentiator)
|
||||
/ (2.0f * pid->tau + pid->T);
|
||||
|
||||
|
||||
/*
|
||||
* Compute output and apply limits
|
||||
*/
|
||||
pid->out = pid->proportional + pid->integrator + pid->differentiator;
|
||||
|
||||
if (pid->proportional > pid->limMax) {
|
||||
|
||||
pid->proportional = pid->limMax;
|
||||
|
||||
} else if (pid->proportional < pid->limMin) {
|
||||
|
||||
pid->proportional = pid->limMin;
|
||||
|
||||
}
|
||||
/*
|
||||
* Compute output and apply limits
|
||||
*/
|
||||
pid->out = pid->offset + pid->proportional + pid->integrator + pid->differentiator;
|
||||
|
||||
if (pid->out > pid->limMax) {
|
||||
|
||||
@@ -96,10 +68,11 @@ float pidControllerUpdate(PidController *pid, float setpoint, float measurement)
|
||||
|
||||
}
|
||||
|
||||
/* Store error and measurement for later use */
|
||||
pid->prevError = error;
|
||||
/* Store error and measurement for later use */
|
||||
pid->prevError = error;
|
||||
pid->prevMeasurement = measurement;
|
||||
|
||||
/* Return controller output */
|
||||
/* Return controller output */
|
||||
return pid->out;
|
||||
|
||||
}
|
||||
|
||||
66
PID.h
66
PID.h
@@ -1,51 +1,43 @@
|
||||
#ifndef PID_CONTROLLER_H
|
||||
#define PID_CONTROLLER_H
|
||||
|
||||
#include <stdbool.h>
|
||||
|
||||
typedef struct {
|
||||
|
||||
/* Controller gains */
|
||||
float Kp;
|
||||
float Ki;
|
||||
float Kd;
|
||||
/* Controller gains */
|
||||
float Kp;
|
||||
float Ki;
|
||||
float Kd;
|
||||
|
||||
/* Default controller gains */
|
||||
float KpDefault;
|
||||
float KiDefault;
|
||||
float KdDefault;
|
||||
/* Derivative low-pass filter time constant */
|
||||
float tau;
|
||||
|
||||
/* Derivative low-pass filter time constant */
|
||||
float tau;
|
||||
/* Output limits */
|
||||
float limMin;
|
||||
float limMax;
|
||||
|
||||
/* Integrator limits */
|
||||
float limMinInt;
|
||||
float limMaxInt;
|
||||
|
||||
/* Output limits */
|
||||
float limMin;
|
||||
float limMax;
|
||||
/* Sample time (in seconds) */
|
||||
float T;
|
||||
|
||||
/* Integrator limits */
|
||||
float limMinInt;
|
||||
float limMaxInt;
|
||||
/* pid coefficient offset for start pid */
|
||||
float offset;
|
||||
|
||||
/* Sample time (in seconds) */
|
||||
float T;
|
||||
/* Controller "memory" */
|
||||
float integrator;
|
||||
float prevError; /* Required for integrator */
|
||||
float proportional;
|
||||
float differentiator;
|
||||
float prevMeasurement; /* Required for differentiator */
|
||||
|
||||
bool enable;
|
||||
/* Controller output */
|
||||
float out;
|
||||
|
||||
/* Controller "memory" */
|
||||
float proportional;
|
||||
float integrator;
|
||||
float prevError; /* Required for integrator */
|
||||
float differentiator;
|
||||
float prevMeasurement; /* Required for differentiator */
|
||||
} PIDController;
|
||||
|
||||
/* Controller output */
|
||||
float out;
|
||||
void PIDController_Init(PIDController *pid);
|
||||
float PIDController_Update(PIDController *pid, float setpoint, float measurement);
|
||||
|
||||
} PidController;
|
||||
|
||||
void pidControllerInit(PidController *pid, float Kp, float Ki, float Kd, float tau, float limMin, float limMax,
|
||||
float limMinInt, float limMaxInt, float T);
|
||||
|
||||
float pidControllerUpdate(PidController *pid, float setpoint, float measurement);
|
||||
|
||||
#endif //PID_CONTROLLER_H
|
||||
#endif
|
||||
|
||||
65
PID_Test.txt
Normal file
65
PID_Test.txt
Normal file
@@ -0,0 +1,65 @@
|
||||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
|
||||
#include "PID.h"
|
||||
|
||||
/* Controller parameters */
|
||||
#define PID_KP 2.0f
|
||||
#define PID_KI 0.5f
|
||||
#define PID_KD 0.25f
|
||||
|
||||
#define PID_TAU 0.02f
|
||||
|
||||
#define PID_LIM_MIN -10.0f
|
||||
#define PID_LIM_MAX 10.0f
|
||||
|
||||
#define PID_LIM_MIN_INT -5.0f
|
||||
#define PID_LIM_MAX_INT 5.0f
|
||||
|
||||
#define SAMPLE_TIME_S 0.01f
|
||||
|
||||
/* Maximum run-time of simulation */
|
||||
#define SIMULATION_TIME_MAX 4.0f
|
||||
|
||||
/* Simulated dynamical system (first order) */
|
||||
float TestSystem_Update(float inp);
|
||||
|
||||
int main()
|
||||
{
|
||||
/* Initialise PID controller */
|
||||
PIDController pid = { PID_KP, PID_KI, PID_KD,
|
||||
PID_TAU,
|
||||
PID_LIM_MIN, PID_LIM_MAX,
|
||||
PID_LIM_MIN_INT, PID_LIM_MAX_INT,
|
||||
SAMPLE_TIME_S };
|
||||
|
||||
PIDController_Init(&pid);
|
||||
|
||||
/* Simulate response using test system */
|
||||
float setpoint = 1.0f;
|
||||
|
||||
printf("Time (s)\tSystem Output\tControllerOutput\r\n");
|
||||
for (float t = 0.0f; t <= SIMULATION_TIME_MAX; t += SAMPLE_TIME_S) {
|
||||
|
||||
/* Get measurement from system */
|
||||
float measurement = TestSystem_Update(pid.out);
|
||||
|
||||
/* Compute new control signal */
|
||||
PIDController_Update(&pid, setpoint, measurement);
|
||||
|
||||
printf("%f\t%f\t%f\r\n", t, measurement, pid.out);
|
||||
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
float TestSystem_Update(float inp) {
|
||||
|
||||
static float output = 0.0f;
|
||||
static const float alpha = 0.02f;
|
||||
|
||||
output = (SAMPLE_TIME_S * inp + output) / (1.0f + alpha * SAMPLE_TIME_S);
|
||||
|
||||
return output;
|
||||
}
|
||||
5
README.md
Normal file
5
README.md
Normal file
@@ -0,0 +1,5 @@
|
||||
# PID
|
||||
PID controller implementation written in C.
|
||||
|
||||
Note on 'derivative-on-measurement': Since the 'error signal' effectively going into the differentiator does not depend on the setpoint: e[n] = 0 - measurement, and therefore (e[n] - e[n - 1]) = (0 - measurement) - (0 - prevMeasurement) = -Kd * (measurement - prevMeasurement). (Note the minus sign compared to derivative-on-error!)
|
||||
I've included the minus sign in the code, so gains will have the effect as normal.
|
||||
Reference in New Issue
Block a user