Arkm/Ftc-12926
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Update to flywheelTuning

Browse Source
This commit is contained in:
Arkm20
2026-03-15 13:00:07 -05:00
parent 22dd346d26
commit 83d6cefc56
1 changed files with 25 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files

30
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/flywheelTuning.java
View File

@@ -40,7 +40,7 @@ public class flywheelTuning extends OpMode {
public static double curTargetVelocity = 0; public static double curTargetVelocity = 0;
public static double targetPOS = 0; public static double targetPOS = 0;
public static int pipeline = 1; public static int pipeline = 1;
public static boolean reverse = false; public static boolean reverse = true;
double distance = 0; double distance = 0;
@@ -51,6 +51,17 @@ public class flywheelTuning extends OpMode {
double snapshotAutoHood = 0.0; double snapshotAutoHood = 0.0;
double calculatedGain = 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 @Override
public void init() { public void init() {
flywheelMotorR = hardwareMap.get(DcMotorEx.class, Constants.TurretConstants.TURRET_RIGHT_MOTOR); 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.setMode(DcMotorEx.RunMode.RUN_USING_ENCODER);
flywheelMotorL.setDirection(reverse ? DcMotorSimple.Direction.FORWARD : DcMotorSimple.Direction.REVERSE); flywheelMotorL.setDirection(reverse ? DcMotorSimple.Direction.FORWARD : DcMotorSimple.Direction.REVERSE);
flywheelMotorR.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(2, 0, 11, 14.3)); flywheelMotorL.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, new PIDFCoefficients(P, I, D, F));
hoodServo = hardwareMap.servo.get("hoodservo"); hoodServo = hardwareMap.servo.get("hoodservo");
hoodServo.setDirection(Servo.Direction.REVERSE); hoodServo.setDirection(Servo.Direction.REVERSE);
@@ -125,6 +136,17 @@ public class flywheelTuning extends OpMode {
double autoHoodPos = getHoodPOS(distance); 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) { if (autoMode) {
switch (calibState) { 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)); hoodServo.setPosition(Range.clip(applyHood + recoilOffset, Constants.TurretConstants.SERVO_MIN, Constants.TurretConstants.SERVO_MAX));
// --- TELEMETRY ---
telemetry.addData("Operating Mode", autoMode ? "AUTOMATIC" : "MANUAL"); telemetry.addData("Operating Mode", autoMode ? "AUTOMATIC" : "MANUAL");
telemetry.addData("Calibration State", calibState.toString()); telemetry.addData("Calibration State", calibState.toString());
telemetry.addLine(); telemetry.addLine();