New dealgify command

Author: owen-beck

src/main/java/frc/robot/RobotContainer.java

@@ -220,8 +220,8 @@ private void configureSecondaryBindings() {
 
         b.whileTrue(superstructure.intakeCoralGroundCommand());
         a.whileTrue(superstructure.intakeCoralHumanPlayerCommand());
-        x.whileTrue(superstructure.intakeAlgaeReefLowCommand());
-        y.whileTrue(superstructure.intakeAlgaeReefHighCommand());
+        x.whileTrue(superstructure.dealgifyLowCommand(overrideAtPose));
+        y.whileTrue(superstructure.dealgifyHighCommand(overrideAtPose));
 
         // bumperRight.whileTrue(climber.climbCommand());
         bumperLeft.whileTrue(climber.setVoltageCommand(-12));

src/main/java/frc/robot/constants/IntakeConstants.java

@@ -18,6 +18,7 @@ public class IntakeConstants {
     public static final double BRANCH_EJECT_VELOCITY = -15;
     public static final double PROCESSOR_EJECT_VELOCITY = -120;
     public static final double NET_EJECT_VELOCITY = -120;
+    public static final double DEALGIFY_VELOCITY = -120;
 
     public static final double CORAL_STALL_DEBOUNCE_TIME = 0.1;
     public static final int CORAL_STALL_CURRENT = 50;

src/main/java/frc/robot/constants/SuperstructureConstants.java

@@ -188,6 +188,32 @@ public class SuperstructureConstants {
         WristConstants.PARALLEL_CURRENT, 
         IntakeConstants.NET_EJECT_VELOCITY);
 
+    // Dealgify states
+
+    public static final SuperstructureState STAGE_LOW_DEALGIFY_STATE = new SuperstructureState(
+        ElevatorConstants.ALGAE_LOW_REEF_INTAKING_POSITION, 
+        ArmConstants.ALGAE_LOW_REEF_INTAKING_POSITION, 
+        WristConstants.PERPENDICULAR_CURRENT, 
+        IntakeConstants.DEALGIFY_VELOCITY);
+
+    public static final SuperstructureState REMOVE_LOW_DEALGIFY_STATE = new SuperstructureState(
+        ElevatorConstants.ALGAE_LOW_REEF_INTAKING_POSITION, 
+        ArmConstants.ALGAE_LOW_REEF_INTAKING_POSITION, 
+        WristConstants.PERPENDICULAR_CURRENT, 
+        IntakeConstants.DEALGIFY_VELOCITY);
+
+    public static final SuperstructureState STAGE_HIGH_DEALGIFY_STATE = new SuperstructureState(
+        ElevatorConstants.ALGAE_HIGH_REEF_INTAKING_POSITION, 
+        ArmConstants.ALGAE_HIGH_REEF_INTAKING_POSITION, 
+        WristConstants.PERPENDICULAR_CURRENT, 
+        IntakeConstants.DEALGIFY_VELOCITY);
+
+    public static final SuperstructureState REMOVE_HIGH_DEALGIFY_STATE = new SuperstructureState(
+        ElevatorConstants.ALGAE_HIGH_REEF_INTAKING_POSITION, 
+        ArmConstants.ALGAE_HIGH_REEF_INTAKING_POSITION, 
+        WristConstants.PERPENDICULAR_CURRENT, 
+        IntakeConstants.DEALGIFY_VELOCITY);
+
     // Error margins
     public static final double ELEVATOR_ERROR_MARGIN = 1;
     public static final double ARM_ERROR_MARGIN = 0.075;

src/main/java/frc/robot/subsystems/SuperstructureSubsystem.java

@@ -345,20 +345,27 @@ public Command scoreEjectCommand(SuperstructureState stagingState, Superstructur
         });
     }
 
-    // Temporary values
-    public Command autoScoreCommand() {
+    // Dealgify
+
+    public Command dealgifyLowCommand(Supplier<Boolean> override) {
+        return dealgifyCommand(SuperstructureConstants.STAGE_LOW_DEALGIFY_STATE, SuperstructureConstants.REMOVE_LOW_DEALGIFY_STATE,
+            SuperstructureConstants.CORAL_STOWED_STATE, override);
+    }
+
+    public Command dealgifyHighCommand(Supplier<Boolean> override) {
+        return dealgifyCommand(SuperstructureConstants.STAGE_HIGH_DEALGIFY_STATE, SuperstructureConstants.REMOVE_HIGH_DEALGIFY_STATE,
+            SuperstructureConstants.CORAL_STOWED_STATE, override);
+    }
+
+    public Command dealgifyCommand(SuperstructureState stagingState, SuperstructureState removingState,
+            SuperstructureState stowedState, Supplier<Boolean> override) {
         return Commands.sequence(
-            Commands.runOnce(() -> setState(SuperstructureConstants.STAGE_L1_STATE),
-                elevator, arm, wrist, intake),
-            Commands.waitUntil(() -> isAtTargetState()),
-            Commands.run(() -> {
-                setState(SuperstructureConstants.EJECT_L1_STATE);
-                led.flashAllPixels(LedConstants.YELLOW, 5);
-            },
-                elevator, arm, wrist, intake, led)
+            Commands.runOnce(() -> setState(stagingState), elevator, arm, wrist, intake),
+            Commands.waitUntil(() -> override.get()),
+            Commands.run(() -> setState(removingState), elevator, arm, wrist, intake)
         )
         .finallyDo(() -> {
-            setState(SuperstructureConstants.CORAL_STOWED_STATE);
+            setState(stowedState);
         });
     }