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 29bd910..766378e 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 @@ -56,6 +56,7 @@ import static org.firstinspires.ftc.teamcode.constants.Poses.ry4a; import static org.firstinspires.ftc.teamcode.constants.Poses.ry4b; import static org.firstinspires.ftc.teamcode.constants.Poses.ryPrep; import static org.firstinspires.ftc.teamcode.constants.Poses.teleStart; +import static org.firstinspires.ftc.teamcode.constants.ServoPositions.hoodOffset; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_intakePos1; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_outtakeBall1; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_outtakeBall2; @@ -95,7 +96,7 @@ import java.util.Objects; @Config @Autonomous(preselectTeleOp = "TeleopV3") public class Auto_LT_Close_12Ball_Indexed extends LinearOpMode { - public static double shoot0Vel = 2300, shoot0Hood = 0.93; + public static double shoot0Vel = 2300, shoot0Hood = 0.93 + hoodOffset; public static double autoSpinStartPos = 0.2; public static double shoot0SpinSpeedIncrease = 0.015; @@ -132,9 +133,8 @@ public class Auto_LT_Close_12Ball_Indexed extends LinearOpMode { public static double pickup1Speed = 23; // ---- SECOND SHOT / PICKUP ---- public static double shoot1Vel = 2300; - public static double shoot1Hood = 0.93; public static double shootAllVelocity = 2500; - public static double shootAllHood = 0.78; + public static double shootAllHood = 0.78 + hoodOffset; // ---- PICKUP POSITION TOLERANCES ---- public static double pickup1XTolerance = 2.0; public static double pickup1YTolerance = 2.0; 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 32a6040..fbc4fa3 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 @@ -33,7 +33,7 @@ public class ServoPositions { public static double hoodAuto = 0.27; - +public static double hoodOffset = -0.05; public static double turret_redClose = 0.42; public static double turret_blueClose = 0.38; 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 968f8db..91355a7 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 @@ -36,7 +36,6 @@ public class TeleopV3 extends LinearOpMode { public static double tp = 0.8, ti = 0.001, td = 0.0315, tf = 0; public static double spinSpeedIncrease = 0.03; public static int resetSpinTicks = 4; - public static double manualOffset = 0.0; public static double hoodSpeedOffset = 0.01; public static double turretSpeedOffset = 0.01; public double vel = 3000; @@ -63,7 +62,6 @@ public class TeleopV3 extends LinearOpMode { double headingOffset = 0.0; int ticker = 0; - double hoodOffset = 0.0; boolean autoSpintake = false; boolean enableSpindexerManager = true; @@ -196,25 +194,23 @@ public class TeleopV3 extends LinearOpMode { if (targetingHood) { robot.hood.setPosition(targetingSettings.hoodAngle + autoHoodOffset); } else { - robot.hood.setPosition(hoodDefaultPos + hoodOffset); + robot.hood.setPosition(hoodDefaultPos); } if (gamepad2.dpadUpWasPressed()) { - hoodOffset -= hoodSpeedOffset; autoHoodOffset -= hoodSpeedOffset; gamepad2.rumble(80); } else if (gamepad2.dpadDownWasPressed()) { - hoodOffset += hoodSpeedOffset; autoHoodOffset += hoodSpeedOffset; gamepad2.rumble(80); } if (gamepad2.dpadLeftWasPressed()) { - manualOffset -= turretSpeedOffset; + Turret.manualOffset -= turretSpeedOffset; gamepad2.rumble(80); } else if (gamepad2.dpadRightWasPressed()) { - manualOffset += turretSpeedOffset; + Turret.manualOffset += turretSpeedOffset; gamepad2.rumble(80); } @@ -230,6 +226,8 @@ public class TeleopV3 extends LinearOpMode { drive = new MecanumDrive(hardwareMap, new Pose2d(0, 0, 0)); } + + if (enableSpindexerManager) { //if (!shootAll) { spindexer.processIntake(); @@ -320,20 +318,20 @@ public class TeleopV3 extends LinearOpMode { // TELE.addData("spinTestCounter", spindexer.counter); // TELE.addData("autoSpintake", autoSpintake); // -// TELE.addData("shootall commanded", shootAll); -// // Targeting Debug -// TELE.addData("robotX", robotX); -// TELE.addData("robotY", robotY); -// TELE.addData("robotInchesX", targeting.robotInchesX); -// TELE.addData( "robotInchesY", targeting.robotInchesY); -// TELE.addData("Targeting Interpolate", turretInterpolate); -// TELE.addData("Targeting GridX", targeting.robotGridX); -// TELE.addData("Targeting GridY", targeting.robotGridY); -// TELE.addData("Targeting FlyWheel", targetingSettings.flywheelRPM); -// TELE.addData("Targeting HoodAngle", targetingSettings.hoodAngle); -// TELE.addData("timeSinceStamp", getRuntime() - shootStamp); -// -// TELE.update(); + TELE.addData("shootall commanded", shootAll); + // Targeting Debug + TELE.addData("robotX", robotX); + TELE.addData("robotY", robotY); + TELE.addData("robotInchesX", targeting.robotInchesX); + TELE.addData( "robotInchesY", targeting.robotInchesY); + TELE.addData("Targeting Interpolate", turretInterpolate); + TELE.addData("Targeting GridX", targeting.robotGridX); + TELE.addData("Targeting GridY", targeting.robotGridY); + TELE.addData("Targeting FlyWheel", targetingSettings.flywheelRPM); + 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/Targeting.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Targeting.java index 57371af..6b32de0 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Targeting.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Targeting.java @@ -4,6 +4,8 @@ import static org.firstinspires.ftc.teamcode.constants.Color.redAlliance; import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; +import org.firstinspires.ftc.teamcode.constants.ServoPositions; + public class Targeting { // Known settings discovered using shooter test. // Keep the fidelity at 1 floor tile for now but we could also half it if more @@ -183,7 +185,7 @@ public class Targeting { if (true) { //!interpolate) { if ((robotGridY < 6) && (robotGridX < 6)) { recommendedSettings.flywheelRPM = KNOWNTARGETING[robotGridX][robotGridY].flywheelRPM; - recommendedSettings.hoodAngle = KNOWNTARGETING[robotGridX][robotGridY].hoodAngle; + recommendedSettings.hoodAngle = KNOWNTARGETING[robotGridX][robotGridY].hoodAngle + ServoPositions.hoodOffset; } return recommendedSettings; } else { 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 a386361..2074ecd 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,8 +1,5 @@ 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; import com.acmerobotics.dashboard.config.Config; @@ -24,11 +21,13 @@ public class Turret { public static double turretTolerance = 0.02; public static double turrPosScalar = 0.00011264432; public static double turret180Range = 0.4; - public static double turrDefault = 0.4; + public static double turrDefault = 0.39; public static double turrMin = 0.15; public static double turrMax = 0.85; public static boolean limelightUsed = true; + public static double manualOffset = 0.0; + public static double visionCorrectionGain = 0.08; // Single tunable gain public static double maxOffsetChangePerCycle = 5.0; // Degrees per cycle public static double cameraBearingEqual = 0.5; // Deadband @@ -36,28 +35,23 @@ public class Turret { // TODO: tune these values for limelight public static double clampTolerance = 0.03; + public static double B_PID_P = 0.105, B_PID_I = 0.0, B_PID_D = 0.0125; Robot robot; MultipleTelemetry TELE; Limelight3A webcam; - double tx = 0.0; double ty = 0.0; double limelightPosX = 0.0; double limelightPosY = 0.0; + LLResult result; + boolean bearingAligned = false; private boolean lockOffset = false; private int obeliskID = 0; - private double offset = 0.0; private double currentTrackOffset = 0.0; private int currentTrackCount = 0; - private double permanentOffset = 0.0; - - LLResult result; - private PIDController bearingPID; - 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) { this.TELE = tele; @@ -149,8 +143,7 @@ public class Turret { Param @deltaPos = Pose2d when subtracting robot x, y, heading from goal x, y, heading */ - - private double bearingAlign (LLResult llResult) { + private double bearingAlign(LLResult llResult) { double bearingOffset = 0.0; double targetTx = llResult.getTx(); // How far left or right the target is (degrees) final double MIN_OFFSET_POWER = 0.15; @@ -166,8 +159,7 @@ public class Turret { } // Only with valid data and if too far off target - if (llResult.isValid() && !bearingAligned) - { + if (llResult.isValid() && !bearingAligned) { // Adjust Robot Speed based on how far the target is located // Only drive at half speed max @@ -189,7 +181,6 @@ public class Turret { return bearingOffset; } - public void trackGoal(Pose2d deltaPos) { /* ---------------- FIELD → TURRET GEOMETRY ---------------- */