Can range

README.md

@@ -52,7 +52,8 @@
 | Funnel    | FXS      | rollers                   | 40  | rio     |          |           | Minion |         |
 | Climb     | FX       | frontMain                 | 45  | rio     |          |           | Minion |         |
 | Climb     | FX       | backFollow                | 46  | rio     |          |           | kraken |         |
-| Climb     | CANcoder | CANCoder                  | 47  | ri0     |          |           | n/a    |         |
+| Climb     | CANcoder | CANCoder                  | 47  | rio     |          |           | n/a    |         |
+| Laser     | CANRange | CANRange                  | 50  | rio     |          |           | n/a    |         |
 | -         | -        | rio                       | -   | both    |          | 12        |        |         |
 | -         | -        | vrm (radio, pigeon)       | -   | -       |          | 13        |        |         |
 | -         | -        | custom circuit (pi power) | -   | -       |          |           |        |         |

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

@@ -91,6 +91,8 @@
 import frc.robot.subsystems.elevator.ElevatorSubsystem.ElevatorStates;
 import frc.robot.subsystems.funnel.FunnelIOFXS;
 import frc.robot.subsystems.funnel.FunnelSubsystem;
+import frc.robot.subsystems.laser.LaserIOCANRange;
+import frc.robot.subsystems.laser.LaserSubsystem;
 import frc.robot.subsystems.led.LEDIO;
 import frc.robot.subsystems.led.LEDSubsystem;
 import frc.robot.subsystems.pathHandler.PathHandler;
@@ -138,6 +140,9 @@ public class RobotContainer {
   private final LEDIO ledIO;
   private final LEDSubsystem ledSubsystem;
 
+  private final LaserIOCANRange laserIO;
+  private final LaserSubsystem laserSubsystem;
+
   private final TagAlignSubsystem tagAlignSubsystem;
   private final BargeAlignSubsystem bargeAlignSubsystem;
 
@@ -200,9 +205,12 @@ public RobotContainer() {
     ledIO = new LEDIO();
     ledSubsystem = new LEDSubsystem(ledIO);
 
+    laserIO = new LaserIOCANRange();
+    laserSubsystem = new LaserSubsystem(laserIO);
+
     visionSubsystem = new VisionSubsystem(driveSubsystem);
 
-    tagAlignSubsystem = new TagAlignSubsystem(driveSubsystem, visionSubsystem);
+    tagAlignSubsystem = new TagAlignSubsystem(driveSubsystem, visionSubsystem, laserSubsystem);
     bargeAlignSubsystem = new BargeAlignSubsystem(flysky, driveSubsystem);
 
     robotStateSubsystem =
@@ -300,6 +308,7 @@ private void configureTelemetry() {
     biscuitSubsystem.registerWith(telemetryService);
     ledSubsystem.registerWith(telemetryService);
     climbSubsystem.registerWith(telemetryService);
+    laserSubsystem.registerWith(telemetryService);
     telemetryService.start();
   }
 

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

@@ -0,0 +1,18 @@
+package frc.robot.constants;
+
+import com.ctre.phoenix6.configs.CANrangeConfiguration;
+import com.ctre.phoenix6.signals.UpdateModeValue;
+
+public class LaserConstants {
+  public static final int LaserCanId = 50;
+
+  public static CANrangeConfiguration getLaserConfig() {
+    CANrangeConfiguration config = new CANrangeConfiguration();
+    config.ToFParams =
+        config.ToFParams.withUpdateMode(UpdateModeValue.LongRangeUserFreq).withUpdateFrequency(50);
+    config.ProximityParams = config.ProximityParams.withProximityThreshold(1.0);
+    config.FovParams = config.FovParams.withFOVRangeX(10.0).withFOVRangeY(10.0);
+    config.ToFParams = config.ToFParams.withUpdateFrequency(25.0);
+    return config;
+  }
+}

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

@@ -95,10 +95,9 @@ public class TagServoingConstants {
   public static final double kEndVelThreshold = 5;
 
   // Stuck coral
-  public static final double kMaxStalledDer = 0.5;
-  public static final double kMinStuckCounts = 5;
-  public static final double kCoralStuckRadius = 1.4;
-  public static final double kCoralStuckAllowence = 0.03;
+  public static final double kMinStuckCounts = 3;
+  public static final double kCoralStuckDistance = 0.19;
+  public static final double kUnfixableCoralStuckDistance = 0.34;
 
   // Reef
   public static final Translation2d kBlueReefPose =

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

@@ -0,0 +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

@@ -0,0 +1,28 @@
+package frc.robot.subsystems.laser;
+
+import static edu.wpi.first.units.Units.Meters;
+
+import com.ctre.phoenix6.hardware.CANrange;
+import frc.robot.constants.LaserConstants;
+
+public class LaserIOCANRange implements LaserIO {
+  private CANrange laser;
+
+  public LaserIOCANRange() {
+    laser = new CANrange(LaserConstants.LaserCanId);
+    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);
+    } else {
+      return -1.0;
+    }
+  }
+}

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

@@ -0,0 +1,31 @@
+package frc.robot.subsystems.laser;
+
+import java.util.Set;
+import org.strykeforce.telemetry.measurable.MeasurableSubsystem;
+import org.strykeforce.telemetry.measurable.Measure;
+
+public class LaserSubsystem extends MeasurableSubsystem {
+
+  private final LaserIO io;
+  private final LaserIOInputsAutoLogged inputs = new LaserIOInputsAutoLogged();
+
+  public LaserSubsystem(LaserIO laserio) {
+    this.io = laserio;
+  }
+
+  public double getDistance() {
+    return io.getDistanceMeters();
+  }
+
+  @Override
+  public void periodic() {
+    io.updateInputs(inputs);
+    org.littletonrobotics.junction.Logger.processInputs("LaserInputs", inputs);
+  }
+
+  @Override
+  public Set<Measure> getMeasures() {
+    return Set.of(
+        new Measure("distance", "Distance measured by the laser in meters", () -> getDistance()));
+  }
+}

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

@@ -172,7 +172,6 @@ public void reassignAlliance() {
     mirrorTrajectory = driveSubsystem.shouldFlip();
     Optional<Trajectory<SwerveSample>> temp;
     for (int i = 0; i < 12; i++) {
-      logger.info(i + "");
       temp = Choreo.loadTrajectory(pathNames[0][i]);
       if (!temp.isEmpty()) {
         fetchPaths.add(temp.get());

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

@@ -673,6 +673,7 @@ public void toPlaceCoral() {
     scoringTimer.stop();
     scoringTimer.reset();
     scoringTimer.start();
+    reefCoralStuckFixable = false;
     setState(RobotStates.PLACE_CORAL);
   }
 
@@ -1043,7 +1044,29 @@ public void periodic() {
           }
         } */
 
-        if ((isAutoPlacing
+        if (tagAlignSubsystem.fixableStuckCoral() == true) {
+          switch (scoringLevel) {
+            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 -> {}
+          }
+          tagAlignSubsystem.terminate();
+        } else if ((isAutoPlacing
                 && (tagAlignSubsystem.getState() == TagAlignSubsystem.TagAlignStates.DONE
                 /*  || reefCoralStuckFixable
                 && tagAlignSubsystem.isAligned()
@@ -1053,7 +1076,6 @@ public void periodic() {
           toPlaceCoral();
           // tagAlignSubsystem.terminate();
           isAutoReadyForEject = false;
-          reefCoralStuckFixable = false;
         }
       }
       case REMOVE_ALGAE -> {
@@ -1078,7 +1100,9 @@ public void periodic() {
       }
 
       case PLACE_CORAL -> {
-        if (!hasCoral() && scoringTimer.hasElapsed(RobotStateConstants.kCoralEjectTimer)) {
+        if (reefCoralStuckFixable) {
+          toPlaceCoral();
+        } else if (!hasCoral() && scoringTimer.hasElapsed(RobotStateConstants.kCoralEjectTimer)) {
 
           coralLoc = CoralLoc.NONE;
           visionSubsystem.setYawUpdateCamera(-1);

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

@@ -9,6 +9,7 @@
 import frc.robot.constants.DriveConstants;
 import frc.robot.constants.TagServoingConstants;
 import frc.robot.subsystems.drive.DriveSubsystem;
+import frc.robot.subsystems.laser.LaserSubsystem;
 import frc.robot.subsystems.robotState.RobotStateSubsystem.ScoringLevel;
 import frc.robot.subsystems.vision.VisionSubsystem;
 import java.util.Set;
@@ -22,6 +23,7 @@ public class TagAlignSubsystem extends MeasurableSubsystem {
   private static final org.slf4j.Logger logger = LoggerFactory.getLogger(DriveSubsystem.class);
   private DriveSubsystem driveSubsystem;
   private VisionSubsystem visionSubsystem;
+  private LaserSubsystem laserSubsystem;
 
   private PIDController driveX;
   private PIDController driveY;
@@ -51,11 +53,16 @@ public class TagAlignSubsystem extends MeasurableSubsystem {
   private double coralOffset = 0;
   private double noProgressCounts = 0;
   private double yAutoOffset = 0;
+  private boolean behindCoral = false;
   private boolean justAlgae = false;
 
-  public TagAlignSubsystem(DriveSubsystem driveSubsystem, VisionSubsystem visionSubsystem) {
+  public TagAlignSubsystem(
+      DriveSubsystem driveSubsystem,
+      VisionSubsystem visionSubsystem,
+      LaserSubsystem laserSubsystem) {
     this.driveSubsystem = driveSubsystem;
     this.visionSubsystem = visionSubsystem;
+    this.laserSubsystem = laserSubsystem;
 
     this.driveX = new PIDController(4, 0, 0);
     this.driveY = new PIDController(4, 0, 0);
@@ -211,6 +218,10 @@ public double getErrYaw() {
     return yawError;
   }
 
+  public boolean getBehindCoral() {
+    return behindCoral;
+  }
+
   public boolean isFinalDrive() {
     return finalDrive;
   }
@@ -220,8 +231,10 @@ public boolean stalled() {
   }
 
   public boolean fixableStuckCoral() {
-    return FastMath.abs(getCurRadius() - TagServoingConstants.kCoralStuckRadius)
-        < TagServoingConstants.kCoralStuckAllowence;
+    return laserSubsystem.getDistance() > TagServoingConstants.kCoralStuckDistance
+        && laserSubsystem.getDistance() < TagServoingConstants.kUnfixableCoralStuckDistance
+        && FastMath.abs(driveOmega.getError()) < TagServoingConstants.kAngleCloseEnough
+        && stalled();
   }
 
   public boolean isAligned() {
@@ -432,6 +445,18 @@ public void periodic() {
             case TAG_ALIGN -> {
               if (justAlgae && (FastMath.abs(alignY.getError()) < driveYCloseEnough)) {
                 finalDrive = true;
+                if (scoreLeft) {
+                  visionSubsystem.setUsingLeftCam(false);
+                  visionSubsystem.setUsingRightCam(true);
+                } else {
+                  visionSubsystem.setUsingLeftCam(true);
+                  visionSubsystem.setUsingRightCam(false);
+                }
+              } else if (stalled()
+                  && fixableStuckCoral()
+                  && FastMath.abs(alignY.getError()) < driveYCloseEnough) {
+                finalDrive = true;
+                behindCoral = true;
               } else {
                 if ((
                     /*isAuto ? true :*/ FastMath.abs(alignX.getError()) < driveXCloseEnough)