void drivePID(double target) {
target *= 360 / (3.25 * M_PI);
DLB.resetRotation();
double kp = 0.33;
double ki = 0.0005;
double kd = 0.2;
double proportion;
double integralRaw;
double integral;
double integralActiveZone = 360 / (3.25 * M_PI);
double integralPowerLimit = 50 / ki;
double derivative;
double finalPower;
double error = target;
double lastError = target;
sleepMs(50);
while(abs(error) > 75){
error = target - DLB.rotation(rotationUnits::deg);
if(error == 0){
break;
}
proportion = kp * error;
if(abs(error) < integralActiveZone && error != 0){
integralRaw += error;
}
else integralRaw = 0.0;
if(integralRaw > integralPowerLimit){
integralRaw = integralPowerLimit;
}
if(integralRaw < -integralPowerLimit){
integralRaw = integralPowerLimit;
}
integral = ki * integralRaw;
derivative = kd * (error - lastError);
lastError = error;
if(error == 0){
derivative = 0;
}
finalPower = 0.5 * (proportion + integral + derivative);
driveForward(finalPower);
sleepMs(20);
}
DLF.stop(brakeType::coast);
DLB.stop(brakeType::coast);
DRF.stop(brakeType::coast);
DRB.stop(brakeType::coast);
}
We had the loop in the actual function, and then passed our target as a parameter. If by coasting past the value you mean that you use brakeType::coast, you could always try a different brakeType.