Check my PID pls

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();
}

the best way to be sure your code is good is to test it, because there might be small problems with the code that don’t stand out right away that are obvious when testing.

there is really not a definite range, because the ideal numbers are different for every robot, so the best way to find the correct constants (IMO) is to brute force the numbers. (i have heard some people say that the integral part of the PID controller is not very important, so if you are having problems with it, you can alwayse just take it out and test it.

you can keep them separate, but the code you have right now waits for both sides to reach the end goal, so if one side gets stuck on a ring (or in any other way stops moving) it will just keep spinning. if you do it so both are connected, this will be a smaller problem (but still a problem).

1 Like