Harrison's Custom Commands + Alerts expansion

src/main/java/frc/robot/Robot.java

@@ -75,7 +75,10 @@ public Robot() {
     // Configure brownout voltage
     RobotController.setBrownoutVoltage(6.0);
 
-    // Create RobotConatiner
+    // Disable joysick warnings in Driver Station
+    DriverStation.silenceJoystickConnectionWarning(true);
+
+    // Create RobotContainer
     robotContainer = new RobotContainer();
   }
 

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

@@ -23,10 +23,15 @@
 import frc.robot.subsystems.intake.IntakeIOSim;
 import frc.robot.subsystems.intake.IntakeIOSpark;
 import frc.robot.util.AllianceFlipUtil;
+import org.littletonrobotics.junction.AutoLogOutput;
 import org.littletonrobotics.junction.networktables.LoggedDashboardChooser;
 import org.littletonrobotics.junction.networktables.LoggedNetworkNumber;
 
 public class RobotContainer {
+  // Button Binding variables
+  @AutoLogOutput private boolean robotRelativeEnabled = false;
+  @AutoLogOutput private boolean slowModeEnabled = false;
+
   // Load PoseEstimator class
   private final PoseEstimator poseEstimator = PoseEstimator.getInstance();
 
@@ -47,8 +52,6 @@ public class RobotContainer {
   private final LoggedNetworkNumber endgameAlert2 =
       new LoggedNetworkNumber("/SmartDashboard/Endgame Alert #2", 15.0);
 
-  private boolean slowModeEnabled;
-
   public RobotContainer() {
     switch (Constants.getMode()) {
       case REAL -> {
@@ -145,6 +148,16 @@ private void configureButtonBindings() {
     // Make slow mode toggleable
     controller.y().toggleOnTrue(Commands.runOnce(() -> slowModeEnabled = !slowModeEnabled));
 
+    controller
+        .leftBumper()
+        .and(controller.rightBumper())
+        .onTrue(Commands.runOnce(() -> robotRelativeEnabled = true));
+
+    controller
+        .leftBumper()
+        .and(controller.rightBumper())
+        .onFalse(Commands.runOnce(() -> robotRelativeEnabled = false));
+
     // Default command, normal field-relative drive
     driveBase.setDefaultCommand(
         DriveCommands.joystickDrive(
@@ -153,44 +166,72 @@ private void configureButtonBindings() {
             () -> -controller.getLeftX(),
             () -> -controller.getRightX(),
             () -> slowModeEnabled,
-            () -> controller.leftBumper().and(controller.rightBumper()).getAsBoolean()));
+            () -> robotRelativeEnabled));
 
     // Stow
     controller.povDown().onTrue(Commands.runOnce(() -> elevatorBase.setGoal(ElevatorState.STOW)));
     // L1
     controller
         .povLeft()
-        .onTrue(Commands.runOnce(() -> elevatorBase.setGoal(ElevatorState.L1_CORAL)));
+        .onTrue(
+            Commands.runOnce(
+                    () -> {
+                      robotRelativeEnabled = true;
+                      slowModeEnabled = true;
+                    })
+                .andThen(Commands.runOnce(() -> elevatorBase.setGoal(ElevatorState.L1_CORAL))));
+
     // L2
     controller
         .povUp()
         .onTrue(
-            Commands.either(
-                Commands.runOnce(() -> elevatorBase.setGoal(ElevatorState.L2_CORAL)),
-                Commands.runOnce(() -> elevatorBase.setGoal(ElevatorState.L2_ALGAE_REMOVAL)),
-                controller.b().negate().debounce(0.25)));
+            Commands.runOnce(
+                    () -> {
+                      robotRelativeEnabled = true;
+                      slowModeEnabled = true;
+                    })
+                .andThen(
+                    Commands.either(
+                        Commands.runOnce(() -> elevatorBase.setGoal(ElevatorState.L2_CORAL)),
+                        Commands.runOnce(
+                            () -> elevatorBase.setGoal(ElevatorState.L2_ALGAE_REMOVAL)),
+                        controller.b().negate().debounce(0.25))));
 
     // L3
     controller
         .povRight()
         .onTrue(
-            Commands.either(
-                Commands.runOnce(() -> elevatorBase.setGoal(ElevatorState.L3_CORAL)),
-                Commands.runOnce(() -> elevatorBase.setGoal(ElevatorState.L3_ALGAE_REMOVAL)),
-                controller.b().negate().debounce(0.25)));
+            Commands.runOnce(
+                    () -> {
+                      robotRelativeEnabled = true;
+                      slowModeEnabled = true;
+                    })
+                .andThen(
+                    Commands.either(
+                        Commands.runOnce(() -> elevatorBase.setGoal(ElevatorState.L3_CORAL)),
+                        Commands.runOnce(
+                            () -> elevatorBase.setGoal(ElevatorState.L3_ALGAE_REMOVAL)),
+                        controller.b().negate().debounce(0.25))));
 
     // Intake
     controller.x().toggleOnTrue(IntakeCommands.intake(elevatorBase, intakeBase, dispenserBase));
 
     controller
         .rightTrigger()
         .onTrue(
-            Commands.either(
-                dispenserBase
-                    .eject(elevatorBase::getGoal)
-                    .andThen(Commands.runOnce(() -> elevatorBase.setGoal(ElevatorState.STOW))),
-                IntakeCommands.reserialize(elevatorBase, intakeBase, dispenserBase),
-                controller.leftTrigger().negate().debounce(0.25)));
+            Commands.runOnce(
+                    () -> {
+                      robotRelativeEnabled = false;
+                      slowModeEnabled = false;
+                    })
+                .andThen(
+                    Commands.either(
+                        dispenserBase
+                            .eject(elevatorBase::getGoal)
+                            .andThen(
+                                Commands.runOnce(() -> elevatorBase.setGoal(ElevatorState.STOW))),
+                        IntakeCommands.reserialize(elevatorBase, intakeBase, dispenserBase),
+                        controller.leftTrigger().negate().debounce(0.25))));
 
     // Home Elevator
     controller

src/main/java/frc/robot/subsystems/drive/Module.java

@@ -24,6 +24,7 @@ class Module {
   private static final LoggedTunableNumber driveIZone =
       new LoggedTunableNumber("Drive/Module/DrivekIZone");
   private static final LoggedTunableNumber turnkP = new LoggedTunableNumber("Drive/Module/TurnkP");
+  private static final LoggedTunableNumber turnkI = new LoggedTunableNumber("Drive/Module/TurnkI");
   private static final LoggedTunableNumber turnkD = new LoggedTunableNumber("Drive/Module/TurnkD");
 
   static {
@@ -46,6 +47,7 @@ class Module {
         drivekD.initDefault(0.0);
         driveIZone.initDefault(0.0);
         turnkP.initDefault(10.0);
+        turnkI.initDefault(0.0);
         turnkD.initDefault(0.0);
       }
     }

src/main/java/frc/robot/subsystems/drive/ModuleIOSpark.java

@@ -169,9 +169,14 @@ public void runTurnOpenLoop(double output) {
 
   @Override
   public void runDriveVelocity(double velocityRadPerSec) {
-    double ffVolts = driveFeedforward.calculate(velocityRadPerSec);
+    double ffVolts =
+        driveFeedforward.calculate(
+            velocityRadPerSec
+            //  * 1 / (Math.PI * 2)
+            );
     driveController.setReference(
         velocityRadPerSec,
+        //  * 1 / (Math.PI * 2),
         ControlType.kVelocity,
         ClosedLoopSlot.kSlot0,
         ffVolts,
@@ -182,7 +187,11 @@ public void runDriveVelocity(double velocityRadPerSec) {
   public void runTurnPosition(Rotation2d rotation) {
     // Because internal encoder is relative, we want to wrap to the range -π to π radians.
     var updatedSetpoint = MathUtil.angleModulus(rotation.getRadians());
-    turnController.setReference(updatedSetpoint, ControlType.kPosition);
+    turnController.setReference(
+        updatedSetpoint
+        //  * 1 / (Math.PI * 2)
+        ,
+        ControlType.kPosition);
   }
 
   @Override

src/main/java/frc/robot/util/AlertsUtil.java

@@ -37,6 +37,9 @@ public static AlertsUtil getInstance() {
   private final Alert lowBatteryVoltageAlert =
       new Alert("Battery voltage is too low, change the battery", AlertType.kWarning);
   private final Debouncer batteryVoltageDebouncer = new Debouncer(1.5);
+  private final Alert joystickDisconnectedAlert =
+      new Alert("At least one joystick button is not detected", AlertType.kWarning);
+  private final Debouncer joystickDebouncer = new Debouncer(2);
 
   // Program Alerts
   private final Alert tuningModeAlert = new Alert("Robot in Tuning Mode", AlertType.kInfo);
@@ -75,6 +78,9 @@ public void periodic() {
         batteryVoltageDebouncer.calculate(
             RobotController.getBatteryVoltage() <= LOW_VOLTAGE_WARNING_THRESHOLD));
 
+    joystickDisconnectedAlert.set(
+        joystickDebouncer.calculate(DriverStation.isJoystickConnected(0)));
+
     // Update Program Alerts
     // TODO