# VEXCode PID Tutorial

What do you want the PID for? If it’s to maintain equilibrium on the lift (maintaining both sides at an equal altitude), you could have a simple function that only executes if the lift is leaning too much to one side. Otherwise, the integrated PID is really great for maintaining motor position. You can also improve performance through hardware optimization.

2 Likes

I want to use the PID loop for maintaining equilibrium (thanks for asking).

What exactly do you mean by hardware optimisation?

In the case of maintaining equilibrium, would an Integral value actually be necessary?

That is one massive “It depends”
If you have a mere PD, it could help but not entirely, as when you add weight the robot will be less and less precise from the desired position. But other than the less precision and inaccuracies, it will work depending on what you are trying to do.

1 Like

Does this mean that in Driver control, where small changes will have to be made for one motor to maintain equilibrium, a PD loop will suffice - however in autonomous a dr4b lift may require an entire PID control loop, like the one shown in the video.

@Connor @mvas8037
I have tried to implement your code into a PID control loop where the Left Motor tries to stay at the same height as the right one. I also changed the encoder readings to raw data units rather than degrees. Could you please tell me if there’s anything wrong with my code before I start trying to tune my PID constants?

Lift PID function and variables

``````///Settings
double kP = 0.0;
double kI = 0.0;
double kD = 0.0;
int maxIntegral = 300;
int integralBound = 20; //If error is outside the bounds, then apply the integral. This is a buffer with +-integralBound degrees

//Autonomous Settings
int desiredValue = 0;

int error; //SensorValue - DesiredValue : Position
int prevError = 0; //Position 20 miliseconds ago
int derivative; // error - prevError : Speed
int totalError = 0; //totalError = totalError + error

bool resetLiftSensors = false;

//Variables modified for use
bool enableLiftPID = true;

//Pasted from a C++ resource
double signnum_c(double x) {
if (x > 0.0) return 1.0;
if (x < 0.0) return -1.0;
return x;
}

int liftPID(){
while(enableLiftPID){
if (resetLiftSensors) {
resetLiftSensors = false;
LiftMotorLeft.setPosition(0,degrees);
LiftMotorRight.setPosition(0,degrees);
}

//Get the position of both motors
int leftMotorPosition = LiftMotorLeft.position(rotationUnits::raw);
int rightMotorPosition = LiftMotorRight.position(rotationUnits::raw);

///////////////////////////////////////////
//Lateral movement PID
/////////////////////////////////////////////////////////////////////

//Potential
error = rightMotorPosition - leftMotorPosition;

//Derivative
derivative = error - prevError;

//Integral
if(abs(error) < integralBound){
totalError+=error;
}  else {
totalError = 0;
}
//totalError += error;

//This would cap the integral
totalError = abs(totalError) > maxIntegral ? signnum_c(totalError) * maxIntegral : totalError;

double lateralMotorPower = error * kP + derivative * kD + totalError * kI;
/////////////////////////////////////////////////////////////////////

LiftMotorLeft.spin(forward, lateralMotorPower, voltageUnits::volt);

prevError = error;

}

return 1;
}
``````

Running the function as a separate task (I put this at the beginning of autonomous):

``vex::task liftEquilibrium(liftPID);``

Thanks a lot for the guidance (having only started out with cpp about a month ago, this guide and your help is proving to be very useful despite having previous programming knowledge).

Remove any and all friction points (these usually form as a result of asymmetrical structure). Brace everything, especially the arms. You can use either boxed c-channel or box bolts to do this. Cross bracing is also extremely important. Using boxed 5 - wide c-channel is incredibly strong and relatively low profile, or an x-brace. Next is rubber banding. A dr4b will have constant tension with a triangular tension, however, maximizing load capacity will require a 2 point tension. Minimize your use of axles, and use as many bearings and screw joints as possible.

These are just the main things a well-built lift uses, there are nuances to every lift, so I can’t comment specifically for how yours can be improved.

You should probably declare this variable with the rest of your variables, the math up until this point seems to check out by the way. I want to note that I use PROS and not VexCode so some of the API looks a bit foreign. I’ll post the pseudocode to how I would approach the problem. Assume all variables have already been declared and the variable names mean what they say.

``````void setLiftMotors(int left, int right){
//sets the voltage of the motors
leftMotor = left;
rightMotor = right;
}
void liftEqual(){
while(!target && buttonPressed){
error = setPoint - averageCurrentPosition;

if(abs(error) < integralBound){
integral += error;
}else
integral = 0;

derivative = error - prevError;
prevError = error;

liftMotorEncoderDifference = leftMotor.get_position() - rightMotor.get_position();

liftPower = error * Kp + integral * Ki + derivative * Kd;

setLiftMotors(liftPower - liftMotorEncoderDifference, liftPower + liftMotorEncoderDifference);

delay(20);
}
setLiftMotors(0,0);
}
``````

This function would work for driver control. Again, this is largely pseudo code for you to understand the logic of what’s going on.

The first function is simply to condense the code; the parameters would be any unit of power (in PROS, you would typically set the voltage of the motor). Then you would put the next function in your driver control loop. It would only run if the button for the lift is pressed and the lift is not at the max height. Then it goes through the PID calculations. The importants line is `liftMotorEncoderDifference = leftMotor.get_position() - rightMotor.get_position();`. Think about what’s happening when the lift is not in equilibrium. One of the motor’s encoder values will be greater than the other, and if that value is positive or negative will tell you which side is leaning. Then you simply subtract that value from the left side and add it to the right. The signs work out because of how a double negative works.

Try and do the math out with this scenario in mind and it should make intuitive sense. The key thing to note is that the difference between these two motor positions will decrease over time as you add and subtract the values because what’s happening is your decreasing the power sent to the side that’s more “up” and increasing the power sent to the side that’s more “down”. Eventually the difference between the two will reach 0 when both sides are equal and the PID calculation value will be the only thing driving the lift. This will obviously impact settling time, however, you gain a much more stable lift.

4 Likes

I will definitely be trying to implement your pseudocode as it seems to build upon what I had previously and indentifies the errors. I’m also grateful for the explanation as to how it works - again thanks for this (I’ll be using it later today).

3 Likes

Hey i really enjoyed Your tutorial. It helped me alot to build a PID escpecially because i am new to VEX. I had one question though. What is the best way to tune the PID. Like the constants kP, ki. and kD. Also i do not understand what
int maxIntegral = 300;
int integralBound = 20;
int integralBound = 3;
mean in the code. Thanks alot for the help!

In the tutorial, I roughly explain how to tune them.
You increase P until there is subtle oscillation, then you increase D until the oscillation stops, and then you increase I until the target is exactly achieved.

2 Likes

In regards to integral, maxIntegral is a cap value you set that prevents integral from getting too high or too low. So if integral is ever above 300 the code will set the code back to 300, and if integral is below -300 then the code will be set back to -300. Integralbound is just another word for a deadzone. If error is within the range of 20 then it will just automatically set integral to 0 to prevent integral overshooting.

1 Like

Sorry, this may sound stupid but When You mean steady oscillations, will the robot move forward and backward slightly as it moves forwards(saying you were driving forward.

2 Likes

And If I understand what you said about integrals completely, if the ki value ever goes above 300, it would set the ki value back to 300. and if it is below -300, the code will be back to -300.

And for the Integralbound : if the error value is in the range given,(3 degrees), it will just stop the robot so it does not overshoot.

sorry for causing you this inconvience and thank you for your help.

It’ll stop the integral (or totalError) but not error itself.

First of all @Connor thank you so much for this Tutorial it helped me a lot because I am kind of new to all these PID stuff, however I had a small question left after your tutorial. So, in my autonomous I’m trying the take in the 4 horizontal cubes near the smaller goal zone, and in order to do so, I need to turn both my intake motors. How can I turn the intake motors simultaneously to the driving PID.

1 Like

Just start them spinning `motor.spin(forward);` and they will keep spinning until you `motor.stop();` them.

2 Likes

Is it possible to make a minimum bound for power in a PID loop. For example, if I wanted the robot not to stop at the end but to go at 30 pct velocity.

Are you able to make the code not go above a specified speed

I have been thinking of solutions for this but I am unsure how to do it. I would suggest just having it so as long as the target is reached, just tell the motor to brake with braketype hold.

@Skynet Just clamp the values https://en.cppreference.com/w/cpp/algorithm/clamp
Use

``````std::clamp(value, minvalue, maxvalue)
``````

Returns value

You may need to include the function with (I believe)

``````#include <cmath>
``````

On the top of the program file

1 Like