RWAR's soon to be PID controller

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

Is everyone here really that afraid of PID controllers? :smiley: Would’ve thought I’d have gotten a response by now.

At any rate, I think I forgot to factor in delta time in the integral and derivative terms. See for corrected code:

/*
    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 int DeltaTime = 20;
    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 + (error * DeltaTime));
        
        // Calculate derivative term
        float derivative = Kd * ((error - PrevError) / DeltaTime);
        
        // 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;
    }
}

for what you have it looks good. Fine tunning the parameters is key once you have live input from your course control. As it stands now it a very good pid class for directional steering.

Thanks Bstag!

I made one more modification to make sure the robot knows how to correct in the direction of least work.

Could you please look at it one last time? ;D

If you could, look specifically at the integral and derivative definitions. Are those correct in the way they factor in dt? They seem to be correct, but I just want to be sure.

/*
 * PID Pseudocode (kinda sorta C#ish)
 *        Chris Seto 2010
 *        
 * Released under the Apache 2.0 license.
 * Copyright Chris Seto 2010
 * */


public static class PIDTest
{
    /// <summary>
    /// Tuning coefficient for the the proportional term
    /// </summary>
    const float Kp = 1;

    /// <summary>
    /// Tuning coefficient for the the integral term
    /// </summary>
    const float Ki = 1;

    /// <summary>
    /// Tuning coefficient for the the derivative term
    /// </summary>
    const float Kd = 1;

    /// <summary>
    /// Time from one PID frame to the next
    /// TODO: Make sure this is more than the time it takes for
    /// the steering servo to move from 0-180 degrees
    /// </summary>
    const int DeltaTime = 20;
    
    /// <summary>
    /// How much of a left/right error are we going to allow?
    /// </summary>
    const float AllowError = 1; 

    /// <summary>
    /// Error from the last PID frame
    /// </summary>
    private float PrevError = 0;

    /// <summary>
    /// Sum of all errors
    /// </summary>
    private float SteadyError = 0;

    /// <summary>
    /// Last servo setting
    /// </summary>
    private float LastServoSet = 90;
    
    /// <summary>
    /// Heading to hold
    /// This is private because we DON'T want anything to change it without
    /// going through the reset method
    /// </summary>
    private float TargetHeading;
    
    /// <summary>
    /// If the target heading needs to be changed, this is the method to use
    /// </summary>
    /// <param name="targetHeading"></param>
    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)
    }
    
    /// <summary>
    /// This will be called by a timer to make the PID calculated correction
    /// The timer needs to run in less time than it takes for the servo to cycle 0-180 degrees
    /// </summary>
    private static void MakeCorrection()
    {
        // Calculate error
        // (let's just assume CurrentHeading really is the current GPS heading, OK?)
        float error = (TargetHeading - CurrentHeading);

        // We calculated the error, but we need to make sure the error is set so that we will be correcting in the 
        // direction of least work. For example, if we are flying a heading of 2 degrees and the error is a few degrees
        // to the left of that ( IE, somewhere around 360) there will be a large error and the rover will try to turn all
        // the way around to correct, when it could just turn to the right a few degrees.
        // In short, we are adjusting for the fact that a compass heading wraps around in a circle instead of continuing
        // infinity on a line
        if (error < -180)
            error = error + 360;
        else if (error > 180)
            error = error - 360;
        
        // 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 + (error * DeltaTime));
        
        // Calculate derivative term
        float derivative = Kd * ((error - PrevError) / DeltaTime);
        
        // 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;
    }
}

Will compare against my C++ pid class i used previously tonight and let you know.

Thanks! I really appreciate it ;D

although i must admit i havent implemented PID using .net, its on the list of classes i need to write when i find the time. The algorithm is pretty close to what i use for industrial process control [ i use plc rockwell controllers for a living], if i dont want to get bogged down in a wash of parameters and pre written blocks then i use something very similar. one thing important in PID as im sure you know is the tuning performance, quite often i will use a fixed ramp to get the process value close to the setpoint then kick in the pid controller, this can help under or overdamping when the two values are far apart ie cold starts, with the benfit of using tighter intergral and derviative settings as the loop is only working around a smaller error.

alternatively if tuning is causeing problems it is possible to utilise two seperate loops one for rough ramping and one for fine tuning during runtime,

For tuning PID loops the old and proven procedure is as follows
1- set the Integral and devivative to 0
2- Increase the proportational value until the system start to hunt around the sp over/undershoot
3-Increase intergral to stop the oscillations
4- increase the deviative for faster response.

As a general note on PID tuning
P= proportional amount of correction to amount of error [Get me Near the Sp]
I=smooths control but decreases response [Stop Over/under Shoot]
D=boosts output for error over time [Fix Response]

Yeah Chris it looks very solid. As nubiarn said I would use the Ziegler-Nichols method for tuning. I would start Kp around .2 to .4

Quickly takes code and runs away giddy. Heh this was on my list for the table top robot this week.

I would do a simulation in Matlab first… once I get the numbers right I will plug into the system.

Alright. I stayed up till almost 3 last night and everything is ready to be tested. I’m waiting for the battery to charge a little bit and then I will take it out.

Integral and Derivative gains are set to 0
Proportional gain is set to .2

I think .8 is a little high. I did some messing with it last night and 1 was waaaayyyy to high. OK, enough talk, I’ll be back in 10 minutes.

ARGH! The stupid new ESC won’t run. I need to turn the lipo cutoff off, but their stupid menu is too hard to navigate. You have to listen and count beeps to see which setting you are on. Of course, the beep go faster than you can count them.

if the Integral and Derivative gain is 0, that means you are having a pure Proportional control.

It might not gonna work…

Exactly. I’m tuning it.

I just got off the phone with Castle Creations support about the ESC. turns out a hosed a few settings up while trying to program it last night. Ooops. Got it all resolved.

Kp is now at 1. Anything below 1 is way too little.

OK, it works, but it hunts more than anything. It does, however travel in a semi straight line that it hunts on.

Wikipedia says to increase Kp until the loop oscillates, but it oscillates no matter what! What do they mean?

try resetting the steadyerror and error values adjust the i term then restart the loop from fresh as build up of errors can distort the tuning results

something that springs to mind, have you tried ramping the setpoint change rather than a large step change.

I don`t know the actual dynamics of your system but assuming a 180degrees full rotaion takes for ease of maths 1 second, then determine current setpoint subtract new setpoint that and equate that number to a ration of the 1 second movment and use that as a static ramp rate for the setpoint change, essentially you end up removing a large setpoint step change and but it has no effect on controller output response.

technically the solution suggested is addressing intergral windup in a PID loop, which is caused by large setpoint changes and causing a increased error build up.

I will have to post some code. But in trying this on Fez robot, the true light sensor path is say 65 on the line between black and white. The problem is, the sweet spot is so small, that minor changes off 65 make motor speed 100%. I never want to see 100% as that will always overshoot. How to limit top speed to say 50 and still stay proportional without just clamping the values? tia

Chris - i think there is a error in your code, the steady stateerror doesnot take into account the delta time.

replace
SteadyError += error

for
SteadyError += (error * DeltaTime));

the reason for this is that steady error terminalolgy can be better descripted as previous intergral term. and your intergral terms is integral = Ki * (SteadyError + (error * DeltaTime)); so the intergral term is effectively (SteadyError + (error * DeltaTime));

that would also explain why adjusting the integral is not removing the oscillations from the loop like it should do.
i hope that makes sense

Hi Nubiarn,

How does this look?

        /// <summary>
        /// Do the PID correction
        /// </summary>
        /// <param name="o"></param>
        private static void DoPID(object o)
        {
            BoardLED.Write(true);
            float currentHeading = (float)RazorIMU.Yaw;

            Debug.Print("Heading: "+ currentHeading.ToString());

            // Calculate error
            // (let's just assume CurrentHeading really is the current GPS heading, OK?)
            float error = (TargetHeading - currentHeading);

            // We calculated the error, but we need to make sure the error is set so that we will be correcting in the 
            // direction of least work. For example, if we are flying a heading of 2 degrees and the error is a few degrees
            // to the left of that ( IE, somewhere around 360) there will be a large error and the rover will try to turn all
            // the way around to correct, when it could just turn to the right a few degrees.
            // In short, we are adjusting for the fact that a compass heading wraps around in a circle instead of continuing
            // infinity on a line
            if (error < -180)
                error = error + 360;
            else if (error > 180)
                error = error - 360;

            // 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 (MyAbs(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) / DeltaTime);

            // 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
            Steering.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 * DeltaTime);

            // Set the last servo position
            LastServoSet = absolutePos;

            BoardLED.Write(false);
        }

Thanks for all the help, ;D I really appreciate it. Next testing opportunity will be tomorrow. For right now, I’m waiting for the battery to charge again.

you need to keep the original intergral term
float integral = Ki * (SteadyError + (error * DeltaTime)); // Correct

correctly made this addition in adding the deltatime when calculating the he previous integral
SteadyError += (error * DeltaTime); // correct