From b0a9859817f7e4104d52d694f8ec7781edb7b68f Mon Sep 17 00:00:00 2001 From: Isaac Hoekstra <122327870+PotatoBoyH4@users.noreply.github.com> Date: Tue, 10 Jun 2025 18:35:13 -0400 Subject: [PATCH 1/5] added some of the rangefinder logic --- src/main/java/frc/robot/RobotContainer.java | 8 +++++++ .../frc/robot/constants/LaserConstants.java | 12 ++++++++++ .../robot/constants/TagServoingConstants.java | 4 +--- .../frc/robot/subsystems/laser/LaserIO.java | 7 ++++++ .../subsystems/laser/LaserIOCANRange.java | 23 ++++++++++++++++++ .../subsystems/laser/LaserSubsystem.java | 24 +++++++++++++++++++ .../tagAlign/TagAlignSubsystem.java | 17 ++++++++++--- 7 files changed, 89 insertions(+), 6 deletions(-) create mode 100644 src/main/java/frc/robot/constants/LaserConstants.java create mode 100644 src/main/java/frc/robot/subsystems/laser/LaserIO.java create mode 100644 src/main/java/frc/robot/subsystems/laser/LaserIOCANRange.java create mode 100644 src/main/java/frc/robot/subsystems/laser/LaserSubsystem.java diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 7a9da27b..bb5d5300 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/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,6 +205,9 @@ 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); diff --git a/src/main/java/frc/robot/constants/LaserConstants.java b/src/main/java/frc/robot/constants/LaserConstants.java new file mode 100644 index 00000000..14f994a0 --- /dev/null +++ b/src/main/java/frc/robot/constants/LaserConstants.java @@ -0,0 +1,12 @@ +package frc.robot.constants; + +import com.ctre.phoenix6.configs.CANrangeConfiguration; + +public class LaserConstants { + public static final int LaserCanId = 0; + + public static CANrangeConfiguration getLaserConfig() { + CANrangeConfiguration config = new CANrangeConfiguration(); + return config; + } +} diff --git a/src/main/java/frc/robot/constants/TagServoingConstants.java b/src/main/java/frc/robot/constants/TagServoingConstants.java index b4c422ed..8e2ab5b1 100644 --- a/src/main/java/frc/robot/constants/TagServoingConstants.java +++ b/src/main/java/frc/robot/constants/TagServoingConstants.java @@ -95,10 +95,8 @@ 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 kCoralStuckDistance = 0.03; // Reef public static final Translation2d kBlueReefPose = new Translation2d(4.489323, 4.0259); diff --git a/src/main/java/frc/robot/subsystems/laser/LaserIO.java b/src/main/java/frc/robot/subsystems/laser/LaserIO.java new file mode 100644 index 00000000..e35b6b37 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/laser/LaserIO.java @@ -0,0 +1,7 @@ +package frc.robot.subsystems.laser; + +public interface LaserIO { + public void LaserIO(); + + public double getDistanceMeters(); +} diff --git a/src/main/java/frc/robot/subsystems/laser/LaserIOCANRange.java b/src/main/java/frc/robot/subsystems/laser/LaserIOCANRange.java new file mode 100644 index 00000000..9ea14dd9 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/laser/LaserIOCANRange.java @@ -0,0 +1,23 @@ +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 void LaserIO() { + laser = new CANrange(LaserConstants.LaserCanId); + laser.getConfigurator().apply(LaserConstants.getLaserConfig()); + } + + public double getDistanceMeters() { + if (laser.getIsDetected().getValue()) { + return laser.getDistance().getValue().in(Meters); + } else { + return -1.0; + } + } +} diff --git a/src/main/java/frc/robot/subsystems/laser/LaserSubsystem.java b/src/main/java/frc/robot/subsystems/laser/LaserSubsystem.java new file mode 100644 index 00000000..cf764555 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/laser/LaserSubsystem.java @@ -0,0 +1,24 @@ +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 laserio; + + public LaserSubsystem(LaserIO laserio) { + this.laserio = laserio; + } + + public double getDistance() { + return laserio.getDistanceMeters(); + } + + @Override + public Set getMeasures() { + return Set.of( + new Measure("distance", "Distance measured by the laser in meters", () -> getDistance())); + } +} diff --git a/src/main/java/frc/robot/subsystems/tagAlign/TagAlignSubsystem.java b/src/main/java/frc/robot/subsystems/tagAlign/TagAlignSubsystem.java index 1020718b..31dec912 100644 --- a/src/main/java/frc/robot/subsystems/tagAlign/TagAlignSubsystem.java +++ b/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,13 @@ 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 +215,10 @@ public double getErrYaw() { return yawError; } + public boolean getBehindCoral() { + return behindCoral; + } + public boolean isFinalDrive() { return finalDrive; } @@ -220,8 +228,7 @@ public boolean stalled() { } public boolean fixableStuckCoral() { - return FastMath.abs(getCurRadius() - TagServoingConstants.kCoralStuckRadius) - < TagServoingConstants.kCoralStuckAllowence; + return laserSubsystem.getDistance() < TagServoingConstants.kCoralStuckDistance; } public boolean isAligned() { @@ -437,6 +444,9 @@ public void periodic() { visionSubsystem.setUsingRightCam(false); } } + } else if (stalled() && fixableStuckCoral() && FastMath.abs(alignY.getError()) < driveYCloseEnough) { + finalDrive = true; + behindCoral = true; } if (finalDrive && driveSubsystem.getAvgDriveCurrent() @@ -505,6 +515,7 @@ public enum TagAlignStates { DRIVE, WAITING, TAG_ALIGN, + DONE } } From 768dfa917492088861ce66f2dc9a0e1c4e22db88 Mon Sep 17 00:00:00 2001 From: Isaac Hoekstra <122327870+PotatoBoyH4@users.noreply.github.com> Date: Tue, 17 Jun 2025 19:58:22 -0400 Subject: [PATCH 2/5] put the code on a sob, it worked, messed with constants, removed useless logger message --- README.md | 3 ++- src/main/java/frc/robot/RobotContainer.java | 3 ++- .../java/frc/robot/constants/LaserConstants.java | 7 ++++++- .../java/frc/robot/subsystems/laser/LaserIO.java | 1 - .../robot/subsystems/laser/LaserIOCANRange.java | 2 +- .../robot/subsystems/pathHandler/PathHandler.java | 1 - .../subsystems/tagAlign/TagAlignSubsystem.java | 15 ++++++++++----- 7 files changed, 21 insertions(+), 11 deletions(-) diff --git a/README.md b/README.md index 90d4afad..f6058597 100644 --- a/README.md +++ b/README.md @@ -36,7 +36,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) | - | - | | | | | diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index bb5d5300..46875561 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -210,7 +210,7 @@ public RobotContainer() { visionSubsystem = new VisionSubsystem(driveSubsystem); - tagAlignSubsystem = new TagAlignSubsystem(driveSubsystem, visionSubsystem); + tagAlignSubsystem = new TagAlignSubsystem(driveSubsystem, visionSubsystem, laserSubsystem); bargeAlignSubsystem = new BargeAlignSubsystem(flysky, driveSubsystem); robotStateSubsystem = @@ -308,6 +308,7 @@ private void configureTelemetry() { biscuitSubsystem.registerWith(telemetryService); ledSubsystem.registerWith(telemetryService); climbSubsystem.registerWith(telemetryService); + laserSubsystem.registerWith(telemetryService); telemetryService.start(); } diff --git a/src/main/java/frc/robot/constants/LaserConstants.java b/src/main/java/frc/robot/constants/LaserConstants.java index 14f994a0..7c38ff64 100644 --- a/src/main/java/frc/robot/constants/LaserConstants.java +++ b/src/main/java/frc/robot/constants/LaserConstants.java @@ -1,12 +1,17 @@ package frc.robot.constants; import com.ctre.phoenix6.configs.CANrangeConfiguration; +import com.ctre.phoenix6.signals.UpdateModeValue; public class LaserConstants { - public static final int LaserCanId = 0; + 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); return config; } } diff --git a/src/main/java/frc/robot/subsystems/laser/LaserIO.java b/src/main/java/frc/robot/subsystems/laser/LaserIO.java index e35b6b37..c1b373c6 100644 --- a/src/main/java/frc/robot/subsystems/laser/LaserIO.java +++ b/src/main/java/frc/robot/subsystems/laser/LaserIO.java @@ -1,7 +1,6 @@ package frc.robot.subsystems.laser; public interface LaserIO { - public void LaserIO(); public double getDistanceMeters(); } diff --git a/src/main/java/frc/robot/subsystems/laser/LaserIOCANRange.java b/src/main/java/frc/robot/subsystems/laser/LaserIOCANRange.java index 9ea14dd9..21cf5762 100644 --- a/src/main/java/frc/robot/subsystems/laser/LaserIOCANRange.java +++ b/src/main/java/frc/robot/subsystems/laser/LaserIOCANRange.java @@ -8,7 +8,7 @@ public class LaserIOCANRange implements LaserIO { private CANrange laser; - public void LaserIO() { + public LaserIOCANRange() { laser = new CANrange(LaserConstants.LaserCanId); laser.getConfigurator().apply(LaserConstants.getLaserConfig()); } diff --git a/src/main/java/frc/robot/subsystems/pathHandler/PathHandler.java b/src/main/java/frc/robot/subsystems/pathHandler/PathHandler.java index c1bbd06e..5646f911 100644 --- a/src/main/java/frc/robot/subsystems/pathHandler/PathHandler.java +++ b/src/main/java/frc/robot/subsystems/pathHandler/PathHandler.java @@ -172,7 +172,6 @@ public void reassignAlliance() { mirrorTrajectory = driveSubsystem.shouldFlip(); Optional> temp; for (int i = 0; i < 12; i++) { - logger.info(i + ""); temp = Choreo.loadTrajectory(pathNames[0][i]); if (!temp.isEmpty()) { fetchPaths.add(temp.get()); diff --git a/src/main/java/frc/robot/subsystems/tagAlign/TagAlignSubsystem.java b/src/main/java/frc/robot/subsystems/tagAlign/TagAlignSubsystem.java index 31dec912..e9ee73d7 100644 --- a/src/main/java/frc/robot/subsystems/tagAlign/TagAlignSubsystem.java +++ b/src/main/java/frc/robot/subsystems/tagAlign/TagAlignSubsystem.java @@ -56,7 +56,10 @@ public class TagAlignSubsystem extends MeasurableSubsystem { private boolean behindCoral = false; private boolean justAlgae = false; - public TagAlignSubsystem(DriveSubsystem driveSubsystem, VisionSubsystem visionSubsystem, LaserSubsystem laserSubsystem) { + public TagAlignSubsystem( + DriveSubsystem driveSubsystem, + VisionSubsystem visionSubsystem, + LaserSubsystem laserSubsystem) { this.driveSubsystem = driveSubsystem; this.visionSubsystem = visionSubsystem; this.laserSubsystem = laserSubsystem; @@ -431,6 +434,11 @@ public void periodic() { 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) @@ -444,9 +452,6 @@ public void periodic() { visionSubsystem.setUsingRightCam(false); } } - } else if (stalled() && fixableStuckCoral() && FastMath.abs(alignY.getError()) < driveYCloseEnough) { - finalDrive = true; - behindCoral = true; } if (finalDrive && driveSubsystem.getAvgDriveCurrent() @@ -515,7 +520,7 @@ public enum TagAlignStates { DRIVE, WAITING, TAG_ALIGN, - + DONE } } From a23d65ccc0e6ee979470fe02478cd6f07d1bcb39 Mon Sep 17 00:00:00 2001 From: Isaac Hoekstra <122327870+PotatoBoyH4@users.noreply.github.com> Date: Tue, 19 Aug 2025 20:06:38 -0400 Subject: [PATCH 3/5] Update LaserConstants.java tiny leetle constant change --- src/main/java/frc/robot/constants/LaserConstants.java | 1 + 1 file changed, 1 insertion(+) diff --git a/src/main/java/frc/robot/constants/LaserConstants.java b/src/main/java/frc/robot/constants/LaserConstants.java index 7c38ff64..547a6f05 100644 --- a/src/main/java/frc/robot/constants/LaserConstants.java +++ b/src/main/java/frc/robot/constants/LaserConstants.java @@ -12,6 +12,7 @@ public static CANrangeConfiguration getLaserConfig() { 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; } } From aaaabd8c76579293ba7d83f1cffacab59b21c776 Mon Sep 17 00:00:00 2001 From: Isaac Hoekstra <122327870+PotatoBoyH4@users.noreply.github.com> Date: Thu, 4 Sep 2025 20:13:00 -0400 Subject: [PATCH 4/5] state logic works, need to test --- .../robot/constants/TagServoingConstants.java | 3 +- .../robotState/RobotStateSubsystem.java | 38 +++++++++++++++++-- .../tagAlign/TagAlignSubsystem.java | 4 +- 3 files changed, 39 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/robot/constants/TagServoingConstants.java b/src/main/java/frc/robot/constants/TagServoingConstants.java index 5e3f733c..2d3b79b2 100644 --- a/src/main/java/frc/robot/constants/TagServoingConstants.java +++ b/src/main/java/frc/robot/constants/TagServoingConstants.java @@ -96,7 +96,8 @@ public class TagServoingConstants { // Stuck coral public static final double kMinStuckCounts = 5; - public static final double kCoralStuckDistance = 0.03; + public static final double kCoralStuckDistance = 0.21; + public static final double kUnfixableCoralStuckDistance = 0.34; // Reef public static final Translation2d kBlueReefPose = diff --git a/src/main/java/frc/robot/subsystems/robotState/RobotStateSubsystem.java b/src/main/java/frc/robot/subsystems/robotState/RobotStateSubsystem.java index 47cbd1aa..55c66e98 100644 --- a/src/main/java/frc/robot/subsystems/robotState/RobotStateSubsystem.java +++ b/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,37 @@ public void periodic() { } } */ - if ((isAutoPlacing + if (tagAlignSubsystem.fixableStuckCoral() == true) { + switch (scoringLevel) { + case L1 -> { + setBiscuitTransferSlow(RobotConstants.kL1CoralSetpoint, true); + elevatorSubsystem.setPosition( + ElevatorConstants.kL1CoralSetpoint.plus( + RobotStateConstants.kStuckCoralElevatorOffset)); + } + case L2 -> { + setBiscuitTransfer(RobotConstants.kL2CoralSetpoint, true); + elevatorSubsystem.setPosition( + ElevatorConstants.kL2CoralSetpoint.plus( + RobotStateConstants.kStuckCoralElevatorOffset)); + } + case L3 -> { + setBiscuitTransfer(RobotConstants.kL3CoralSetpoint, true); + elevatorSubsystem.setPosition( + ElevatorConstants.kL3CoralSetpoint.plus( + RobotStateConstants.kStuckCoralElevatorOffset)); + } + case L4 -> { + setBiscuitTransfer(RobotConstants.kL4CoralSetpoint, true); + elevatorSubsystem.setPosition( + ElevatorConstants.kL4CoralSetpoint.plus( + RobotStateConstants.kStuckCoralElevatorOffset)); + } + } + tagAlignSubsystem.terminate(); + setState(RobotStates.PLACE_CORAL, true); + reefCoralStuckFixable = tagAlignSubsystem.fixableStuckCoral(); + } else if ((isAutoPlacing && (tagAlignSubsystem.getState() == TagAlignSubsystem.TagAlignStates.DONE /* || reefCoralStuckFixable && tagAlignSubsystem.isAligned() @@ -1053,7 +1084,6 @@ public void periodic() { toPlaceCoral(); // tagAlignSubsystem.terminate(); isAutoReadyForEject = false; - reefCoralStuckFixable = false; } } case REMOVE_ALGAE -> { @@ -1078,7 +1108,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); diff --git a/src/main/java/frc/robot/subsystems/tagAlign/TagAlignSubsystem.java b/src/main/java/frc/robot/subsystems/tagAlign/TagAlignSubsystem.java index cbbc82c6..b3073056 100644 --- a/src/main/java/frc/robot/subsystems/tagAlign/TagAlignSubsystem.java +++ b/src/main/java/frc/robot/subsystems/tagAlign/TagAlignSubsystem.java @@ -231,7 +231,8 @@ public boolean stalled() { } public boolean fixableStuckCoral() { - return laserSubsystem.getDistance() < TagServoingConstants.kCoralStuckDistance; + return laserSubsystem.getDistance() > TagServoingConstants.kCoralStuckDistance + && laserSubsystem.getDistance() < TagServoingConstants.kUnfixableCoralStuckDistance && stalled(); } public boolean isAligned() { @@ -528,7 +529,6 @@ public enum TagAlignStates { DRIVE, WAITING, TAG_ALIGN, - DONE } } From b396db68526c92215c9bff1eb1e61b90b29e6e05 Mon Sep 17 00:00:00 2001 From: Isaac Hoekstra <122327870+PotatoBoyH4@users.noreply.github.com> Date: Thu, 11 Sep 2025 20:03:46 -0400 Subject: [PATCH 5/5] scoring over coral works now --- .../robot/constants/TagServoingConstants.java | 4 ++-- .../frc/robot/subsystems/laser/LaserIO.java | 9 +++++++++ .../subsystems/laser/LaserIOCANRange.java | 5 +++++ .../subsystems/laser/LaserSubsystem.java | 13 +++++++++--- .../robotState/RobotStateSubsystem.java | 20 ++++++------------- .../tagAlign/TagAlignSubsystem.java | 4 +++- 6 files changed, 35 insertions(+), 20 deletions(-) diff --git a/src/main/java/frc/robot/constants/TagServoingConstants.java b/src/main/java/frc/robot/constants/TagServoingConstants.java index 2d3b79b2..5abc2390 100644 --- a/src/main/java/frc/robot/constants/TagServoingConstants.java +++ b/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 diff --git a/src/main/java/frc/robot/subsystems/laser/LaserIO.java b/src/main/java/frc/robot/subsystems/laser/LaserIO.java index c1b373c6..018bba06 100644 --- a/src/main/java/frc/robot/subsystems/laser/LaserIO.java +++ b/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(); } diff --git a/src/main/java/frc/robot/subsystems/laser/LaserIOCANRange.java b/src/main/java/frc/robot/subsystems/laser/LaserIOCANRange.java index 21cf5762..8d485e51 100644 --- a/src/main/java/frc/robot/subsystems/laser/LaserIOCANRange.java +++ b/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); diff --git a/src/main/java/frc/robot/subsystems/laser/LaserSubsystem.java b/src/main/java/frc/robot/subsystems/laser/LaserSubsystem.java index cf764555..716e771a 100644 --- a/src/main/java/frc/robot/subsystems/laser/LaserSubsystem.java +++ b/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 diff --git a/src/main/java/frc/robot/subsystems/robotState/RobotStateSubsystem.java b/src/main/java/frc/robot/subsystems/robotState/RobotStateSubsystem.java index 55c66e98..b859a7fa 100644 --- a/src/main/java/frc/robot/subsystems/robotState/RobotStateSubsystem.java +++ b/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 diff --git a/src/main/java/frc/robot/subsystems/tagAlign/TagAlignSubsystem.java b/src/main/java/frc/robot/subsystems/tagAlign/TagAlignSubsystem.java index b3073056..0d28cc44 100644 --- a/src/main/java/frc/robot/subsystems/tagAlign/TagAlignSubsystem.java +++ b/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() {