diff --git a/README.md b/README.md index 8f9c53a..dd91f2e 100644 --- a/README.md +++ b/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) | - | - | | | | | diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 7a9da27..4687556 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,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(); } 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 0000000..547a6f0 --- /dev/null +++ b/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; + } +} diff --git a/src/main/java/frc/robot/constants/TagServoingConstants.java b/src/main/java/frc/robot/constants/TagServoingConstants.java index 0061dc8..5abc239 100644 --- a/src/main/java/frc/robot/constants/TagServoingConstants.java +++ b/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 = 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 0000000..018bba0 --- /dev/null +++ b/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(); +} 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 0000000..8d485e5 --- /dev/null +++ b/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; + } + } +} 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 0000000..716e771 --- /dev/null +++ b/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 getMeasures() { + return Set.of( + new Measure("distance", "Distance measured by the laser in meters", () -> getDistance())); + } +} diff --git a/src/main/java/frc/robot/subsystems/pathHandler/PathHandler.java b/src/main/java/frc/robot/subsystems/pathHandler/PathHandler.java index c1bbd06..5646f91 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/robotState/RobotStateSubsystem.java b/src/main/java/frc/robot/subsystems/robotState/RobotStateSubsystem.java index 47cbd1a..b859a7f 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,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); diff --git a/src/main/java/frc/robot/subsystems/tagAlign/TagAlignSubsystem.java b/src/main/java/frc/robot/subsystems/tagAlign/TagAlignSubsystem.java index a3b1eba..0d28cc4 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,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)