98ba591357
If you want to use derivative-on-measurement, you will need to negate the 'D'-gain you would typically use in a 'standard derivative-on-error' PID controller - I did not mention that in the video. 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). So, for example, if you require a controller with a D-gain of 10, you would set pid->Kd = -10. This is a slight quirk of the derivative-on-measurement form - however, the code can of course be adapted to include the minus sign.
62 lines
1.3 KiB
C
62 lines
1.3 KiB
C
#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.01f
|
|
|
|
#define PID_LIM_MIN -10.0f
|
|
#define PID_LIM_MAX 10.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,
|
|
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;
|
|
}
|