umm forget this message i typed somethiong and then realised after i typed it what i wrote was wrong <<<<< ignore this … bed time i think brain seems to be malfuctioning
Ok, great!
I put the original term back.
As for tuning, do you have any recommendations?
IT WORKS!!!
I had to change a few things in code, but the biggest issue was something nobody noticed. Basically, if I keep the code the same way with the correction delta constantly added to the servo’s last position, it will eventually add the servo degree up to the point where it could never be corrected by simply adding the (presumably negative) correction delta.
Instead, I started thinking about it for a little while, and I realized that whole approach was totally convoluted.
Instead, I set it up so that the correction delta is added to 90 (servo center). Now, the pid controller is only deciding how much to deflect off center.
With all terms except for the proportional term zero’d out, this makes perfect sense. If the car has a heading error of -1 degree, the servo will not be at 0 (as it may have been in the last version of the controller), it will be at 89, which is one degree less than a center position. This means the car will drift over into the error band and then the controller will go inactive until the car escapes.
Here is the final version that probably needs to be tuned a little more and cleaned up:
// Bring the LED up to signify frame start
BoardLED.Write(true);
// Get IMU heading
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);
//Debug.Print("Error: " + error.ToString());
// 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;
//Debug.Print("P: " + proportional.ToString());
// 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 + ServoLast;
//Debug.Print("Pos: " + absolutePos.ToString());
// Set the steering servo to the correction
//Steering.Degree = absolutePos;
Steering.Degree = 90 + correction;
// 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;
//ServoLast = absolutePos;
// Add the error calculated in this frame to the running total
SteadyError += (error * DeltaTime);
BoardLED.Write(false);
effectively adding a fixed offset to to pid loop, thats a good call, as you need a output to achieve a central positon where as most loops or actuators would be outputting zero when at rest.
glad you got the code working and great work as i`m sure it will help alot of people as pid is often misunderstood or steered away.
So I did as much testing today as my battery permitted, and it looks like the FEZ is able to keep RWAR within about ~ 6 degrees of it’s target heading. Don’t ask me why it’s minus six degrees, it just is. I think I will add a trim variable to trim that error out, otherwise, I think one of the PID terms will handle steady errors.
Does anyone have any tips of tuning the controller? I aim to do that tomorrow.
it it always 6 degrees away from the setpoint ? is it the same amount out both sides of the servo central position ?
Yeah, it’s the damnedest thing, it’s always around -6 degrees. I have never seen a positive error, only negative.
float error = (TargetHeading - currentHeading); // current code
float error = (TargetHeading - currentHeading) + Offset; // try this
try adding a fixed offset to the error of 6, it would be interesting to see if the loop still had a -6 error or whther the loop owuld effectively shove the servo 6 degrees across, i think it should just shove the output 6 deg.
sometimes when a control system requires a certain value when the proceess varible = the setpoint which is not zero then a fixed offset can be added straight to the pid code such as
output = P + I + D + Offset.
i wish i had some hardware to test this out , i must get myself organised fast 
What your describing Chris pid should correct for. If you need a offset the the inital values something else is up. Ok well we can talk this weekend in chat. I will get my blitz converted to fez so i can work on the same type of issue with you. I need to spend some time on this anyways.
Awesome, had a 3 hour conversation with my EE buddy. We ironed out some of the calculus, so I am doing more testing now.
Can’t wait to talk to you, Bstag!
Hmm…
I spent a good hour in the front lawn today testing by doing runs then making adjustments from my laptop.
For whatever the reason, I am having a really big problem with the integral term. If I take it to even .1 the truck will go totally nuts and steer in what appears to be a random pattern.
I did add .2 Kd, which seemed to flatten out the response. I don’t notice it oscillating as much.
Also, large code update:
/// <summary>
/// Do the PID correction
/// </summary>
/// <param name="o"></param>
private static void DoPID(object o)
{
// Bring the LED up to signify frame start
BoardLED.Write(true);
// Get IMU heading
float currentHeading = (float)RazorIMU.Yaw;
int deltaTime = (int)((LastCorrectionTime - DateTime.Now.Ticks) / 10000);
// Calculate error
// (let's just assume CurrentHeading really is the current GPS heading, OK?)
float error = (TargetHeading - currentHeading);
LCD.Lines[0].Text = "Heading: "+ currentHeading.ToString("F2");
// 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;
// Add the error calculated in this frame to the running total
SteadyError = SteadyError + (error * deltaTime);
// 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;
LCD.Lines[2].Text = "Error: " + error.ToString("F2");
// 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;
// Set the steering servo to the correction
Steering.Degree = 88 + correction;
LastCorrectionTime = DateTime.Now.Ticks;
// At this point, the current PID frame is finished
// ------------------------------------------------------------
// Now, we need to setup for the next PID frame and close out
// 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;
// Done
BoardLED.Write(false);
}
I found out the proper term for the issues that I have been having with the integral term is “integral windup”.
I came up with a patch, represented below.
/// <summary>
/// Do the PID correction
/// </summary>
/// <param name="o"></param>
private static void DoPID(object o)
{
// Bring the LED up to signify frame start
BoardLED.Write(true);
// Get IMU heading
float currentHeading = (float)RazorIMU.Yaw;
int deltaTime = (int)((LastCorrectionTime - DateTime.Now.Ticks) / 10000);
// Calculate error
// (let's just assume CurrentHeading really is the current GPS heading, OK?)
float error = (TargetHeading - currentHeading);
LCD.Lines[0].Text = "Heading: "+ currentHeading.ToString("F2");
// 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;
// Add the error calculated in this frame to the running total
SteadyError = SteadyError + (error * deltaTime);
// Solve for integral windup
if (MyAbs(SteadyError) > 180)
SteadyError = 0;
// 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;
LCD.Lines[2].Text = "Error: " + error.ToString("F2");
// 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;
// Set the steering servo to the correction
Steering.Degree = 88 + correction;
LastCorrectionTime = DateTime.Now.Ticks;
// At this point, the current PID frame is finished
// ------------------------------------------------------------
// Now, we need to setup for the next PID frame and close out
// 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;
// Done
BoardLED.Write(false);
}
What do you guys think?
Interesting, thanks for sharing.
What are the Kd, Ki and Kp values? Are they program constants?
Why are you passing in the object? I don’t see it being used any where. Is it for future use?
Chris
looks like you read up on what we talked about. looks even more awesome then before.
mhectorgato
P I D are tuned for his loop so him giving default values would be not good for you. if you need some direction in that let us know we can help.
During the chat today he sent me a few links about it - the wiki just confused me to no ends. I have the concept in mind, but not the actual details.
I was just wondering what those were (constants or values set in a different section of code) in his code, as the snippet didn’t show it.
How did he go about getting those values for his application?
Hi bstag,
I wrote a little thing about it on my blog: [url]http://www.chrisseto.com/wordpress/?p=279[/url]
The important part isn’t the little burb, though, it’s the code link: [url]http://files.chrisseto.com/jQN[/url]
What do you think about integral windup? Do you think that is what was causing my previous errors? Remember, it (was) running at 10Hz, so any error added up quite rapidly.
Changes also include a 2X increase in PID speed. It now runs at 20Hz.
@ mhectorgato: You might consider looking at the code at the link above. It includes the complete controller, so it should be able to clear up any confusion.
As for the wiki, I know how you feel. I had no idea how it worked until I had a friend explain it to me. It’s very easy to understand when it isn’t clouded by math.
Thanks for the code; makes a lot more sense to see how your fitting it all together now.
How did you come about the 200ms timing for the PID processing?
Is there a way to determine how well the FEZ chip is keeping up? In another words, could you go to 20ms and would there be lag?
Very cool stuff. Some day, hopefully soon, I’ll have my bot scampering around as well. Concepts gained from this forum will make that task easier and sophisticated!
You might be looking at outdated code if you see 200ms. It’s actually 100ms now. That link has been updated to the latest version…
At any rate, most all of those values were guessed from tuning sessions. Basically, I brought my laptop outside, ran the truck a few times, changed a few things and repeated. Eventually, I got closer and closer accuracy.
I’m not real sure how the USBizi is holding up. I do know that I don’t think I will be able to do GPS parsing on the same chip due to the intense load required to parse a veritable fire hose. In fact, when I had the IMU setup to just spew data out in the same way NMEA does, the USBizi almost never even had time to run the PID.
I assume in the future you’re going to be adding in sensors for obstacle avoidance.
How are you planning on fitting that in - in relation to the PID?
I don’t think I will be doing obstacle avoidance for a long time. If I do, it will temporarily take over control itself. The PID controller is only required to track a heading.