Straight Drive PID Problem VEXcode Pro V5

Here is robot’s straight drive PID control loop:

double distkP = 2;
double distkI = 0.005;
double distkD = 0.1;

double diffkP = 2;
double diffkI = 0;
double diffkD = 0;
double driveTarget;
double distSpeed;
double diffSpeed;
double distError; 
double diffError;
double prevDistError;
double prevDiffError;
double diffIntegral;
double distIntegral;
double distDerivative; 
double diffDerivative;

bool enableDrivePID = true; 
void resetEncoders(){
void driveStraight(int distance) // cm
  while (distError != -1)
    Brain.Screen.printAt( 10, 50, "distError %6.2f", distError);
    Brain.Screen.printAt( 10, 80, "diffError %6.2f", diffError);
    double leftEncoder = (LeftMotor.position(degrees) / 360) * 31.92;
    double rightEncoder = (RightMotor.position(degrees) / 360) * 31.92;

    distError = distance - ((leftEncoder + rightEncoder)/2); //Calculate distance error
    diffError = leftEncoder - rightEncoder; //Calculate difference error
    // Find the integral ONLY if within controllable range AND if the distance error is not equal to zero
    if( std::abs(distError) < 2 && distError != 0)
      distIntegral = distIntegral + distError;
      distIntegral = 0; //Otherwise, reset the integral
    // Find the integral ONLY if within controllable range AND if the difference error is not equal to zero
    if( std::abs(diffError) < 60 && diffError != 0)
      diffIntegral = diffIntegral + diffError;
      diffIntegral = 0; //Otherwise, reset the integral
    distDerivative = distError - prevDistError; //Calculate distance derivative
    distDerivative = diffError - prevDiffError; //Calculate difference derivative
    prevDistError = distError; //Update previous distance error
    prevDiffError = diffError; //Update previous difference error
    distSpeed = (distError * distkP) + (distIntegral * distkI) + (distDerivative * distkD); //Calculate distance speed
    diffSpeed = (diffError * diffkP) + (diffIntegral * diffkI) + (diffDerivative* diffkD); //Calculate difference (turn) speed
    prevDistError = 0;
    prevDiffError = 0;
    LeftMotor.spin(forward, distSpeed - diffSpeed, voltageUnits::volt); //Set motor values
    RightMotor.spin(forward, distSpeed + diffSpeed, voltageUnits::volt); //Set motor values
    if(distError < 1 && distError > -1){

The problem is, the robot stops when the break statement comes in but the right motor is continuing to move like 1 sec more. And the main question is, is there any problem with my code?

Could perhaps the Left motor be set to brakeType::brake and the Right motor set to brakeType::coast? That could explain why it goes through the entire loop only to have a problem after it finishes.

I would alter your stopping code to


And see if it does anything.


Thank you for the answer. I realized my Omni-wheel which located rear is causing movement at the end of the loop. Because of that, I moved them to the front. Apart from these do you see any mistake in the code?

The only (obvious) mistake I see is this.

You set distDerivative twice, when the second should probably be diffDerivative.

Also, your exit condition is not the strongest, even though it should work. Perhaps use a timer.


Good catch @trontech569!

Also, I don’t see any delay inside the loop.

Perhaps, initialize both Integrals to zero, add vex::task::sleep(20) to the loop, and don’t reset prev*Error to zero.


I’m not really a code guy, so im gonna shoot for friction issues in your drivebase?

Thank you for everything, I handled the problem. The problem was like I said Omni-wheels on the back. But your PID comments made it better :slight_smile: