Fixed L1 command, added elevator reset command

chopshop-166 · Jan 31, 2025 · a470457 · a470457
1 parent 8d39c9d
commit a470457
Show file tree

Hide file tree

Showing 2 changed files with 17 additions and 11 deletions.
diff --git a/src/main/java/frc/robot/CommandSequences.java b/src/main/java/frc/robot/CommandSequences.java
@@ -31,9 +31,8 @@ public CommandSequences(Drive drive, Led led, AlgaeDestage algaeDestage, CoralMa
     }
 
     public Command intake() {
-        return led.intaking().andThen(elevator.moveTo(ElevatorPresets.INTAKE), led.elevatorAtPreset(),
-                coralManip.intake(), led.gamePieceAquired());
-
+        return led.elevatorToPreset().andThen(elevator.moveTo(ElevatorPresets.INTAKE), led.elevatorAtPreset(),
+                led.intaking(), coralManip.intake(), led.gamePieceAquired());
     }
 
     public Command moveElevator(ElevatorPresets level) {
@@ -45,10 +44,14 @@ public Command score() {
                 led.elevatorAtPreset());
     }
 
-    public Command scoreCoral(ElevatorPresets level) {
+    public Command scoreCoralAuto(ElevatorPresets level) {
         return led.elevatorToPreset().andThen(elevator.moveTo(level), led.elevatorAtPreset(), coralManip.score());
     }
 
+    public Command scoreL1Auto() {
+        return moveElevator(ElevatorPresets.SCOREL1).andThen(scoreL1());
+    }
+
     public Command scoreL1() {
         return coralManip.scoreL1().andThen(led.elevatorToPreset(), elevator.moveTo(ElevatorPresets.STOW),
                 led.elevatorAtPreset());
@@ -61,6 +64,6 @@ public Command setRumble(ButtonXboxController controller, int rumbleAmount) {
     }
 
     public Command resetAll() {
-        return drive.resetCmd().andThen(coralManip.resetCmd(), algaeDestage.resetCmd());
+        return drive.resetCmd().andThen(coralManip.resetCmd(), algaeDestage.resetCmd(), elevator.resetCmd());
     }
-}
+}
diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
@@ -60,9 +60,9 @@ public final class Robot extends CommandRobot {
     public void registerNamedCommands() {
 
         NamedCommands.registerCommand("Intake Game Piece", commandSequences.intake());
-        NamedCommands.registerCommand("Score Coral L1", commandSequences.scoreL1());
-        NamedCommands.registerCommand("Score Coral L2", commandSequences.scoreCoral(ElevatorPresets.SCOREL2));
-        NamedCommands.registerCommand("Score Coral L3", commandSequences.scoreCoral(ElevatorPresets.SCOREL3));
+        NamedCommands.registerCommand("Score Coral L1", commandSequences.scoreL1Auto());
+        NamedCommands.registerCommand("Score Coral L2", commandSequences.scoreCoralAuto(ElevatorPresets.SCOREL2));
+        NamedCommands.registerCommand("Score Coral L3", commandSequences.scoreCoralAuto(ElevatorPresets.SCOREL3));
     }
 
     @Autonomous(name = "No Auto", defaultAuto = true)
@@ -126,10 +126,13 @@ public void configureButtonBindings() {
                 .whileTrue(drive.robotCentricDrive());
 
         copilotController.a().onTrue(commandSequences.intake());
-        copilotController.b().whileTrue(commandSequences.moveElevator(ElevatorPresets.SCOREL2))
-                .onFalse(commandSequences.score());
+
         copilotController.x().whileTrue(commandSequences.moveElevator(ElevatorPresets.SCOREL1))
+                .onFalse(commandSequences.scoreL1());
+
+        copilotController.b().whileTrue(commandSequences.moveElevator(ElevatorPresets.SCOREL2))
                 .onFalse(commandSequences.score());
+
         copilotController.y().whileTrue(commandSequences.moveElevator(ElevatorPresets.SCOREL3))
                 .onFalse(commandSequences.score());