code quit bug, other bugs

Author: ds12a

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

@@ -20,6 +20,7 @@
 import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.button.JoystickButton;
 import edu.wpi.first.wpilibj2.command.button.Trigger;
+import frc.robot.commands.algae.EjectAlgaeCommand;
 import frc.robot.commands.algae.IntakeAlgaeCommand;
 import frc.robot.commands.algae.OpenLoopAlgaeCommand;
 import frc.robot.commands.algae.ProcessorAlgaeCommand;
@@ -44,15 +45,14 @@
 import frc.robot.commands.elevator.ZeroElevatorCommand;
 import frc.robot.commands.robotState.AutoReefCycleCommand;
 import frc.robot.commands.robotState.DepthChargeHealthCheckCommand;
-import frc.robot.commands.robotState.EjectAlgaeCommand;
+import frc.robot.commands.robotState.ForceBargeCommand;
 import frc.robot.commands.robotState.ForceLowFloorAlgaeCommand;
 import frc.robot.commands.robotState.ForceProcessorCommand;
 import frc.robot.commands.robotState.HPAlgaeCommand;
 import frc.robot.commands.robotState.InterruptAutoCommand;
 import frc.robot.commands.robotState.LockWheelsCommand;
 import frc.robot.commands.robotState.OperatorRumbleCommand;
 import frc.robot.commands.robotState.ReefCycleCommand;
-import frc.robot.commands.robotState.ScoreAlgaeCommand;
 import frc.robot.commands.robotState.SetScoreSideCommand;
 import frc.robot.commands.robotState.SetScoreSideRightCommand;
 import frc.robot.commands.robotState.SetScoringLevelCommand;
@@ -362,10 +362,13 @@ private void configureDriverBindings() {
 
     new JoystickButton(driveJoystick, Button.M_SWE.id)
         .onTrue(
-            new ScoreAlgaeCommand(
-                robotStateSubsystem, elevatorSubsystem, biscuitSubsystem, algaeSubsystem));
-    // new ForceBargeCommand(
-    //     robotStateSubsystem, elevatorSubsystem, biscuitSubsystem, algaeSubsystem)); // for easy
+            // new ScoreAlgaeCommand(
+            //     robotStateSubsystem, elevatorSubsystem, biscuitSubsystem, algaeSubsystem));
+            new ForceBargeCommand(
+                robotStateSubsystem,
+                elevatorSubsystem,
+                biscuitSubsystem,
+                algaeSubsystem)); // for easy
     // swapping
     Command floorAlgaeCommand =
         // new FloorAlgaeCommand(
@@ -440,8 +443,7 @@ private void configureOperatorBindings() {
     new Trigger(() -> robotStateSubsystem.isStuckAndMisaligned())
         .onTrue(new OperatorRumbleCommand(robotStateSubsystem, xboxController));
 
-    new Trigger(() -> xboxController.getPOV() != 0)
-        .onTrue(new EjectAlgaeCommand(algaeSubsystem, robotStateSubsystem));
+    new Trigger(() -> xboxController.getPOV() != -1).onTrue(new EjectAlgaeCommand(algaeSubsystem));
 
     // // Move biscuit
     // new Trigger((() -> xboxController.getRightY() < -RobotConstants.kTestingDeadband))

src/main/java/frc/robot/commands/algae/EjectAlgaeCommand.java

@@ -0,0 +1,18 @@
+package frc.robot.commands.algae;
+
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+import frc.robot.subsystems.algae.AlgaeSubsystem;
+
+public class EjectAlgaeCommand extends InstantCommand {
+  private AlgaeSubsystem algaeSubsystem;
+
+  public EjectAlgaeCommand(AlgaeSubsystem algaeSubsystem) {
+    this.algaeSubsystem = algaeSubsystem;
+    addRequirements(algaeSubsystem);
+  }
+
+  @Override
+  public void initialize() {
+    algaeSubsystem.scoreProcessor();
+  }
+}

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

@@ -38,16 +38,21 @@ public void initialize() {
 
   @Override
   public boolean isFinished() {
-    if (startState == RobotStates.PROCESSOR_ALGAE || hasEjectedToBarge) {
+    if (startState == RobotStates.PROCESSOR_ALGAE
+        || startState == RobotStates.BARGE_ALGAE
+        || hasEjectedToBarge) {
       return robotStateSubsystem.getState() == RobotStates.FUNNEL_LOAD
           || robotStateSubsystem.getState() == RobotStates.STOW
           || !startingElevatorFinished;
-    } else if (robotStateSubsystem.getState() == RobotStates.BARGE_ALGAE) {
+    } else if (robotStateSubsystem.getState() == RobotStates.BARGE_ALGAE
+        && robotStateSubsystem.getIsAutoPlacing()) {
       hasEjectedToBarge = true;
+      return false;
     } else {
       return robotStateSubsystem.getState() == RobotStates.PROCESSOR_ALGAE
+          || (robotStateSubsystem.getState() == RobotStates.BARGE_ALGAE
+              && !robotStateSubsystem.getIsAutoPlacing())
           || !robotStateSubsystem.getIsBargeSafe();
     }
-    return false;
   }
 }

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

@@ -41,7 +41,7 @@ public class TagServoingConstants {
   public static final int[] kBlueTargetTag = {18, 17, 22, 21, 20, 19};
   public static final int[] kRedTargetTag = {7, 8, 9, 10, 11, 6};
 
-  // Offsets
+  // Offsets (left is negative, right is pos)
   public static final double[][][] kBlueCoralOffset = {
     {{0, 0}, {0, 0}, {0, 0}, {0, 0}},
     {{0, 0}, {0, 0}, {0, 0}, {0, 0}},

src/main/java/frc/robot/subsystems/led/LEDSubsystem.java

@@ -172,7 +172,7 @@ public boolean getCurrentLimiting() {
   }
 
   private void buildBase() {
-
+    base = LEDPattern.solid(Color.kBlack);
     algae =
         LEDPattern.steps(
             Map.of(

src/main/java/frc/robot/subsystems/pathHandler/PathHandler.java

@@ -331,6 +331,7 @@ public void killPathHandler() {
     isHandling = false;
     curState = PathStates.DONE;
     runningPath = false;
+    robotStateSubsystem.setLEDLoadCoral(false);
     robotStateSubsystem.setIsAutoPlacing(true);
     pathTimer.stop();
     pathTimer.reset();