I am having problems tuning my PID, I know that this isn’t the clean PID code, but for some reason it takes the robot a long time to change speed and slow down, so instead of being like a smooth graph it is jumpy. It still gets to the position, but not efficiently. Any help is wanted, even with things that I didn’t mention.
void preAutonomous(void) {
// actions to do when the program starts
Brain.Screen.clearScreen();
goal.set(true);
corner.set(false);
Insensor.calibrate();
while(Insensor.isCalibrating()) //Calibrates the gyro in pre-auton
{
wait(10,msec);
Brain.Screen.setCursor(1, 1);
Brain.Screen.print("is calibrating");
}
Brain.Screen.clearScreen();
Brain.Screen.setCursor(1, 1);
Brain.Screen.print("Is calibrated");
}
float KP = .185; //PID Constants subject to changes
float KI = 0.000001;
float KD = 0.005;
float turnKP = 0.5;
float turnKI = 0.000001;
float turnKD = 0.1;
void DrivePID(float desire, float turnDesire, float maxp, bool turning, float maxtime) //maxp is the max power,
{ //turning if I'm turning or not
Brain.Timer.clear(); //Resets and initalizes all variables //maxtime as a redudant matter
float totalError = 0.0;
float prevError = 0.0;
float turnTotalError = 0;
float prevTurnError = 0;
float error = 100;
float turnError = 100;
bool PID = true;
Insensor.setHeading(0,degrees);
RS.setPosition(0,degrees);
while(PID) //actual PID loop
{
float average = RS.position(degrees)/1.625;
float averageTurn = Insensor.heading(); //gathers gyro position
turnError = turnDesire - averageTurn; //calculates error based gyro heading
error = desire - average; //calculates error based on motor positon and desire
if (turning) // if robot is turning don't calculate drive error
{
error = 0;
}
if(fabs(turnError) >= 180) //Allows for left turns
{
turnError = -fabs(turnError)/turnError * 360 - fabs(turnError);
}
if (fabs(error) < 1 && fabs(turnError) < .8) // Exit Loop Once we are close to desire
{
PID = false;
}
if(Brain.Timer.time(seconds) > maxtime) //Exit loop if running too long
{
PID = false;
}
totalError += error; //Calculate Derivative if stuck
turnTotalError += turnError;
if (turnTotalError > 5000) // Doesn't allow derivative to build up
{
turnTotalError = 0;
}
if (totalError > 5000)
{
totalError = 0;
}
float LMP = (error * KP) + (totalError * KI) + ((error - prevError) * KD);// Calculates drive power and turn power
float turnPower = (turnError * turnKP) + (turnTotalError * turnKI) + ((turnError - prevTurnError) * turnKD);
if(LMP > maxp) // restricts power
{
LMP = maxp;
}
if (LMP < -maxp)
{
LMP = -maxp;
}
if (turnPower > maxp)
{
turnPower = maxp;
}
if (turnPower < -maxp)
{
turnPower = -maxp;
}
LMS.spin(forward,LMP + turnPower,percent); //actual output
RMS.spin(forward,LMP - turnPower,percent);
prevError = error; //turns error into previous error
prevTurnError = turnError;
Brain.Screen.setCursor(1, 1); //prints value to screen for debugging and testing
Brain.Screen.print("Error: %.3f", error);
Brain.Screen.newLine();
Brain.Screen.print("Power: %.3f", LMP);
Brain.Screen.newLine();
Brain.Screen.print("Turn Error: %.3f", turnError);
Brain.Screen.newLine();
Brain.Screen.print("Heading: %.3f", averageTurn);
Brain.Screen.newLine();
Brain.Screen.print("Turn Power: %.3f", turnPower);
Brain.Screen.newLine();
Brain.Screen.print("Time: %.3f", Brain.Timer.time(seconds));
wait(25,msec);
}
LMS.stop(hold); //sets motors to hold at end of function
RMS.stop(hold);
}
Thank you so much!! I do have one question, though. What is a smooth ramp, and how does this function work? I am relatively new to C++, so I don’t know what fmin means or the input values. Also, where do you then use the variables maxPower and maxTurnPower?
A smooth ramp is just a way to make something increase or decrease gradually instead of jumping straight to a new value. Think of a car speeding up slowly instead of slamming the gas pedal.
What are maxPower and maxTurnPower?
maxPower: The highest speed or power you want something (like a motor) to go.
maxTurnPower: The highest speed or power allowed when turning, so it doesn’t spin out of control.
You use maxPower and maxTurnPower to make sure your values don’t go too high. For example:
cpp
Copy code
double motorSpeed = 12; // Speed we want
double maxPower = 10; // Limit it to 10
motorSpeed = fmin(motorSpeed, maxPower); // Now motorSpeed is 10
Your PID code looks like it should work fine, my guess is your kP, kI, and kD aren’t tuned right. I would recommend setting kI to 0 because you usually don’t need it (at least in most vex contexts), and just focus on kP and kD. Start by setting kD to 0, and increase kP until it overshoots the target and starts oscillating. Then, increase kD until it stops overshooting, and start increasing kP again. Repeat until you think it works well enough.
You can probably ignore what @sweetspotmaster said about the smooth ramp thing, they’ve AI generated a lot of posts recently and this looks like another one. Some parts of what the AI said might be useful and based in the truth (the smooth ramp thing is a real, just probably not useful here), but other parts will probably just be confusing because the AI doesn’t always know important details and is sometimes just wrong.
It would also help if you could give a little more description of what is happening. Is it speeding up slowly? Is it overshooting the target? Does it change directions really fast? etc.
I am accelerating too fast at the start, possibly throwing off my values or tipping my robot; I don’t think it has too much effect on the overall code, however I would rather it be smoother
When I tell the robot to drive backwards using the programming, it always either undershoots or overshoots the mark. This has to do with how my function deals with negative values, but as of now I don’t know how to fit it.
Thank you for helping me, I may or may not have spent an hour researching profiling PID’s before seeing your post.
For issue 1, that sounds like the center of mass of your robot is too high, generally if you can get the weight low down on your robot you shouldn’t have a problem with tipping. If you don’t want to change the robot, you could (rather ironically) fix that with the smooth ramp thing. Here’s some example code for that:
// get how much the PID's output has changed
double output_change = pid_output - last_pid_output;
// if the change is too big, cap it at the maximum
if (output_change > max_output_change)
{
output_change = max_output_change;
}
else if (output_change < -max_output_change)
{
output_change = -max_output_change;
}
// use our newly calculated change to get the new output value
pid_output = last_pid_output + output_change;
last_pid_output = pid_output;
For issue 2, I think this is because your maximum for totalError only works for positive values. If error is negative, then totalError will be negative, and totalError > 5000 will always return false even if total error is very far from 0. Switching that to account for negatives should fix the issue.
Also another thing I just noticed, I would recommend not using percent when spinning the motors with your PID, use volts instead. The motors are actually spun with voltage, and giving a percent speed means VexOS has to put your values into a speed controller to convert them to voltage. This can cause unreliability, because while the speed controller is generally fine it gives your PID less control over the robot movement. You’ll need to retune if you do this though.