diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close_12Ball_Indexed.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close_12Ball_Indexed.java index 41c736f..0a739a9 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close_12Ball_Indexed.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close_12Ball_Indexed.java @@ -485,16 +485,11 @@ public class Auto_LT_Close_12Ball_Indexed extends LinearOpMode { if (ticker == 0) { stamp = System.currentTimeMillis(); + robot.limelight.pipelineSwitch(1); } ticker++; - robot.limelight.pipelineSwitch(1); motif = turret.detectObelisk(); - if (redAlliance){ - robot.limelight.pipelineSwitch(4); - } else { - robot.limelight.pipelineSwitch(2); - } robot.turr1.setPosition(turrPos); robot.turr2.setPosition(1 - turrPos); @@ -511,7 +506,16 @@ public class Auto_LT_Close_12Ball_Indexed extends LinearOpMode { TELE.addData("Hood", robot.hood.getPosition()); TELE.update(); - return !shouldFinish; + if (shouldFinish){ + if (redAlliance){ + robot.limelight.pipelineSwitch(4); + } else { + robot.limelight.pipelineSwitch(2); + } + return false; + } else { + return true; + } } }; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/ServoPositions.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/ServoPositions.java index 42bb3bf..58492c7 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/ServoPositions.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/ServoPositions.java @@ -20,7 +20,7 @@ public class ServoPositions { public static double spinEndPos = 0.85; public static double shootAllAutoSpinStartPos = 0.2; - public static double shootAllSpindexerSpeedIncrease = 0.02; + public static double shootAllSpindexerSpeedIncrease = 0.014; public static double shootAllTime = 1.8; public static double transferServo_out = 0.15; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV3.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV3.java index 8e31068..9eda7ec 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV3.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV3.java @@ -5,6 +5,7 @@ import static org.firstinspires.ftc.teamcode.constants.Poses.teleStart; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_intakePos1; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_in; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_out; +import static org.firstinspires.ftc.teamcode.utils.Turret.limelightUsed; import com.acmerobotics.dashboard.FtcDashboard; import com.acmerobotics.dashboard.config.Config; @@ -39,10 +40,12 @@ public class TeleopV3 extends LinearOpMode { public double vel = 3000; public boolean autoVel = true; public boolean targetingHood = true; - public double manualOffset = 0.0; + public static double manualOffset = 0.0; public boolean autoHood = true; public double shootStamp = 0.0; + public static double hoodSpeedOffset = 0.01; + public static double turretSpeedOffset = 0.01; boolean fixedTurret = false; Robot robot; @@ -191,43 +194,40 @@ public class TeleopV3 extends LinearOpMode { //HOOD: if (targetingHood) { - robot.hood.setPosition(targetingSettings.hoodAngle); + robot.hood.setPosition(targetingSettings.hoodAngle + autoHoodOffset); } else { robot.hood.setPosition(hoodDefaultPos + hoodOffset); } - if (gamepad2.dpadUpWasPressed() || gamepad1.dpadUpWasPressed()) { - hoodOffset -= 0.03; - autoHoodOffset -= 0.02; - - } else if (gamepad2.dpadDownWasPressed() || gamepad1.dpadDownWasPressed()) { - hoodOffset += 0.03; - autoHoodOffset += 0.02; + if (gamepad2.dpadUpWasPressed()) { + hoodOffset -= hoodSpeedOffset; + autoHoodOffset -= hoodSpeedOffset; + gamepad2.rumble(80); + } else if (gamepad2.dpadDownWasPressed()) { + hoodOffset += hoodSpeedOffset; + autoHoodOffset += hoodSpeedOffset; + gamepad2.rumble(80); } - if (gamepad2.cross) { - manualOffset = 0; - overrideTurr = true; + if (gamepad2.dpadLeftWasPressed()){ + manualOffset -= turretSpeedOffset; + gamepad2.rumble(80); + } else if (gamepad2.dpadRightWasPressed()){ + manualOffset += turretSpeedOffset; + gamepad2.rumble(80); } - if (gamepad2.squareWasPressed()) { - drive = new MecanumDrive(hardwareMap, new Pose2d(20, 0, 0)); - sleep(1500); + if (gamepad2.rightBumperWasPressed()){ + limelightUsed = true; + gamepad2.rumble(80); + } else if (gamepad2.leftBumperWasPressed()){ + limelightUsed = false; + gamepad2.rumble(80); } - if (gamepad2.triangle) { - autoHood = false; - hoodOffset = 0; - } + if (gamepad2.crossWasPressed()){ - if (gamepad2.circleWasPressed()) { - xOffset = robotX; - yOffset = robotY; - headingOffset = robotHeading; - - autoHood = true; - fixedTurret = false; } if (enableSpindexerManager) { @@ -330,8 +330,6 @@ public class TeleopV3 extends LinearOpMode { // TELE.addData("Targeting HoodAngle", targetingSettings.hoodAngle); // TELE.addData("timeSinceStamp", getRuntime() - shootStamp); -// TELE.update(); - ticker++; } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Turret.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Turret.java index e02b496..a386361 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Turret.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Turret.java @@ -1,6 +1,7 @@ package org.firstinspires.ftc.teamcode.utils; import static org.firstinspires.ftc.teamcode.constants.Color.redAlliance; +import static org.firstinspires.ftc.teamcode.teleop.TeleopV3.manualOffset; import static java.lang.Math.abs; @@ -26,6 +27,7 @@ public class Turret { public static double turrDefault = 0.4; public static double turrMin = 0.15; public static double turrMax = 0.85; + public static boolean limelightUsed = true; public static double visionCorrectionGain = 0.08; // Single tunable gain public static double maxOffsetChangePerCycle = 5.0; // Degrees per cycle @@ -54,7 +56,7 @@ public class Turret { LLResult result; private PIDController bearingPID; - public static double B_PID_P = 0.15, B_PID_I = 0.0, B_PID_D = 0.025; + public static double B_PID_P = 0.105, B_PID_I = 0.0, B_PID_D = 0.0125; boolean bearingAligned = false; public Turret(Robot rob, MultipleTelemetry tele, Limelight3A cam) { @@ -84,11 +86,6 @@ public class Turret { } private void limelightRead() { // only for tracking purposes, not general reads - if (redAlliance) { - webcam.pipelineSwitch(4); - } else { - webcam.pipelineSwitch(2); - } result = webcam.getLatestResult(); if (result != null) { @@ -223,7 +220,7 @@ public class Turret { limelightRead(); // Active correction if we see the target - if (result.isValid() && !lockOffset) { + if (result.isValid() && !lockOffset && limelightUsed) { currentTrackOffset += bearingAlign(result); currentTrackCount++; // double bearingError = Math.abs(tagBearingDeg); @@ -282,8 +279,8 @@ public class Turret { } // Set servo positions - robot.turr1.setPosition(turretPos); - robot.turr2.setPosition(1.0 - turretPos); + robot.turr1.setPosition(turretPos + manualOffset); + robot.turr2.setPosition(1.0 - turretPos - manualOffset); /* ---------------- TELEMETRY ---------------- */