Better Controls

src/main/java/frc/trigon/robot/RobotContainer.java

@@ -10,6 +10,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;
@@ -93,6 +94,9 @@ 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.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());
@@ -107,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) {

src/main/java/frc/trigon/robot/commands/CommandConstants.java

@@ -26,9 +26,19 @@ 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
+            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
+    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(
+                    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),
@@ -57,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.

src/main/java/frc/trigon/robot/commands/commandclasses/IntakeAssistCommand.java

@@ -119,7 +119,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;
     }
 

src/main/java/frc/trigon/robot/commands/commandclasses/ShootingSafeDriveCommand.java

@@ -0,0 +1,108 @@
+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.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;
+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 double
+            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(
+                getInitializeCommand(),
+                SwerveCommands.getClosedLoopFieldRelativeDriveCommand(
+                        ShootingSafeDriveCommand::calculateSafeDriveTranslationPower,
+                        ShootingSafeDriveCommand::calculateSafeDriveRotationPower
+                ).asProxy()
+        );
+    }
+
+    private static Command getInitializeCommand() {
+        return new InstantCommand(() -> {
+            X_SLEW_RATE_LIMITER.reset(0);
+            Y_SLEW_RATE_LIMITER.reset(0);
+            ROTATION_SLEW_RATE_LIMITER.reset(0);
+        });
+    }
+
+    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 double
+                rawXPower = OperatorConstants.DRIVER_CONTROLLER.getLeftY(),
+                rawYPower = OperatorConstants.DRIVER_CONTROLLER.getLeftX();
+
+        return new Translation2d(
+                X_SLEW_RATE_LIMITER.calculate(rawXPower),
+                Y_SLEW_RATE_LIMITER.calculate(rawYPower)
+        );
+    }
+
+    private static Translation2d limitTranslationPowerToEaseShooting(Translation2d targetPower) {
+        final Rotation2d hubDirection = calculateFieldRelativeAngleToHub().plus(Rotation2d.k180deg);
+
+        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(
+                radialVelocity * hubDirection.getCos(),
+                radialVelocity * hubDirection.getSin()
+        );
+    }
+
+    private static Rotation2d calculateFieldRelativeAngleToHub() {
+        final Translation2d
+                robotPosition = RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose().getTranslation(),
+                hubPosition = FieldConstants.HUB_POSITION.get();
+        return hubPosition.minus(robotPosition).getAngle();
+    }
+}

src/main/java/frc/trigon/robot/commands/commandfactories/ClimbCommands.java

@@ -1,33 +1,37 @@
 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;
+import org.littletonrobotics.junction.networktables.LoggedNetworkBoolean;
 
 public class ClimbCommands {
-    public static boolean IS_CLIMBING = false;
+    public static final 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.negate()),
+                new InstantCommand(() -> IS_CLIMBING.set(false))
         );
     }
 
     public static Command getClimberDefaultCommand() {
         return new ConditionalCommand(
-                getClimbDownFromL1Command().andThen(() -> IS_CLIMBING = false),
+                getReleaseL1Command(),
                 ClimberCommands.getSetTargetStateCommand(ClimberConstants.ClimberState.REST),
-                () -> IS_CLIMBING
+                IS_CLIMBING::get
         );
     }
 }

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() {

src/main/java/frc/trigon/robot/commands/commandfactories/autonomous/GeneralAutonomousCommands.java

@@ -120,8 +120,8 @@ public static Command getClimbCommand(Supplier<FlippablePose2d> climbPosition) {
 
     private static Command getClimbToL1Command(Supplier<FlippablePose2d> 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);
     }

src/main/java/frc/trigon/robot/constants/OperatorConstants.java

@@ -9,6 +9,7 @@
 import frc.trigon.lib.hardware.misc.XboxController;
 import frc.trigon.lib.utilities.flippable.FlippablePose2d;
 import frc.trigon.robot.RobotContainer;
+import frc.trigon.robot.commands.commandfactories.ClimbCommands;
 import frc.trigon.robot.misc.MatchTracker;
 
 import java.util.function.DoubleUnaryOperator;
@@ -23,6 +24,8 @@ 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
             POV_DIVIDER = 2,
@@ -41,19 +44,22 @@ 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(),
+            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(),
             BACKWARD_DYNAMIC_CHARACTERIZATION_TRIGGER = OPERATOR_CONTROLLER.down();
     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),
@@ -65,9 +71,9 @@ 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()),
-            CANCEL_CLIMB_TRIGGER = DRIVER_CONTROLLER.leftBumper().or(OPERATOR_CONTROLLER.x()),
-            CONTINUE_CLIMB_TRIGGER = DRIVER_CONTROLLER.rightBumper().or(OPERATOR_CONTROLLER.v());
+            OPEN_CLIMBER_TRIGGER = DRIVER_CONTROLLER.back().or(OPERATOR_CONTROLLER.c()).multiPress(2, DOUBLE_TAP_TIMEOUT_SECONDS),
+            CANCEL_CLIMB_TRIGGER = new Trigger(ClimbCommands.IS_CLIMBING).and(DRIVER_CONTROLLER.leftBumper()).or(OPERATOR_CONTROLLER.x()),
+            CONTINUE_CLIMB_TRIGGER = new Trigger(ClimbCommands.IS_CLIMBING).and(DRIVER_CONTROLLER.rightBumper()).or(OPERATOR_CONTROLLER.v());
     public static final Trigger //Debugging Triggers
             UNJAM_TRIGGER = DRIVER_CONTROLLER.start().or(OPERATOR_CONTROLLER.q()),
             SHORT_EJECTION_TRIGGER = DRIVER_CONTROLLER.x().or(OPERATOR_CONTROLLER.e());