From 10222a7bc29fb2787f68dfec5dadce349231461f Mon Sep 17 00:00:00 2001 From: PatribotsProgramming Date: Tue, 24 Sep 2024 16:25:07 -0700 Subject: [PATCH] tuned to 12 feet - should run through speeds again --- .../pathplanner/paths/S W3-1 Bayou.path | 12 +++++----- src/main/java/frc/robot/RobotContainer.java | 2 ++ .../frc/robot/subsystems/drive/Swerve.java | 1 + .../robot/subsystems/vision/Limelight.java | 2 +- src/main/java/frc/robot/util/Constants.java | 22 ++++++++++--------- 5 files changed, 22 insertions(+), 17 deletions(-) diff --git a/src/main/deploy/pathplanner/paths/S W3-1 Bayou.path b/src/main/deploy/pathplanner/paths/S W3-1 Bayou.path index d60df045..a55091c9 100644 --- a/src/main/deploy/pathplanner/paths/S W3-1 Bayou.path +++ b/src/main/deploy/pathplanner/paths/S W3-1 Bayou.path @@ -36,12 +36,12 @@ "y": 5.378253593386619 }, "prevControl": { - "x": 1.15356049764184, - "y": 5.234393015427714 + "x": 1.145914832282723, + "y": 5.291535853302232 }, "nextControl": { - "x": 3.1831127492042217, - "y": 5.574752709789107 + "x": 3.1606075951005406, + "y": 5.4934000313860505 }, "isLocked": false, "linkedName": null @@ -73,7 +73,7 @@ }, { "waypointRelativePos": 2.55, - "rotationDegrees": -168.469589071078, + "rotationDegrees": -146.99446263627644, "rotateFast": false } ], @@ -150,7 +150,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": -149.36, + "rotation": -156.9427737708922, "rotateFast": false }, "reversed": false, diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 309a1dfb..08f54625 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -122,6 +122,8 @@ public class RobotContainer { public static Transform2d visionErrorPose = new Transform2d(); @AutoLogOutput (key = "Draggables/DistanceToSpeakerMeters") public static double distanceToSpeakerMeters = 0; + @AutoLogOutput (key = "Draggables/DistanceToSpeakerFeet") + public static double distanceToSpeakerFeet = 0; @AutoLogOutput (key = "Draggables/RobotPose3d") public static Pose3d robotPose3d = new Pose3d(); @AutoLogOutput (key = "Draggables/SwerveMeasuredStates") diff --git a/src/main/java/frc/robot/subsystems/drive/Swerve.java b/src/main/java/frc/robot/subsystems/drive/Swerve.java index 55053608..8ec5daa8 100644 --- a/src/main/java/frc/robot/subsystems/drive/Swerve.java +++ b/src/main/java/frc/robot/subsystems/drive/Swerve.java @@ -205,6 +205,7 @@ public void logPositions() { rotation3d); RobotContainer.distanceToSpeakerMeters = currentPose.getTranslation().getDistance(FieldConstants.GET_SPEAKER_TRANSLATION()); + RobotContainer.distanceToSpeakerFeet = Units.metersToFeet(RobotContainer.distanceToSpeakerMeters); Logger.recordOutput("Subsystems/Swerve/RobotPose3d", RobotContainer.robotPose3d); Logger.recordOutput("Subsystems/Swerve/ChassisSpeeds", speeds); Logger.recordOutput("Subsystems/Swerve/IsUnderStage", PoseCalculations.inStageTriangle(currentPose)); diff --git a/src/main/java/frc/robot/subsystems/vision/Limelight.java b/src/main/java/frc/robot/subsystems/vision/Limelight.java index d00442f1..e0776fd6 100644 --- a/src/main/java/frc/robot/subsystems/vision/Limelight.java +++ b/src/main/java/frc/robot/subsystems/vision/Limelight.java @@ -318,7 +318,7 @@ public boolean hasTarget() { double singleTagAmbiguityThreshold = Robot.gameMode == GameMode.AUTONOMOUS ? 0.175 : 0.141; if (!inputs.validResult || (inputs.limelightTA < singleTagAmbiguityThreshold && inputs.targetIDs.length == 1) - || (inputs.targetIDs.length > 1 && inputs.limelightTA < 0.05) + || (inputs.targetIDs.length > 1 && inputs.limelightTA < CameraConstants.LIMELIGHT_3G_TA_CUTOFF) || (estimatedRobotPose.getX() == 0 && estimatedRobotPose.getY() == 0) || Double.isNaN(estimatedRobotPose.getX()) || Double.isNaN(estimatedRobotPose.getY()) diff --git a/src/main/java/frc/robot/util/Constants.java b/src/main/java/frc/robot/util/Constants.java index 55db688f..06029cf8 100644 --- a/src/main/java/frc/robot/util/Constants.java +++ b/src/main/java/frc/robot/util/Constants.java @@ -215,7 +215,7 @@ public static final class ShooterConstants { public static final HashMap SPEAKER_DISTANCES_TO_SPEEDS_AND_ANGLE_MAP = new HashMap() { { put(4, SpeedAngleTriplet.of(1763.0, 2316.0, 56.1)); - put(5, SpeedAngleTriplet.of(1763.0, 2316.0, 57.6)); + put(5, SpeedAngleTriplet.of(1763.0, 2316.0, 54.5)); // put(6, SpeedAngleTriplet.of(2088.0, 2088.0, 50.0)); // put(7, SpeedAngleTriplet.of(2188.0, 2188.0, 45.7)); // put(8, SpeedAngleTriplet.of(2313.0, 2313.0, 41.3)); @@ -261,14 +261,14 @@ public static final class ShooterConstants { // put(15, SpeedAngleTriplet.of(3586.0, 3371.0, 34)); // NEW SHOOTER WOOOHOO - put(6, SpeedAngleTriplet.of(3007.0, 2850.0, 50.1)); - put(7, SpeedAngleTriplet.of(3160.0, 2865.0, 46.3)); - put(8, SpeedAngleTriplet.of(3310.0, 3017.0, 43.4)); - put(9, SpeedAngleTriplet.of(3502.0, 3202.0, 40.2)); - put(10, SpeedAngleTriplet.of(3706.0, 3305.0, 37.8)); + put(6, SpeedAngleTriplet.of(3007.0, 2850.0, 48.2)); + put(7, SpeedAngleTriplet.of(3160.0, 2865.0, 45.0)); + put(8, SpeedAngleTriplet.of(3314.0, 3020.0, 42.5)); + put(9, SpeedAngleTriplet.of(3502.0, 3202.0, 38.1)); + put(10, SpeedAngleTriplet.of(3706.0, 3305.0, 36.3)); put(11, SpeedAngleTriplet.of(3856.0, 3539.0, 34.5)); - put(12, SpeedAngleTriplet.of(3947.0, 3580.0, 33.8)); - put(13, SpeedAngleTriplet.of(4075.0, 3691.0, 31.8)); + put(12, SpeedAngleTriplet.of(3947.0, 3580.0, 32.7)); + put(13, SpeedAngleTriplet.of(4075.0, 3691.0, 31.8)); // start here // Future note, 13.2ft is a common shot which should have its own calibration point put(14, SpeedAngleTriplet.of(4214.0, 3755.0, 30.9)); put(15, SpeedAngleTriplet.of(4515.0, 4043.0, 30.9)); @@ -1171,11 +1171,13 @@ public static final class CameraConstants { public static final long LIMELIGHT_MAX_UPDATE_TIME = 200_000; // Micro Seconds = 0.2 Seconds + public static final double LIMELIGHT_3G_TA_CUTOFF = 0.078; + public static final Pose3d LL3Pose = new Pose3d( // forward positive, right positive, up positive - 0.204, - -0.234, + 0.2872, // Mind control ll pose + 0.234, 0.651, new Rotation3d(0, Units.degreesToRadians(15), 0)); public static final Pose3d LL2Pose =