From 072677d1001c1a5e4d4f7b6499c6be2220ae9fc4 Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Sun, 1 Feb 2026 12:43:00 +0200 Subject: [PATCH 01/16] Implement double tap controls --- .../java/frc/trigon/robot/constants/OperatorConstants.java | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/trigon/robot/constants/OperatorConstants.java b/src/main/java/frc/trigon/robot/constants/OperatorConstants.java index bf0a277b..c409a3cd 100644 --- a/src/main/java/frc/trigon/robot/constants/OperatorConstants.java +++ b/src/main/java/frc/trigon/robot/constants/OperatorConstants.java @@ -23,6 +23,7 @@ public class OperatorConstants { DRIVER_CONTROLLER_PORT, DRIVER_CONTROLLER_RIGHT_STICK_EXPONENT, DRIVER_CONTROLLER_LEFT_STICK_EXPONENT, DRIVER_CONTROLLER_DEADBAND ); public static final KeyboardController OPERATOR_CONTROLLER = new KeyboardController(); + private static final double DOUBLE_TAP_TIMEOUT_SECONDS = 0.5; public static final double POV_DIVIDER = 2, @@ -41,7 +42,7 @@ public class OperatorConstants { ); public static final Trigger //General Triggers - RESET_HEADING_TRIGGER = DRIVER_CONTROLLER.y(), + RESET_HEADING_TRIGGER = DRIVER_CONTROLLER.y().multiPress(2, DOUBLE_TAP_TIMEOUT_SECONDS), DRIVE_FROM_DPAD_TRIGGER = new Trigger(() -> DRIVER_CONTROLLER.getPov() != -1), TOGGLE_BRAKE_TRIGGER = OPERATOR_CONTROLLER.g().or(RobotController::getUserButton), DEBUGGING_TRIGGER = OPERATOR_CONTROLLER.f2(), @@ -65,7 +66,7 @@ public class OperatorConstants { SET_FIXED_SHOOTING_POSITION_CLOSE_TO_TOWER_TRIGGER = DRIVER_CONTROLLER.povDown().or(OPERATOR_CONTROLLER.k()), SET_FIXED_SHOOTING_POSITION_CLOSE_TO_OUTPOST_TRIGGER = DRIVER_CONTROLLER.povRight().or(OPERATOR_CONTROLLER.l()); public static final Trigger //Climb Triggers - OPEN_CLIMBER_TRIGGER = DRIVER_CONTROLLER.back().or(OPERATOR_CONTROLLER.c()); + OPEN_CLIMBER_TRIGGER = DRIVER_CONTROLLER.back().or(OPERATOR_CONTROLLER.c()).multiPress(2, DOUBLE_TAP_TIMEOUT_SECONDS); // CANCEL_CLIMB_TRIGGER = new Trigger(ClimberCommands::IS_CLIMBING).and(DRIVER_CONTROLLER.leftBumper()).or(OPERATOR_CONTROLLER.x()), // CONTINUE_CLIMB_TRIGGER = new Trigger(ClimberCommands::IS_CLIMBING).and(DRIVER_CONTROLLER.rightBumper()).or(OPERATOR_CONTROLLER.v()); public static final Trigger //Debugging Triggers From dd6ce30f6c8d5bfbaca6de3e0cb3f43f7f579010 Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Sun, 1 Feb 2026 12:45:30 +0200 Subject: [PATCH 02/16] Double tap for operator toggle controls --- .../java/frc/trigon/robot/constants/OperatorConstants.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/trigon/robot/constants/OperatorConstants.java b/src/main/java/frc/trigon/robot/constants/OperatorConstants.java index c409a3cd..b46382db 100644 --- a/src/main/java/frc/trigon/robot/constants/OperatorConstants.java +++ b/src/main/java/frc/trigon/robot/constants/OperatorConstants.java @@ -53,8 +53,8 @@ public class OperatorConstants { public static final Trigger //Intake Triggers INTAKE_TRIGGER = DRIVER_CONTROLLER.leftTrigger(), TOGGLE_SHOULD_KEEP_INTAKE_OPEN_TRIGGER = DRIVER_CONTROLLER.b().or(OPERATOR_CONTROLLER.u()), - ENABLE_INTAKE_ASSIST_TRIGGER = OPERATOR_CONTROLLER.o(), - DISABLE_INTAKE_ASSIST_TRIGGER = DRIVER_CONTROLLER.a().or(OPERATOR_CONTROLLER.p()); + ENABLE_INTAKE_ASSIST_TRIGGER = OPERATOR_CONTROLLER.o().multiPress(2, DOUBLE_TAP_TIMEOUT_SECONDS), + DISABLE_INTAKE_ASSIST_TRIGGER = DRIVER_CONTROLLER.a().or(OPERATOR_CONTROLLER.p().multiPress(2, DOUBLE_TAP_TIMEOUT_SECONDS)); public static final Trigger //Shooting Triggers DISABLE_AUTO_SHOOT_TRIGGER = DRIVER_CONTROLLER.rightStick().or(OPERATOR_CONTROLLER.a()), AUTO_SHOOT_AT_HUB_TRIGGER = new Trigger(OperatorConstants::shouldAutoShootAtHub), From 4fdc575de833b52458b604363d29225dd845812e Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Sun, 8 Feb 2026 12:23:06 +0200 Subject: [PATCH 03/16] Indicate no vision update --- src/main/java/frc/trigon/robot/RobotContainer.java | 1 + .../frc/trigon/robot/commands/CommandConstants.java | 7 ++++++- .../frc/trigon/robot/constants/OperatorConstants.java | 2 ++ .../robotposeestimator/RobotPoseEstimator.java | 10 +++++++--- 4 files changed, 16 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/trigon/robot/RobotContainer.java b/src/main/java/frc/trigon/robot/RobotContainer.java index 7de344c1..36b6a1d0 100644 --- a/src/main/java/frc/trigon/robot/RobotContainer.java +++ b/src/main/java/frc/trigon/robot/RobotContainer.java @@ -89,6 +89,7 @@ private void bindControllerCommands() { OperatorConstants.RESET_HEADING_TRIGGER.onTrue(CommandConstants.RESET_HEADING_COMMAND); OperatorConstants.DRIVE_FROM_DPAD_TRIGGER.whileTrue(CommandConstants.SELF_RELATIVE_DRIVE_FROM_DPAD_COMMAND); OperatorConstants.TOGGLE_BRAKE_TRIGGER.onTrue(GeneralCommands.getToggleBrakeCommand()); + OperatorConstants.CAMERAS_DISCONNECTED_TRIGGER.onTrue(CommandConstants.INDICATE_CAMERAS_DISCONNECTED_COMMAND); OperatorConstants.INTAKE_TRIGGER.whileTrue(FuelIntakeCommands.getIntakeCommand()); OperatorConstants.TOGGLE_SHOULD_KEEP_INTAKE_OPEN_TRIGGER.onTrue(FuelIntakeCommands.getToggleDefaultIntakeStateCommand()); diff --git a/src/main/java/frc/trigon/robot/commands/CommandConstants.java b/src/main/java/frc/trigon/robot/commands/CommandConstants.java index d9d41127..436b47c9 100644 --- a/src/main/java/frc/trigon/robot/commands/CommandConstants.java +++ b/src/main/java/frc/trigon/robot/commands/CommandConstants.java @@ -26,8 +26,11 @@ public class CommandConstants { MINIMUM_TRANSLATION_SHIFT_POWER = 0.30, MINIMUM_ROTATION_SHIFT_POWER = 0.4; private static final double JOYSTICK_ORIENTED_ROTATION_DEADBAND = 0.07; + private static final double + RUMBLE_WHEN_CAMERAS_DISCONNECTED_DURATION_SECONDS = 0.5, + RUMBLE_WHEN_CAMERAS_DISCONNECTED_POWER = 1; - public static final Command + public static final Command //General Commands RESET_HEADING_COMMAND = new InstantCommand(RobotContainer.ROBOT_POSE_ESTIMATOR::resetHeading), SELF_RELATIVE_DRIVE_FROM_DPAD_COMMAND = SwerveCommands.getClosedLoopSelfRelativeDriveCommand( () -> getXPowerFromPov(DRIVER_CONTROLLER.getPov()) / OperatorConstants.POV_DIVIDER / calculateShiftModeValue(MINIMUM_TRANSLATION_SHIFT_POWER), @@ -57,6 +60,8 @@ public class CommandConstants { (omegaRadiansPerSecond) -> RobotContainer.SWERVE.selfRelativeDrive(new ChassisSpeeds(0, 0, omegaRadiansPerSecond)), RobotContainer.SWERVE ); + public static final Command //Robot-specific commands + INDICATE_CAMERAS_DISCONNECTED_COMMAND = new InstantCommand(() -> OperatorConstants.DRIVER_CONTROLLER.rumble(RUMBLE_WHEN_CAMERAS_DISCONNECTED_DURATION_SECONDS, RUMBLE_WHEN_CAMERAS_DISCONNECTED_POWER)); /** * Calculates the target drive power from an axis value by dividing it by the shift mode value. diff --git a/src/main/java/frc/trigon/robot/constants/OperatorConstants.java b/src/main/java/frc/trigon/robot/constants/OperatorConstants.java index b46382db..f75ce00f 100644 --- a/src/main/java/frc/trigon/robot/constants/OperatorConstants.java +++ b/src/main/java/frc/trigon/robot/constants/OperatorConstants.java @@ -23,6 +23,7 @@ public class OperatorConstants { DRIVER_CONTROLLER_PORT, DRIVER_CONTROLLER_RIGHT_STICK_EXPONENT, DRIVER_CONTROLLER_LEFT_STICK_EXPONENT, DRIVER_CONTROLLER_DEADBAND ); public static final KeyboardController OPERATOR_CONTROLLER = new KeyboardController(); + private static final double ARE_CAMERAS_DISCONNECTED_CHECK_DEBOUNCE_SECONDS = 3; private static final double DOUBLE_TAP_TIMEOUT_SECONDS = 0.5; public static final double @@ -46,6 +47,7 @@ public class OperatorConstants { DRIVE_FROM_DPAD_TRIGGER = new Trigger(() -> DRIVER_CONTROLLER.getPov() != -1), TOGGLE_BRAKE_TRIGGER = OPERATOR_CONTROLLER.g().or(RobotController::getUserButton), DEBUGGING_TRIGGER = OPERATOR_CONTROLLER.f2(), + CAMERAS_DISCONNECTED_TRIGGER = new Trigger(() -> !RobotContainer.ROBOT_POSE_ESTIMATOR.hasUpdateFromCameras()).debounce(ARE_CAMERAS_DISCONNECTED_CHECK_DEBOUNCE_SECONDS), FORWARD_QUASISTATIC_CHARACTERIZATION_TRIGGER = OPERATOR_CONTROLLER.right(), BACKWARD_QUASISTATIC_CHARACTERIZATION_TRIGGER = OPERATOR_CONTROLLER.left(), FORWARD_DYNAMIC_CHARACTERIZATION_TRIGGER = OPERATOR_CONTROLLER.up(), diff --git a/src/main/java/frc/trigon/robot/poseestimation/robotposeestimator/RobotPoseEstimator.java b/src/main/java/frc/trigon/robot/poseestimation/robotposeestimator/RobotPoseEstimator.java index 84c63c08..27710b43 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/robotposeestimator/RobotPoseEstimator.java +++ b/src/main/java/frc/trigon/robot/poseestimation/robotposeestimator/RobotPoseEstimator.java @@ -49,7 +49,7 @@ public RobotPoseEstimator(RelativeRobotPoseSource relativeRobotPoseSource, April this.relativeRobotPoseSource = relativeRobotPoseSource; this.shouldUseRelativeRobotPoseSource = true; - initialize(); + initializeFieldWidget(); } /** @@ -63,7 +63,7 @@ public RobotPoseEstimator(AprilTagCamera... aprilTagCameras) { this.relativeRobotPoseSource = null; this.shouldUseRelativeRobotPoseSource = false; - initialize(); + initializeFieldWidget(); } @Override @@ -120,6 +120,10 @@ public Pose2d getEstimatedOdometryPose() { return swerveDriveOdometry.getPoseMeters(); } + public boolean hasUpdateFromCameras() { + return getCamerasWithResults().length > 0; + } + /** * Updates the pose estimator with the given swerve wheel positions and gyro rotations. * This function accepts an array of swerve wheel positions and an array of gyro rotations because the odometry can be updated at a faster rate than the main loop (which is 50 hertz). @@ -161,7 +165,7 @@ public Pose2d getPredictedRobotPose(double seconds) { return getEstimatedRobotPose().transformBy(new Transform2d(predictedX, predictedY, predictedRotation)); } - private void initialize() { + private void initializeFieldWidget() { putAprilTagsOnFieldWidget(); SmartDashboard.putData("Field", field); logTargetPath(); From 3e119e4c6061b5865ca5762d897a6365e2043ca0 Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Sun, 8 Feb 2026 12:30:23 +0200 Subject: [PATCH 04/16] Hald update bad climb logic --- src/main/java/frc/trigon/lib | 2 +- .../java/frc/trigon/robot/RobotContainer.java | 4 +++- .../commands/commandfactories/ClimbCommands.java | 15 ++++++++------- .../trigon/robot/subsystems/climber/Climber.java | 2 +- .../subsystems/climber/ClimberConstants.java | 4 ++-- 5 files changed, 15 insertions(+), 12 deletions(-) diff --git a/src/main/java/frc/trigon/lib b/src/main/java/frc/trigon/lib index be4a5859..6b6c7adb 160000 --- a/src/main/java/frc/trigon/lib +++ b/src/main/java/frc/trigon/lib @@ -1 +1 @@ -Subproject commit be4a5859599b2fe2bc0363d533191b634e4d569e +Subproject commit 6b6c7adbf9d1111fa67fdc0b801e0520044bc7f8 diff --git a/src/main/java/frc/trigon/robot/RobotContainer.java b/src/main/java/frc/trigon/robot/RobotContainer.java index 3d695bd3..0249a316 100644 --- a/src/main/java/frc/trigon/robot/RobotContainer.java +++ b/src/main/java/frc/trigon/robot/RobotContainer.java @@ -111,8 +111,10 @@ private void bindControllerCommands() { OperatorConstants.SET_FIXED_SHOOTING_POSITION_LEFT_CORNER_TRIGGER.onTrue(ShootingCommands.getChangeFixedShootingPositionCommand(ShootingCommands.FixedShootingPosition.LEFT_CORNER)); OperatorConstants.SET_FIXED_SHOOTING_POSITION_CLOSE_TO_TOWER_TRIGGER.onTrue(ShootingCommands.getChangeFixedShootingPositionCommand(ShootingCommands.FixedShootingPosition.CLOSE_TO_TOWER)); OperatorConstants.SET_FIXED_SHOOTING_POSITION_CLOSE_TO_OUTPOST_TRIGGER.onTrue(ShootingCommands.getChangeFixedShootingPositionCommand(ShootingCommands.FixedShootingPosition.CLOSE_TO_OUTPOST)); - OperatorConstants.SHORT_EJECTION_TRIGGER.whileTrue(ShootingCommands.getShortEjectFuelCommand()); + OperatorConstants.OPEN_CLIMBER_TRIGGER.onTrue(ClimbCommands.getClimbToL1Command()); + + OperatorConstants.SHORT_EJECTION_TRIGGER.whileTrue(ShootingCommands.getShortEjectFuelCommand()); } private void configureSysIDBindings(MotorSubsystem subsystem) { diff --git a/src/main/java/frc/trigon/robot/commands/commandfactories/ClimbCommands.java b/src/main/java/frc/trigon/robot/commands/commandfactories/ClimbCommands.java index 0f2776aa..5c792490 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/ClimbCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/ClimbCommands.java @@ -4,30 +4,31 @@ import frc.trigon.robot.constants.OperatorConstants; import frc.trigon.robot.subsystems.climber.ClimberCommands; import frc.trigon.robot.subsystems.climber.ClimberConstants; +import org.littletonrobotics.junction.networktables.LoggedNetworkBoolean; public class ClimbCommands { - private static boolean IS_CLIMBING = false; + public static LoggedNetworkBoolean IS_CLIMBING = new LoggedNetworkBoolean("IsClimbing", false); public static Command getClimbToL1Command() { return new SequentialCommandGroup( - ClimberCommands.getSetTargetStateCommand(ClimberConstants.ClimberState.CLIMB_PREPARE).until(OperatorConstants.CONTINUE_CLIMB_TRIGGER), - new InstantCommand(() -> IS_CLIMBING = true), + new InstantCommand(() -> IS_CLIMBING.set(true)), + ClimberCommands.getSetTargetStateCommand(ClimberConstants.ClimberState.PREPARE_CLIMB).until(OperatorConstants.CONTINUE_CLIMB_TRIGGER), ClimberCommands.getSetTargetStateCommand(ClimberConstants.ClimberState.CLIMB_L1) ).until(OperatorConstants.CANCEL_CLIMB_TRIGGER); } - public static Command getClimbDownFromL1Command() { + public static Command getReleaseL1Command() { return new SequentialCommandGroup( new WaitUntilCommand(OperatorConstants.CANCEL_CLIMB_TRIGGER.negate()), - ClimberCommands.getSetTargetStateCommand(ClimberConstants.ClimberState.CLIMB_DOWN).until(OperatorConstants.CANCEL_CLIMB_TRIGGER) + ClimberCommands.getSetTargetStateCommand(ClimberConstants.ClimberState.RELEASE_CLIMB).until(OperatorConstants.CANCEL_CLIMB_TRIGGER) ); } public static Command getClimberDefaultCommand() { return new ConditionalCommand( - getClimbDownFromL1Command().andThen(() -> IS_CLIMBING = false), + getReleaseL1Command().andThen(() -> IS_CLIMBING.set(false)), ClimberCommands.getSetTargetStateCommand(ClimberConstants.ClimberState.REST), - () -> IS_CLIMBING + IS_CLIMBING::get ); } } diff --git a/src/main/java/frc/trigon/robot/subsystems/climber/Climber.java b/src/main/java/frc/trigon/robot/subsystems/climber/Climber.java index 6f4a4382..549dd46e 100644 --- a/src/main/java/frc/trigon/robot/subsystems/climber/Climber.java +++ b/src/main/java/frc/trigon/robot/subsystems/climber/Climber.java @@ -73,7 +73,7 @@ public void sysIDDrive(double targetVoltage) { } public boolean isClimbing() { - return targetState == ClimberConstants.ClimberState.CLIMB_PREPARE || + return targetState == ClimberConstants.ClimberState.PREPARE_CLIMB || targetState == ClimberConstants.ClimberState.CLIMB_L1; } diff --git a/src/main/java/frc/trigon/robot/subsystems/climber/ClimberConstants.java b/src/main/java/frc/trigon/robot/subsystems/climber/ClimberConstants.java index 4cd7b62e..b163dcc5 100644 --- a/src/main/java/frc/trigon/robot/subsystems/climber/ClimberConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/climber/ClimberConstants.java @@ -134,9 +134,9 @@ private static void configureMotor() { public enum ClimberState { REST(0, false), - CLIMB_PREPARE(0.8, false), + PREPARE_CLIMB(0.8, false), CLIMB_L1(0.5, true), - CLIMB_DOWN(0.8, true); + RELEASE_CLIMB(0.8, true); final double targetPositionMeters; final boolean affectedByRobotWeight; From 16ba74f106c9664d0849a09909543d6f2186a02f Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Sun, 8 Feb 2026 12:39:13 +0200 Subject: [PATCH 05/16] Fix climb logic + error + minor logic fix --- .../commands/commandfactories/ClimbCommands.java | 11 +++++++---- .../robotposeestimator/RobotPoseEstimator.java | 5 ++++- .../frc/trigon/robot/subsystems/swerve/Swerve.java | 4 ++-- 3 files changed, 13 insertions(+), 7 deletions(-) diff --git a/src/main/java/frc/trigon/robot/commands/commandfactories/ClimbCommands.java b/src/main/java/frc/trigon/robot/commands/commandfactories/ClimbCommands.java index 5c792490..f1b04643 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/ClimbCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/ClimbCommands.java @@ -1,6 +1,9 @@ package frc.trigon.robot.commands.commandfactories; -import edu.wpi.first.wpilibj2.command.*; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.ConditionalCommand; +import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import frc.trigon.robot.constants.OperatorConstants; import frc.trigon.robot.subsystems.climber.ClimberCommands; import frc.trigon.robot.subsystems.climber.ClimberConstants; @@ -19,14 +22,14 @@ public static Command getClimbToL1Command() { public static Command getReleaseL1Command() { return new SequentialCommandGroup( - new WaitUntilCommand(OperatorConstants.CANCEL_CLIMB_TRIGGER.negate()), - ClimberCommands.getSetTargetStateCommand(ClimberConstants.ClimberState.RELEASE_CLIMB).until(OperatorConstants.CANCEL_CLIMB_TRIGGER) + ClimberCommands.getSetTargetStateCommand(ClimberConstants.ClimberState.RELEASE_CLIMB).until(OperatorConstants.CANCEL_CLIMB_TRIGGER.negate()), + new InstantCommand(() -> IS_CLIMBING.set(false)) ); } public static Command getClimberDefaultCommand() { return new ConditionalCommand( - getReleaseL1Command().andThen(() -> IS_CLIMBING.set(false)), + getReleaseL1Command(), ClimberCommands.getSetTargetStateCommand(ClimberConstants.ClimberState.REST), IS_CLIMBING::get ); diff --git a/src/main/java/frc/trigon/robot/poseestimation/robotposeestimator/RobotPoseEstimator.java b/src/main/java/frc/trigon/robot/poseestimation/robotposeestimator/RobotPoseEstimator.java index 27710b43..7bbed318 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/robotposeestimator/RobotPoseEstimator.java +++ b/src/main/java/frc/trigon/robot/poseestimation/robotposeestimator/RobotPoseEstimator.java @@ -36,6 +36,7 @@ public class RobotPoseEstimator implements AutoCloseable { private final AprilTagCamera[] aprilTagCameras; private final RelativeRobotPoseSource relativeRobotPoseSource; private final boolean shouldUseRelativeRobotPoseSource; + private boolean hasUpdateFromCameras = false; /** * Constructs a new RobotPoseEstimator and sets the relativeRobotPoseSource. @@ -121,7 +122,7 @@ public Pose2d getEstimatedOdometryPose() { } public boolean hasUpdateFromCameras() { - return getCamerasWithResults().length > 0; + return hasUpdateFromCameras; } /** @@ -213,6 +214,8 @@ private void updateFromRelativeRobotPoseSource() { private void updateFromAprilTagCameras() { final AprilTagCamera[] newResultCameras = getCamerasWithResults(); + + this.hasUpdateFromCameras = newResultCameras.length > 0; // sortCamerasByLatestResultTimestamp(newResultCameras); for (AprilTagCamera aprilTagCamera : newResultCameras) { diff --git a/src/main/java/frc/trigon/robot/subsystems/swerve/Swerve.java b/src/main/java/frc/trigon/robot/subsystems/swerve/Swerve.java index c5d559b4..1e1c9124 100644 --- a/src/main/java/frc/trigon/robot/subsystems/swerve/Swerve.java +++ b/src/main/java/frc/trigon/robot/subsystems/swerve/Swerve.java @@ -58,11 +58,11 @@ public void updateLog(SysIdRoutineLog log) { @Override public void updatePeriodically() { - Phoenix6SignalThread.QUEUES_LOCK.lock(); + Phoenix6SignalThread.SIGNALS_LOCK.lock(); try { updateHardware(); } finally { - Phoenix6SignalThread.QUEUES_LOCK.unlock(); + Phoenix6SignalThread.SIGNALS_LOCK.unlock(); } updatePoseEstimatorStates(); From 5fd022d6038200e0ef5a684335f8e2326bcdd44f Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Sun, 8 Feb 2026 12:48:46 +0200 Subject: [PATCH 06/16] Reorder --- src/main/java/frc/trigon/robot/commands/CommandConstants.java | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/main/java/frc/trigon/robot/commands/CommandConstants.java b/src/main/java/frc/trigon/robot/commands/CommandConstants.java index 436b47c9..5d7919e3 100644 --- a/src/main/java/frc/trigon/robot/commands/CommandConstants.java +++ b/src/main/java/frc/trigon/robot/commands/CommandConstants.java @@ -32,6 +32,7 @@ public class CommandConstants { public static final Command //General Commands RESET_HEADING_COMMAND = new InstantCommand(RobotContainer.ROBOT_POSE_ESTIMATOR::resetHeading), + INDICATE_CAMERAS_DISCONNECTED_COMMAND = new InstantCommand(() -> OperatorConstants.DRIVER_CONTROLLER.rumble(RUMBLE_WHEN_CAMERAS_DISCONNECTED_DURATION_SECONDS, RUMBLE_WHEN_CAMERAS_DISCONNECTED_POWER)), SELF_RELATIVE_DRIVE_FROM_DPAD_COMMAND = SwerveCommands.getClosedLoopSelfRelativeDriveCommand( () -> getXPowerFromPov(DRIVER_CONTROLLER.getPov()) / OperatorConstants.POV_DIVIDER / calculateShiftModeValue(MINIMUM_TRANSLATION_SHIFT_POWER), () -> getYPowerFromPov(DRIVER_CONTROLLER.getPov()) / OperatorConstants.POV_DIVIDER / calculateShiftModeValue(MINIMUM_TRANSLATION_SHIFT_POWER), @@ -60,8 +61,6 @@ public class CommandConstants { (omegaRadiansPerSecond) -> RobotContainer.SWERVE.selfRelativeDrive(new ChassisSpeeds(0, 0, omegaRadiansPerSecond)), RobotContainer.SWERVE ); - public static final Command //Robot-specific commands - INDICATE_CAMERAS_DISCONNECTED_COMMAND = new InstantCommand(() -> OperatorConstants.DRIVER_CONTROLLER.rumble(RUMBLE_WHEN_CAMERAS_DISCONNECTED_DURATION_SECONDS, RUMBLE_WHEN_CAMERAS_DISCONNECTED_POWER)); /** * Calculates the target drive power from an axis value by dividing it by the shift mode value. From 89fb34d79dfaa85c4ba5fb4e257def9c742d9fcd Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Tue, 10 Feb 2026 12:13:39 +0200 Subject: [PATCH 07/16] Made intake assist toggles ignore disable --- .../robot/commands/commandfactories/FuelIntakeCommands.java | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/trigon/robot/commands/commandfactories/FuelIntakeCommands.java b/src/main/java/frc/trigon/robot/commands/commandfactories/FuelIntakeCommands.java index beac4875..945db6ae 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/FuelIntakeCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/FuelIntakeCommands.java @@ -28,19 +28,19 @@ public static Command getIntakeCommand() { public static Command getToggleDefaultIntakeStateCommand() { return new InstantCommand( () -> SHOULD_KEEP_INTAKE_OPEN.set(!SHOULD_KEEP_INTAKE_OPEN.get()) - ); + ).ignoringDisable(true); } public static Command getEnableIntakeAssistCommand() { return new InstantCommand( () -> SHOULD_ASSIST_INTAKE.set(true) - ); + ).ignoringDisable(true); } public static Command getDisableIntakeAssistCommand() { return new InstantCommand( () -> SHOULD_ASSIST_INTAKE.set(false) - ); + ).ignoringDisable(true); } private static Command getIntakeAssistCommand() { From cffe1ee43e18acc4d773204df1f4b499fa4ca11d Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Thu, 12 Feb 2026 12:06:20 +0200 Subject: [PATCH 08/16] Finla --- .../trigon/robot/commands/commandfactories/ClimbCommands.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/trigon/robot/commands/commandfactories/ClimbCommands.java b/src/main/java/frc/trigon/robot/commands/commandfactories/ClimbCommands.java index f1b04643..d7157117 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/ClimbCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/ClimbCommands.java @@ -10,7 +10,7 @@ import org.littletonrobotics.junction.networktables.LoggedNetworkBoolean; public class ClimbCommands { - public static LoggedNetworkBoolean IS_CLIMBING = new LoggedNetworkBoolean("IsClimbing", false); + public static final LoggedNetworkBoolean IS_CLIMBING = new LoggedNetworkBoolean("IsClimbing", false); public static Command getClimbToL1Command() { return new SequentialCommandGroup( From 5706aac4ef7581d9f54029556a2c7dce893f20e9 Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Thu, 12 Feb 2026 14:56:08 +0200 Subject: [PATCH 09/16] =?UTF-8?q?Commit=20so=20that=20CodeRabbit=20can=20r?= =?UTF-8?q?eview=20=F0=9F=91=8D?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../java/frc/trigon/robot/RobotContainer.java | 2 + .../ShootingSafeDriveCommand.java | 76 +++++++++++++++++++ .../robot/constants/OperatorConstants.java | 1 + .../robot/subsystems/swerve/Swerve.java | 11 +++ .../subsystems/swerve/SwerveCommands.java | 18 ++++- 5 files changed, 107 insertions(+), 1 deletion(-) create mode 100644 src/main/java/frc/trigon/robot/commands/commandclasses/ShootingSafeDriveCommand.java diff --git a/src/main/java/frc/trigon/robot/RobotContainer.java b/src/main/java/frc/trigon/robot/RobotContainer.java index 0249a316..edb16bcb 100644 --- a/src/main/java/frc/trigon/robot/RobotContainer.java +++ b/src/main/java/frc/trigon/robot/RobotContainer.java @@ -11,6 +11,7 @@ import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.trigon.lib.utilities.flippable.Flippable; import frc.trigon.robot.commands.CommandConstants; +import frc.trigon.robot.commands.commandclasses.ShootingSafeDriveCommand; import frc.trigon.robot.commands.commandfactories.ClimbCommands; import frc.trigon.robot.commands.commandfactories.FuelIntakeCommands; import frc.trigon.robot.commands.commandfactories.GeneralCommands; @@ -96,6 +97,7 @@ private void bindControllerCommands() { OperatorConstants.RESET_HEADING_TRIGGER.onTrue(CommandConstants.RESET_HEADING_COMMAND); OperatorConstants.DRIVE_FROM_DPAD_TRIGGER.whileTrue(CommandConstants.SELF_RELATIVE_DRIVE_FROM_DPAD_COMMAND); OperatorConstants.TOGGLE_BRAKE_TRIGGER.onTrue(GeneralCommands.getToggleBrakeCommand()); + OperatorConstants.SHOOTING_SAFE_DRIVE_TRIGGER.whileTrue(new ShootingSafeDriveCommand()); OperatorConstants.CAMERAS_DISCONNECTED_TRIGGER.onTrue(CommandConstants.INDICATE_CAMERAS_DISCONNECTED_COMMAND); OperatorConstants.INTAKE_TRIGGER.whileTrue(FuelIntakeCommands.getIntakeCommand()); diff --git a/src/main/java/frc/trigon/robot/commands/commandclasses/ShootingSafeDriveCommand.java b/src/main/java/frc/trigon/robot/commands/commandclasses/ShootingSafeDriveCommand.java new file mode 100644 index 00000000..be626263 --- /dev/null +++ b/src/main/java/frc/trigon/robot/commands/commandclasses/ShootingSafeDriveCommand.java @@ -0,0 +1,76 @@ +package frc.trigon.robot.commands.commandclasses; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.filter.SlewRateLimiter; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import frc.trigon.robot.RobotContainer; +import frc.trigon.robot.constants.FieldConstants; +import frc.trigon.robot.constants.OperatorConstants; +import frc.trigon.robot.subsystems.swerve.SwerveCommands; + +/** + * A command class that limits the swerve powers to ease shooting to the hub and increase accuracy. + */ +public class ShootingSafeDriveCommand extends ParallelCommandGroup { + private static final SlewRateLimiter + TRANSLATION_SLEW_RATE_LIMITER = new SlewRateLimiter(0.3), + ROTATION_SLEW_RATE_LIMITER = new SlewRateLimiter(0.8); + private static final double + MINIMUM_DRIVE_POWER_TOWARDS_HUB = 0.3; + + + public ShootingSafeDriveCommand() { + addCommands( + SwerveCommands.getClosedLoopFieldRelativeDriveCommand( + ShootingSafeDriveCommand::calculateSafeDriveTranslationPower, + ShootingSafeDriveCommand::calculateSafeDriveRotationPower + ).asProxy() + ); + } + + private static Translation2d calculateSafeDriveTranslationPower() { + return limitTranslationPowerToEaseShooting(getSlewRateLimitedTranslationPower()); + } + + private static double calculateSafeDriveRotationPower() { + final double rawPower = OperatorConstants.DRIVER_CONTROLLER.getRightX(); + return ROTATION_SLEW_RATE_LIMITER.calculate(rawPower); + } + + private static Translation2d getSlewRateLimitedTranslationPower() { + final Translation2d rawPower = new Translation2d( + OperatorConstants.DRIVER_CONTROLLER.getLeftY(), + OperatorConstants.DRIVER_CONTROLLER.getLeftX() + ); + + return new Translation2d( + TRANSLATION_SLEW_RATE_LIMITER.calculate(rawPower.getNorm()), + rawPower.getAngle() + ); + } + + private static Translation2d limitTranslationPowerToEaseShooting(Translation2d targetPower) { + final Rotation2d fieldRelativeAngleToHub = calculateFieldRelativeAngleToHub(); + + final double + rawXPower = targetPower.getX(), + rawYPower = targetPower.getY(); + final double + xPowerLimiter = MathUtil.clamp(Math.abs(fieldRelativeAngleToHub.getCos()), 0, 1 - MINIMUM_DRIVE_POWER_TOWARDS_HUB), + yPowerLimiter = MathUtil.clamp(Math.abs(fieldRelativeAngleToHub.getSin()), 0, 1 - MINIMUM_DRIVE_POWER_TOWARDS_HUB); + + return new Translation2d( + rawXPower * xPowerLimiter, + rawYPower * yPowerLimiter + ); + } + + private static Rotation2d calculateFieldRelativeAngleToHub() { + final Translation2d + robotPosition = RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose().getTranslation(), + hubPosition = FieldConstants.HUB_POSITION.get(); + return hubPosition.minus(robotPosition).getAngle(); + } +} diff --git a/src/main/java/frc/trigon/robot/constants/OperatorConstants.java b/src/main/java/frc/trigon/robot/constants/OperatorConstants.java index c3dac279..b71b1672 100644 --- a/src/main/java/frc/trigon/robot/constants/OperatorConstants.java +++ b/src/main/java/frc/trigon/robot/constants/OperatorConstants.java @@ -48,6 +48,7 @@ public class OperatorConstants { DRIVE_FROM_DPAD_TRIGGER = new Trigger(() -> DRIVER_CONTROLLER.getPov() != -1), TOGGLE_BRAKE_TRIGGER = OPERATOR_CONTROLLER.g().or(RobotController::getUserButton), DEBUGGING_TRIGGER = OPERATOR_CONTROLLER.f2(), + SHOOTING_SAFE_DRIVE_TRIGGER = DRIVER_CONTROLLER.leftStick(), CAMERAS_DISCONNECTED_TRIGGER = new Trigger(() -> !RobotContainer.ROBOT_POSE_ESTIMATOR.hasUpdateFromCameras()).debounce(ARE_CAMERAS_DISCONNECTED_CHECK_DEBOUNCE_SECONDS), FORWARD_QUASISTATIC_CHARACTERIZATION_TRIGGER = OPERATOR_CONTROLLER.right(), BACKWARD_QUASISTATIC_CHARACTERIZATION_TRIGGER = OPERATOR_CONTROLLER.left(), diff --git a/src/main/java/frc/trigon/robot/subsystems/swerve/Swerve.java b/src/main/java/frc/trigon/robot/subsystems/swerve/Swerve.java index 1e1c9124..46077a5b 100644 --- a/src/main/java/frc/trigon/robot/subsystems/swerve/Swerve.java +++ b/src/main/java/frc/trigon/robot/subsystems/swerve/Swerve.java @@ -228,6 +228,17 @@ void fieldRelativeDrive(double xPower, double yPower, double thetaPower) { selfRelativeDrive(speeds); } + /** + * Drives the swerve with the given powers, relative to the field's frame of reference. + * + * @param translationPowers the translation powers + * @param rotationPower the rotation power + */ + void fieldRelativeDrive(Translation2d translationPowers, double rotationPower) { + final ChassisSpeeds speeds = selfRelativeSpeedsFromFieldRelativePowers(translationPowers.getX(), translationPowers.getY(), rotationPower); + selfRelativeDrive(speeds); + } + /** * Drives the swerve with the given powers, relative to the robot's frame of reference. * diff --git a/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveCommands.java b/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveCommands.java index 243d5e3f..80450b57 100644 --- a/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveCommands.java +++ b/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveCommands.java @@ -2,11 +2,12 @@ import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.path.PathConstraints; +import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj2.command.*; -import frc.trigon.robot.RobotContainer; import frc.trigon.lib.commands.InitExecuteCommand; import frc.trigon.lib.utilities.flippable.FlippablePose2d; import frc.trigon.lib.utilities.flippable.FlippableRotation2d; +import frc.trigon.robot.RobotContainer; import java.util.Set; import java.util.function.DoubleSupplier; @@ -29,6 +30,21 @@ public static Command getClosedLoopFieldRelativeDriveCommand(DoubleSupplier xSup ); } + /** + * Creates a command that drives the swerve with the given powers, relative to the field's frame of reference, in closed loop mode. + * + * @param translationSupplier the target translation powers + * @param rotationSupplier the target rotation power, CCW+ + * @return the command + */ + public static Command getClosedLoopFieldRelativeDriveCommand(Supplier translationSupplier, DoubleSupplier rotationSupplier) { + return new InitExecuteCommand( + () -> RobotContainer.SWERVE.initializeDrive(true), + () -> RobotContainer.SWERVE.fieldRelativeDrive(translationSupplier.get(), rotationSupplier.getAsDouble()), + RobotContainer.SWERVE + ); + } + /** * Creates a command that drives the swerve with the given powers, relative to the field's frame of reference, in closed loop mode. * This command will use pid to reach the target angle. From b2206b757ec4added63941d32d6e0cff2701bb6c Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Wed, 18 Feb 2026 09:10:00 +0200 Subject: [PATCH 10/16] Added indicate alliance shift --- .../java/frc/trigon/robot/RobotContainer.java | 1 + .../robot/commands/CommandConstants.java | 17 +++++++++++--- .../commandclasses/IntakeAssistCommand.java | 2 +- .../robot/constants/OperatorConstants.java | 1 + .../frc/trigon/robot/misc/MatchTracker.java | 22 ++++++++++++++----- 5 files changed, 33 insertions(+), 10 deletions(-) diff --git a/src/main/java/frc/trigon/robot/RobotContainer.java b/src/main/java/frc/trigon/robot/RobotContainer.java index edb16bcb..2e29749c 100644 --- a/src/main/java/frc/trigon/robot/RobotContainer.java +++ b/src/main/java/frc/trigon/robot/RobotContainer.java @@ -99,6 +99,7 @@ private void bindControllerCommands() { OperatorConstants.TOGGLE_BRAKE_TRIGGER.onTrue(GeneralCommands.getToggleBrakeCommand()); OperatorConstants.SHOOTING_SAFE_DRIVE_TRIGGER.whileTrue(new ShootingSafeDriveCommand()); OperatorConstants.CAMERAS_DISCONNECTED_TRIGGER.onTrue(CommandConstants.INDICATE_CAMERAS_DISCONNECTED_COMMAND); + OperatorConstants.INDICATE_ALLIANCE_SHIFT_TRIGGER.onTrue(CommandConstants.INDICATE_ALLIANCE_SHIFT_COMMAND); OperatorConstants.INTAKE_TRIGGER.whileTrue(FuelIntakeCommands.getIntakeCommand()); OperatorConstants.TOGGLE_SHOULD_KEEP_INTAKE_OPEN_TRIGGER.onTrue(FuelIntakeCommands.getToggleDefaultIntakeStateCommand()); diff --git a/src/main/java/frc/trigon/robot/commands/CommandConstants.java b/src/main/java/frc/trigon/robot/commands/CommandConstants.java index 5d7919e3..c643a9ed 100644 --- a/src/main/java/frc/trigon/robot/commands/CommandConstants.java +++ b/src/main/java/frc/trigon/robot/commands/CommandConstants.java @@ -27,12 +27,18 @@ public class CommandConstants { MINIMUM_ROTATION_SHIFT_POWER = 0.4; private static final double JOYSTICK_ORIENTED_ROTATION_DEADBAND = 0.07; private static final double - RUMBLE_WHEN_CAMERAS_DISCONNECTED_DURATION_SECONDS = 0.5, - RUMBLE_WHEN_CAMERAS_DISCONNECTED_POWER = 1; + INDICATE_CAMERAS_DISCONNECTED_RUMBLE_DURATION_SECONDS = 0.5, + INDICATE_CAMERAS_DISCONNECTED_RUMBLE_POWER = 1; + private static final double + INDICATE_ALLIANCE_SHIFT_RUMBLE_DURATION_SECONDS = 1, + INDICATE_ALLIANCE_SHIFT_RUMBLE_POWER = 0.5; public static final Command //General Commands RESET_HEADING_COMMAND = new InstantCommand(RobotContainer.ROBOT_POSE_ESTIMATOR::resetHeading), - INDICATE_CAMERAS_DISCONNECTED_COMMAND = new InstantCommand(() -> OperatorConstants.DRIVER_CONTROLLER.rumble(RUMBLE_WHEN_CAMERAS_DISCONNECTED_DURATION_SECONDS, RUMBLE_WHEN_CAMERAS_DISCONNECTED_POWER)), + INDICATE_CAMERAS_DISCONNECTED_COMMAND = new InstantCommand(() -> OperatorConstants.DRIVER_CONTROLLER.rumble( + INDICATE_CAMERAS_DISCONNECTED_RUMBLE_DURATION_SECONDS, + INDICATE_CAMERAS_DISCONNECTED_RUMBLE_POWER + )), SELF_RELATIVE_DRIVE_FROM_DPAD_COMMAND = SwerveCommands.getClosedLoopSelfRelativeDriveCommand( () -> getXPowerFromPov(DRIVER_CONTROLLER.getPov()) / OperatorConstants.POV_DIVIDER / calculateShiftModeValue(MINIMUM_TRANSLATION_SHIFT_POWER), () -> getYPowerFromPov(DRIVER_CONTROLLER.getPov()) / OperatorConstants.POV_DIVIDER / calculateShiftModeValue(MINIMUM_TRANSLATION_SHIFT_POWER), @@ -61,6 +67,11 @@ public class CommandConstants { (omegaRadiansPerSecond) -> RobotContainer.SWERVE.selfRelativeDrive(new ChassisSpeeds(0, 0, omegaRadiansPerSecond)), RobotContainer.SWERVE ); + public static final Command //Robot-specific commands + INDICATE_ALLIANCE_SHIFT_COMMAND = new InstantCommand(() -> OperatorConstants.DRIVER_CONTROLLER.rumble( + INDICATE_ALLIANCE_SHIFT_RUMBLE_DURATION_SECONDS, + INDICATE_ALLIANCE_SHIFT_RUMBLE_POWER + )); /** * Calculates the target drive power from an axis value by dividing it by the shift mode value. diff --git a/src/main/java/frc/trigon/robot/commands/commandclasses/IntakeAssistCommand.java b/src/main/java/frc/trigon/robot/commands/commandclasses/IntakeAssistCommand.java index fce03ab1..908136f3 100644 --- a/src/main/java/frc/trigon/robot/commands/commandclasses/IntakeAssistCommand.java +++ b/src/main/java/frc/trigon/robot/commands/commandclasses/IntakeAssistCommand.java @@ -120,7 +120,7 @@ private Translation2d calculateSelfRelativeDistanceFromBestGamePiece() { final Translation2d fieldRelativeDistanceFromBestGamePiece = robotPose.getTranslation().minus(bestGamePieceFieldRelativePosition); final Translation2d selfRelativeDistanceFromBestGamePiece = fieldRelativeDistanceFromBestGamePiece.rotateBy(robotPose.getRotation().unaryMinus()); - Logger.recordOutput("IntakeAssist/selfRelativeDistanceFromBestGamePiece", selfRelativeDistanceFromBestGamePiece); + Logger.recordOutput("Assists/selfRelativeDistanceFromBestGamePiece", selfRelativeDistanceFromBestGamePiece); return selfRelativeDistanceFromBestGamePiece; } diff --git a/src/main/java/frc/trigon/robot/constants/OperatorConstants.java b/src/main/java/frc/trigon/robot/constants/OperatorConstants.java index b71b1672..fed4d0a6 100644 --- a/src/main/java/frc/trigon/robot/constants/OperatorConstants.java +++ b/src/main/java/frc/trigon/robot/constants/OperatorConstants.java @@ -50,6 +50,7 @@ public class OperatorConstants { DEBUGGING_TRIGGER = OPERATOR_CONTROLLER.f2(), SHOOTING_SAFE_DRIVE_TRIGGER = DRIVER_CONTROLLER.leftStick(), CAMERAS_DISCONNECTED_TRIGGER = new Trigger(() -> !RobotContainer.ROBOT_POSE_ESTIMATOR.hasUpdateFromCameras()).debounce(ARE_CAMERAS_DISCONNECTED_CHECK_DEBOUNCE_SECONDS), + INDICATE_ALLIANCE_SHIFT_TRIGGER = new Trigger(MatchTracker::shouldIndicateAllianceShift), FORWARD_QUASISTATIC_CHARACTERIZATION_TRIGGER = OPERATOR_CONTROLLER.right(), BACKWARD_QUASISTATIC_CHARACTERIZATION_TRIGGER = OPERATOR_CONTROLLER.left(), FORWARD_DYNAMIC_CHARACTERIZATION_TRIGGER = OPERATOR_CONTROLLER.up(), diff --git a/src/main/java/frc/trigon/robot/misc/MatchTracker.java b/src/main/java/frc/trigon/robot/misc/MatchTracker.java index 7deeadca..43289d44 100644 --- a/src/main/java/frc/trigon/robot/misc/MatchTracker.java +++ b/src/main/java/frc/trigon/robot/misc/MatchTracker.java @@ -5,8 +5,19 @@ import org.littletonrobotics.junction.AutoLogOutput; public final class MatchTracker { + private static final double TIME_BEFORE_ALLIANCE_SHIFT_TO_INDICATE_SECONDS = 5; + + @AutoLogOutput(key = "Assists/ShouldIndicateAllianceShift") + public static boolean shouldIndicateAllianceShift() { + return isHubActive(getMatchTimeSeconds() - TIME_BEFORE_ALLIANCE_SHIFT_TO_INDICATE_SECONDS); + } + @AutoLogOutput(key = "IsHubActive") public static boolean isHubActive() { + return isHubActive(getMatchTimeSeconds()); + } + + public static boolean isHubActive(double matchTimeSeconds) { if (!DriverStation.isTeleop()) return true; @@ -15,8 +26,8 @@ public static boolean isHubActive() { if (!"R".equals(gameMessage) && !"B".equals(gameMessage)) return true; final boolean didRedAllianceWinAutonomous = "R".equals(gameMessage); - final boolean isRedHubActive = isRedHubActive(didRedAllianceWinAutonomous); - return isRedAlliance ? isRedHubActive : !isRedHubActive; + final boolean isRedHubActive = isRedHubActive(didRedAllianceWinAutonomous, matchTimeSeconds); + return isRedAlliance == isRedHubActive; } @AutoLogOutput(key = "MatchTimeSeconds") @@ -24,8 +35,8 @@ public static double getMatchTimeSeconds() { return DriverStation.getMatchTime(); } - private static boolean isRedHubActive(boolean didRedAllianceWinAutonomous) { - final int currentShiftNumber = getCurrentShiftNumber(); + private static boolean isRedHubActive(boolean didRedAllianceWinAutonomous, double matchTimeSeconds) { + final int currentShiftNumber = getShiftNumber(matchTimeSeconds); if (currentShiftNumber == -1) return true; if (didRedAllianceWinAutonomous && currentShiftNumber % 2 != 0) @@ -35,8 +46,7 @@ private static boolean isRedHubActive(boolean didRedAllianceWinAutonomous) { return true; } - private static int getCurrentShiftNumber() { - final double matchTimeSeconds = getMatchTimeSeconds(); + private static int getShiftNumber(double matchTimeSeconds) { if (matchTimeSeconds > 30 && matchTimeSeconds <= 55) return 4; if (matchTimeSeconds > 55 && matchTimeSeconds <= 80) From 391db9d73ec2483d32b6c9eef4250df0410a86d6 Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Wed, 18 Feb 2026 09:13:18 +0200 Subject: [PATCH 11/16] Refactor and reset PID controllers --- .../ShootingSafeDriveCommand.java | 20 ++++++++++++++----- 1 file changed, 15 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/trigon/robot/commands/commandclasses/ShootingSafeDriveCommand.java b/src/main/java/frc/trigon/robot/commands/commandclasses/ShootingSafeDriveCommand.java index be626263..f24e2f4a 100644 --- a/src/main/java/frc/trigon/robot/commands/commandclasses/ShootingSafeDriveCommand.java +++ b/src/main/java/frc/trigon/robot/commands/commandclasses/ShootingSafeDriveCommand.java @@ -4,7 +4,9 @@ import edu.wpi.first.math.filter.SlewRateLimiter; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import frc.trigon.robot.RobotContainer; import frc.trigon.robot.constants.FieldConstants; import frc.trigon.robot.constants.OperatorConstants; @@ -13,16 +15,17 @@ /** * A command class that limits the swerve powers to ease shooting to the hub and increase accuracy. */ -public class ShootingSafeDriveCommand extends ParallelCommandGroup { +public class ShootingSafeDriveCommand extends SequentialCommandGroup { private static final SlewRateLimiter TRANSLATION_SLEW_RATE_LIMITER = new SlewRateLimiter(0.3), ROTATION_SLEW_RATE_LIMITER = new SlewRateLimiter(0.8); private static final double - MINIMUM_DRIVE_POWER_TOWARDS_HUB = 0.3; + MAXIMUM_DRIVE_POWER_LIMIT_TOWARDS_HUB = 0.7; public ShootingSafeDriveCommand() { addCommands( + getInitializeCommand(), SwerveCommands.getClosedLoopFieldRelativeDriveCommand( ShootingSafeDriveCommand::calculateSafeDriveTranslationPower, ShootingSafeDriveCommand::calculateSafeDriveRotationPower @@ -30,6 +33,13 @@ public ShootingSafeDriveCommand() { ); } + private static Command getInitializeCommand() { + return new InstantCommand(() -> { + TRANSLATION_SLEW_RATE_LIMITER.reset(0); + ROTATION_SLEW_RATE_LIMITER.reset(0); + }); + } + private static Translation2d calculateSafeDriveTranslationPower() { return limitTranslationPowerToEaseShooting(getSlewRateLimitedTranslationPower()); } @@ -58,8 +68,8 @@ private static Translation2d limitTranslationPowerToEaseShooting(Translation2d t rawXPower = targetPower.getX(), rawYPower = targetPower.getY(); final double - xPowerLimiter = MathUtil.clamp(Math.abs(fieldRelativeAngleToHub.getCos()), 0, 1 - MINIMUM_DRIVE_POWER_TOWARDS_HUB), - yPowerLimiter = MathUtil.clamp(Math.abs(fieldRelativeAngleToHub.getSin()), 0, 1 - MINIMUM_DRIVE_POWER_TOWARDS_HUB); + xPowerLimiter = MathUtil.clamp(Math.abs(fieldRelativeAngleToHub.getCos()), 0, MAXIMUM_DRIVE_POWER_LIMIT_TOWARDS_HUB), + yPowerLimiter = MathUtil.clamp(Math.abs(fieldRelativeAngleToHub.getSin()), 0, MAXIMUM_DRIVE_POWER_LIMIT_TOWARDS_HUB); return new Translation2d( rawXPower * xPowerLimiter, From a831a93e7228cb5165493c212449b6f801dcb19f Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Thu, 19 Feb 2026 15:22:56 +0200 Subject: [PATCH 12/16] Update auton commands with main update + fix logic --- .../ShootingSafeDriveCommand.java | 64 +++++++++++++------ .../autonomous/GeneralAutonomousCommands.java | 4 +- 2 files changed, 45 insertions(+), 23 deletions(-) diff --git a/src/main/java/frc/trigon/robot/commands/commandclasses/ShootingSafeDriveCommand.java b/src/main/java/frc/trigon/robot/commands/commandclasses/ShootingSafeDriveCommand.java index f24e2f4a..bc8cc8ed 100644 --- a/src/main/java/frc/trigon/robot/commands/commandclasses/ShootingSafeDriveCommand.java +++ b/src/main/java/frc/trigon/robot/commands/commandclasses/ShootingSafeDriveCommand.java @@ -11,17 +11,19 @@ import frc.trigon.robot.constants.FieldConstants; import frc.trigon.robot.constants.OperatorConstants; import frc.trigon.robot.subsystems.swerve.SwerveCommands; +import frc.trigon.robot.subsystems.swerve.SwerveConstants; /** * A command class that limits the swerve powers to ease shooting to the hub and increase accuracy. */ public class ShootingSafeDriveCommand extends SequentialCommandGroup { - private static final SlewRateLimiter - TRANSLATION_SLEW_RATE_LIMITER = new SlewRateLimiter(0.3), - ROTATION_SLEW_RATE_LIMITER = new SlewRateLimiter(0.8); private static final double - MAXIMUM_DRIVE_POWER_LIMIT_TOWARDS_HUB = 0.7; - + TRANSLATION_SLEW_RATE = 2, + MAXIMUM_DRIVE_POWER_TOWARDS_HUB = 3 / SwerveConstants.MAXIMUM_SPEED_METERS_PER_SECOND; + private static final SlewRateLimiter + X_SLEW_RATE_LIMITER = new SlewRateLimiter(TRANSLATION_SLEW_RATE), + Y_SLEW_RATE_LIMITER = new SlewRateLimiter(TRANSLATION_SLEW_RATE), + ROTATION_SLEW_RATE_LIMITER = new SlewRateLimiter(1); public ShootingSafeDriveCommand() { addCommands( @@ -35,7 +37,8 @@ public ShootingSafeDriveCommand() { private static Command getInitializeCommand() { return new InstantCommand(() -> { - TRANSLATION_SLEW_RATE_LIMITER.reset(0); + X_SLEW_RATE_LIMITER.reset(0); + Y_SLEW_RATE_LIMITER.reset(0); ROTATION_SLEW_RATE_LIMITER.reset(0); }); } @@ -50,30 +53,49 @@ private static double calculateSafeDriveRotationPower() { } private static Translation2d getSlewRateLimitedTranslationPower() { - final Translation2d rawPower = new Translation2d( - OperatorConstants.DRIVER_CONTROLLER.getLeftY(), - OperatorConstants.DRIVER_CONTROLLER.getLeftX() - ); + final double + rawXPower = OperatorConstants.DRIVER_CONTROLLER.getLeftY(), + rawYPower = OperatorConstants.DRIVER_CONTROLLER.getLeftX(); return new Translation2d( - TRANSLATION_SLEW_RATE_LIMITER.calculate(rawPower.getNorm()), - rawPower.getAngle() + X_SLEW_RATE_LIMITER.calculate(rawXPower), + Y_SLEW_RATE_LIMITER.calculate(rawYPower) ); } private static Translation2d limitTranslationPowerToEaseShooting(Translation2d targetPower) { - final Rotation2d fieldRelativeAngleToHub = calculateFieldRelativeAngleToHub(); + final Rotation2d hubDirection = calculateFieldRelativeAngleToHub().plus(Rotation2d.k180deg); - final double - rawXPower = targetPower.getX(), - rawYPower = targetPower.getY(); - final double - xPowerLimiter = MathUtil.clamp(Math.abs(fieldRelativeAngleToHub.getCos()), 0, MAXIMUM_DRIVE_POWER_LIMIT_TOWARDS_HUB), - yPowerLimiter = MathUtil.clamp(Math.abs(fieldRelativeAngleToHub.getSin()), 0, MAXIMUM_DRIVE_POWER_LIMIT_TOWARDS_HUB); + final double radialVelocity = calculateRadialVelocityToHub(targetPower, hubDirection); + final double limitedRadialVelocity = limitRadialVelocity(radialVelocity); + + final Translation2d tangentialComponent = calculateTangentialComponent(targetPower, radialVelocity, hubDirection); + final Translation2d limitedRadialComponent = calculateRadialComponent(limitedRadialVelocity, hubDirection); + + return tangentialComponent.plus(limitedRadialComponent); + } + + private static double calculateRadialVelocityToHub(Translation2d velocity, Rotation2d hubDirection) { + return velocity.getX() * hubDirection.getCos() + velocity.getY() * hubDirection.getSin(); + } + + private static double limitRadialVelocity(double radialVelocity) { + return MathUtil.clamp( + radialVelocity, + -MAXIMUM_DRIVE_POWER_TOWARDS_HUB, + MAXIMUM_DRIVE_POWER_TOWARDS_HUB + ); + } + + private static Translation2d calculateTangentialComponent(Translation2d velocity, double radialVelocity, Rotation2d hubDirection) { + final Translation2d radialComponent = calculateRadialComponent(radialVelocity, hubDirection); + return velocity.minus(radialComponent); + } + private static Translation2d calculateRadialComponent(double radialVelocity, Rotation2d hubDirection) { return new Translation2d( - rawXPower * xPowerLimiter, - rawYPower * yPowerLimiter + radialVelocity * hubDirection.getCos(), + radialVelocity * hubDirection.getSin() ); } diff --git a/src/main/java/frc/trigon/robot/commands/commandfactories/autonomous/GeneralAutonomousCommands.java b/src/main/java/frc/trigon/robot/commands/commandfactories/autonomous/GeneralAutonomousCommands.java index 16792f78..fcf52bd6 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/autonomous/GeneralAutonomousCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/autonomous/GeneralAutonomousCommands.java @@ -120,8 +120,8 @@ public static Command getClimbCommand(Supplier climbPosition) { private static Command getClimbToL1Command(Supplier climbPosition) { return new SequentialCommandGroup( - ClimberCommands.getSetTargetStateCommand(ClimberConstants.ClimberState.CLIMB_PREPARE).until(() -> RobotContainer.CLIMBER.atTargetState() && RobotContainer.SWERVE.atPose(climbPosition.get())), - new InstantCommand(() -> ClimbCommands.IS_CLIMBING = true), + ClimberCommands.getSetTargetStateCommand(ClimberConstants.ClimberState.PREPARE_CLIMB).until(() -> RobotContainer.CLIMBER.atTargetState() && RobotContainer.SWERVE.atPose(climbPosition.get())), + new InstantCommand(() -> ClimbCommands.IS_CLIMBING.set(true)), ClimberCommands.getSetTargetStateCommand(ClimberConstants.ClimberState.CLIMB_L1) ).until(OperatorConstants.CANCEL_CLIMB_TRIGGER); } From e8d27be6a3bbbe9df0a674e7cb2203e19147521c Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Thu, 19 Feb 2026 17:58:05 +0200 Subject: [PATCH 13/16] Update lib --- src/main/java/frc/trigon/lib | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/trigon/lib b/src/main/java/frc/trigon/lib index 6b6c7adb..33193d5c 160000 --- a/src/main/java/frc/trigon/lib +++ b/src/main/java/frc/trigon/lib @@ -1 +1 @@ -Subproject commit 6b6c7adbf9d1111fa67fdc0b801e0520044bc7f8 +Subproject commit 33193d5c42b4f36076569d714d255f85c1acc381 From 7f0b775e13045c77de918b585899c233004ef678 Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Thu, 19 Feb 2026 17:59:02 +0200 Subject: [PATCH 14/16] Make it so don't go brrr during practice --- src/main/java/frc/trigon/robot/misc/MatchTracker.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/trigon/robot/misc/MatchTracker.java b/src/main/java/frc/trigon/robot/misc/MatchTracker.java index bc5d84c9..88394929 100644 --- a/src/main/java/frc/trigon/robot/misc/MatchTracker.java +++ b/src/main/java/frc/trigon/robot/misc/MatchTracker.java @@ -9,7 +9,7 @@ public final class MatchTracker { @AutoLogOutput(key = "Assists/ShouldIndicateAllianceShift") public static boolean shouldIndicateAllianceShift() { - return isHubActive(getMatchTimeSeconds() - TIME_BEFORE_ALLIANCE_SHIFT_TO_INDICATE_SECONDS); + return DriverStation.isDSAttached() && isHubActive(getMatchTimeSeconds() - TIME_BEFORE_ALLIANCE_SHIFT_TO_INDICATE_SECONDS); } @AutoLogOutput(key = "IsHubActive") From 7d114b64e94d4fce0e3dc14b107be8b812c4f99c Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Thu, 19 Feb 2026 18:01:52 +0200 Subject: [PATCH 15/16] Make last commit better --- src/main/java/frc/trigon/robot/misc/MatchTracker.java | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/trigon/robot/misc/MatchTracker.java b/src/main/java/frc/trigon/robot/misc/MatchTracker.java index 88394929..c4a0592f 100644 --- a/src/main/java/frc/trigon/robot/misc/MatchTracker.java +++ b/src/main/java/frc/trigon/robot/misc/MatchTracker.java @@ -9,7 +9,8 @@ public final class MatchTracker { @AutoLogOutput(key = "Assists/ShouldIndicateAllianceShift") public static boolean shouldIndicateAllianceShift() { - return DriverStation.isDSAttached() && isHubActive(getMatchTimeSeconds() - TIME_BEFORE_ALLIANCE_SHIFT_TO_INDICATE_SECONDS); + return DriverStation.getMatchType() != DriverStation.MatchType.None && + isHubActive(getMatchTimeSeconds() - TIME_BEFORE_ALLIANCE_SHIFT_TO_INDICATE_SECONDS); } @AutoLogOutput(key = "IsHubActive") From 9c19cb448bcd20902053b2f1ece212e2fa8d2081 Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Thu, 19 Feb 2026 18:10:11 +0200 Subject: [PATCH 16/16] Fix bug --- src/main/java/frc/trigon/robot/subsystems/swerve/Swerve.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/trigon/robot/subsystems/swerve/Swerve.java b/src/main/java/frc/trigon/robot/subsystems/swerve/Swerve.java index ec74dee0..c337be1b 100644 --- a/src/main/java/frc/trigon/robot/subsystems/swerve/Swerve.java +++ b/src/main/java/frc/trigon/robot/subsystems/swerve/Swerve.java @@ -59,11 +59,11 @@ public void updateLog(SysIdRoutineLog log) { @Override public void updatePeriodically() { - Phoenix6SignalThread.SIGNALS_LOCK.lock(); + Phoenix6SignalThread.QUEUES_LOCK.lock(); try { updateHardware(); } finally { - Phoenix6SignalThread.SIGNALS_LOCK.unlock(); + Phoenix6SignalThread.QUEUES_LOCK.unlock(); } updatePoseEstimatorStates();