diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/flywheelTuning.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/flywheelTuning.java index 3b43674..a6350e3 100755 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/flywheelTuning.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/flywheelTuning.java @@ -40,7 +40,7 @@ public class flywheelTuning extends OpMode { public static double curTargetVelocity = 0; public static double targetPOS = 0; public static int pipeline = 1; - public static boolean reverse = false; + public static boolean reverse = true; double distance = 0; @@ -51,6 +51,17 @@ public class flywheelTuning extends OpMode { double snapshotAutoHood = 0.0; double calculatedGain = 0.0; + public static double P = 5; + public static double I = 0; + public static double D = 11; + public static double F = 14.3; + + private double lastP = P; + private double lastI = I; + private double lastD = D; + private double lastF = F; + + @Override public void init() { flywheelMotorR = hardwareMap.get(DcMotorEx.class, Constants.TurretConstants.TURRET_RIGHT_MOTOR); @@ -61,8 +72,8 @@ public class flywheelTuning extends OpMode { flywheelMotorL.setMode(DcMotorEx.RunMode.RUN_USING_ENCODER); flywheelMotorL.setDirection(reverse ? DcMotorSimple.Direction.FORWARD : DcMotorSimple.Direction.REVERSE); - flywheelMotorR.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, new PIDFCoefficients(2, 0, 11, 14.3)); - flywheelMotorL.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, new PIDFCoefficients(2, 0, 11, 14.3)); + flywheelMotorR.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, new PIDFCoefficients(P, I, D, F)); + flywheelMotorL.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, new PIDFCoefficients(P, I, D, F)); hoodServo = hardwareMap.servo.get("hoodservo"); hoodServo.setDirection(Servo.Direction.REVERSE); @@ -125,6 +136,17 @@ public class flywheelTuning extends OpMode { double autoHoodPos = getHoodPOS(distance); + if (P != lastP || I != lastI || D != lastD || F != lastF) { + PIDFCoefficients newCoefficients = new PIDFCoefficients(P, I, D, F); + flywheelMotorR.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, newCoefficients); + flywheelMotorL.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, newCoefficients); + + lastP = P; + lastI = I; + lastD = D; + lastF = F; + } + if (autoMode) { switch (calibState) { @@ -188,8 +210,6 @@ public class flywheelTuning extends OpMode { hoodServo.setPosition(Range.clip(applyHood + recoilOffset, Constants.TurretConstants.SERVO_MIN, Constants.TurretConstants.SERVO_MAX)); - - // --- TELEMETRY --- telemetry.addData("Operating Mode", autoMode ? "AUTOMATIC" : "MANUAL"); telemetry.addData("Calibration State", calibState.toString()); telemetry.addLine();