I want make my PID like go instant onto next PID without stopping but can slow down. For example: Transition into next movement.
void DrivePID(double targetInches, double Kp = 2.7, double Ki = 0.02, double Kd = 0.2,
int maxSpeed = 90, double momentum = 0.5, double headingKp = 0.5) {
ForwardTrackingWheel.setPosition(0,degrees);
double initialHeading = Inertial1.heading(degrees); // store starting direction
double error = targetInches - encoderToInches(ForwardTrackingWheel.position(degrees));
double integral = 0;
double lastError = error;
while (fabs(error) > momentum) {
double currentPosition = encoderToInches(ForwardTrackingWheel.position(degrees));
error = targetInches - currentPosition;
// PID distance control
if (fabs(error) < 10) integral += error;
if (integral > 100) integral = 100;
if (integral < -100) integral = -100;
double derivative = error - lastError;
double pid = Kp * error + Ki * integral + Kd * derivative;
// Clamp drive power
double drivePower = pid;
if (drivePower > maxSpeed) drivePower = maxSpeed;
if (drivePower < -maxSpeed) drivePower = -maxSpeed;
// Heading correction
double headingError = normalizeError(initialHeading - Inertial1.heading(degrees));
double turnCorrection = headingError * headingKp; // higher = tighter correction
// Apply correction to left/right sides
double leftPower = drivePower + turnCorrection;
double rightPower = drivePower - turnCorrection;
// Clamp both sides
if (leftPower > 100) leftPower = 100;
if (leftPower < -100) leftPower = -100;
if (rightPower > 100) rightPower = 100;
if (rightPower < -100) rightPower = -100;
// Send to motors
LeftDrive.setVelocity(leftPower, percent);
Left3rd.setVelocity(leftPower, percent);
RightDrive.setVelocity(rightPower, percent);
Right3rd.setVelocity(rightPower, percent);
LeftDrive.spin(forward);
Left3rd.spin(forward);
RightDrive.spin(forward);
Right3rd.spin(forward);
lastError = error;
wait(20, msec);
}
// Stop
LeftDrive.stop(brake);
Left3rd.stop(brake);
RightDrive.stop(brake);
Right3rd.stop(brake);
}