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(){
RightMotor.setPosition(0,degrees);
LeftMotor.setPosition(0,degrees);
}
void driveStraight(int distance) // cm
{
resetEncoders();
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;
}
else
{
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;
}
else
{
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){
RightMotor.stop();
LeftMotor.stop();
break;
}
}
}
```

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?