I wrote out a quick rough draft of RWAR’s heading-hold autopilot PID controller. Anybody have any comments or suggestions?

Not that this is largely pseudocode. It really only serves to show how I intend on coding the final optimized controller.

```
/*
PID Pseudocode (kinda sorta C#ish)
Chris Seto 2010
Released under the Apache 2.0 license.
Copyright Chris Seto 2010
*/
public static class PIDTest
{
// Tuning coefficient constants
const float Kp = 1;
const float Ki = 1;
const float Kd = 1;
// How much error is allowable?
const float AllowError = 1;
// Last error
private float PrevError = 0;
// Total error
private float SteadyError = 0;
// Last servo setting (0-180)
private float LastServoSet = 90;
// Heading to hold
// This is private because we DON'T want anything to change it without
// going through the reset method
private float TargetHeading;
// If the target heading needs to be changed, this is the method to use
public static void ChangeTargetHeading(float targetHeading)
{
// (Some code here to pause the PID timer)
PrevError = 0;
SteadyError = 0;
TargetHeading = targetHeading;
// (Some code here to resume the PID timer)
}
// This method will be called by an timer. Maybe 10Hz?
private static void MakeCorrection()
{
// Calculate error
// (let's just assume CurrentHeading really is the current GPS heading, OK?)
float error = (TargetHeading - CurrentHeading);
// We need to allow for a certain amount of tolerance.
// If the abs(error) is less than the set amount, we will
// set error to 0, effectively telling the equation that the
// rover is perfectly on course.
if (abs(error) < AllowError)
error = 0;
// Calculate proportional term
float proportional = Kp * error;
// Calculate integral term
float integral = Ki * SteadyError;
// Calculate derivative term
float derivative = Kd * (error - prevError);
// Add them all together to get the correction delta
float correction = proportional + integral + derivative;
// Add the delta to the last servo setting to
// get the absolute servo setting
float absolutePos = correction + lastServoSet;
// Make sure we aren't going out of range of the steering servo
if (absolutePos > 180)
absolutePos = 180;
else if (absolutePos < 0)
absolutePos = 0;
// Set the steering servo to the correction
servo.Degree = absolutePos;
// At this point, the current PID frame is finished
// ------------------------------------------------------------
// Now, we need to setup for the next PID frame
// The "current" error is now the previous error
// (Remember, we are done with the current frame, so in
// relative terms, the previous frame IS the "current" frame)
PrevError = error;
// Add the error calculated in this frame to the running total
SteadyError += error;
// Set the last servo position
LastServoSet = absolutePos;
}
}
```