scoring over coral works now

Author: PotatoBoyH4

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

@@ -95,8 +95,8 @@ public class TagServoingConstants {
   public static final double kEndVelThreshold = 5;
 
   // Stuck coral
-  public static final double kMinStuckCounts = 5;
-  public static final double kCoralStuckDistance = 0.21;
+  public static final double kMinStuckCounts = 3;
+  public static final double kCoralStuckDistance = 0.19;
   public static final double kUnfixableCoralStuckDistance = 0.34;
 
   // Reef

src/main/java/frc/robot/subsystems/laser/LaserIO.java

@@ -1,6 +1,15 @@
 package frc.robot.subsystems.laser;
 
+import org.littletonrobotics.junction.AutoLog;
+
 public interface LaserIO {
 
+  @AutoLog
+  public static class LaserIOInputs {
+    public double distance = 0.0;
+  }
+
+  public void updateInputs(LaserIOInputs inputs);
+
   public double getDistanceMeters();
 }

src/main/java/frc/robot/subsystems/laser/LaserIOCANRange.java

@@ -13,6 +13,11 @@ public LaserIOCANRange() {
     laser.getConfigurator().apply(LaserConstants.getLaserConfig());
   }
 
+  @Override
+  public void updateInputs(LaserIOInputs inputs) {
+    inputs.distance = getDistanceMeters();
+  }
+
   public double getDistanceMeters() {
     if (laser.getIsDetected().getValue()) {
       return laser.getDistance().getValue().in(Meters);

src/main/java/frc/robot/subsystems/laser/LaserSubsystem.java

@@ -6,14 +6,21 @@
 
 public class LaserSubsystem extends MeasurableSubsystem {
 
-  private final LaserIO laserio;
+  private final LaserIO io;
+  private final LaserIOInputsAutoLogged inputs = new LaserIOInputsAutoLogged();
 
   public LaserSubsystem(LaserIO laserio) {
-    this.laserio = laserio;
+    this.io = laserio;
   }
 
   public double getDistance() {
-    return laserio.getDistanceMeters();
+    return io.getDistanceMeters();
+  }
+
+  @Override
+  public void periodic() {
+    io.updateInputs(inputs);
+    org.littletonrobotics.junction.Logger.processInputs("LaserInputs", inputs);
   }
 
   @Override

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

@@ -1046,34 +1046,26 @@ public void periodic() {
 
         if (tagAlignSubsystem.fixableStuckCoral() == true) {
           switch (scoringLevel) {
-            case L1 -> {
-              setBiscuitTransferSlow(RobotConstants.kL1CoralSetpoint, true);
-              elevatorSubsystem.setPosition(
-                  ElevatorConstants.kL1CoralSetpoint.plus(
-                      RobotStateConstants.kStuckCoralElevatorOffset));
-            }
+            case L1 -> {}
             case L2 -> {
               setBiscuitTransfer(RobotConstants.kL2CoralSetpoint, true);
               elevatorSubsystem.setPosition(
                   ElevatorConstants.kL2CoralSetpoint.plus(
                       RobotStateConstants.kStuckCoralElevatorOffset));
+              setState(RobotStates.PLACE_CORAL, true);
+              reefCoralStuckFixable = tagAlignSubsystem.fixableStuckCoral();
             }
             case L3 -> {
               setBiscuitTransfer(RobotConstants.kL3CoralSetpoint, true);
               elevatorSubsystem.setPosition(
                   ElevatorConstants.kL3CoralSetpoint.plus(
                       RobotStateConstants.kStuckCoralElevatorOffset));
+              setState(RobotStates.PLACE_CORAL, true);
+              reefCoralStuckFixable = tagAlignSubsystem.fixableStuckCoral();
             }
-            case L4 -> {
-              setBiscuitTransfer(RobotConstants.kL4CoralSetpoint, true);
-              elevatorSubsystem.setPosition(
-                  ElevatorConstants.kL4CoralSetpoint.plus(
-                      RobotStateConstants.kStuckCoralElevatorOffset));
-            }
+            case L4 -> {}
           }
           tagAlignSubsystem.terminate();
-          setState(RobotStates.PLACE_CORAL, true);
-          reefCoralStuckFixable = tagAlignSubsystem.fixableStuckCoral();
         } else if ((isAutoPlacing
                 && (tagAlignSubsystem.getState() == TagAlignSubsystem.TagAlignStates.DONE
                 /*  || reefCoralStuckFixable

src/main/java/frc/robot/subsystems/tagAlign/TagAlignSubsystem.java

@@ -232,7 +232,9 @@ public boolean stalled() {
 
   public boolean fixableStuckCoral() {
     return laserSubsystem.getDistance() > TagServoingConstants.kCoralStuckDistance
-        && laserSubsystem.getDistance() < TagServoingConstants.kUnfixableCoralStuckDistance && stalled();
+        && laserSubsystem.getDistance() < TagServoingConstants.kUnfixableCoralStuckDistance
+        && FastMath.abs(driveOmega.getError()) < TagServoingConstants.kAngleCloseEnough
+        && stalled();
   }
 
   public boolean isAligned() {