PID Turning Function Causing Skipping

We’ve been running this code for a bit now, and inconsistently it will skip random lines.
The spot in which it skips is not identical each run, and we’ve run certain autons 50-100 times and have seen it skip in every turn. Previously, the PID Drive was skipping as well but this was fixed by changing the initial speed of the PID Drive. This quick fix was not successful with the PID Turn.

If anybody has any recommendations or has experienced this please let us know of any fixes.
Thank you!

Code attached below:

void pidTurn(double angle, int maxpower=6) {

//ENDPOINT defines the final value which the gyro should
//report after it turns through the desired angle
const double ENDPOINTTURN = angle;

double currentValueTurn = inertial1.rotation(degrees);
double currentErrorTurn = ENDPOINTTURN - currentValueTurn;
double previousErrorTurn = 0.00;
double totalErrorTurn = 0.00;
const double INTEGRAL_LIMIT = 100.0;

if(currentErrorTurn > 0) {
    LF.spin(directionType::fwd, 3, percentUnits::pct);
    RF.spin(directionType::rev, 3, percentUnits::pct);
    RB.spin(directionType::rev, 3, percentUnits::pct);
    LB.spin(directionType::fwd, 3, percentUnits::pct);
} else {
    LF.spin(directionType::fwd, 3, percentUnits::pct);
    RF.spin(directionType::rev, 3, percentUnits::pct);
    RB.spin(directionType::rev, 3, percentUnits::pct);
    LB.spin(directionType::fwd, 3, percentUnits::pct);
}
task::sleep(200);
//While loop ensures that the robot will keep
//turning until it reaches the endpoint
while(fabs(currentErrorTurn) > 5) {
    
    if(fabs(currentErrorTurn) < INTEGRAL_LIMIT)
        totalErrorTurn += currentErrorTurn;
    else
        totalErrorTurn = 0;
    
    if(previousErrorTurn * currentErrorTurn <= 0) totalErrorTurn = 0;
    
    double kP = .135; //0.1350
    double kI = 0.0; //0.03
    double kD = 0.0; //0.74
    
    double p = kP * currentErrorTurn;
    double i = kI * totalErrorTurn;
    double d = kD * (currentErrorTurn - previousErrorTurn);

    double power = (p+i+d);
    
    int finalpower;
    
  if(power > 0)
  {
    if(power>maxpower){
      finalpower=maxpower;
    }else{
      finalpower = power;
    }
  }
  else
  {
    if((fabs(power))>maxpower){
      finalpower=-maxpower;
    }else{
      finalpower = power;
    }
  }
    

    LF.spin(directionType::fwd, (finalpower), voltageUnits::volt);
    RF.spin(directionType::rev, (finalpower), voltageUnits::volt);
    LB.spin(directionType::fwd, (finalpower), voltageUnits::volt);
    RB.spin(directionType::rev, (finalpower), voltageUnits::volt);
    
    currentValueTurn = inertial1.rotation(degrees);
    previousErrorTurn = currentErrorTurn;
    currentErrorTurn = ENDPOINTTURN - currentValueTurn;
    
    task::sleep(20);
    if((fabs(LB.velocity(velocityUnits::pct))) * (fabs(RB.velocity(velocityUnits::pct))) < 1) break;

}
drivestop();
task::sleep(50);
drivestopcoast();

}

Bumping this up because I’m still having issues with this. If anyone could help it would be greatly appreciated.

Are you using internal or external encoders? If external, are the insides clean of all dust?

PID turn is inertial. I am pretty sure there is a code issue.