@anon4126930@mvas
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;
vex::task::sleep(20);
}
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.
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).
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.
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.
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.
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.
First of all @anon4126930 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.
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.
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.
I want to provide additional detail for teams who are utilizing pid on their tray and it’s going too fast. I would advise having the error changed from:
This would set it to the 3rd power, which would turn it into a curve that would speed up quick then slow down the tray much more as it’s nearing the target. We don’t do 2nd power because if something is to the second power any negative value will be multiplied by a negative, resulting in always positive values. Note, you will have to re-tune accordingly.
Thanks a lot for the tutorial it was really helpful. So the only question i had was if I need to add thresholds or time-stops for the tuning part of the code. If you had to include this stuff to exit the loop, where do you put it, and how exactly do you write this part of the code?
