I’ve been workng on PID and I’m not sure how to adjust the values and also make sure that everything is coded correctly. It’s my first time trying and I work best by seeing example code. Unfortunately, no one seems to have vexcode examples that are detailed. Wondering if someone could share their code for me to look at and learn off of? I just need to see some examples.
- Tuning is specific to your robot. Test it and change kp, ki, and kd depending on your results.
- Can you post your cose so we can see if it is programmed correctly?
Also, I should note that @Connor made a great tutorial on PID. Link to that is here.
1 Like
void drivePID(double target){
double error;
double prevError = 0; // position 10ms ago
double derivative; // difference between error and prevError: calculates speed
double integral = 0;
// get position of four motors
LFDrive.setPosition(0, degrees);
LBDrive.setPosition(0, degrees);
RFDrive.setPosition(0, degrees);
RBDrive.setPosition(0, degrees);
while (true){
double LFMotorPosition = LFDrive.position(degrees);
double LBMotorPosition = LBDrive.position(degrees);
double RFMotorPosition = RFDrive.position(degrees);
double RBMotorPosition = RBDrive.position(degrees);
double kP = 0.6, kI = 0.0, kD = 4;
double accelRate = 0.0;
// get average of the four motors (sensor value)
double averagePosition = ((LFMotorPosition + LBMotorPosition) / 2 + (RFMotorPosition + RBMotorPosition) / 2);
// position
error = target - averagePosition;
// derivative
derivative = error - prevError;
// integral
integral += error;
double lateralMotorPower = (error * kP + derivative * kD + integral * kI);
if (fabs(target)/target==1){
if(target<=error){
accelRate += 1000;
lateralMotorPower = accelRate;
}
else if(fabs(target)/target==-1){
if (target >= error){
accelRate += -1000;
lateralMotorPower = accelRate;
}
}
}
if (lateralMotorPower >= 430){
lateralMotorPower=430;
}
if (lateralMotorPower<=-430){
lateralMotorPower=-430;
}
if(error<=0){
integral=0;
}
LFDrive.spin(forward, lateralMotorPower, velocityUnits::rpm );
LBDrive.spin(forward, lateralMotorPower, velocityUnits::rpm );
RFDrive.spin(forward, lateralMotorPower, velocityUnits::rpm );
RBDrive.spin(forward, lateralMotorPower, velocityUnits::rpm );
prevError = error;
task::sleep(10);
}
}
The code formatted:
void drivePID(double target){
double error;
double prevError = 0; // position 10ms ago
double derivative; // difference between error and prevError: calculates speed
double integral = 0;
// get position of four motors
LFDrive.setPosition(0, degrees);
LBDrive.setPosition(0, degrees);
RFDrive.setPosition(0, degrees);
RBDrive.setPosition(0, degrees);
while (true){
double LFMotorPosition = LFDrive.position(degrees);
double LBMotorPosition = LBDrive.position(degrees);
double RFMotorPosition = RFDrive.position(degrees);
double RBMotorPosition = RBDrive.position(degrees);
double kP = 0.6, kI = 0.0, kD = 4;
double accelRate = 0.0;
// get average of the four motors (sensor value)
double averagePosition = ((LFMotorPosition + LBMotorPosition) / 2 + (RFMotorPosition + RBMotorPosition) / 2);
// position
error = target - averagePosition;
// derivative
derivative = error - prevError;
// integral
integral += error;
double lateralMotorPower = (error * kP + derivative * kD + integral * kI);
if (fabs(target)/target==1){
if(target<=error){
accelRate += 1000;
lateralMotorPower = accelRate;
}
else if(fabs(target)/target==-1){
if (target >= error){
accelRate += -1000;
lateralMotorPower = accelRate;
}
}
}
if (lateralMotorPower >= 430){
lateralMotorPower=430;
}
if (lateralMotorPower<=-430){
lateralMotorPower=-430;
}
if(error<=0){
integral=0;
}
LFDrive.spin(forward, lateralMotorPower, velocityUnits::rpm );
LBDrive.spin(forward, lateralMotorPower, velocityUnits::rpm );
RFDrive.spin(forward, lateralMotorPower, velocityUnits::rpm );
RBDrive.spin(forward, lateralMotorPower, velocityUnits::rpm );
prevError = error;
task::sleep(10);
}
}
By the way, If you want to format code, put ``` at the top and bottom of your code.
3 Likes
I’m currently having troubles going from a turn to drive PID or vice versa. It will complete the first function, but during auton, it will not move on. Any ideas as to why this is happening?
You don’t have any conditions to exit the while loop so the first time you call this method, it will start and keep running infinitely until the autonomous period is over.
To answer your original question, the reason you do not see VexCode specific examples of PID algorithms is because the theory behind PID is the same no matter what environment you are in. If you have a solid understanding of PID, feedback loops, and how to use sensors you could write a PID algorithm in virtually any environment. For that reason, authors of guides and tutorials often tend to rely on math and pseudocode since that can be translated into any situation. I would encourage you to familiarize yourself with the theory behind the calculations because it will make sight-reading non-VexCode-specific examples easier. Furthermore, if you have solid understanding of the underlying theory you could write your own custom implementation very easily. Luckily for you, I put together a collection of resources that span from abstract theory to concrete (RobotC) code. It’s a collection of several years of information and you can really learn a lot if you dedicate the time to understand it.
5 Likes
Yeah I realized the while loop was running infinitely. Now I just need to figure out how to be able to put in a value for drivePID in inches so I can say drive forward 10 inches and it will do that in the PID. Any thoughts?
void drivePID(double target){
double error; // sensor value - desired value : calculates position (derivative of position = speed)&(derivative of speed = acceleration)
double prevError = 0; // position 20ms ago
double derivative; // difference between error and prevError: calculates speed
double integral = 0; // totalError = totalError + error =(integral)
bool running =true;
// get position of four motors
LFDrive.setPosition(0, degrees);
LBDrive.setPosition(0, degrees);
RFDrive.setPosition(0, degrees);
RBDrive.setPosition(0, degrees);
while (running){
double LFMotorPosition = LFDrive.position(degrees);
double LBMotorPosition = LBDrive.position(degrees);
double RFMotorPosition = RFDrive.position(degrees);
double RBMotorPosition = RBDrive.position(degrees);
double kP = 0.4, kI = 0.0, kD = 0.0;
double accelRate = 0.0;
// get average of the four motors (sensor value)
double averagePosition = ((LFMotorPosition + LBMotorPosition) / 2 + (RFMotorPosition + RBMotorPosition) / 2);
// position
error = target - averagePosition;
// derivative
derivative = error - prevError;
// integral (velocity -> position -> absement)
integral += error;
double lateralMotorPower = (error * kP + derivative * kD + integral * kI);
if (fabs(target)/target==1){
if(target<=error){
accelRate += 1000;
lateralMotorPower = accelRate;
}
else if(fabs(target)/target==-1){
if (target >= error){
accelRate += -1000;
lateralMotorPower = accelRate;
}
}
}
if (lateralMotorPower >= 430){
lateralMotorPower=430;
}
if (lateralMotorPower<=-430){
lateralMotorPower=-430;
}
if(error<=0){
integral=0;
}
LFDrive.spin(forward, lateralMotorPower, velocityUnits::rpm );
LBDrive.spin(forward, lateralMotorPower, velocityUnits::rpm );
RFDrive.spin(forward, lateralMotorPower, velocityUnits::rpm );
RBDrive.spin(forward, lateralMotorPower, velocityUnits::rpm );
prevError = error;
task::sleep(12);
Brain.Screen.clearScreen();
Brain.Screen.setCursor(1,1);
Brain.Screen.print("error:");
Brain.Screen.print(error);
Brain.Screen.newLine();
Brain.Screen.print("lmp:");
Brain.Screen.print(lateralMotorPower);
if((error=0)){
running=false;
}
}
}