more drive practice tweaks

Author: mwitcpalek

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

@@ -41,6 +41,7 @@
 import frc.robot.commands.elevator.ZeroElevatorCommand;
 import frc.robot.commands.robotState.AutoReefCycleCommand;
 import frc.robot.commands.robotState.FloorAlgaeCommand;
+import frc.robot.commands.robotState.ForceProcessorCommand;
 import frc.robot.commands.robotState.HPAlgaeCommand;
 import frc.robot.commands.robotState.InterruptAutoCommand;
 import frc.robot.commands.robotState.ReefCycleCommand;
@@ -289,21 +290,15 @@ private void configureDriverBindings() {
 
     // Reset Gyro Command, stow, interrupt Auton, and zero elev
     new JoystickButton(driveJoystick, Button.M_SWC.id).onTrue(new ResetGyroCommand(driveSubsystem));
+
+    // Force Processor
     new JoystickButton(driveJoystick, Button.SWD.id)
         .onTrue(
-            new StowCommand(
-                robotStateSubsystem,
-                elevatorSubsystem,
-                coralSubsystem,
-                biscuitSubsystem,
-                algaeSubsystem))
+            new ForceProcessorCommand(
+                robotStateSubsystem, elevatorSubsystem, biscuitSubsystem, algaeSubsystem))
         .onFalse(
-            new StowCommand(
-                robotStateSubsystem,
-                elevatorSubsystem,
-                coralSubsystem,
-                biscuitSubsystem,
-                algaeSubsystem));
+            new ForceProcessorCommand(
+                robotStateSubsystem, elevatorSubsystem, biscuitSubsystem, algaeSubsystem));
 
     // Interupt
     new JoystickButton(driveJoystick, Button.SWG_UP.id)

src/main/java/frc/robot/commands/robotState/ForceProcessorCommand.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 ForceProcessorCommand extends Command {
+  RobotStateSubsystem robotStateSubsystem;
+  ElevatorSubsystem elevatorSubsystem;
+  boolean hasTriedToPickup = false;
+  private RobotStates startState;
+  private boolean startingElevatorFinished;
+
+  public ForceProcessorCommand(
+      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.LOW);
+    if (startState == RobotStates.PROCESSOR_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/TagServoingConstants.java

@@ -47,12 +47,12 @@ public class TagServoingConstants {
   public static final double kNoUpdateMicrosec = 500_000;
 
   // Drive
-  public static final double kCoralInitialDriveRadius = 1.7; // 0.36 away from reef wall
+  public static final double kCoralInitialDriveRadius = 1.75; // 0.36 away from reef wall
   public static final double kCoralAlignRadius = 1.32; // 1.293823 is perfectly against the reef
   // public static final double kCoralStopXDriveRadius =
   //     kCoralInitialDriveRadius; // Should be closer to reef than target pose
-  public static final double kAlgaeInitialDriveRadius = 1.6;
-  public static final double kAlgaeAlignRadius = 1.49; // was 1.34
+  public static final double kAlgaeInitialDriveRadius = 1.;
+  public static final double kAlgaeAlignRadius = 1.34; // was 1.34
   public static final double kAlgaeStopXDriveRadius =
       kAlgaeInitialDriveRadius; // Should be closer to reef than target pose
   // public static final double kMinVelX = 0.85;

src/main/java/frc/robot/subsystems/algae/AlgaeSubsystem.java

@@ -50,7 +50,7 @@ public void scoreBarge() {
 
   public void hold() {
     // setSpeed(AlgaeConstants.kHoldSpeed);
-    setPct(-0);
+    setPct(0.04);
   }
 
   public boolean hasAlgae() {