Created command sequences

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

Hide file tree

Showing 2 changed files with 40 additions and 13 deletions.
diff --git a/src/main/java/frc/robot/CommandSequences.java b/src/main/java/frc/robot/CommandSequences.java
@@ -6,8 +6,11 @@
 
 import edu.wpi.first.wpilibj.GenericHID.RumbleType;
 import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.maps.subsystems.ElevatorMap.ElevatorPresets;
 import frc.robot.subsystems.AlgaeDestage;
+import frc.robot.subsystems.CoralManip;
 import frc.robot.subsystems.Drive;
+import frc.robot.subsystems.Elevator;
 import frc.robot.subsystems.Led;
 import frc.robot.subsystems.Outtake;
 
@@ -16,21 +19,39 @@ public class CommandSequences {
     Drive drive;
     Led led;
     AlgaeDestage algaeDestage;
-    Outtake outtake;
+    CoralManip coralManip;
+    Elevator elevator;
 
-    public CommandSequences(Drive drive, Led led, AlgaeDestage algaeDestage, Outtake outtake) {
+    public CommandSequences(Drive drive, Led led, AlgaeDestage algaeDestage, CoralManip coralManip, Elevator elevator) {
         this.drive = drive;
         this.led = led;
         this.algaeDestage = algaeDestage;
-        this.outtake = outtake;
+        this.coralManip = coralManip;
+        this.elevator = elevator;
     }
 
     public Command intake() {
-        return outtake.intake();
+        return led.intaking().andThen(elevator.moveTo(ElevatorPresets.INTAKE), led.elevatorAtPreset(),
+                coralManip.intake(), led.gamePieceAquired());
+
+    }
+
+    public Command moveElevator(ElevatorPresets level) {
+        return led.elevatorToPreset().andThen(elevator.moveTo(level), led.elevatorAtPreset());
+    }
+
+    public Command score() {
+        return coralManip.score().andThen(led.elevatorToPreset(), elevator.moveTo(ElevatorPresets.STOW),
+                led.elevatorAtPreset());
+    }
+
+    public Command scoreCoral(ElevatorPresets level) {
+        return led.elevatorToPreset().andThen(elevator.moveTo(level), led.elevatorAtPreset(), coralManip.score());
     }
 
-    public Command scoreCoral() {
-        return outtake.score();
+    public Command scoreL1() {
+        return coralManip.scoreL1().andThen(led.elevatorToPreset(), elevator.moveTo(ElevatorPresets.STOW),
+                led.elevatorAtPreset());
     }
 
     public Command setRumble(ButtonXboxController controller, int rumbleAmount) {
@@ -40,6 +61,6 @@ public Command setRumble(ButtonXboxController controller, int rumbleAmount) {
     }
 
     public Command resetAll() {
-        return drive.resetCmd().andThen(outtake.resetCmd(), algaeDestage.resetCmd());
+        return drive.resetCmd().andThen(coralManip.resetCmd(), algaeDestage.resetCmd());
     }
 }
diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
@@ -21,6 +21,7 @@
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
 import frc.robot.maps.RobotMap;
+import frc.robot.maps.subsystems.ElevatorMap.ElevatorPresets;
 import frc.robot.subsystems.AlgaeDestage;
 import frc.robot.subsystems.CoralManip;
 import frc.robot.subsystems.DeepClimb;
@@ -52,14 +53,16 @@ public final class Robot extends CommandRobot {
     private Elevator elevator = new Elevator(map.getElevatorMap());
     private DeepClimb deepClimb = new DeepClimb(map.getDeepClimbMap());
 
-    private CommandSequences commandSequences = new CommandSequences(drive, led, algaeDestage, outtake);
+    private CommandSequences commandSequences = new CommandSequences(drive, led, algaeDestage, coralManip, elevator);
 
     NetworkTableInstance ntinst = NetworkTableInstance.getDefault();
 
     public void registerNamedCommands() {
 
         NamedCommands.registerCommand("Intake Game Piece", commandSequences.intake());
-        NamedCommands.registerCommand("Score Coral", commandSequences.scoreCoral());
+        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));
     }
 
     @Autonomous(name = "No Auto", defaultAuto = true)
@@ -122,10 +125,13 @@ public void configureButtonBindings() {
         driveController.leftBumper()
                 .whileTrue(drive.robotCentricDrive());
 
-        driveController.a().onTrue(outtake.intake());
-        driveController.b().whileTrue(outtake.score());
-        driveController.x().whileTrue(outtake.scoreL1());
-        driveController.y().whileTrue(algaeDestage.destageAlgae());
+        copilotController.a().onTrue(commandSequences.intake());
+        copilotController.b().whileTrue(commandSequences.moveElevator(ElevatorPresets.SCOREL2))
+                .onFalse(commandSequences.score());
+        copilotController.x().whileTrue(commandSequences.moveElevator(ElevatorPresets.SCOREL1))
+                .onFalse(commandSequences.score());
+        copilotController.y().whileTrue(commandSequences.moveElevator(ElevatorPresets.SCOREL3))
+                .onFalse(commandSequences.score());
 
         driveController.rightBumper().whileTrue(drive.aimAtReefCenter());