// 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; }