Merge pull request #67 from strykeforce/statesHotfixes

mwitcpalek · web-flow · commit 5400f9417fe2 · 2025-04-08T19:28:04.000-04:00
States hotfixes
diff --git a/src/main/java/frc/robot/Robot.java b/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
diff --git a/src/main/java/frc/robot/RobotContainer.java b/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() {
diff --git a/src/main/java/frc/robot/commands/algae/EjectAlgaeCommand.java b/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();
+  }
+}
diff --git a/src/main/java/frc/robot/commands/drive/DriveAutonCommand.java b/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)
diff --git a/src/main/java/frc/robot/commands/robotState/EjectAlgaeCommand.java b/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();
+  }
+}
diff --git a/src/main/java/frc/robot/commands/robotState/ForceBargeCommand.java b/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;
   }
 }
diff --git a/src/main/java/frc/robot/commands/robotState/ForceProcessorCommand.java b/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);
diff --git a/src/main/java/frc/robot/commands/robotState/ScoreAlgaeCommand.java b/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;
diff --git a/src/main/java/frc/robot/constants/AlgaeConstants.java b/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
diff --git a/src/main/java/frc/robot/constants/ClimbConstants.java b/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();
diff --git a/src/main/java/frc/robot/constants/TagServoingConstants.java b/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
diff --git a/src/main/java/frc/robot/subsystems/algae/AlgaeIO.java b/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;
   }
diff --git a/src/main/java/frc/robot/subsystems/algae/AlgaeIOFX.java b/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
diff --git a/src/main/java/frc/robot/subsystems/algae/AlgaeSubsystem.java b/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() {
diff --git a/src/main/java/frc/robot/subsystems/climb/ClimbSubsystem.java b/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;
diff --git a/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java b/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();
diff --git a/src/main/java/frc/robot/subsystems/led/LEDSubsystem.java b/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(
diff --git a/src/main/java/frc/robot/subsystems/pathHandler/PathHandler.java b/src/main/java/frc/robot/subsystems/pathHandler/PathHandler.java
@@ -219,7 +219,7 @@ private void drivePath() {
       driveSubsystem.calculateController(
           mirrorToProcessor(currPath.sampleAt(pathTimer.get(), mirrorTrajectory).get()));
       if (pathTimer.hasElapsed(currPath.getTotalTime() + AutonConstants.kAutoTimeout)
-          || (FastMath.sqrtQuick(
+          || (Math.sqrt(
                       FastMath.pow2(
                               driveSubsystem.getPoseMeters().getX() - currPathFinalPose.getX())
                           + FastMath.pow2(
@@ -331,6 +331,7 @@ public void killPathHandler() {
     isHandling = false;
     curState = PathStates.DONE;
     runningPath = false;
+    robotStateSubsystem.setLEDLoadCoral(false);
     robotStateSubsystem.setIsAutoPlacing(true);
     pathTimer.stop();
     pathTimer.reset();
@@ -451,7 +452,7 @@ public void periodic() {
           startPath(nextPath());
         }
         double fetchDistance =
-            FastMath.sqrtQuick(
+            Math.sqrt(
                 FastMath.pow2(driveSubsystem.getPoseMeters().getX() - currPathFinalPose.getX())
                     + FastMath.pow2(
                         driveSubsystem.getPoseMeters().getY() - currPathFinalPose.getY()));
diff --git a/src/main/java/frc/robot/subsystems/robotState/RobotStateSubsystem.java b/src/main/java/frc/robot/subsystems/robotState/RobotStateSubsystem.java
diff --git a/src/main/java/frc/robot/subsystems/tagAlign/TagAlignSubsystem.java b/src/main/java/frc/robot/subsystems/tagAlign/TagAlignSubsystem.java
diff --git a/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java b/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java

Original file line number	Diff line number	Diff line change
`@@ -61,7 +61,7 @@ public void robotPeriodic() {`
`61`	`61`
`62`	`62`	`@Override`
`63`	`63`	`public void disabledInit() {`
`64`		`- m_robotContainer.stopTagAlign();`
	`64`	`+ m_robotContainer.stopTagAlignAndPathHandler();`
`65`	`65`	`}`
`66`	`66`
`67`	`67`	`@Override`
Original file line number	Diff line number	Diff line change
`@@ -153,7 +153,7 @@ public boolean isFinished() {`
`153`	`153`	`return true;`
`154`	`154`	`}`
`155`	`155`	`return (timer.hasElapsed(trajectory.getTotalTime() + AutonConstants.kAutoTimeout)`
`156`		`- \|\| (FastMath.sqrtQuick(`
	`156`	`+ \|\| (Math.sqrt(`
`157`	`157`	`FastMath.pow2(driveSubsystem.getPoseMeters().getX() - finalPose.getX())`
`158`	`158`	`+ FastMath.pow2((driveSubsystem.getPoseMeters().getY() - finalPose.getY())))`
`159`	`159`	`< AutonConstants.kMaxPathErrorMeters)`