fix ground intake selection

Iron-Panthers · Nov 11, 2023 · f00e61a · f00e61a
1 parent 6e30f1f
commit f00e61a
Show file tree

Hide file tree

Showing 3 changed files with 36 additions and 33 deletions.
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -328,10 +328,6 @@ private void configureButtonBindings() {
             new GroundPickupCommand(
                 intakeSubsystem,
                 elevatorSubsystem,
-                () ->
-                    anthony.rightBumper().getAsBoolean()
-                        ? Elevator.Setpoints.GROUND_INTAKE_CUBE
-                        : Elevator.Setpoints.GROUND_INTAKE_CONE,
                 anthony.rightBumper()));
 
     jacob
@@ -340,10 +336,6 @@ private void configureButtonBindings() {
             new GroundPickupCommand(
                 intakeSubsystem,
                 elevatorSubsystem,
-                () ->
-                    jacob.leftBumper().getAsBoolean()
-                        ? Elevator.Setpoints.GROUND_INTAKE_CUBE
-                        : Elevator.Setpoints.GROUND_INTAKE_CONE,
                 jacob.leftBumper()));
 
     jacobLayer
@@ -406,13 +398,13 @@ private void configureButtonBindings() {
               intakeSubsystem,
               elevatorSubsystem,
               Constants.SCORE_STEP_MAP.get(scoreType),
-              jacob.b()));
-    //   anthony.povRight()));
+            //   jacob.b()));
+      anthony.povRight()));
 
-    // anthony
-    //     .povRight()
-    jacob
-        .b()
+    anthony
+        .povRight()
+    // jacob
+    //     .b()
         .onTrue(
             new HashMapCommand<>(
                 scoreCommandMap, () -> currentNodeSelection.get().getScoreTypeIdentifier()));

diff --git a/src/main/java/frc/robot/commands/GroundIntakeElevatorCommand.java b/src/main/java/frc/robot/commands/GroundIntakeElevatorCommand.java
@@ -0,0 +1,29 @@
+package frc.robot.commands;
+
+import java.util.function.BooleanSupplier;
+
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import frc.robot.Constants;
+import frc.robot.subsystems.ElevatorSubsystem;
+
+public class GroundIntakeElevatorCommand extends CommandBase {
+
+    private ElevatorSubsystem elevatorSubsystem;
+    private BooleanSupplier isCube;
+
+    public GroundIntakeElevatorCommand(ElevatorSubsystem elevatorSubsystem, BooleanSupplier isCube) {
+        this.elevatorSubsystem = elevatorSubsystem;
+        this.isCube = isCube;
+
+        addRequirements(this.elevatorSubsystem);
+    }
+
+    @Override
+    public void execute() {
+        if(isCube.getAsBoolean()) {
+            elevatorSubsystem.setTargetState(Constants.Elevator.Setpoints.GROUND_INTAKE_CUBE);
+        } else {
+            elevatorSubsystem.setTargetState(Constants.Elevator.Setpoints.GROUND_INTAKE_CONE);
+        }
+    }
+}
diff --git a/src/main/java/frc/robot/commands/GroundPickupCommand.java b/src/main/java/frc/robot/commands/GroundPickupCommand.java
@@ -19,33 +19,15 @@
 // https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html
 public class GroundPickupCommand extends SequentialCommandGroup {
   /** Creates a new GroundPickupCommand. */
-  private BooleanSupplier isCube;
-
-  private ElevatorSubsystem elevatorSubsystem;
-
-  private Supplier<ElevatorState> setpoint;
-
   public GroundPickupCommand(
       IntakeSubsystem intakeSubsystem,
       ElevatorSubsystem elevatorSubsystem,
-      Supplier<ElevatorState> setpoint,
       BooleanSupplier isCube) {
     // Add your commands in the addCommands() call, e.g.
-    this.isCube = isCube;
-    this.elevatorSubsystem = elevatorSubsystem;
-    this.setpoint = setpoint;
 
     addCommands(
-        new ElevatorPositionCommand(elevatorSubsystem, setpoint.get())
+        new GroundIntakeElevatorCommand(elevatorSubsystem, isCube)
             .alongWith(new IntakeModeCommand(intakeSubsystem, Modes.INTAKE, isCube))
             .andThen(new ElevatorPositionCommand(elevatorSubsystem, Elevator.Setpoints.STOWED)));
   }
-
-  private Command determineIntakeType() {
-    if (isCube.getAsBoolean()) {
-      return new ElevatorPositionCommand(elevatorSubsystem, Elevator.Setpoints.GROUND_INTAKE_CUBE);
-    } else {
-      return new ElevatorPositionCommand(elevatorSubsystem, Elevator.Setpoints.GROUND_INTAKE_CONE);
-    }
-  }
 }