diff --git a/src/main/deploy/constants/comp/ClimbConstants.json b/src/main/deploy/constants/comp/ClimbConstants.json index adb46a5f..bd8c4226 100644 --- a/src/main/deploy/constants/comp/ClimbConstants.json +++ b/src/main/deploy/constants/comp/ClimbConstants.json @@ -11,7 +11,7 @@ "value": 0.6, "unit": "Radian" }, - "climbEncoderOffsetRotations": -0.40234375, + "climbEncoderOffsetRotations": -0.238037109375, "leadClimbMotorId": 16, "followerClimbMotorId": 17, "climbkP": 45.0, diff --git a/src/main/deploy/constants/comp/DrivetrainConstants.json b/src/main/deploy/constants/comp/DrivetrainConstants.json index 96253972..4a23be85 100644 --- a/src/main/deploy/constants/comp/DrivetrainConstants.json +++ b/src/main/deploy/constants/comp/DrivetrainConstants.json @@ -38,11 +38,20 @@ "driveRotationKi": 0.0, "driveRotationKd": 0.0, "driveAlongTrackMultiplier": 1, - "driveAlongTrackOffset": 0.24, - "driveCrossTrackFrontRightOffset": -0.03, + "driveAlongTrackFrontRightOffset": 0.25, + "driveAlongTrackFrontLeftOffset": 0.24, + "driveCrossTrackFrontRightOffset": -0.02, "driveCrossTrackFrontLeftOffset": -0.03, "driveCrossTrackFrontRightAlgaeOffset": 0.25, "driveCrossTrackFrontLeftAlgaeOffset": 0.25, + "driveLineupHeadingFrontRightOffset": { + "value": 0.0, + "unit": "Degree" + }, + "driveLineupHeadingFrontLeftOffset": { + "value": 0.0, + "unit": "Degree" + }, "lineupRotationMarginRadians": 0.05, "lineupAlongTrackThresholdMeters": 0.05, "lineupCrossTrackThresholdMeters": 0.04, diff --git a/src/main/deploy/constants/demo/DrivetrainConstants.json b/src/main/deploy/constants/demo/DrivetrainConstants.json index 54cbfeea..185ba238 100644 --- a/src/main/deploy/constants/demo/DrivetrainConstants.json +++ b/src/main/deploy/constants/demo/DrivetrainConstants.json @@ -38,11 +38,20 @@ "driveRotationKi": 0.0, "driveRotationKd": 0.0, "driveAlongTrackMultiplier": 1, - "driveAlongTrackOffset": 0.27, + "driveAlongTrackFrontRightOffset": 0.25, + "driveAlongTrackFrontLeftOffset": 0.24, "driveCrossTrackFrontRightOffset": -0.02, "driveCrossTrackFrontLeftOffset": -0.03, "driveCrossTrackFrontRightAlgaeOffset": 0.25, "driveCrossTrackFrontLeftAlgaeOffset": 0.25, + "driveLineupHeadingFrontRightOffset": { + "value": 0.0, + "unit": "Degree" + }, + "driveLineupHeadingFrontLeftOffset": { + "value": 0.0, + "unit": "Degree" + }, "lineupRotationMarginRadians": 0.05, "lineupAlongTrackThresholdMeters": 0.05, "lineupCrossTrackThresholdMeters": 0.04, diff --git a/src/main/deploy/constants/hitl/DrivetrainConstants.json b/src/main/deploy/constants/hitl/DrivetrainConstants.json index bcf5049f..db5a0c22 100644 --- a/src/main/deploy/constants/hitl/DrivetrainConstants.json +++ b/src/main/deploy/constants/hitl/DrivetrainConstants.json @@ -28,8 +28,17 @@ "driveRotationKi": 0.0, "driveRotationKd": 0.0, "driveAlongTrackMultiplier": 1, - "driveAlongTrackOffset": 0.25, + "driveAlongTrackFrontRightOffset": 0.25, + "driveAlongTrackFrontLeftOffset": 0.25, "driveCrossTrackFrontRightOffset": 0.0, + "driveLineupHeadingFrontRightOffset": { + "value": 0.0, + "unit": "Degree" + }, + "driveLineupHeadingFrontLeftOffset": { + "value": 0.0, + "unit": "Degree" + }, "otfPoseDistanceLimit": 0.1, "lineupMaxVelocity": 3.0, "lineupMaxAcceleration": 1.5, diff --git a/src/main/deploy/constants/test_drivebase/DrivetrainConstants.json b/src/main/deploy/constants/test_drivebase/DrivetrainConstants.json index ef4f7c89..efa27f65 100644 --- a/src/main/deploy/constants/test_drivebase/DrivetrainConstants.json +++ b/src/main/deploy/constants/test_drivebase/DrivetrainConstants.json @@ -28,8 +28,17 @@ "driveRotationKi": 0.0, "driveRotationKd": 0.0, "driveAlongTrackMultiplier": 1, - "driveAlongTrackOffset": 0.25, + "driveAlongTrackFrontRightOffset": 0.25, + "driveAlongTrackFrontLeftOffset": 0.25, "driveCrossTrackFrontRightOffset": 0.0, + "driveLineupHeadingFrontRightOffset": { + "value": 0.0, + "unit": "Degree" + }, + "driveLineupHeadingFrontLeftOffset": { + "value": 0.0, + "unit": "Degree" + }, "otfPoseDistanceLimit": 0.1, "lineupMaxVelocity": 3.0, "lineupMaxAcceleration": 1.5, diff --git a/src/main/java/frc/robot/BuildConstants.java b/src/main/java/frc/robot/BuildConstants.java index cb579028..5d9c81ca 100644 --- a/src/main/java/frc/robot/BuildConstants.java +++ b/src/main/java/frc/robot/BuildConstants.java @@ -5,12 +5,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "2025-Robot-Code"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 94; - public static final String GIT_SHA = "8f0396d74d2a8c697941aada0550766844303745"; - public static final String GIT_DATE = "2025-08-22 17:46:12 EDT"; - public static final String GIT_BRANCH = "248-add-readme"; - public static final String BUILD_DATE = "2025-08-23 21:35:05 EDT"; - public static final long BUILD_UNIX_TIME = 1755999305676L; + public static final int GIT_REVISION = 90; + public static final String GIT_SHA = "13ec3b6f046838dd98b05d9f82f2362eb77383c4"; + public static final String GIT_DATE = "2025-10-24 15:19:25 EDT"; + public static final String GIT_BRANCH = "252-separate-lineup-offsets"; + public static final String BUILD_DATE = "2025-10-24 15:21:57 EDT"; + public static final long BUILD_UNIX_TIME = 1761333717458L; public static final int DIRTY = 1; private BuildConstants() {} diff --git a/src/main/java/frc/robot/constants/VisionConstants.java b/src/main/java/frc/robot/constants/VisionConstants.java index 828dae52..dba40e59 100644 --- a/src/main/java/frc/robot/constants/VisionConstants.java +++ b/src/main/java/frc/robot/constants/VisionConstants.java @@ -15,7 +15,7 @@ public class VisionConstants { EnvironmentHandler.getEnvironmentHandler().getEnvironmentPathProvider(), new JSONSyncConfigBuilder().setPrettyPrinting(true).build()); - public final Integer FrontLeftCameraIndex = 0; + public final Integer FrontLeftCameraIndex = 1; public final Integer FrontRightCameraIndex = 0; public final Transform3d FrontLeftTransform = new Transform3d(); public final Transform3d FrontRightTransform = diff --git a/src/main/java/frc/robot/constants/subsystems/DrivetrainConstants.java b/src/main/java/frc/robot/constants/subsystems/DrivetrainConstants.java index 5b117e1f..cded3202 100644 --- a/src/main/java/frc/robot/constants/subsystems/DrivetrainConstants.java +++ b/src/main/java/frc/robot/constants/subsystems/DrivetrainConstants.java @@ -84,11 +84,14 @@ public class DrivetrainConstants { public final Double driveRotationKi = 0.0; public final Double driveRotationKd = 0.0; public final Double driveAlongTrackMultiplier = -1.0; - public final Double driveAlongTrackOffset = 0.25; + public final Double driveAlongTrackFrontRightOffset = 0.25; + public final Double driveAlongTrackFrontLeftOffset = 0.25; public final Double driveCrossTrackFrontRightOffset = 0.0; public final Double driveCrossTrackFrontLeftOffset = 0.0; public final Double driveCrossTrackFrontRightAlgaeOffset = 0.25; public final Double driveCrossTrackFrontLeftAlgaeOffset = 0.25; + public final Angle driveLineupHeadingFrontRightOffset = Degrees.zero(); + public final Angle driveLineupHeadingFrontLeftOffset = Degrees.zero(); public final Double lineupMaxVelocity = 3.0; public final Double lineupMaxAcceleration = 1.5; diff --git a/src/main/java/frc/robot/subsystems/drive/states/LineupState.java b/src/main/java/frc/robot/subsystems/drive/states/LineupState.java index f443a518..0a39d0a0 100644 --- a/src/main/java/frc/robot/subsystems/drive/states/LineupState.java +++ b/src/main/java/frc/robot/subsystems/drive/states/LineupState.java @@ -1,5 +1,7 @@ package frc.robot.subsystems.drive.states; +import static edu.wpi.first.units.Units.Radians; + import coppercore.controls.state_machine.state.PeriodicStateInterface; import coppercore.controls.state_machine.transition.Transition; import coppercore.parameter_tools.LoggedTunableNumber; @@ -11,6 +13,7 @@ import edu.wpi.first.math.geometry.Transform2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.units.measure.Angle; import edu.wpi.first.wpilibj.DriverStation; import frc.robot.StrategyManager; import frc.robot.StrategyManager.AutonomyMode; @@ -337,13 +340,21 @@ public DistanceToTag tryOtherCamera( ? 0 : JsonConstants.visionConstants.FrontLeftTransform.getY() - JsonConstants.visionConstants.FrontRightTransform.getY(); + + double alongTrackOffset; + if (otherCameraIndex == JsonConstants.visionConstants.FrontLeftCameraIndex) { + alongTrackOffset = JsonConstants.drivetrainConstants.driveAlongTrackFrontLeftOffset; + } else { + alongTrackOffset = JsonConstants.drivetrainConstants.driveAlongTrackFrontRightOffset; + } + DistanceToTag observationOtherCamera = alignmentSupplier.get( tagId, otherCameraIndex, ReefLineupUtil.getCrossTrackOffset(otherCameraIndex) + (signOfError * offsetErrorCorrection), - JsonConstants.drivetrainConstants.driveAlongTrackOffset); + alongTrackOffset); return observationOtherCamera; } @@ -392,12 +403,16 @@ public void LineupWithReefLocation() { return; } + double alongTrackOffset; + if (cameraIndex == JsonConstants.visionConstants.FrontLeftCameraIndex) { + alongTrackOffset = JsonConstants.drivetrainConstants.driveAlongTrackFrontLeftOffset; + } else { + alongTrackOffset = JsonConstants.drivetrainConstants.driveAlongTrackFrontRightOffset; + } + DistanceToTag observation = alignmentSupplier.get( - tagId, - cameraIndex, - ReefLineupUtil.getCrossTrackOffset(cameraIndex), - JsonConstants.drivetrainConstants.driveAlongTrackOffset); + tagId, cameraIndex, ReefLineupUtil.getCrossTrackOffset(cameraIndex), alongTrackOffset); DistanceToTag otherCameraObs = tryOtherCamera(alignmentSupplier, tagId, cameraIndex); @@ -460,9 +475,18 @@ public void LineupWithReefLocation() { * getAlongTrackVelocityReductionFactor(observation.crossTrackDistance()) * getAlongTrackVelocity(observation.alongTrackDistance()); // Maybe use filtered? double vy = driveCrossTrackLineupController.calculate(observation.crossTrackDistance()); + + Angle headingOffset; + if (cameraIndex == JsonConstants.visionConstants.FrontLeftCameraIndex) { + headingOffset = JsonConstants.drivetrainConstants.driveLineupHeadingFrontLeftOffset; + } else { + headingOffset = JsonConstants.drivetrainConstants.driveLineupHeadingFrontRightOffset; + } + double omega = rotationController.calculate( - drive.getRotation().getRadians(), this.getRotationForReefSide().getRadians()); + drive.getRotation().getRadians(), + this.getRotationForReefSide().getRadians() + headingOffset.in(Radians)); if (usingOtherCamera) { vy = driveCrossTrackOtherCameraLineupController.calculate(observation.crossTrackDistance());