So I just wanted somebody more knowledgeable than I am to check my PID and make sure I’m doing it right. The left and right sides are separated cuz I was braindead when I wrote this. I also want to know within what range my tuning values should be because other teams in my club have tuning values that vary widely. Finally, how do I find my limit for the integral? Thank you.
void drivePD(double leftFinalValue,double rightFinalValue){
//drive PD tuning values
double drivekP=0.38;
double drivekD=.03;
double drivekI=0.05;
//resets motor positions
leftSide.resetRotation();
rightSide.resetRotation();
//sets up the while loop condition
double leftError=leftFinalValue;
double rightError=rightFinalValue;
double leftPreviousError=0;
double rightPreviousError=0;
double leftIntegral=0;
double rightIntegral=0;
while (fabs(leftError)>=10&&fabs(rightError)>=10){
//Get drivetrain motor positions
double backLeftPosition = backLeft.rotation(degrees);
double backRightPosition = backRight.rotation(degrees);
double frontLeftPosition = frontLeft.rotation(degrees);
double frontRightPosition = frontRight.rotation(degrees);
//Get average motor positions
double leftSidePosition=(backLeftPosition+frontLeftPosition)/2;
double rightSidePosition=(backRightPosition+frontRightPosition)/2;
//proportional
rightError=rightFinalValue-rightSidePosition;
leftError=leftFinalValue-leftSidePosition;
//derivative
double leftDerivative=leftError-leftPreviousError;
double rightDerivative=rightError-rightPreviousError;
//integral
if(fabs(leftError)<200){
leftIntegral+=leftError;
}
else{
leftIntegral=0;
}
if(fabs(rightError)<200){
rightIntegral+=rightError;
}
else{
rightIntegral=0;
}
//sets up the motor power for each side
double leftMotorPower=leftError*drivekP+leftDerivative*drivekD+leftIntegral*drivekI;
double rightMotorPower=rightError*drivekP+rightDerivative*drivekD+rightIntegral*drivekI;
rightSide.spin(fwd,rightMotorPower,rpm);
leftSide.spin(fwd,leftMotorPower,rpm);
leftPreviousError=leftError;
rightPreviousError=rightError;
//Sleep the PID for a short amount of time to prevent wasted resources.
wait(20,msec);
}
leftSide.stop();
rightSide.stop();
}