# Exiting a PID loop

Do you have any examples of how you called it later on in the auto section of the match that you could possibly share

This doesn’t look like a pid loop to me. You never change your voltage after initial calling.

``````void drivePID(double target) {
target *= 360 / (3.25 * M_PI);
DLB.resetRotation();

double kp = 0.33;
double ki = 0.0005;
double kd = 0.2;

double proportion;
double integralRaw;
double integral;
double integralActiveZone = 360 / (3.25 * M_PI);
double integralPowerLimit = 50 / ki;
double derivative;
double finalPower;

double error = target;
double lastError = target;
sleepMs(50);
while(abs(error) > 75){
error = target - DLB.rotation(rotationUnits::deg);
if(error == 0){
break;
}
proportion = kp * error;

if(abs(error) < integralActiveZone && error != 0){
integralRaw += error;
}
else integralRaw = 0.0;

if(integralRaw > integralPowerLimit){
integralRaw = integralPowerLimit;
}
if(integralRaw < -integralPowerLimit){
integralRaw = integralPowerLimit;
}

integral = ki * integralRaw;

derivative = kd * (error - lastError);
lastError = error;

if(error == 0){
derivative = 0;

}
finalPower = 0.5 * (proportion + integral + derivative);
driveForward(finalPower);
sleepMs(20);
}
DLF.stop(brakeType::coast);
DLB.stop(brakeType::coast);
DRF.stop(brakeType::coast);
DRB.stop(brakeType::coast);
}
``````

We had the loop in the actual function, and then passed our target as a parameter. If by coasting past the value you mean that you use brakeType::coast, you could always try a different brakeType.

7 Likes

Is this written in VCS

We wrote it in Robot Mesh.

Ok Thank You So Much

driveForward(finalPower);

Is the driveForward a function that you have in your code

It’s just a function to make the robot drive forward or back.

Oh sorry my question was if the driveForward is a function that you created or another function embedded inside of the coding language itself

It’s a function he made. You can see the code is customizable and has void right before the function name.

1 Like

The 2 main parameters are error (obviously) and maxSpeed so the robot doesn’t go too fast if I need to slow it a bit down

1 Like

Could You possibly send a snippet of your code if possible for like your parameter and the pid loop

I’ll just send the basic part of it and how it used parameters

``````void forward(int error, int maxSpeed){
while(absolute of error greater than 10){
//PID Calculations and whatever else;
int speed = P + I + D;
If speed > maxSpeed, then speed = maxSpeed
Set motor to speed
}
``````

The 1st parameter is used to for the target distance and if it’s within 10 units away, it will end the function. The 2nd parameter is used to compare with the calculated speed from the PID calculations. If the calculated speed is greater than the maxSpeed, it will limit itself to maxSpeed.

4 Likes

Hol’up. Are you declaring variables every time this function is called? Are you also sleeping every time the function is called?

If this is for auton, then I would recommend including a few args like “timeout” and “precision” so that your pid exits as needed.

1 Like

Hi, I have another question about toggles in VCS

How do you make a toggle on a button for the lift?

In your while loop for user control add something along the lines of `if(Controller1.ButttonX.pressing()){`
And then in the if add lift spin code

careful about dividing by zero in the signleft/signright lines. If your error ever reaches zero, there’s gonna be a problem with youe code. normally for this I would use a signum function (make one yourself if there isnt one in cmath).

2 Likes

I was very happy to see that almost every important aspect of the robust PID algorithm was already covered by many people.

The most important thing to know for PID exit condition, is whether it is intended for user control or autonomous.

In case of the user control - PID shouldn’t have its own while loop, but the main while(true) loop that has all the code for checking user inputs would call PID function once in 10-20 mS and let it determine necessary motor power. Variables like accumulated integral and prevSensorValue need to persist outside of the PID function.

This way if user presses a key, the PID control could be easily interrupted and user could take over with manual control. You code will have to have a variable that indicates if it is in the PID or manual control state (for each specific subsystem).

This could be very useful for cases when you want to press a macro key to set lift target and continue driving while the lift moves to target position under the control of PID. If you have multiple subsystems that could benefit from PID, then you can interleave all those PIDs and user control under a single while(true) loop.

This method of running multiple PIDs at once is great both for user control and high performance autonomous, but it requires more experienced programmer to do it right.

However, since @OK_BOOMER said in OP that they need PID for autonomous, then the code could be simplified by having PID function with its own while() loop that doesn’t return control back to the main program until it is finished.

@Anomalocaris has posted a very good code example that handles both integralActiveZone and integralPowerLimit (aka integral windup). Then if you add guards against division by zero as @mattjiang had suggested and max motor power cap guard per @thorstenl312 example, then you will have almost ready to use algorithm.

One thing I will add is that calculating of the derivative Kd can sometimes be noisy and, if that is the case you have an option to either not use it at all or employ some type of smoothing over multiple samples, or just increase sampling interval.

The remaining part of the puzzle for the case of autonomous, when drivers cannot interfere, is that you need your exit condition to be either `abs(error)<threshold` or `timeRunningThisStep > maxAllotedTime` for which you need to remember system time before starting PID loop and check current system time on every iteration with `vex::timer::system()`.

https://api.vexcode.cloud/v5/html/classvex_1_1timer.html#aa9e59b87a0bb93463e1e8d580cb7bea6

Finally, the original @OK_BOOMER’s code had special handling for driving straight vs turning, which is very important to have for good autonomous driving PID. This had been discussed in the past and I am going to link several threads with good examples.

This is an older RobotC example, but is still good for reference:

http://www.robotc.net/wikiarchive/Tutorials/Arduino_Projects/Mobile_Robotics/VEX/Using_encoders_to_drive_straight

In general, once you start feeling comfortable with using multiple interleaving PID loops at once, you will want to have one set of PID coefficients to control robot moving forward and another to control it heading.

11 Likes