Lift after floor intaking (#124)

* double voltage setpoint for holding algae

* restore missing event binding

* raise to processor height after acquiring algae from floor

* rotate algae wrist instead of lifting elevator; apply to all elevator states

* restore hold-algae function to event binding

* separate bindings

* formatting

Author: nlaverdure

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

@@ -43,8 +43,10 @@
 import frc.robot.drivetrain.commands.DriveToPoseCommand;
 import frc.robot.drivetrain.commands.ZorroDriveCommand;
 import frc.robot.elevator.AlgaeRoller;
+import frc.robot.elevator.AlgaeWrist;
 import frc.robot.elevator.CoralRoller;
 import frc.robot.elevator.Elevator;
+import frc.robot.elevator.ElevatorConstants.AlgaeWristConstants.AlgaeWristState;
 import frc.robot.elevator.Lifter;
 import frc.robot.vision.Vision;
 import frc.util.Gamepiece;
@@ -65,6 +67,7 @@ public class Robot extends TimedRobot {
   private final Lifter lifter = elevator.getLifter();
   private final CoralRoller coralRoller = elevator.getCoralRoller();
   private final AlgaeRoller algaeRoller = elevator.getAlgaeRoller();
+  private final AlgaeWrist algaeWrist = elevator.getAlgaeWrist();
 
   private final Drivetrain swerve =
       new Drivetrain(allianceSelector::fieldRotated, lifter::getProportionOfMaxHeight);
@@ -298,8 +301,7 @@ public boolean getAsBoolean() {
     // Intake coral and algae
     operator.rightBumper()
         .whileTrue(algaeRoller.createIntakeCommand()
-        .alongWith(coralRoller.createIntakeCommand())
-        .andThen(new ConditionalCommand(algaeRoller.createHoldAlgaeCommand(), algaeRoller.createStopCommand(), algaeRoller.hasAlgae)));
+        .alongWith(coralRoller.createIntakeCommand()));
 
     // Force joystick operation of the elevator
     Trigger elevatorTriggerHigh = operator.axisGreaterThan(Axis.kLeftY.value, 0.9, loop).debounce(0.1);
@@ -313,7 +315,7 @@ public boolean getAsBoolean() {
 
     operator.leftTrigger().onTrue(climber.createDeployCommand());
 
-    // Auto climbe to position
+    // Auto climb to position
     operator.povUp().onTrue(climber.createRetractCommand());
 
     // just for testing roller animation.
@@ -331,7 +333,10 @@ private void configureEventBindings() {
         .andThen(climber.lockRatchet())
         .andThen(climber.resetEncoder()));
 
-    algaeRoller.hasAlgae.whileTrue(algaeRoller.createHoldAlgaeCommand());
+    algaeRoller.hasAlgae
+        .whileTrue(algaeRoller.createHoldAlgaeCommand());
+    algaeRoller.hasAlgae
+        .onTrue(algaeWrist.createSetAngleCommand(AlgaeWristState.Barge));
     coralRoller.isRolling.whileTrue(createRollerAnimationCommand());
   }
 

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

@@ -26,6 +26,7 @@
 import frc.robot.Constants.RobotConstants;
 import frc.robot.elevator.ElevatorConstants.AlgaeWristConstants;
 import frc.robot.elevator.ElevatorConstants.AlgaeWristConstants.AlgaeWristState;
+import java.util.function.BooleanSupplier;
 
 public class AlgaeWrist extends SubsystemBase {
 
@@ -35,16 +36,19 @@ public class AlgaeWrist extends SubsystemBase {
 
   private final ProfiledPIDController feedback =
       new ProfiledPIDController(
-          AlgaeWristConstants.kP,
+          AlgaeWristConstants.kPWithoutAlgae,
           AlgaeWristConstants.kI,
           AlgaeWristConstants.kD,
           AlgaeWristConstants.kConstraints);
   private final ArmFeedforward feedforward =
       new ArmFeedforward(AlgaeWristConstants.kS, AlgaeWristConstants.kG, AlgaeWristConstants.kV);
 
   private AlgaeWristState targetState = AlgaeWristState.Initial;
+  private BooleanSupplier hasAlgae;
+
+  public AlgaeWrist(BooleanSupplier hasAlgaeSupplier) {
+    hasAlgae = hasAlgaeSupplier;
 
-  public AlgaeWrist() {
     // spotless:off
     config
         .inverted(false)
@@ -112,6 +116,12 @@ public void resetController() {
   }
 
   private void control() {
+    if (hasAlgae.getAsBoolean()) {
+      feedback.setP(AlgaeWristConstants.kPWithAlgae);
+    } else {
+      feedback.setP(AlgaeWristConstants.kPWithoutAlgae);
+    }
+
     double currentPosition = encoder.getPosition();
     double offsetPosition =
         currentPosition + AlgaeWristConstants.kCenterOfGravityOffset.in(Radians);

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

@@ -12,8 +12,8 @@ public class Elevator {
   Lifter lifter = new Lifter();
   CoralWrist coralWrist = new CoralWrist();
   CoralRoller coralRoller = new CoralRoller();
-  AlgaeWrist algaeWrist = new AlgaeWrist();
   AlgaeRoller algaeRoller = new AlgaeRoller();
+  AlgaeWrist algaeWrist = new AlgaeWrist(algaeRoller.hasAlgae);
 
   public Elevator() {}
 
@@ -83,7 +83,7 @@ public Command coralIntakeCG() {
 
   public Command algaeBargePositionCG() {
     return Commands.parallel(
-        lifter.createSetHeightCommand(LifterState.Max),
+        lifter.createSetHeightCommand(LifterState.AlgaeBarge),
         coralWrist.createSetAngleCommand(CoralWristState.AlgaeMode),
         algaeWrist.createSetAngleCommand(AlgaeWristState.Barge));
   }

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

@@ -73,8 +73,9 @@ public static enum LifterState {
       CoralIntake(11.7),
       AlgaeProcessor(12.0),
       AlgaeL2(28),
-      AlgaeL3(44.5),
-      Max(67.8);
+      AlgaeL3(43.5),
+      AlgaeBarge(67.8),
+      Max(68.3);
 
       public Distance height;
 
@@ -226,9 +227,10 @@ public static final class AlgaeWristConstants {
     public static final double kS = 0.15; // Found empirically 2/22/2025
     public static final double kV = (12.0 - kS) / maxArmVelocityTheoretical.in(RadiansPerSecond);
 
-    public static final double kP = 1.5;
-    public static final double kI = 1.0;
-    public static final double kD = 0.1;
+    public static final double kPWithAlgae = 3.0;
+    public static final double kPWithoutAlgae = 1.0;
+    public static final double kI = 0;
+    public static final double kD = 0;
     public static final Angle kIZone = Degrees.of(30.0);
 
     public static enum AlgaeWristState {

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

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