separate commands for outtaking algae to barge and processor (#122)

Co-authored-by: DylanTaylor29 <[email protected]>

Author: nlaverdure

src/main/java/frc/robot/Robot.java

@@ -248,10 +248,13 @@ private void configureDriverButtonBindings() {
         .whileTrue(new DriveToPoseCommand(swerve, vision, () -> swerve.getNearestPose()));
 
     // Outtake grippers
-    driver.HIn()
-        .whileTrue(coralRoller.createOuttakeCommand()
-        .alongWith(algaeRoller.createOuttakeCommand()));
-
+    var outtaking = driver.HIn();
+    lifter.atProcessorHeight.and(outtaking)
+        .whileTrue(algaeRoller.createOuttakeToProcessorCommand());
+    lifter.atBargeHeight.and(outtaking)
+        .whileTrue(algaeRoller.createOuttakeToBargeCommand());
+    outtaking
+        .whileTrue(coralRoller.createOuttakeCommand());
   }
 
   private void configureOperatorButtonBindings() {

src/main/java/frc/robot/elevator/AlgaeRoller.java

@@ -89,8 +89,12 @@ public Command createIntakeCommand() {
     return this.run(() -> setVoltage(AlgaeRollerConstants.kIntakeVoltage));
   }
 
-  public Command createOuttakeCommand() {
-    return this.run(() -> setVoltage(AlgaeRollerConstants.kOuttakeVoltage));
+  public Command createOuttakeToBargeCommand() {
+    return this.run(() -> setVoltage(AlgaeRollerConstants.kOuttakeToBargeVoltage));
+  }
+
+  public Command createOuttakeToProcessorCommand() {
+    return this.run(() -> setVoltage(AlgaeRollerConstants.kOuttakeToProcessorVoltage));
   }
 
   public Command createHoldAlgaeCommand() {

src/main/java/frc/robot/elevator/ElevatorConstants.java

@@ -185,7 +185,8 @@ public static final class AlgaeRollerConstants {
     public static final Voltage kIntakeVoltage = Volts.of(5.0);
     // Found 3/9/2025 that 5V applied at stall will draw ~5A, which is acceptable for motor lifetime
     public static final Voltage kHoldVoltage = Volts.of(5.0);
-    public static final Voltage kOuttakeVoltage = kIntakeVoltage.unaryMinus();
+    public static final Voltage kOuttakeToProcessorVoltage = Volts.of(-5.0);
+    public static final Voltage kOuttakeToBargeVoltage = Volts.of(-2.5);
   }
 
   public static final class AlgaeWristConstants {

src/main/java/frc/robot/elevator/Lifter.java

@@ -154,6 +154,8 @@ public Boolean inState(LifterState state) {
   }
 
   public Trigger atIntakingHeight = new Trigger(() -> inState(LifterState.CoralIntake));
+  public Trigger atProcessorHeight = new Trigger(() -> inState(LifterState.AlgaeProcessor));
+  public Trigger atBargeHeight = new Trigger(() -> inState(LifterState.AlgaeBarge));
   public Trigger atFloorIntakingHeight = new Trigger(() -> inState(LifterState.AlgaeIntakeFloor));
   public Trigger tooHighForCoralWrist =
       new Trigger(() -> getCurrentHeight().gt(LifterState.CoralL3.height.plus(Inches.of(2.0))));