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