Over the summer I’ve been working with Odom and PID and I was able to get both working separately: if I was driving my bot Odom would show its coordinates or if I gave the PID values it would drive and turn. Since then, I’ve been trying to combine the two and it’s been really challenging. I am at a point where when I tell the bot to move in a singular motion such as turning to a point or driving forward in a straight line it will succeed in doing so. The trouble comes in when I try and do complex movements like turn 45 degrees and drive or more specifically drive to a point that has different X and Y coordinates. In the last test I ran, I placed the robot in the center of the field (72, 72) and I coded it so that it should end up one tile length up and to the left (48, 96), however, what happens is that when the robot turns to the point and starts to drive it is off by a couple of inches and the PID ends up driving into the wall because it isn’t able to reach zero or close to it. I have summed this problem to be a PID issue rather than an Odom issue because driving the robot without PID, Odom correctly tracks the location.
What are some steps I can take to fix this issue? So far I’m thinking that my tolerances for the PID are too precise, but I want to see if there could be any other issue rather than just starting to mess with my code.
PID Code
double PID_Data::error = 0; // Target distance difference
double PID_Data::prevError = 0; // Last cycle distance difference
const double maxError = 0.75; // Error tolerance
// 0.5
double PID_Data::integral = 0; // The accumulated error
const double integralBound = 10; // The minium distance to the target to use the integral
// 1.5
double PID_Data::derivative = 0; // The change in error between cycles (prev and current)
const double kP = 0.5; // Error Gain
// 0.059// 0.07 // 0.025 // 0.03 // 0.05 // 0.055
const double kI = 0.0045; // Integral Gain
// 0.008// 0.007 //0.006 // 0.005
const double kD = 0.255; // Derivative Gain
// 0.255 // 0.40 // 0.20 //0.25
double PID_Data::drivePowerPID = 0; // The power that will be sent to the motors
double PID_Data::turn_Error = 0; // Target angle difference
double PID_Data::turn_PrevError = 0; // Last cycle angle difference
const double turn_MaxError = 0.010472; // tolerance of 0.6 degrees (in rads)
//0.01309; // 0.75 deg //0.01745329 // 1 deg //0.00087266 // 0.05 deg
double PID_Data::turn_Integral = 0; // The accumulated error
const double turn_IntegralBound = 0.34906585; // Minimum angle to target to use the integral (20 degrees)
// 0.17453293; // 10 degrees
double PID_Data::turn_Derivative = 0; // The change in error between cycles (prev and current)
const double turn_kP = 4.75; // Turn Error Gain
// 1.50 // 1.25 // 1.125 // 1.0625 // 1 // 2
const double turn_kI = 0.45; // Turn Integral Gain
// 0.3 //0.1
const double turn_kD = 1.0; // Turn Derivative Gain
// 0.45; //0.5
double PID_Data::turnPowerPID = 0; // The power that will be sent to the motors
void PID(){
// update the previous error
PID_Data::prevError = PID_Data::error;
PID_Data::turn_PrevError = PID_Data::turn_Error;
//Error is equal to the total distance away from the target
PID_Data::error = (targetX - odom.xPosGlobal) + (targetY - odom.yPosGlobal);
PID_Data::turn_Error = targetAngle - odom.currentAbsoluteOrientation;
// If the error is greater than pi radians, wrap it around to the other side of the circle
if(fabs(PID_Data::turn_Error) > M_PI) {
PID_Data::turn_Error = -fabs(PID_Data::turn_Error)/PID_Data::turn_Error * (M_PI * 2) - fabs(PID_Data::turn_Error);
}
//only use integral if close enough to target but allows for tolerance
if(fabs(PID_Data::error) < integralBound && fabs(PID_Data::error) > maxError) {
PID_Data::integral += PID_Data::error; // Add the error to the integral
} else PID_Data::integral = 0; // Reset the integral if we are too far from the target
// only use integral if close enough to target but allows for tolerance
if(fabs(PID_Data::turn_Error) < turn_IntegralBound && fabs(PID_Data::turn_Error) > turn_MaxError) {
PID_Data::turn_Integral += PID_Data::turn_Error; // Add the error to the integral
} else PID_Data::turn_Integral = 0; // Reset the integral if we are too far from the target
PID_Data::derivative = PID_Data::error - PID_Data::prevError; // calculate the derivative
// calculate the derivative
PID_Data::turn_Derivative = PID_Data::turn_Error - PID_Data::turn_PrevError;
// calculate the power output
PID_Data::drivePowerPID = (PID_Data::error * kP + PID_Data::integral * kI + PID_Data::derivative * kD);
PID_Data::turnPowerPID = (PID_Data::turn_Error * turn_kP +
PID_Data::turn_Integral * turn_kI +
PID_Data::turn_Derivative * turn_kD);
//Limit power output to 12V
PID_Data::drivePowerPID = clamp(PID_Data::drivePowerPID, -12, 12);
PID_Data::turnPowerPID = clamp(PID_Data::turnPowerPID, -12, 12);
// If in tolerance range, cut power
if(fabs(PID_Data::error) < maxError) PID_Data::drivePowerPID = 0;
if(fabs(PID_Data::turn_Error) < turn_MaxError) PID_Data::turnPowerPID = 0;
}
The variables that are used in the PID are attributes of an object, however, they are all static and are only used for our team’s data collection. Also, out of our whole project, only one PID data object is instantiated.
Most of this seems pretty good! A few things I noticed: are you running your PID code with a while loop? It sounds like you’ve been able to multi thread it along with the Odom, but I don’t see where you have a refresh for your pid function. In addition, your distance error calculation shouldn’t be (x_error)+(y_error). Using trig, the distance to any point on an xy plane should be sqrt((x_error)^2 + (y_error)^2). Let me know if any of this helps!
Thank you. Later in my code, the function is being called in a while loop.
loop
int driveControl() {
//loop to constantly execute commands and update values
while(1) {
//Check if we should run the control
if(runControl) {
//get PID values for driving and turning
PID();
//set power for each motor
leftFront.spin(fwd, PID_Data::drivePowerPID - PID_Data::turnPowerPID, volt);
leftMid.spin(fwd, PID_Data::drivePowerPID - PID_Data::turnPowerPID, volt);
leftBack.spin(fwd, PID_Data::drivePowerPID - PID_Data::turnPowerPID, volt);
rightBack.spin(fwd, PID_Data::drivePowerPID + PID_Data::turnPowerPID, volt);
rightMid.spin(fwd, PID_Data::drivePowerPID + PID_Data::turnPowerPID, volt);
rightFront.spin(fwd, PID_Data::drivePowerPID + PID_Data::turnPowerPID, volt);
// If the time out is reached, stop the loop
if(Brain_Timeout_timer.timer(timeUnits::msec) > timeOutValue) {
runControl = false;
}
}else{
leftFront.stop(coast);
leftMid.stop(coast);
leftBack.stop(coast);
rightBack.stop(coast);
rightMid.stop(coast);
rightFront.stop(coast);
}
// Set cycle time
task::sleep(20);
}
return 1;
}
As for the error equation, I started with the distance formula however it wouldn’t work correctly. I forget the behavior but it was worse and I tried to sub it out for the hypot function which was slightly better but neither really worked. We are using a standard drive so when I was debugging with the distance formula I thought that the type of drivetrain changed how it would respond to it. Meaning that since we can’t strafe it wouldn’t work as well.
I know that you said that distance didn’t work well, but I think* that you should find the perpendicular distance to the point, or whatever it’s called (read the post in the *).
(This applies to the turn integral too)
Even though you address integral windup by only letting the integral take effect when the error is in a certain range, you should still reset the total error (integral variable in your code) when the error crosses 0 (goes from positive to negative or vice versa), as this also helps with integral windup**.
**
According to Wikepedia (Integral windup - Wikipedia) that is called a Clegg integrator, but the article linked doesn’t even exist, so I am not sure. It seems to be mentioned in some research articles though, so that’s probably what it’s called.
I would use motor groups for this.
I think that the distance calculation is likely the main reason your code is failing, but I would also test it with both kI’s set to 0 to see if it is integral windup.
Sorry for not responding to this earlier, my builder needed to tinker. So without making any changes I tested the code again and I recorded it: Video. I understand why in the first test it didn’t move, and thats because I am not using the distance formula as mentioned by @Max_Waldman and @ProgrammingKid. And as for the second test, it was drastically different than the behavior I was originally describing. In both tests though, the robot turned to the right instead of the left (the open side of the drivetrain is the front) which is odd. During the recordings I also was collecting the data from the robot, again the first test looks mostly correct whereas the second doesn’t: Data Spreadsheet.
The pid and loop code is the same as the previous posts.
This is the function that I call for PID and Odom and the function call
Function:
void driveToPoint(double Target_X, double Target_Y, double timeOutLength) {
targetX = Target_X; // Set target X
targetY = Target_Y; // Set target Y
/*if (targetAngle < 0) targetAngle += 360;
targetAngle = Target_Angle * M_PI / 180;*/ // convert deg to rad
targetAngle = atan2(odom.xPosGlobal- Target_X, odom.yPosGlobal - Target_Y) - odom.currentAbsoluteOrientation - M_PI_2;
timeOutValue = timeOutLength; // Set timeout
runControl = true; // Let the feedback loop move the drivetrain
Brain_Timeout_timer.resetTimer(); // Reset and start countdown
}
call: driveToPoint(48, 48, 50000);
To make sure that the other functions work I’ll test those but for now this is what the issue is. Thanks!
I’m not sure if this is the only issue or if this is something you’ve already addressed, but looking at the data table I see a lot of integral windup. For your drive power that might be ok because you have a low Ki, but your turning Ki is ~0.5, so when your turning integral jumps past 24, you are giving full turning power to the motors. The second issue is that your code is assuming the starting position has a previous error of 0, meaning your initial derivative is very large - as can be seen by your drivetrain going to full power the moment the auton starts. This is usually fixed by putting the previous error update code after the motor output line. Lmk if any of this helps or you have any questions!
You should be using Target_X - odom.xPosGlobal instead of odom.xPosGlobal - Target_X (same for y), because you want to subtract your target from your position to get the correct direction. This is likely why your robot turned the wrong way (because it was trying to go the opposite direction). The reason it went the wrong direction in the second test is that your distance calculation doesn’t take the robot’s heading into account, and since the robot was already facing the wrong direction, instead of turning toward the target while driving forward (which would get it to the target eventually), it just drove forward into the wall. I hope this helps!