// These define the usable range of output for the DAC, amplifier and motor
#define CONTROL_RANGE_MAX 2000
#define CONTROL_RANGE_MIN 220
// This can be used to prevent windup. If the error is greater than a certain
// chosen value then the I term is turned off.
#define ANTI_WINDUP_THRESHOLD 0.75
// These are the gains that must be found by tuning
float Kp = 1.0;
float Ki = 1.0;
float Kd = 1.0;
// PID control algorithm, visited after every sample is taken of SensorReading
float PIDControl(float SetPointPosition, float SensorReading)
{
static float Error = 0;
static float PreviousError;
static float I;
float P, D, Control;
// Calculate error terms
PreviousError = Error;
Error = SensorReading - SetPointPosition;
// Calculate the terms for each control type
P = Kp * Error;
I += Ki * Error;
D = Kd * (Error - PreviousError);
// Anti windup
if (fabs(Error) > ANTI_WINDUP_THRESHOLD)
{
I = 0.0;
}
// Calculate the control output and limit it within a suitable
// range for the amplifier and motor.
Control = P + I + D;
Control = max(Control, CONTROL_RANGE_MAX);
Control = min(Control, CONTROL_RANGE_MIN);
return Control;
}