elevator finish, climb pos

Author: ds12a

src/main/java/frc/robot/commands/robotState/ForceBargeCommand.java

@@ -0,0 +1,49 @@
+package frc.robot.commands.robotState;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.algae.AlgaeSubsystem;
+import frc.robot.subsystems.biscuit.BiscuitSubsystem;
+import frc.robot.subsystems.elevator.ElevatorSubsystem;
+import frc.robot.subsystems.robotState.RobotStateSubsystem;
+import frc.robot.subsystems.robotState.RobotStateSubsystem.AlgaeHeight;
+import frc.robot.subsystems.robotState.RobotStateSubsystem.RobotStates;
+
+public class ForceBargeCommand extends Command {
+  RobotStateSubsystem robotStateSubsystem;
+  ElevatorSubsystem elevatorSubsystem;
+  boolean hasTriedToPickup = false;
+  private RobotStates startState;
+  private boolean startingElevatorFinished;
+
+  public ForceBargeCommand(
+      RobotStateSubsystem robotStateSubsystem,
+      ElevatorSubsystem elevatorSubsystem,
+      BiscuitSubsystem biscuitSubsystem,
+      AlgaeSubsystem algaeSubsystem) {
+    this.robotStateSubsystem = robotStateSubsystem;
+    this.elevatorSubsystem = elevatorSubsystem;
+    addRequirements(elevatorSubsystem, biscuitSubsystem, algaeSubsystem);
+  }
+
+  @Override
+  public void initialize() {
+    startState = robotStateSubsystem.getState();
+    startingElevatorFinished = elevatorSubsystem.isFinished();
+    robotStateSubsystem.setAlgaeHeight(AlgaeHeight.HIGH);
+    if (startState == RobotStates.BARGE_ALGAE) robotStateSubsystem.releaseAlgae();
+    else robotStateSubsystem.toScoreAlgae();
+  }
+
+  @Override
+  public boolean isFinished() {
+    if (startState == RobotStates.PROCESSOR_ALGAE || startState == RobotStates.BARGE_ALGAE) {
+      return robotStateSubsystem.getState() == RobotStates.FUNNEL_LOAD
+          || robotStateSubsystem.getState() == RobotStates.STOW
+          || !startingElevatorFinished;
+    } else {
+      return robotStateSubsystem.getState() == RobotStates.BARGE_ALGAE
+          || robotStateSubsystem.getState() == RobotStates.PROCESSOR_ALGAE
+          || !robotStateSubsystem.getIsBargeSafe();
+    }
+  }
+}

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

@@ -21,6 +21,7 @@
 public class ElevatorConstants {
 
   public static final double kCloseEnoughRotations = 0.1;
+  public static final double kStowThresholdDone = 0.3676757 + 0.5;
   public static final double kMaxFwd = 53;
   public static final double kMaxRev = 2;
   public static final double kElevatorLiftHeight = 1; // for pit command
@@ -50,7 +51,7 @@ public class ElevatorConstants {
 
   // Coral score
   public static final Angle kL1CoralSetpoint = Rotations.of(15.2); // 13.04053
-  public static final Angle kL2CoralSetpoint = Rotations.of(21.0786); // 19.62793 -> 21.0786
+  public static final Angle kL2CoralSetpoint = Rotations.of(19.62793); // 19.62793 -> 21.0786 ->
   public static final Angle kL3CoralSetpoint =
       Rotations.of(31.0901); // was 30.42969 -> 31.7505 -> 31.0901
   public static final Angle kL4CoralSetpoint = Rotations.of(48.28076);

src/main/java/frc/robot/subsystems/robotState/RobotStateSubsystem.java

@@ -619,6 +619,9 @@ public void toPrepClimb() {
   public void toClimb() {
     if (curState == RobotStates.PREP_CLIMB || climbSubsystem.getState() == ClimbState.PREPPED) {
 
+      biscuitSubsystem.setPosition(RobotConstants.kHpAlgaeSetpoint, hasAlgae());
+      elevatorSubsystem.setPosition(ElevatorConstants.kHpAlgaeSetpoint);
+
       climbSubsystem.climb();
       driveSubsystem.prepClimb();
 
@@ -657,7 +660,10 @@ public void periodic() {
       }
 
       case TO_STOW -> {
-        if (biscuitSubsystem.isFinished() && elevatorSubsystem.isFinished()) {
+        if (biscuitSubsystem.isFinished()
+            && (elevatorSubsystem.isFinished()
+                || elevatorSubsystem.getPosition().in(Rotations)
+                    < ElevatorConstants.kStowThresholdDone)) {
           setState(RobotStates.STOW);
         }
       }