Merging main into barge-buffer-zone to fix a tag align bug fixed at states

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

@@ -61,7 +61,7 @@ public void robotPeriodic() {
 
   @Override
   public void disabledInit() {
-    m_robotContainer.stopTagAlign();
+    m_robotContainer.stopTagAlignAndPathHandler();
   }
 
   @Override

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;
@@ -361,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));
-    // new ForceBargeCommand(
-    //     robotStateSubsystem, elevatorSubsystem, biscuitSubsystem, algaeSubsystem)); // for easy
+                robotStateSubsystem,
+                elevatorSubsystem,
+                biscuitSubsystem,
+                algaeSubsystem)); // for easy
     // swapping
     Command floorAlgaeCommand =
         // new FloorAlgaeCommand(
@@ -439,29 +443,32 @@ private void configureOperatorBindings() {
     new Trigger(() -> robotStateSubsystem.isStuckAndMisaligned())
         .onTrue(new OperatorRumbleCommand(robotStateSubsystem, xboxController));
 
-    // Move biscuit
-    new Trigger((() -> xboxController.getRightY() < -RobotConstants.kTestingDeadband))
-        .onTrue(
-            new JogBiscuitCommand(
-                biscuitSubsystem, Angle.ofBaseUnits(BiscuitConstants.kJogAmountUp, Rotations)))
-        .onFalse(new HoldBiscuitCommand(biscuitSubsystem));
-    new Trigger((() -> xboxController.getRightY() > RobotConstants.kTestingDeadband))
-        .onTrue(
-            new JogBiscuitCommand(
-                biscuitSubsystem, Angle.ofBaseUnits(BiscuitConstants.kJogAmountDown, Rotations)))
-        .onFalse(new HoldBiscuitCommand(biscuitSubsystem));
-
-    // Move elevator
-    new Trigger((() -> xboxController.getLeftY() < -RobotConstants.kTestingDeadband))
-        .onTrue(
-            new JogElevatorCommand(
-                elevatorSubsystem, Angle.ofBaseUnits(ElevatorConstants.kJogAmountUp, Rotations)))
-        .onFalse(new HoldElevatorCommand(elevatorSubsystem));
-    new Trigger((() -> xboxController.getLeftY() > RobotConstants.kTestingDeadband))
-        .onTrue(
-            new JogElevatorCommand(
-                elevatorSubsystem, Angle.ofBaseUnits(ElevatorConstants.kJogAmountDown, Rotations)))
-        .onFalse(new HoldElevatorCommand(elevatorSubsystem));
+    new Trigger(() -> xboxController.getPOV() != -1).onTrue(new EjectAlgaeCommand(algaeSubsystem));
+
+    // // Move biscuit
+    // new Trigger((() -> xboxController.getRightY() < -RobotConstants.kTestingDeadband))
+    //     .onTrue(
+    //         new JogBiscuitCommand(
+    //             biscuitSubsystem, Angle.ofBaseUnits(BiscuitConstants.kJogAmountUp, Rotations)))
+    //     .onFalse(new HoldBiscuitCommand(biscuitSubsystem));
+    // new Trigger((() -> xboxController.getRightY() > RobotConstants.kTestingDeadband))
+    //     .onTrue(
+    //         new JogBiscuitCommand(
+    //             biscuitSubsystem, Angle.ofBaseUnits(BiscuitConstants.kJogAmountDown, Rotations)))
+    //     .onFalse(new HoldBiscuitCommand(biscuitSubsystem));
+
+    // // Move elevator
+    // new Trigger((() -> xboxController.getLeftY() < -RobotConstants.kTestingDeadband))
+    //     .onTrue(
+    //         new JogElevatorCommand(
+    //             elevatorSubsystem, Angle.ofBaseUnits(ElevatorConstants.kJogAmountUp, Rotations)))
+    //     .onFalse(new HoldElevatorCommand(elevatorSubsystem));
+    // new Trigger((() -> xboxController.getLeftY() > RobotConstants.kTestingDeadband))
+    //     .onTrue(
+    //         new JogElevatorCommand(
+    //             elevatorSubsystem, Angle.ofBaseUnits(ElevatorConstants.kJogAmountDown,
+    // Rotations)))
+    //     .onFalse(new HoldElevatorCommand(elevatorSubsystem));
   }
 
   private void configureTestOperatorBindings() {
@@ -801,8 +808,9 @@ public Command getAutonomousCommand() {
     return Commands.print("No autonomous command configured");
   }
 
-  public void stopTagAlign() {
+  public void stopTagAlignAndPathHandler() {
     tagAlignSubsystem.terminate();
+    pathHandler.killPathHandler();
   }
 
   public void setIsAuto(boolean isAuto) {
@@ -830,8 +838,8 @@ public void updateCanivoreStatus() {
   }
 
   public void updateCANErrorCount() {
-    rCanErrors = RobotController.getCANStatus().receiveErrorCount;
-    tCanErrors = RobotController.getCANStatus().transmitErrorCount;
+    rCanErrors += RobotController.getCANStatus().receiveErrorCount;
+    tCanErrors += RobotController.getCANStatus().transmitErrorCount;
   }
 
   public void disableNoMotionCal() {

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/drive/DriveAutonCommand.java

@@ -153,7 +153,7 @@ public boolean isFinished() {
       return true;
     }
     return (timer.hasElapsed(trajectory.getTotalTime() + AutonConstants.kAutoTimeout)
-        || (FastMath.sqrtQuick(
+        || (Math.sqrt(
                     FastMath.pow2(driveSubsystem.getPoseMeters().getX() - finalPose.getX())
                         + FastMath.pow2((driveSubsystem.getPoseMeters().getY() - finalPose.getY())))
                 < AutonConstants.kMaxPathErrorMeters)

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

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

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

@@ -28,6 +28,7 @@ public ForceBargeCommand(
 
   @Override
   public void initialize() {
+    hasEjectedToBarge = false;
     startState = robotStateSubsystem.getState();
     startingElevatorFinished = elevatorSubsystem.isFinished();
     robotStateSubsystem.setAlgaeHeight(AlgaeHeight.HIGH);
@@ -38,16 +39,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/commands/robotState/ForceProcessorCommand.java

@@ -28,6 +28,7 @@ public ForceProcessorCommand(
 
   @Override
   public void initialize() {
+    hasEjectedToBarge = false;
     startState = robotStateSubsystem.getState();
     startingElevatorFinished = elevatorSubsystem.isFinished();
     robotStateSubsystem.setAlgaeHeight(AlgaeHeight.LOW);

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

@@ -26,9 +26,10 @@ public ScoreAlgaeCommand(
 
   @Override
   public void initialize() {
+    hasEjectedToBarge = false;
     startState = robotStateSubsystem.getState();
     startingElevatorFinished = elevatorSubsystem.isFinished();
-    if (startState == RobotStates.PROCESSOR_ALGAE) {
+    if (startState == RobotStates.PROCESSOR_ALGAE || startState == RobotStates.BARGE_ALGAE) {
       robotStateSubsystem.releaseAlgae();
     } else {
       robotStateSubsystem.toScoreAlgae();
@@ -37,14 +38,19 @@ 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;
     } else {
       return robotStateSubsystem.getState() == RobotStates.PROCESSOR_ALGAE
+          || (robotStateSubsystem.getState() == RobotStates.BARGE_ALGAE
+              && !robotStateSubsystem.getIsAutoPlacing())
           || !robotStateSubsystem.getIsBargeSafe();
     }
     return false;

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

@@ -32,7 +32,7 @@ public class AlgaeConstants {
   public static final double kIntakingSpeed = -1;
 
   public static final double kHasAlgaeVelThreshold = 10;
-  public static final double kSuperCycleHasAlgaeVelThres = 50; // was 40
+  public static final double kSuperCycleHasAlgaeVelThres = 40; // was 40
   public static final int kHasAlgaeCounts = 2;
 
   public static final double kHasCoralVelThreshold = 70; // FIXME

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

@@ -7,10 +7,8 @@ public class BargeAlignConstants {
 
   public static final double kBlueEjectAlgaeX = 7.67; // 7.72
   public static final double kRedEjectAlgaeX = 9.86; // 9.81
-  public static final double kBlueRaiseElevatorX = 7.22; // 7.22
-  public static final double kRedRaiseElevatorX = 10.71; // 10.31
-  public static final double kBlueUnsafeX = 7.02;
-  public static final double kRedUnsafeX = 10.51;
+  public static final double kBlueRaiseElevatorX = 7.02; // 7.22
+  public static final double kRedRaiseElevatorX = 10.51; // 10.31
 
   public static final Rotation2d kBlueDesiredYaw = Rotation2d.fromDegrees(0.0);
   public static final Rotation2d kRedDesiredYaw = Rotation2d.fromDegrees(180.0);

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

@@ -63,7 +63,7 @@ public class ClimbConstants {
   public static final double kCloseEnoughX = 0.1;
   public static final double kCloseEnoughY = 0.03;
   public static final double kClimbAngleGood = RobotStateConstants.kClimbAngleSmall;
-  public static final double kFastClimbAfter = 3.5;
+  public static final double kFastClimbAt = 4.01;
 
   public static TalonFXConfiguration getPivotArmFxConfig() {
     TalonFXConfiguration armFxConfig = new TalonFXConfiguration();

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

@@ -8,9 +8,8 @@ public class RobotStateConstants {
   public static final double[] kNodeAngles = {0.0, 60.0, 120.0, 180.0, -120.0, -60.0};
   public static final double kAlgaeRetreatDistance = 0;
 
-  // Use Barge align constants
-  // public static final double kBlueBargeSafeX = 7; // 7.6
-  // public static final double kRedBargeSafeX = DriveConstants.kFieldMaxX - kBlueBargeSafeX;
+  public static final double kBlueBargeSafeX = 7; // 7.6
+  public static final double kRedBargeSafeX = DriveConstants.kFieldMaxX - kBlueBargeSafeX;
 
   public static final double kCoralEjectTimer = 0.25;
   public static final double kAlgaeEjectTimer = 0.5;

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}},
@@ -83,9 +83,9 @@ public class TagServoingConstants {
 
   // End conditions
   public static final double kInitialCloseEnough = 0.1;
-  public static final double kCoralDriveXCloseEnough = 0.03;
+  public static final double kCoralDriveXCloseEnough = 0.04; // was 0.03
   public static final double kCoralDriveYCloseEnough = 0.02; // was 0.025
-  public static final double kAlgaeDriveXCloseEnough = 0.03;
+  public static final double kAlgaeDriveXCloseEnough = 0.04; // was 0.03
   public static final double kAlgaeDriveYCloseEnough = kCoralDriveYCloseEnough;
 
   public static final double kSmallYThres = 0.3; // For elevator staging

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

@@ -7,6 +7,7 @@ public interface AlgaeIO {
   @AutoLog
   public static class AlgaeIOInputs {
     public double velocity;
+    public double statorCurrent;
     public boolean isBeamBroken;
     public boolean isCoralBeamBroken;
   }

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

@@ -10,6 +10,7 @@
 import com.ctre.phoenix6.signals.ForwardLimitValue;
 import com.ctre.phoenix6.signals.ReverseLimitValue;
 import edu.wpi.first.units.measure.AngularVelocity;
+import edu.wpi.first.units.measure.Current;
 import frc.robot.constants.AlgaeConstants;
 import org.slf4j.Logger;
 import org.slf4j.LoggerFactory;
@@ -37,6 +38,7 @@ public class AlgaeIOFX implements AlgaeIO, Checkable {
   private StatusSignal<ReverseLimitValue> revLimitSwitch;
   private VelocityVoltage speedRequest = new VelocityVoltage(0).withEnableFOC(false).withSlot(0);
   private DutyCycleOut dutyCycleRequest = new DutyCycleOut(0).withEnableFOC(false);
+  private StatusSignal<Current> statorCurrent;
 
   public AlgaeIOFX() {
     logger = LoggerFactory.getLogger(this.getClass());
@@ -49,14 +51,16 @@ public AlgaeIOFX() {
     fwdLimitSwitch = talonFXS.getForwardLimit();
     revLimitSwitch = talonFXS.getReverseLimit();
     curVelocity = talonFXS.getVelocity();
+    statorCurrent = talonFXS.getStatorCurrent();
   }
 
   @Override
   public void updateInputs(AlgaeIOInputs inputs) {
-    BaseStatusSignal.refreshAll(curVelocity, fwdLimitSwitch, revLimitSwitch);
+    BaseStatusSignal.refreshAll(curVelocity, fwdLimitSwitch, revLimitSwitch, statorCurrent);
     inputs.velocity = curVelocity.getValueAsDouble();
     inputs.isBeamBroken = fwdLimitSwitch.getValue().value == 0;
     inputs.isCoralBeamBroken = revLimitSwitch.getValue().value == 1;
+    inputs.statorCurrent = statorCurrent.getValueAsDouble();
   }
 
   @Override

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

@@ -63,7 +63,7 @@ public void scoreCoral() {
 
   public void holdAlgae() {
     // setSpeed(AlgaeConstants.kHoldSpeed);
-    setPct(0.06);
+    setPct(0.08);
   }
 
   public void holdCoral() {

src/main/java/frc/robot/subsystems/climb/ClimbSubsystem.java

@@ -102,7 +102,7 @@ public void prepClimb() {
   public void climb() {
     if (curState == ClimbState.PREPPED) {
       // setPosition(ClimbConstants.kClimbCagePos);
-      if (DriverStation.getMatchTime() < ClimbConstants.kFastClimbAfter) {
+      if (DriverStation.getMatchTime() <= ClimbConstants.kFastClimbAt) {
         Logger.recordOutput("Climb/isDesparate", true);
         io.setPercent(ClimbConstants.kClimbOpenLoopFastSpeed);
         spedUp = true;

src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java

@@ -304,7 +304,7 @@ public boolean isDriveStill() {
     double vY = cs.vyMetersPerSecond;
 
     // Take fieldRel Speed and get the magnitude of the vector
-    double wheelSpeed = FastMath.sqrtQuick(FastMath.pow2(vX) + FastMath.pow2(vY));
+    double wheelSpeed = Math.sqrt(FastMath.pow2(vX) + FastMath.pow2(vY));
 
     boolean velStill = Math.abs(wheelSpeed) <= DriveConstants.kSpeedStillThreshold;
     boolean gyroStill = isGyroStill();

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(