Merge branch 'main' of https://github.com/team581/2024-offseason-bot

team581 · Sep 11, 2024 · e2d68d2 · e2d68d2
2 parents ffb8c03 + cb7f6c5
commit e2d68d2
Show file tree

Hide file tree

Showing 4 changed files with 128 additions and 3 deletions.
diff --git a/src/main/java/frc/robot/Hardware.java b/src/main/java/frc/robot/Hardware.java
@@ -6,6 +6,7 @@
 import edu.wpi.first.wpilibj.DigitalInput;
 import edu.wpi.first.wpilibj.PowerDistribution;
 import edu.wpi.first.wpilibj.PowerDistribution.ModuleType;
+import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import frc.robot.config.RobotConfig;
 
 public class Hardware {
@@ -29,4 +30,7 @@ public class Hardware {
       new TalonFX(RobotConfig.get().arm().leftMotorID(), RobotConfig.get().arm().canBusName());
   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);
 }
diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
@@ -15,6 +15,8 @@
 import frc.robot.imu.ImuSubsystem;
 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;
@@ -31,10 +33,10 @@ public class Robot extends TimedRobot {
   private final ArmSubsystem arm = new ArmSubsystem(hardware.armLeft, hardware.armRight);
   private final IntakeSubsystem intake =
       new IntakeSubsystem(hardware.intakeMain, hardware.intakeCenteringMotor);
-  // TODO: Add controllers to hardware & use them here
-  private final SwerveSubsystem swerve = new SwerveSubsystem(null);
+  private final SwerveSubsystem swerve = new SwerveSubsystem(hardware.driverController);
   private final ImuSubsystem imu = new ImuSubsystem(swerve.drivetrainPigeon);
   private final Autos autos = new Autos();
+  private final RobotCommands robotCommands = new RobotCommands(null);
 
   public Robot() {
     System.out.println("roboRIO serial number: " + RobotConfig.SERIAL_NUMBER);
@@ -130,5 +132,14 @@ public void testPeriodic() {}
   @Override
   public void testExit() {}
 
-  private void configureBindings() {}
+  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());
+
+  }
 }
diff --git a/src/main/java/frc/robot/robot_manager/RobotCommands.java b/src/main/java/frc/robot/robot_manager/RobotCommands.java
@@ -0,0 +1,73 @@
+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;
+
+public class RobotCommands {
+      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);
+    requirements = requirementsList.toArray(Subsystem[]::new);
+  }
+
+  public Command intakeCommand() {
+    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 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 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 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 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 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 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 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
@@ -315,4 +315,41 @@ public void previousClimbStateRequest() {
       default -> setStateFromRequest(RobotState.CLIMBING_1_LINEUP);
     }
   }
+  public void prepareSpeakerRequest(){
+    switch (getState()) {
+      case CLIMBING_1_LINEUP,CLIMBING_2_HANGING->{}
+
+      default->setStateFromRequest(RobotState.SPEAKER_PREPARE_TO_SCORE);
+    }
+  }
+  public void prepareAmpRequest(){
+    switch (getState()) {
+      case CLIMBING_1_LINEUP,CLIMBING_2_HANGING->{}
+
+      default->setStateFromRequest(RobotState.AMP_PREPARE_TO_SCORE);
+    }
+  }
+  public void prepareFeedRequest(){
+    switch (getState()) {
+      case CLIMBING_1_LINEUP,CLIMBING_2_HANGING->{}
+
+      default->setStateFromRequest(RobotState.FEEDING_PREPARE_TO_SHOOT);
+    }
+  }
+  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);
+    }
+  }
+  public void prepareSubwooferRequest(){
+    switch (getState()) {
+      case CLIMBING_1_LINEUP,CLIMBING_2_HANGING->{}
+
+      default->setStateFromRequest(RobotState.SUBWOOFER_PREPARE_TO_SCORE);
+    }
+  }
+
 }