Format with Spotless and add TODOs

team581 · Sep 11, 2024 · 9df4054 · 9df4054
1 parent e2d68d2
commit 9df4054
Show file tree

Hide file tree

Showing 5 changed files with 132 additions and 83 deletions.
diff --git a/src/main/java/frc/robot/Hardware.java b/src/main/java/frc/robot/Hardware.java
@@ -31,6 +31,6 @@ public class Hardware {
   public final TalonFX armRight =
       new TalonFX(RobotConfig.get().arm().rightMotorID(), RobotConfig.get().arm().canBusName());
 
-      public final CommandXboxController driverController = new CommandXboxController(0);
-      public final CommandXboxController operatorController = new CommandXboxController(0);
+  public final CommandXboxController driverController = new CommandXboxController(0);
+  public final CommandXboxController operatorController = new CommandXboxController(1);
 }
diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
@@ -2,7 +2,6 @@
 
 import dev.doglog.DogLog;
 import dev.doglog.DogLogOptions;
-import edu.wpi.first.wpilibj.PowerDistribution;
 import edu.wpi.first.wpilibj.TimedRobot;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
@@ -16,7 +15,6 @@
 import frc.robot.intake.IntakeSubsystem;
 import frc.robot.queuer.QueuerSubsystem;
 import frc.robot.robot_manager.RobotCommands;
-import frc.robot.robot_manager.RobotManager;
 import frc.robot.shooter.ShooterSubsystem;
 import frc.robot.swerve.SwerveSubsystem;
 import frc.robot.util.Stopwatch;
@@ -43,7 +41,7 @@ public Robot() {
 
     DogLog.setOptions(
         new DogLogOptions().withCaptureNt(false).withNtPublish(RobotConfig.IS_DEVELOPMENT));
-    DogLog.setPdh(new PowerDistribution());
+    DogLog.setPdh(hardware.pdh);
 
     // Record metadata
     DogLog.log("Metadata/ProjectName", BuildConstants.MAVEN_NAME);
@@ -133,13 +131,37 @@ public void testPeriodic() {}
   public void testExit() {}
 
   private void configureBindings() {
-    hardware.driverController.rightTrigger().onTrue(robotCommands.confirmShotCommand()).onFalse(robotCommands.stowCommand());
-    hardware.driverController.leftTrigger().onTrue(robotCommands.intakeCommand()).onFalse(robotCommands.stowCommand());
-    hardware.driverController.rightBumper().onTrue(robotCommands.feedingCommand()).onFalse(robotCommands.stowCommand());
-    hardware.driverController.rightBumper().onTrue(robotCommands.passCommand()).onFalse(robotCommands.stowCommand());
-
-    hardware.operatorController.rightTrigger().onTrue(robotCommands.ampCommand()).onFalse(robotCommands.stowCommand());
-    hardware.operatorController.leftTrigger().onTrue(robotCommands.subwooferCommand()).onFalse(robotCommands.stowCommand());
-
+    hardware
+        .driverController
+        .rightTrigger()
+        .onTrue(robotCommands.confirmShotCommand())
+        .onFalse(robotCommands.stowCommand());
+    hardware
+        .driverController
+        .leftTrigger()
+        .onTrue(robotCommands.intakeCommand())
+        .onFalse(robotCommands.stowCommand());
+    // TODO: Two of these bindings use right bumper
+    hardware
+        .driverController
+        .rightBumper()
+        .onTrue(robotCommands.feedingCommand())
+        .onFalse(robotCommands.stowCommand());
+    hardware
+        .driverController
+        .rightBumper()
+        .onTrue(robotCommands.passCommand())
+        .onFalse(robotCommands.stowCommand());
+
+    hardware
+        .operatorController
+        .rightTrigger()
+        .onTrue(robotCommands.ampCommand())
+        .onFalse(robotCommands.stowCommand());
+    hardware
+        .operatorController
+        .leftTrigger()
+        .onTrue(robotCommands.subwooferCommand())
+        .onFalse(robotCommands.stowCommand());
   }
 }
diff --git a/src/main/java/frc/robot/robot_manager/RobotCommands.java b/src/main/java/frc/robot/robot_manager/RobotCommands.java
@@ -1,73 +1,92 @@
 package frc.robot.robot_manager;
 
-import java.util.List;
-
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.Subsystem;
+import java.util.List;
 
 public class RobotCommands {
-      private final RobotManager robot;
+  private final RobotManager robot;
   private final Subsystem[] requirements;
 
   public RobotCommands(RobotManager robot) {
     this.robot = robot;
-    var requirementsList =
-        List.of(
-            robot.arm,
-            robot.intake,
-            robot.queuer,
-            robot.shooter);
+    var requirementsList = List.of(robot.arm, robot.intake, robot.queuer, robot.shooter);
     requirements = requirementsList.toArray(Subsystem[]::new);
   }
 
   public Command intakeCommand() {
-    return Commands.runOnce(robot::intakeRequest, requirements).andThen(robot.waitForState(RobotState.IDLE_WITH_GP));
+    return Commands.runOnce(robot::intakeRequest, requirements)
+        .andThen(robot.waitForState(RobotState.IDLE_WITH_GP));
   }
-  public Command outtakeCommand(){
-    return Commands.runOnce(robot::outtakeRequest, requirements).andThen(robot.waitForState(RobotState.IDLE_NO_GP));
+
+  public Command outtakeCommand() {
+    return Commands.runOnce(robot::outtakeRequest, requirements)
+        .andThen(robot.waitForState(RobotState.IDLE_NO_GP));
   }
-  public Command speakerCommand(){
-    return Commands.runOnce(robot::prepareSpeakerRequest, requirements).andThen(robot.waitForState(RobotState.IDLE_NO_GP));
+
+  public Command speakerCommand() {
+    return Commands.runOnce(robot::prepareSpeakerRequest, requirements)
+        .andThen(robot.waitForState(RobotState.IDLE_NO_GP));
   }
-  public Command passCommand(){
-    return Commands.runOnce(robot::preparePassRequest,requirements).andThen(robot.waitForState(RobotState.IDLE_NO_GP));
+
+  public Command passCommand() {
+    return Commands.runOnce(robot::preparePassRequest, requirements)
+        .andThen(robot.waitForState(RobotState.IDLE_NO_GP));
   }
 
-  public Command stowCommand(){
-    return Commands.runOnce(robot::stowRequest,requirements).andThen(robot.waitForState(RobotState.IDLE_NO_GP));
+  public Command stowCommand() {
+    return Commands.runOnce(robot::stowRequest, requirements)
+        .andThen(robot.waitForState(RobotState.IDLE_NO_GP));
   }
-  public Command idleWithGpCommand(){
-    return Commands.runOnce(robot::idleWithGpRequest,requirements).andThen(robot.waitForState(RobotState.IDLE_WITH_GP));
+
+  public Command idleWithGpCommand() {
+    return Commands.runOnce(robot::idleWithGpRequest, requirements)
+        .andThen(robot.waitForState(RobotState.IDLE_WITH_GP));
   }
-  public Command stopIntakingCommand(){
-        return Commands.runOnce(robot::stopIntakingRequest,requirements).andThen(robot.waitForState(RobotState.IDLE_NO_GP));
 
+  public Command stopIntakingCommand() {
+    return Commands.runOnce(robot::stopIntakingRequest, requirements)
+        .andThen(robot.waitForState(RobotState.IDLE_NO_GP));
   }
-
-  public Command waitSubwooferCommand(){
-        return Commands.runOnce(robot::waitSubwooferRequest,requirements).andThen(robot.waitForState(RobotState.IDLE_NO_GP));
+
+  public Command waitSubwooferCommand() {
+    return Commands.runOnce(robot::waitSubwooferRequest, requirements)
+        .andThen(robot.waitForState(RobotState.IDLE_NO_GP));
   }
-    public Command waitAmpCommand(){
-        return Commands.runOnce(robot::waitAmpRequest,requirements).andThen(robot.waitForState(RobotState.IDLE_NO_GP));
+
+  public Command waitAmpCommand() {
+    return Commands.runOnce(robot::waitAmpRequest, requirements)
+        .andThen(robot.waitForState(RobotState.IDLE_NO_GP));
   }
-  public Command waitSpeakerCommand(){
-        return Commands.runOnce(robot::waitSpeakerRequest,requirements).andThen(robot.waitForState(RobotState.IDLE_NO_GP));
+
+  public Command waitSpeakerCommand() {
+    return Commands.runOnce(robot::waitSpeakerRequest, requirements)
+        .andThen(robot.waitForState(RobotState.IDLE_NO_GP));
   }
-  public Command confirmShotCommand(){
-        return Commands.runOnce(robot::confirmShotRequest,requirements).andThen(robot.waitForState(RobotState.IDLE_NO_GP));
+
+  public Command confirmShotCommand() {
+    return Commands.runOnce(robot::confirmShotRequest, requirements)
+        .andThen(robot.waitForState(RobotState.IDLE_NO_GP));
   }
-  public Command ampCommand(){
-        return Commands.runOnce(robot::prepareAmpRequest,requirements).andThen(robot.waitForState(RobotState.IDLE_NO_GP));
+
+  public Command ampCommand() {
+    return Commands.runOnce(robot::prepareAmpRequest, requirements)
+        .andThen(robot.waitForState(RobotState.IDLE_NO_GP));
   }
-  public Command stopShootingCommand(){
-        return Commands.runOnce(robot::stopShootingRequest,requirements).andThen(robot.waitForState(RobotState.IDLE_WITH_GP));
+
+  public Command stopShootingCommand() {
+    return Commands.runOnce(robot::stopShootingRequest, requirements)
+        .andThen(robot.waitForState(RobotState.IDLE_WITH_GP));
   }
-  public Command feedingCommand(){
-    return Commands.runOnce(robot::prepareFeedRequest, requirements).andThen(robot.waitForState(RobotState.IDLE_NO_GP));
+
+  public Command feedingCommand() {
+    return Commands.runOnce(robot::prepareFeedRequest, requirements)
+        .andThen(robot.waitForState(RobotState.IDLE_NO_GP));
   }
-  public Command subwooferCommand(){
-        return Commands.runOnce(robot::prepareSubwooferRequest, requirements).andThen(robot.waitForState(RobotState.IDLE_NO_GP));
 
+  public Command subwooferCommand() {
+    return Commands.runOnce(robot::prepareSubwooferRequest, requirements)
+        .andThen(robot.waitForState(RobotState.IDLE_NO_GP));
   }
 }
diff --git a/src/main/java/frc/robot/robot_manager/RobotManager.java b/src/main/java/frc/robot/robot_manager/RobotManager.java
@@ -252,6 +252,8 @@ public void intakeRequest() {
     }
   }
 
+  // TODO: This seems like we ended up not really needing it, can remove it in favor of
+  // stowRequest()
   public void stopIntakingRequest() {
     switch (getState()) {
       case INTAKING -> setStateFromRequest(RobotState.IDLE_NO_GP);
@@ -272,6 +274,7 @@ public void idleWithGpRequest() {
 
   public void stowRequest() {
     switch (getState()) {
+        // TODO: Intaking and unjam should not be IDLE_WITH_GP
       case INTAKING,
               AMP_PREPARE_TO_SCORE,
               SPEAKER_PREPARE_TO_SCORE,
@@ -315,41 +318,48 @@ public void previousClimbStateRequest() {
       default -> setStateFromRequest(RobotState.CLIMBING_1_LINEUP);
     }
   }
-  public void prepareSpeakerRequest(){
+
+  public void prepareSpeakerRequest() {
     switch (getState()) {
-      case CLIMBING_1_LINEUP,CLIMBING_2_HANGING->{}
-
-      default->setStateFromRequest(RobotState.SPEAKER_PREPARE_TO_SCORE);
+      case CLIMBING_1_LINEUP, CLIMBING_2_HANGING -> {}
+      default -> setStateFromRequest(RobotState.SPEAKER_PREPARE_TO_SCORE);
     }
   }
-  public void prepareAmpRequest(){
+
+  public void prepareAmpRequest() {
     switch (getState()) {
-      case CLIMBING_1_LINEUP,CLIMBING_2_HANGING->{}
-
-      default->setStateFromRequest(RobotState.AMP_PREPARE_TO_SCORE);
+      case CLIMBING_1_LINEUP, CLIMBING_2_HANGING -> {}
+      default -> setStateFromRequest(RobotState.AMP_PREPARE_TO_SCORE);
     }
   }
-  public void prepareFeedRequest(){
+
+  public void prepareFeedRequest() {
     switch (getState()) {
-      case CLIMBING_1_LINEUP,CLIMBING_2_HANGING->{}
-
-      default->setStateFromRequest(RobotState.FEEDING_PREPARE_TO_SHOOT);
+      case CLIMBING_1_LINEUP, CLIMBING_2_HANGING -> {}
+      default -> setStateFromRequest(RobotState.FEEDING_PREPARE_TO_SHOOT);
     }
   }
-  public void stopShootingRequest(){
+
+  public void stopShootingRequest() {
     switch (getState()) {
-      case SPEAKER_SCORING,SUBWOOFER_SCORING,AMP_SCORING,FEEDING_SHOOTING,PASS_SHOOTING,
-      SPEAKER_PREPARE_TO_SCORE,SUBWOOFER_PREPARE_TO_SCORE,AMP_PREPARE_TO_SCORE,FEEDING_PREPARE_TO_SHOOT,PASS_PREPARE_TO_SHOOT->{}
-
-      default->setStateFromRequest(RobotState.IDLE_WITH_GP);
+      case SPEAKER_SCORING,
+          SUBWOOFER_SCORING,
+          AMP_SCORING,
+          FEEDING_SHOOTING,
+          PASS_SHOOTING,
+          SPEAKER_PREPARE_TO_SCORE,
+          SUBWOOFER_PREPARE_TO_SCORE,
+          AMP_PREPARE_TO_SCORE,
+          FEEDING_PREPARE_TO_SHOOT,
+          PASS_PREPARE_TO_SHOOT -> {}
+      default -> setStateFromRequest(RobotState.IDLE_WITH_GP);
     }
   }
-  public void prepareSubwooferRequest(){
+
+  public void prepareSubwooferRequest() {
     switch (getState()) {
-      case CLIMBING_1_LINEUP,CLIMBING_2_HANGING->{}
-
-      default->setStateFromRequest(RobotState.SUBWOOFER_PREPARE_TO_SCORE);
+      case CLIMBING_1_LINEUP, CLIMBING_2_HANGING -> {}
+      default -> setStateFromRequest(RobotState.SUBWOOFER_PREPARE_TO_SCORE);
     }
   }
-
 }
diff --git a/src/main/java/frc/robot/swerve/SwerveSubsystem.java b/src/main/java/frc/robot/swerve/SwerveSubsystem.java
@@ -1,7 +1,5 @@
 package frc.robot.swerve;
 
-import java.util.List;
-
 import com.ctre.phoenix6.hardware.Pigeon2;
 import com.ctre.phoenix6.mechanisms.swerve.SwerveModule;
 import com.ctre.phoenix6.mechanisms.swerve.SwerveModule.DriveRequestType;
@@ -15,10 +13,12 @@
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import frc.robot.util.scheduling.SubsystemPriority;
 import frc.robot.util.state_machines.StateMachine;
+import java.util.List;
 
 public class SwerveSubsystem extends StateMachine<SwerveState> {
   /** Max speed allowed to make a speaker shot and feeding. */
   private static final double MAX_SPEED_SHOOTING = Units.feetToMeters(0.5);
+
   private static final double MAX_FLOOR_SPEED_SHOOTING = Units.feetToMeters(18);
 
   public static final double MaxSpeed = 4.75;
@@ -93,7 +93,7 @@ public boolean isSlowEnoughToFeed() {
     return slowEnoughToFeed;
   }
 
-  public List<SwerveModulePosition> getModulePositions(){
+  public List<SwerveModulePosition> getModulePositions() {
     return modulePositions;
   }
 
@@ -102,13 +102,11 @@ public SwerveSubsystem(CommandXboxController driveController) {
     this.controller = driveController;
   }
 
-
-
   @Override
   protected void collectInputs() {
     modulePositions = calculateModulePositions();
-robotRelativeSpeeds = calculateRobotRelativeSpeeds();
-fieldRelativeSpeeds = calculateFieldRelativeSpeeds();
+    robotRelativeSpeeds = calculateRobotRelativeSpeeds();
+    fieldRelativeSpeeds = calculateFieldRelativeSpeeds();
     slowEnoughToShoot = calculateMovingSlowEnoughForSpeakerShot(robotRelativeSpeeds);
     slowEnoughToFeed = calculateMovingSlowEnoughForFloorShot(robotRelativeSpeeds);
   }
@@ -127,9 +125,9 @@ private ChassisSpeeds calculateRobotRelativeSpeeds() {
 
   private ChassisSpeeds calculateFieldRelativeSpeeds() {
     return ChassisSpeeds.fromRobotRelativeSpeeds(
-        robotRelativeSpeeds,
-        Rotation2d.fromDegrees(drivetrainPigeon.getYaw().getValueAsDouble()));
+        robotRelativeSpeeds, Rotation2d.fromDegrees(drivetrainPigeon.getYaw().getValueAsDouble()));
   }
+
   private static boolean calculateMovingSlowEnoughForSpeakerShot(ChassisSpeeds speeds) {
     double linearSpeed =
         Math.sqrt(Math.pow(speeds.vxMetersPerSecond, 2) + Math.pow(speeds.vyMetersPerSecond, 2));