diff --git a/ascopeAssets/Robot_Offseason/config.json b/ascopeAssets/Robot_Offseason/config.json index 85e25e0b..d77295ef 100644 --- a/ascopeAssets/Robot_Offseason/config.json +++ b/ascopeAssets/Robot_Offseason/config.json @@ -1,3 +1,159 @@ { - "name": "offseason" + "name": "butch", + "sourceUrl": "https://cad.onshape.com/documents/8f873ac3a74d8fd06cda742a/w/88e5df6f1994d26987d1f315/e/eb5a3c451ca2580d6f4169f7", + "rotations": [ + { + "axis": "x", + "degrees": 90 + }, + { + "axis": "z", + "degrees": 0 + } + ], + "position": [ + 0, + 0, + 0.04 + ], + "cameras": [ + { + "name": "FrontCam", + "position": [ + 0.3, + 0.0, + 0.2 + ], + "fov": 59.99999999999999, + "rotations": [ + { + "axis": "y", + "degrees": -29.999999999999996 + }, + { + "axis": "z", + "degrees": 0.0 + } + ], + "resolution": [ + 640, + 480 + ] + }, + { + "name": "FrontLeftCam", + "position": [ + 0.25, + 0.3, + 0.2 + ], + "fov": 59.99999999999999, + "rotations": [ + { + "axis": "y", + "degrees": -30.000000000000004 + }, + { + "axis": "z", + "degrees": 29.999999999999996 + } + ], + "resolution": [ + 640, + 480 + ] + }, + { + "name": "FrontRightCam", + "position": [ + 0.25, + -0.3, + 0.2 + ], + "fov": 59.99999999999999, + "rotations": [ + { + "axis": "y", + "degrees": -29.999999999999996 + }, + { + "axis": "z", + "degrees": -30.000000000000004 + } + ], + "resolution": [ + 640, + 480 + ] + }, + { + "name": "BackLeftCam", + "position": [ + 0.25, + 0.05, + 0.2 + ], + "fov": 59.99999999999999, + "rotations": [ + { + "axis": "y", + "degrees": -35.000000000000014 + }, + { + "axis": "z", + "degrees": 150.0 + } + ], + "resolution": [ + 640, + 480 + ] + }, + { + "name": "BackRightCam", + "position": [ + 0.25, + -0.05, + 0.2 + ], + "fov": 59.99999999999999, + "rotations": [ + { + "axis": "y", + "degrees": -35.00000000000001 + }, + { + "axis": "z", + "degrees": -150.0 + } + ], + "resolution": [ + 640, + 480 + ] + } + ], + "components": [ + { + "zeroedRotations": [ + { + "axis": "x", + "degrees": 180 + }, + { + "axis": "y", + "degrees": 60 + }, + { + "axis": "z", + "degrees": 0 + } + ], + "zeroedPosition": [ + 0.25, + 0, + -0.02 + ] + } + ] } diff --git a/ascopeAssets/Robot_Offseason/model.glb b/ascopeAssets/Robot_Offseason/model.glb index ab693a64..8da9ea59 100644 Binary files a/ascopeAssets/Robot_Offseason/model.glb and b/ascopeAssets/Robot_Offseason/model.glb differ diff --git a/src/main/java/frc/robot/BuildConstants.java b/src/main/java/frc/robot/BuildConstants.java new file mode 100644 index 00000000..fcb85e86 --- /dev/null +++ b/src/main/java/frc/robot/BuildConstants.java @@ -0,0 +1,17 @@ +package frc.robot; + +/** Automatically generated file containing build version information. */ +public final class BuildConstants { + public static final String MAVEN_GROUP = ""; + public static final String MAVEN_NAME = "offseason-robot-code-2024-1"; + public static final String VERSION = "unspecified"; + public static final int GIT_REVISION = 534; + public static final String GIT_SHA = "bf661f9f8d1233875f154280c54da2e4c3473fed"; + public static final String GIT_DATE = "2025-01-03 21:20:40 EST"; + public static final String GIT_BRANCH = "vision-sim"; + public static final String BUILD_DATE = "2025-01-04 02:29:02 EST"; + public static final long BUILD_UNIX_TIME = 1735975742416L; + public static final int DIRTY = 1; + + private BuildConstants() {} +} diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 56c8bbd7..38742b18 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -6,15 +6,15 @@ import frc.robot.subsystems.swerve.SwerveConstants.DriveConstants; import frc.robot.subsystems.swerve.SwerveConstants.ModuleConstants; +/** Values are statically stored here used globally throughout the code. */ public final class Constants { public static final class LogPaths { public static final String SYSTEM_PERFORMANCE_PATH = "SystemPerformance/"; public static final String PHYSICS_SIMULATION_PATH = "MaplePhysicsSimulation/"; - public static final String APRIL_TAGS_VISION_PATH = "Vision/AprilTags/"; } - public static final Mode currentMode = Mode.SIM; + public static final Mode CURRENT_MODE = Mode.SIM; public static enum Mode { /** Running on a real robot. */ @@ -133,8 +133,8 @@ public class SimulationConstants { public static final class FieldConstants { // TODO: Now that I think about it, I'm pretty sure these measurements stay the same every year, // so consider setting them in the base code - public static final double FIELD_LENGTH_METERS = Units.inchesToMeters(0 - 9); - public static final double FIELD_WIDTH_METERS = Units.inchesToMeters(0 - 9); + public static final double FIELD_LENGTH_METERS = Units.inchesToMeters(653); + public static final double FIELD_WIDTH_METERS = Units.inchesToMeters(325); } public static final class JoystickConstants { diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 6086a926..b3a2417a 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -24,27 +24,27 @@ public class Robot extends LoggedRobot { public Robot() { // Record metadata // Logger.recordMetadata("ProjectName", BuildConstants.MAVEN_NAME); - // Logger.recordMetadata("BuildDate", BuildConstants.BUILD_DATE); - // Logger.recordMetadata("GitSHA", BuildConstants.GIT_SHA); - // Logger.recordMetadata("GitDate", BuildConstants.GIT_DATE); - // Logger.recordMetadata("GitBranch", BuildConstants.GIT_BRANCH); - // switch (BuildConstants.DIRTY) { - // case 0: - // Logger.recordMetadata("GitDirty", "All changes committed"); - // break; - // case 1: - // Logger.recordMetadata("GitDirty", "Uncomitted changes"); - // break; - // default: - // Logger.recordMetadata("GitDirty", "Unknown"); - // break; - // } + Logger.recordMetadata("BuildDate", BuildConstants.BUILD_DATE); + Logger.recordMetadata("GitSHA", BuildConstants.GIT_SHA); + Logger.recordMetadata("GitDate", BuildConstants.GIT_DATE); + Logger.recordMetadata("GitBranch", BuildConstants.GIT_BRANCH); + switch (BuildConstants.DIRTY) { + case 0: + Logger.recordMetadata("GitDirty", "All changes committed"); + break; + case 1: + Logger.recordMetadata("GitDirty", "Uncomitted changes"); + break; + default: + Logger.recordMetadata("GitDirty", "Unknown"); + break; + } // Set up data receivers & replay source - switch (Constants.currentMode) { + switch (Constants.CURRENT_MODE) { case REAL: // Running on a real robot, log to a USB stick ("/U/logs") - Logger.addDataReceiver(new WPILOGWriter()); + // Logger.addDataReceiver(new WPILOGWriter()); Logger.addDataReceiver(new NT4Publisher()); break; diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 65f87f71..b3c60e7e 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -25,9 +25,10 @@ import frc.robot.subsystems.swerve.module.ModuleInterface; import frc.robot.subsystems.swerve.module.PhysicalModule; import frc.robot.subsystems.swerve.module.SimulatedModule; +import frc.robot.subsystems.vision.PhysicalVision; +import frc.robot.subsystems.vision.SimulatedVision; import frc.robot.subsystems.vision.Vision; -import frc.robot.subsystems.vision.VisionIO; -import frc.robot.subsystems.vision.VisionIOReal; +import frc.robot.subsystems.vision.VisionInterface; import java.util.function.DoubleSupplier; import org.littletonrobotics.junction.Logger; @@ -51,7 +52,7 @@ public class RobotContainer { // private final XboxController driverController = new XboxController(0); public RobotContainer() { - switch (Constants.currentMode) { + switch (Constants.CURRENT_MODE) { case REAL -> { /* Real robot, instantiate hardware IO implementations */ @@ -67,7 +68,7 @@ public RobotContainer() { new PhysicalModule(SwerveConstants.moduleConfigs[1]), new PhysicalModule(SwerveConstants.moduleConfigs[2]), new PhysicalModule(SwerveConstants.moduleConfigs[3])); - visionSubsystem = new Vision(new VisionIOReal()); + visionSubsystem = new Vision(new PhysicalVision()); } case SIM -> { @@ -85,8 +86,8 @@ public RobotContainer() { DriveConstants.TRACK_WIDTH + .2, DriveConstants.WHEEL_BASE + .2, SwerveModuleSimulation.getModule( - DCMotor.getFalcon500(1), - DCMotor.getFalcon500(1), + DCMotor.getKrakenX60(1).withReduction(ModuleConstants.DRIVE_GEAR_RATIO), + DCMotor.getFalcon500(1).withReduction(11), 60, WHEEL_GRIP.TIRE_WHEEL, ModuleConstants.DRIVE_GEAR_RATIO), @@ -105,8 +106,9 @@ public RobotContainer() { new SimulatedModule(swerveDriveSimulation.getModules()[2]), new SimulatedModule(swerveDriveSimulation.getModules()[3])); - // TODO: add sim impl - visionSubsystem = new Vision(new VisionIO() {}); + visionSubsystem = + new Vision( + new SimulatedVision(() -> swerveDriveSimulation.getSimulatedDriveTrainPose())); SimulatedField.getInstance().resetFieldForAuto(); resetFieldAndOdometryForAuto( @@ -114,7 +116,7 @@ public RobotContainer() { } default -> { - visionSubsystem = new Vision(new VisionIO() {}); + visionSubsystem = new Vision(new VisionInterface() {}); /* Replayed robot, disable IO implementations */ /* physics simulations are also not needed */ @@ -141,7 +143,7 @@ private void resetFieldAndOdometryForAuto(Pose2d robotStartingPoseAtBlueAlliance updateFieldSimAndDisplay(); } - swerveDrive.periodic(); + // swerveDrive.periodic(); swerveDrive.setPose(startingPose); } @@ -164,8 +166,8 @@ private void configureButtonBindings() { DoubleSupplier driverRightStickX = driverController::getRightX; DoubleSupplier driverLeftStick[] = new DoubleSupplier[] { - () -> JoystickUtil.modifyAxisCubedPolar(driverLeftStickX, driverLeftStickY)[0], - () -> JoystickUtil.modifyAxisCubedPolar(driverLeftStickX, driverLeftStickY)[1] + () -> JoystickUtil.modifyAxisPolar(driverLeftStickX, driverLeftStickY, 3)[0], + () -> JoystickUtil.modifyAxisPolar(driverLeftStickX, driverLeftStickY, 3)[1] }; DoubleSupplier operatorLeftStickX = operatorController::getLeftX; @@ -214,7 +216,7 @@ private void configureButtonBindings() { visionSubsystem, driverLeftStick[1], driverLeftStick[0], - () -> JoystickUtil.modifyAxisCubed(driverRightStickX), + () -> JoystickUtil.modifyAxis(driverRightStickX, 3), () -> !driverRightBumper.getAsBoolean(), () -> driverLeftBumper.getAsBoolean()); swerveDrive.setDefaultCommand(driveCommand); @@ -248,9 +250,14 @@ private void configureButtonBindings() { swerveDrive.getPose().getX(), swerveDrive.getPose().getY(), Rotation2d.fromDegrees(swerveDrive.getAllianceAngleOffset()))))); + driverController + .x() + .onTrue( + new InstantCommand( + () -> swerveDrive.setPose(swerveDriveSimulation.getSimulatedDriveTrainPose()))); // // // Reset robot odometry based on vision pose measurement from april tags - // driverLeftDirectionPad.onTrue(new InstantCommand(() -> - // swerveDrive.resetOdometry(visionSubsystem.getLastSeenPose()))); + driverLeftDirectionPad.onTrue( + new InstantCommand(() -> swerveDrive.setPose(visionSubsystem.getLastSeenPose()))); // // driverLeftDpad.onTrue(new InstantCommand(() -> swerveDrive.resetOdometry(new // Pose2d(15.251774787902832, 5.573054313659668, Rotation2d.fromRadians(3.14159265))))); // // driverBButton.whileTrue(new ShootPass(swerveDrive, shooterSubsystem, pivotSubsystem, diff --git a/src/main/java/frc/robot/commands/drive/DriveCommandBase.java b/src/main/java/frc/robot/commands/drive/DriveCommandBase.java index a15af59d..efadafa3 100644 --- a/src/main/java/frc/robot/commands/drive/DriveCommandBase.java +++ b/src/main/java/frc/robot/commands/drive/DriveCommandBase.java @@ -2,12 +2,14 @@ package frc.robot.commands.drive; -import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.extras.interpolators.MultiLinearInterpolator; +import frc.robot.extras.util.TimeUtil; import frc.robot.subsystems.swerve.SwerveDrive; import frc.robot.subsystems.vision.Vision; import frc.robot.subsystems.vision.VisionConstants; +import frc.robot.subsystems.vision.VisionConstants.Limelight; +import org.littletonrobotics.junction.Logger; public abstract class DriveCommandBase extends Command { private final MultiLinearInterpolator oneAprilTagLookupTable = @@ -39,32 +41,28 @@ public void execute() { swerveDrive.addPoseEstimatorSwerveMeasurement(); vision.setHeadingInfo( swerveDrive.getPose().getRotation().getDegrees(), swerveDrive.getGyroRate()); - calculatePoseFromLimelight(VisionConstants.SHOOTER_LIMELIGHT_NUMBER); - calculatePoseFromLimelight(VisionConstants.FRONT_LEFT_LIMELIGHT_NUMBER); - calculatePoseFromLimelight(VisionConstants.FRONT_RIGHT_LIMELIGHT_NUMBER); + calculatePoseFromLimelight(Limelight.SHOOTER); + // calculatePoseFromLimelight(Limelight.FRONT_LEFT); + // calculatePoseFromLimelight(Limelight.FRONT_RIGHT); } - public void calculatePoseFromLimelight(int limelightNumber) { + public void calculatePoseFromLimelight(Limelight limelight) { double currentTimeStampSeconds = lastTimeStampSeconds; // Updates the robot's odometry with april tags - if (vision.canSeeAprilTags(limelightNumber)) { - currentTimeStampSeconds = vision.getTimeStampSeconds(limelightNumber); + if (vision.canSeeAprilTags(limelight)) { + currentTimeStampSeconds = vision.getTimeStampSeconds(limelight); - double distanceFromClosestAprilTag = vision.getLimelightAprilTagDistance(limelightNumber); + double distanceFromClosestAprilTag = vision.getLimelightAprilTagDistance(limelight); // Depending on how many april tags we see, we change our confidence as more april tags // results in a much more accurate pose estimate - // TODO: check if this is necessary anymore with MT2, also we might want to set the limelight - // so it only uses 1 april tag, if they set up the field wrong (they can set april tags +-1 - // inch I believe) - // using multiple *could* really mess things up. - if (vision.getNumberOfAprilTags(limelightNumber) == 1) { + if (vision.getNumberOfAprilTags(limelight) == 1) { double[] standardDeviations = oneAprilTagLookupTable.getLookupValue(distanceFromClosestAprilTag); swerveDrive.setPoseEstimatorVisionConfidence( standardDeviations[0], standardDeviations[1], standardDeviations[2]); - } else if (vision.getNumberOfAprilTags(limelightNumber) > 1) { + } else if (vision.getNumberOfAprilTags(limelight) > 1) { double[] standardDeviations = twoAprilTagLookupTable.getLookupValue(distanceFromClosestAprilTag); swerveDrive.setPoseEstimatorVisionConfidence( @@ -72,10 +70,14 @@ public void calculatePoseFromLimelight(int limelightNumber) { } swerveDrive.addPoseEstimatorVisionMeasurement( - vision.getPoseFromAprilTags(limelightNumber), - Timer.getFPGATimestamp() - vision.getLatencySeconds(limelightNumber)); + vision.getPoseFromAprilTags(limelight), + TimeUtil.getLogTimeSeconds() - vision.getLatencySeconds(limelight)); } + Logger.recordOutput( + "Odometry/CurrentVisionPose" + limelight.getName(), vision.getPoseFromAprilTags(limelight)); + Logger.recordOutput("Odometry/CurrentCalculatedPose", swerveDrive.getPose()); + lastTimeStampSeconds = currentTimeStampSeconds; } } diff --git a/src/main/java/frc/robot/extras/simulation/field/SimulatedField.java b/src/main/java/frc/robot/extras/simulation/field/SimulatedField.java index 317cbc71..e73bc04a 100644 --- a/src/main/java/frc/robot/extras/simulation/field/SimulatedField.java +++ b/src/main/java/frc/robot/extras/simulation/field/SimulatedField.java @@ -13,7 +13,7 @@ import frc.robot.extras.simulation.mechanismSim.swerve.AbstractDriveTrainSimulation; import frc.robot.extras.simulation.mechanismSim.swerve.BrushlessMotorSim; import frc.robot.extras.util.GeomUtil; -import java.lang.ref.WeakReference; +// import java.lang.ref.WeakReference; import java.util.*; import org.dyn4j.dynamics.Body; import org.dyn4j.dynamics.BodyFixture; @@ -130,7 +130,7 @@ public static void overrideSimulationTimings( protected final Set driveTrainSimulations; protected final Set gamePieces; protected final List simulationSubTickActions; - protected final List> motors; + protected final List motors; private final List intakeSimulations; /** @@ -284,7 +284,7 @@ public void simulationPeriodic() { } public void addMotor(BrushlessMotorSim motor) { - motors.add(new WeakReference<>(motor)); + motors.add(motor); } /** @@ -306,7 +306,7 @@ public void addMotor(BrushlessMotorSim motor) { private void simulationSubTick() { ArrayList motorCurrents = new ArrayList<>(); for (var motor : motors) { - BrushlessMotorSim motorRef = motor.get(); + BrushlessMotorSim motorRef = motor; if (motorRef != null) { motorRef.update(); } diff --git a/src/main/java/frc/robot/extras/simulation/mechanismSim/swerve/GyroSimulation.java b/src/main/java/frc/robot/extras/simulation/mechanismSim/swerve/GyroSimulation.java index 74235ba3..0702d5e8 100644 --- a/src/main/java/frc/robot/extras/simulation/mechanismSim/swerve/GyroSimulation.java +++ b/src/main/java/frc/robot/extras/simulation/mechanismSim/swerve/GyroSimulation.java @@ -170,7 +170,7 @@ public static GyroSimulation createPigeon2() { * IMU */ public static GyroSimulation createNavX2() { - return new GyroSimulation(2, 0.04); + return new GyroSimulation(0.00208333333, 0.04); } /** diff --git a/src/main/java/frc/robot/extras/util/GeomUtil.java b/src/main/java/frc/robot/extras/util/GeomUtil.java index 3b52c448..017c98ed 100644 --- a/src/main/java/frc/robot/extras/util/GeomUtil.java +++ b/src/main/java/frc/robot/extras/util/GeomUtil.java @@ -50,7 +50,38 @@ public static ChassisSpeeds toWpilibChassisSpeeds( dyn4jLinearVelocity.x, dyn4jLinearVelocity.y, angularVelocityRadPerSec); } + /** + * Gets the x and y velocities of a ChassisSpeeds + * + * @param chassisSpeeds the ChassisSpeeds to retrieve velocities from + * @return a Translation2d containing the velocities in the x and y direction in meters per second + */ public static Translation2d getChassisSpeedsTranslationalComponent(ChassisSpeeds chassisSpeeds) { return new Translation2d(chassisSpeeds.vxMetersPerSecond, chassisSpeeds.vyMetersPerSecond); } + + /** + * Checks if two translations are within a certain threshold in meters + * + * @param t1 Translation 1 as a Translation2d + * @param t2 Translation 2 as a Translation2d + * @param thresholdMeters the threshold between the two translations in meters + * @return true if the two translations are within thresholdMeters + */ + public static boolean isTranslationWithinThreshold( + Translation2d t1, Translation2d t2, double thresholdMeters) { + return t1.getDistance(t2) <= thresholdMeters; + } + + /** + * Checks if two rotations are within a certain threshold in degrees + * + * @param r1 Rotations 1 as a Rotation2d + * @param r2 Rotation 2 as a Rotation2d + * @param thresholdMeters the threshold between the two rotations in degrees + * @return true if the two rotations are within thresholdDegrees + */ + public static boolean isRotationWithinThreshold(double r1, double r2, double thresholdDegrees) { + return Math.abs(r1 - r2) <= thresholdDegrees; + } } diff --git a/src/main/java/frc/robot/extras/util/JoystickUtil.java b/src/main/java/frc/robot/extras/util/JoystickUtil.java index 1ffc4bb9..444024e1 100644 --- a/src/main/java/frc/robot/extras/util/JoystickUtil.java +++ b/src/main/java/frc/robot/extras/util/JoystickUtil.java @@ -43,18 +43,6 @@ public static double modifyAxis(DoubleSupplier supplierValue, int exponent) { return value; } - public static double modifyAxisCubed(DoubleSupplier supplierValue) { - double value = supplierValue.getAsDouble(); - - // Deadband - value = JoystickUtil.deadband(value, JoystickConstants.DEADBAND_VALUE); - - // Cube the axis - value = Math.copySign(value * value * value, value); - - return value; - } - /** * Converts the two axis coordinates to polar to get both the angle and radius, so they can work * in a double[] list. @@ -81,22 +69,4 @@ public static double[] modifyAxisPolar( Math.copySign(Math.pow(yInput, exponent), yInput) }; } - - public static double[] modifyAxisCubedPolar(DoubleSupplier xJoystick, DoubleSupplier yJoystick) { - double xInput = - JoystickUtil.deadband(xJoystick.getAsDouble(), JoystickConstants.DEADBAND_VALUE); - double yInput = deadband(yJoystick.getAsDouble(), JoystickConstants.DEADBAND_VALUE); - if (Math.abs(xInput) > 0 && Math.abs(yInput) > 0) { - double theta = Math.atan(xInput / yInput); - double hypotenuse = Math.sqrt(xInput * xInput + yInput * yInput); - double cubedHypotenuse = Math.pow(hypotenuse, 3); - xInput = Math.copySign(Math.sin(theta) * cubedHypotenuse, xInput); - yInput = Math.copySign(Math.cos(theta) * cubedHypotenuse, yInput); - return new double[] {xInput, yInput}; - } - return new double[] { - Math.copySign(xInput * xInput * xInput, xInput), - Math.copySign(yInput * yInput * yInput, yInput) - }; - } } diff --git a/src/main/java/frc/robot/extras/util/TimeUtil.java b/src/main/java/frc/robot/extras/util/TimeUtil.java index 754968af..3cad1701 100644 --- a/src/main/java/frc/robot/extras/util/TimeUtil.java +++ b/src/main/java/frc/robot/extras/util/TimeUtil.java @@ -1,5 +1,6 @@ package frc.robot.extras.util; +import edu.wpi.first.wpilibj.RobotController; import org.littletonrobotics.junction.Logger; public class TimeUtil { @@ -26,6 +27,6 @@ public static double getLogTimeSeconds() { } public static double getRealTimeSeconds() { - return Logger.getRealTimestamp() / 1_000_000.0; + return RobotController.getFPGATime() / 1_000_000.0; } } diff --git a/src/main/java/frc/robot/extras/vision/FiducialObservation.java b/src/main/java/frc/robot/extras/vision/FiducialObservation.java new file mode 100644 index 00000000..6c5b8ad4 --- /dev/null +++ b/src/main/java/frc/robot/extras/vision/FiducialObservation.java @@ -0,0 +1,85 @@ +package frc.robot.extras.vision; + +import edu.wpi.first.util.struct.Struct; +import edu.wpi.first.util.struct.StructSerializable; +import java.nio.ByteBuffer; + +public class FiducialObservation implements StructSerializable { + public static class FiducialObservationStruct implements Struct { + public int id; + public double txnc; + public double tync; + public double ambiguity; + + @Override + public Class getTypeClass() { + return FiducialObservation.class; + } + + @Override + public String getTypeString() { + return "struct:FiducialObservation"; + } + + @Override + public int getSize() { + return kSizeInt32 + 3 * kSizeDouble; + } + + @Override + public String getSchema() { + return "int id;double txnc;double tync;double ambiguity"; + } + + @Override + public FiducialObservation unpack(ByteBuffer bb) { + FiducialObservation rv = new FiducialObservation(); + rv.id = bb.getInt(); + rv.txnc = bb.getDouble(); + rv.tync = bb.getDouble(); + rv.ambiguity = bb.getDouble(); + return rv; + } + + @Override + public void pack(ByteBuffer bb, FiducialObservation value) { + bb.putInt(value.id); + bb.putDouble(value.txnc); + bb.putDouble(value.tync); + bb.putDouble(value.ambiguity); + } + + @Override + public String getTypeName() { + // TODO Auto-generated method stub + throw new UnsupportedOperationException("Unimplemented method 'getTypeName'"); + } + } + + public int id; + public double txnc; + public double tync; + public double ambiguity; + + public FiducialObservation() {} + + public static FiducialObservation fromLimelight(LimelightHelpers.RawFiducial fiducial) { + FiducialObservation rv = new FiducialObservation(); + rv.id = fiducial.id; + rv.txnc = fiducial.txnc; + rv.tync = fiducial.tync; + rv.ambiguity = fiducial.ambiguity; + + return rv; + } + + public static FiducialObservation[] fromLimelight(LimelightHelpers.RawFiducial[] fiducials) { + FiducialObservation[] rv = new FiducialObservation[fiducials.length]; + for (int i = 0; i < fiducials.length; ++i) { + rv[i] = fromLimelight(fiducials[i]); + } + return rv; + } + + public static final FiducialObservationStruct struct = new FiducialObservationStruct(); +} diff --git a/src/main/java/frc/robot/extras/vision/LimelightHelpers.java b/src/main/java/frc/robot/extras/vision/LimelightHelpers.java index a859a061..a7ddf38b 100644 --- a/src/main/java/frc/robot/extras/vision/LimelightHelpers.java +++ b/src/main/java/frc/robot/extras/vision/LimelightHelpers.java @@ -516,7 +516,7 @@ private static PoseEstimate getBotPoseEstimate(String limelightName, String entr pose, timestamp, latency, tagCount, tagSpan, tagDist, tagArea, rawFiducials); } - private static void printPoseEstimate(PoseEstimate pose) { + public static void printPoseEstimate(PoseEstimate pose) { if (pose == null) { System.out.println("No PoseEstimate available."); return; @@ -679,12 +679,14 @@ public static double[] getCameraPose_TargetSpace(String limelightName) { return getLimelightNTDoubleArray(limelightName, "camerapose_targetspace"); } - public static double[] getTargetPose_CameraSpace(String limelightName) { - return getLimelightNTDoubleArray(limelightName, "targetpose_cameraspace"); + public static Pose2d getTargetPose_CameraSpace(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "targetpose_cameraspace"); + return toPose2D(poseArray); } - public static double[] getTargetPose_RobotSpace(String limelightName) { - return getLimelightNTDoubleArray(limelightName, "targetpose_robotspace"); + public static Pose2d getTargetPose_RobotSpace(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "targetpose_robotspace"); + return toPose2D(poseArray); } public static double[] getTargetColor(String limelightName) { diff --git a/src/main/java/frc/robot/extras/vision/MegatagPoseEstimate.java b/src/main/java/frc/robot/extras/vision/MegatagPoseEstimate.java new file mode 100644 index 00000000..04a7d7a2 --- /dev/null +++ b/src/main/java/frc/robot/extras/vision/MegatagPoseEstimate.java @@ -0,0 +1,99 @@ +package frc.robot.extras.vision; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.util.struct.Struct; +import edu.wpi.first.util.struct.StructSerializable; +import java.nio.ByteBuffer; + +public class MegatagPoseEstimate implements StructSerializable { + public static class MegatagPoseEstimateStruct implements Struct { + public Pose2d fieldToCamera = Pose2d.kZero; + public double timestampSeconds; + public double latency; + public double avgTagArea; + public double avgTagDist; + public int tagCount; + + @Override + public Class getTypeClass() { + return MegatagPoseEstimate.class; + } + + @Override + public String getTypeString() { + return "struct:MegatagPoseEstimate"; + } + + @Override + public int getSize() { + return Pose2d.struct.getSize() + kSizeDouble * 3; + } + + @Override + public String getSchema() { + return "Pose2d fieldToCamera;double timestampSeconds;double latency;double avgTagArea; int tagCount; double avgTagDist"; + } + + @Override + public Struct[] getNested() { + return new Struct[] {Pose2d.struct}; + } + + @Override + public MegatagPoseEstimate unpack(ByteBuffer bb) { + MegatagPoseEstimate rv = new MegatagPoseEstimate(); + rv.fieldToCamera = Pose2d.struct.unpack(bb); + rv.timestampSeconds = bb.getDouble(); + rv.latency = bb.getDouble(); + rv.avgTagArea = bb.getDouble(); + rv.fiducialIds = new int[0]; + rv.avgTagDist = bb.getDouble(); + rv.timestampSeconds = bb.getInt(); + return rv; + } + + @Override + public void pack(ByteBuffer bb, MegatagPoseEstimate value) { + Pose2d.struct.pack(bb, value.fieldToCamera); + bb.putDouble(value.timestampSeconds); + bb.putDouble(value.latency); + bb.putDouble(value.avgTagArea); + bb.putDouble(value.avgTagDist); + } + + @Override + public String getTypeName() { + // TODO Auto-generated method stub + throw new UnsupportedOperationException("Unimplemented method 'getTypeName'"); + } + } + + public Pose2d fieldToCamera = Pose2d.kZero; + public double timestampSeconds; + public double latency; + public double avgTagArea; + public int[] fiducialIds; + public int tagCount; + public double avgTagDist; + + public MegatagPoseEstimate() {} + + public static MegatagPoseEstimate fromLimelight(LimelightHelpers.PoseEstimate poseEstimate) { + MegatagPoseEstimate rv = new MegatagPoseEstimate(); + rv.fieldToCamera = poseEstimate.pose; + if (rv.fieldToCamera == null) rv.fieldToCamera = Pose2d.kZero; + rv.timestampSeconds = poseEstimate.timestampSeconds; + rv.latency = poseEstimate.latency; + rv.avgTagArea = poseEstimate.avgTagArea; + rv.avgTagDist = poseEstimate.avgTagDist; + rv.tagCount = poseEstimate.tagCount; + rv.fiducialIds = new int[poseEstimate.rawFiducials.length]; + for (int i = 0; i < rv.fiducialIds.length; ++i) { + rv.fiducialIds[i] = poseEstimate.rawFiducials[i].id; + } + + return rv; + } + + public static final MegatagPoseEstimateStruct struct = new MegatagPoseEstimateStruct(); +} diff --git a/src/main/java/frc/robot/extras/vision/VisionFieldPoseEstimate.java b/src/main/java/frc/robot/extras/vision/VisionFieldPoseEstimate.java new file mode 100644 index 00000000..577ed3c5 --- /dev/null +++ b/src/main/java/frc/robot/extras/vision/VisionFieldPoseEstimate.java @@ -0,0 +1,34 @@ +package frc.robot.extras.vision; + +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.numbers.N1; +import edu.wpi.first.math.numbers.N3; + +public class VisionFieldPoseEstimate { + + private final Pose2d visionRobotPoseMeters; + private final double timestampSeconds; + private final Matrix visionMeasurementStdDevs; + + public VisionFieldPoseEstimate( + Pose2d visionRobotPoseMeters, + double timestampSeconds, + Matrix visionMeasurementStdDevs) { + this.visionRobotPoseMeters = visionRobotPoseMeters; + this.timestampSeconds = timestampSeconds; + this.visionMeasurementStdDevs = visionMeasurementStdDevs; + } + + public Pose2d getVisionRobotPoseMeters() { + return visionRobotPoseMeters; + } + + public double getTimestampSeconds() { + return timestampSeconds; + } + + public Matrix getVisionMeasurementStdDevs() { + return visionMeasurementStdDevs; + } +} diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveConstants.java b/src/main/java/frc/robot/subsystems/swerve/SwerveConstants.java index c3d3f1d2..ed08692f 100644 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveConstants.java +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveConstants.java @@ -8,7 +8,7 @@ import edu.wpi.first.math.trajectory.TrapezoidProfile.Constraints; import edu.wpi.first.math.util.Units; -/** Add your docs here. */ +/** Swerve Constants */ public class SwerveConstants { public static final class DriveConstants { diff --git a/src/main/java/frc/robot/subsystems/swerve/module/PhysicalModule.java b/src/main/java/frc/robot/subsystems/swerve/module/PhysicalModule.java index 2d052fe2..f89a9bfb 100644 --- a/src/main/java/frc/robot/subsystems/swerve/module/PhysicalModule.java +++ b/src/main/java/frc/robot/subsystems/swerve/module/PhysicalModule.java @@ -11,7 +11,6 @@ import com.ctre.phoenix6.controls.VoltageOut; import com.ctre.phoenix6.hardware.CANcoder; import com.ctre.phoenix6.hardware.TalonFX; -import com.ctre.phoenix6.signals.AbsoluteSensorRangeValue; import com.ctre.phoenix6.signals.FeedbackSensorSourceValue; import com.ctre.phoenix6.signals.NeutralModeValue; import edu.wpi.first.math.geometry.Rotation2d; @@ -49,15 +48,13 @@ public class PhysicalModule implements ModuleInterface { private final BaseStatusSignal[] periodicallyRefreshedSignals; public PhysicalModule(ModuleConfig moduleConfig) { - driveMotor = new TalonFX(moduleConfig.driveMotorChannel(), DeviceCANBus.RIO.name); - turnMotor = new TalonFX(moduleConfig.turnMotorChannel(), DeviceCANBus.RIO.name); - turnEncoder = new CANcoder(moduleConfig.turnEncoderChannel(), DeviceCANBus.RIO.name); + driveMotor = new TalonFX(moduleConfig.driveMotorChannel(), DeviceCANBus.CANIVORE.name); + turnMotor = new TalonFX(moduleConfig.turnMotorChannel(), DeviceCANBus.CANIVORE.name); + turnEncoder = new CANcoder(moduleConfig.turnEncoderChannel(), DeviceCANBus.CANIVORE.name); CANcoderConfiguration turnEncoderConfig = new CANcoderConfiguration(); turnEncoderConfig.MagnetSensor.MagnetOffset = -moduleConfig.angleZero(); turnEncoderConfig.MagnetSensor.SensorDirection = moduleConfig.encoderReversed(); - turnEncoderConfig.MagnetSensor.AbsoluteSensorRange = - AbsoluteSensorRangeValue.Signed_PlusMinusHalf; turnEncoder.getConfigurator().apply(turnEncoderConfig, HardwareConstants.TIMEOUT_S); TalonFXConfiguration driveConfig = new TalonFXConfiguration(); @@ -173,29 +170,15 @@ public void setTurnVoltage(Voltage volts) { @Override public void setDesiredState(SwerveModuleState desiredState) { - double turnRotations = getTurnRotations(); - // Optimize the reference state to avoid spinning further than 90 degrees - SwerveModuleState setpoint = - new SwerveModuleState(desiredState.speedMetersPerSecond, desiredState.angle); - - setpoint.optimize(Rotation2d.fromRotations(turnRotations)); - setpoint.cosineScale(Rotation2d.fromRotations(turnRotations)); - - if (Math.abs(setpoint.speedMetersPerSecond) < 0.01) { - driveMotor.set(0); - turnMotor.set(0); - return; - } - // Converts meters per second to rotations per second double desiredDriveRPS = - setpoint.speedMetersPerSecond + desiredState.speedMetersPerSecond * ModuleConstants.DRIVE_GEAR_RATIO / ModuleConstants.WHEEL_CIRCUMFERENCE_METERS; driveMotor.setControl(velocityRequest.withVelocity(RotationsPerSecond.of(desiredDriveRPS))); turnMotor.setControl( - mmPositionRequest.withPosition(Rotations.of(setpoint.angle.getRotations()))); + mmPositionRequest.withPosition(Rotations.of(desiredState.angle.getRotations()))); } public double getTurnRotations() { diff --git a/src/main/java/frc/robot/subsystems/swerve/odometryThread/OdometryThread.java b/src/main/java/frc/robot/subsystems/swerve/odometryThread/OdometryThread.java index c29d61b5..ebc92167 100644 --- a/src/main/java/frc/robot/subsystems/swerve/odometryThread/OdometryThread.java +++ b/src/main/java/frc/robot/subsystems/swerve/odometryThread/OdometryThread.java @@ -48,7 +48,7 @@ static Queue registerInput(Supplier supplier) { } static OdometryThread createInstance(DeviceCANBus canBus) { - return switch (Constants.currentMode) { + return switch (Constants.CURRENT_MODE) { case REAL -> new OdometryThreadReal( canBus, diff --git a/src/main/java/frc/robot/subsystems/swerve/setpointGen/SwerveSetpointGenerator.java b/src/main/java/frc/robot/subsystems/swerve/setpointGen/SwerveSetpointGenerator.java index 392f9e1c..4f2f230d 100644 --- a/src/main/java/frc/robot/subsystems/swerve/setpointGen/SwerveSetpointGenerator.java +++ b/src/main/java/frc/robot/subsystems/swerve/setpointGen/SwerveSetpointGenerator.java @@ -130,11 +130,7 @@ public SwerveSetpoint generateSetpoint( vars.prevSpeeds.vxMetersPerSecond + vars.minS * vars.dx, vars.prevSpeeds.vyMetersPerSecond + vars.minS * vars.dy, vars.prevSpeeds.omegaRadiansPerSecond + vars.minS * vars.dtheta); - retSpeeds.discretize( - retSpeeds.vxMetersPerSecond, - retSpeeds.vyMetersPerSecond, - retSpeeds.omegaRadiansPerSecond, - dt); + retSpeeds.discretize(retSpeeds, dt); double chassisAccelX = (retSpeeds.vxMetersPerSecond - vars.prevSpeeds.vxMetersPerSecond) / dt; double chassisAccelY = (retSpeeds.vyMetersPerSecond - vars.prevSpeeds.vyMetersPerSecond) / dt; diff --git a/src/main/java/frc/robot/subsystems/vision/PhysicalVision.java b/src/main/java/frc/robot/subsystems/vision/PhysicalVision.java new file mode 100644 index 00000000..4ceab058 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/vision/PhysicalVision.java @@ -0,0 +1,410 @@ +package frc.robot.subsystems.vision; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.wpilibj.DriverStation; +import frc.robot.Constants.FieldConstants; +import frc.robot.extras.util.GeomUtil; +import frc.robot.extras.vision.LimelightHelpers; +import frc.robot.extras.vision.LimelightHelpers.PoseEstimate; +import frc.robot.extras.vision.MegatagPoseEstimate; +import frc.robot.subsystems.vision.VisionConstants.Limelight; +// import frc.robot.subsystems.vision.VisionInterface.VisionInputs; +import java.util.concurrent.ConcurrentHashMap; +// import java.util.concurrent.ExecutorService; +// import java.util.concurrent.Executors; +import java.util.concurrent.atomic.AtomicReference; + +public class PhysicalVision implements VisionInterface { + + private Pose2d lastSeenPose = new Pose2d(); + private double headingDegrees = 0; + private double headingRateDegreesPerSecond = 0; + private final ConcurrentHashMap> limelightThreads = + new ConcurrentHashMap<>(); + // private final ExecutorService executorService = Executors.newFixedThreadPool(3); + private final AtomicReference latestInputs = + new AtomicReference<>(new VisionInputs()); + private final ThreadManager threadManager = new ThreadManager(Limelight.values().length); + + /** + * The pose estimates from the limelights in the following order {shooterLimelight, + * frontLeftLimelight, frontRightLimelight} + */ + private MegatagPoseEstimate[] limelightEstimates = + new MegatagPoseEstimate[] { + new MegatagPoseEstimate(), new MegatagPoseEstimate(), new MegatagPoseEstimate() + }; + + public PhysicalVision() { + for (Limelight limelight : Limelight.values()) { + limelightThreads.put(limelight, new AtomicReference<>(latestInputs.get())); + + // Start a vision input task for each Limelight + threadManager.startVisionInputTask( + limelight.getName(), latestInputs.get(), () -> visionThreadTask(latestInputs.get())); + } + } + + @Override + public void updateInputs(VisionInputs inputs) { + // Combine inputs into the main inputs object + synchronized (inputs) { + for (Limelight limelight : Limelight.values()) { + inputs.isLimelightConnected[limelight.getId()] = isLimelightConnected(limelight); + inputs.limelightLatency[limelight.getId()] = getLatencySeconds(limelight); + inputs.limelightTargets[limelight.getId()] = getNumberOfAprilTags(limelight); + inputs.limelightSeesAprilTags[limelight.getId()] = canSeeAprilTags(limelight); + inputs.limelightMegatagPose[limelight.getId()] = enabledPoseUpdate(limelight); + inputs.limelightAprilTagDistance[limelight.getId()] = + getLimelightAprilTagDistance(limelight); + inputs.limelightCalculatedPose[limelight.getId()] = getPoseFromAprilTags(limelight); + inputs.limelightTimestamp[limelight.getId()] = getTimeStampSeconds(limelight); + inputs.limelightLastSeenPose = getLastSeenPose(); + inputs.limelightAprilTagDistance[limelight.getId()] = + getLimelightAprilTagDistance(limelight); + + latestInputs.set(inputs); + limelightThreads.get(limelight).set(latestInputs.get()); + } + } + } + + /** + * Checks if the specified limelight can fully see one or more April Tag. + * + * @param limelight a limelight (SHOOTER, FRONT_LEFT, FRONT_RIGHT). + * @return true if the limelight can fully see one or more April Tag + */ + @Override + public boolean canSeeAprilTags(Limelight limelight) { + // First checks if it can see an april tag, then checks if it is fully in frame + // Different Limelights have different FOVs + if (getNumberOfAprilTags(limelight) > VisionConstants.MIN_APRIL_TAG_ID + && getNumberOfAprilTags(limelight) <= VisionConstants.MAX_APRIL_TAG_ID) { + if (limelight.getName().equals(Limelight.SHOOTER.getName())) { + return Math.abs(LimelightHelpers.getTX(limelight.getName())) + <= VisionConstants.LL3G_FOV_MARGIN_OF_ERROR; + } else { + // return false; + return Math.abs(LimelightHelpers.getTX(limelight.getName())) + <= VisionConstants.LL3_FOV_MARGIN_OF_ERROR; + } + } + // return latestInputs.get().limelightSeesAprilTags[limelight.getId()] = false; + // return LimelightHelpers.getTV(limelight.getName()); + return false; + // latestInputs.get().limelightSeesAprilTags[limelight.getId()] = + } + + /** + * Gets the JSON dump from the specified limelight and puts it into a PoseEstimate object, which + * is then placed into its corresponding spot in the limelightEstimates array. + * + * @param limelight a limelight (SHOOTER, FRONT_LEFT, FRONT_RIGHT). + */ + public MegatagPoseEstimate enabledPoseUpdate(Limelight limelight) { + PoseEstimate megatag1Estimate = getMegaTag1PoseEstimate(limelight); + PoseEstimate megatag2Estimate = getMegaTag2PoseEstimate(limelight); + + if (canSeeAprilTags(limelight) + && isValidPoseEstimate(limelight, megatag1Estimate, megatag2Estimate)) { + if (isLargeDiscrepancyBetweenMegaTag1And2(limelight, megatag1Estimate, megatag2Estimate) + && getLimelightAprilTagDistance(limelight) + < VisionConstants.MEGA_TAG_2_DISTANCE_THRESHOLD) { + return limelightEstimates[limelight.getId()] = + MegatagPoseEstimate.fromLimelight(getMegaTag1PoseEstimate(limelight)); + } else if (headingRateDegreesPerSecond < VisionConstants.MEGA_TAG_2_MAX_HEADING_RATE) { + LimelightHelpers.SetRobotOrientation(limelight.getName(), headingDegrees, 0, 0, 0, 0, 0); + return limelightEstimates[limelight.getId()] = + MegatagPoseEstimate.fromLimelight(getMegaTag2PoseEstimate(limelight)); + } else { + return limelightEstimates[limelight.getId()] = + MegatagPoseEstimate.fromLimelight(getMegaTag1PoseEstimate(limelight)); + } + } + return limelightEstimates[limelight.getId()] = new MegatagPoseEstimate(); + } + + /** + * If the robot is not enabled, update the pose using MegaTag1 and after it is enabled, run {@link + * #enabledPoseUpdate(int)} + * + * @param limelight a limelight (SHOOTER, FRONT_LEFT, FRONT_RIGHT). + */ + public void updatePoseEstimate(Limelight limelight, VisionInputs inputs) { + synchronized (inputs) { + limelightEstimates[limelight.getId()] = + DriverStation.isEnabled() + ? enabledPoseUpdate(limelight) + : MegatagPoseEstimate.fromLimelight(getMegaTag1PoseEstimate(limelight)); + } + } + + /** + * Checks if there is a large discrepancy between the MegaTag1 and MegaTag2 estimates. + * + * @param limelight a limelight (SHOOTER, FRONT_LEFT, FRONT_RIGHT). + * @return true if the discrepancy is larger than the defined threshold, false otherwise + */ + public boolean isLargeDiscrepancyBetweenMegaTag1And2( + Limelight limelight, PoseEstimate mt1, PoseEstimate mt2) { + return !GeomUtil.isTranslationWithinThreshold( + mt1.pose.getTranslation(), + mt2.pose.getTranslation(), + VisionConstants.MEGA_TAG_TRANSLATION_DISCREPANCY_THRESHOLD) + || !GeomUtil.isRotationWithinThreshold( + mt1.pose.getRotation().getDegrees(), + mt2.pose.getRotation().getDegrees(), + VisionConstants.MEGA_TAG_ROTATION_DISCREPANCY_THREASHOLD); + // return true; + } + + /** + * Gets the MegaTag1 pose of the robot calculated by specified limelight via any April Tags it + * sees + * + * @param limelight a limelight (SHOOTER, FRONT_LEFT, FRONT_RIGHT). + * @return the MegaTag1 pose of the robot, if the limelight can't see any April Tags, it will + * return 0 for x, y, and theta + */ + public PoseEstimate getMegaTag1PoseEstimate(Limelight limelight) { + return LimelightHelpers.getBotPoseEstimate_wpiBlue(limelight.getName()); + } + + /** + * Gets the MegaTag2 pose of the robot calculated by specified limelight via any April Tags it + * sees + * + * @param limelight a limelight (SHOOTER, FRONT_LEFT, FRONT_RIGHT). + * @return the MegaTag2 pose of the robot, if the limelight can't see any April Tags, it will + * return 0 for x, y, and theta + */ + public PoseEstimate getMegaTag2PoseEstimate(Limelight limelight) { + return LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(limelight.getName()); + } + + /** + * Checks if the MT1 and MT2 pose estimate exists and whether it is within the field + * + * @param limelight a limelight (SHOOTER, FRONT_LEFT, FRONT_RIGHT). + * @return true if the pose estimate exists within the field and the pose estimate is not null + */ + public boolean isValidPoseEstimate(Limelight limelight, PoseEstimate mt1, PoseEstimate mt2) { + return LimelightHelpers.isValidPoseEstimate(mt1) + && LimelightHelpers.isValidPoseEstimate(mt2) + && isWithinFieldBounds(mt1, mt2); + } + + /** + * Checks whether the pose estimate for MT1 and MT2 is within the field + * + * @param megaTag1Estimate the MT1 pose estimate to check + * @param megaTag2Estimate the MT2 pose estimate to check + */ + private boolean isWithinFieldBounds( + PoseEstimate megaTag1Estimate, PoseEstimate megaTag2Estimate) { + return (megaTag1Estimate.pose.getX() > 0 + && megaTag1Estimate.pose.getX() <= FieldConstants.FIELD_WIDTH_METERS) + && (megaTag1Estimate.pose.getY() > 0 + && megaTag1Estimate.pose.getY() <= FieldConstants.FIELD_WIDTH_METERS) + && (megaTag2Estimate.pose.getX() > 0 + && megaTag2Estimate.pose.getX() <= FieldConstants.FIELD_WIDTH_METERS) + && (megaTag2Estimate.pose.getY() > 0 + && megaTag2Estimate.pose.getY() <= FieldConstants.FIELD_WIDTH_METERS); + } + + /** + * Gets the pose of the robot calculated by specified limelight via any April Tags it sees + * + * @param limelight a limelight (SHOOTER, FRONT_LEFT, FRONT_RIGHT). + * @return the pose of the robot, if the limelight can't see any April Tags, it will return 0 for + * x, y, and theta + */ + @Override + public Pose2d getPoseFromAprilTags(Limelight limelight) { + return limelightEstimates[limelight.getId()].fieldToCamera; + } + + // public Pose2d getAprilTagPositionToLimelight(Limelight limelight) { + // return LimelightHelpers.getTargetPose_CameraSpace(limelight.getName()); + // } + + // public Pose2d getAprilTagPositionToRobot(Limelight limelight) { + // return latestInputs.get().limelightRobotToTargetPose[limelight.getId()] + // =LimelightHelpers.getTargetPose_RobotSpace(limelight.getName()); + // } + + /** Returns how many april tags the limelight that is being used for pose estimation can see. */ + @Override + public int getNumberOfAprilTags(Limelight limelight) { + return limelightEstimates[limelight.getId()].tagCount; + // latestInputs.get().limelightMegatagPose[limelight.getId()].fiducialIds.length; + // return limelightEstimates[limelight.getId()].tagCount; + } + + /** + * Returns the timestamp for the vision measurement from the limelight that is being used for pose + * estimation. + */ + @Override + public double getTimeStampSeconds(Limelight limelight) { + return limelightEstimates[limelight.getId()].timestampSeconds / 1000.0; + } + + /** + * Returns the latency in seconds of when the limelight that is being used for pose estimation + * calculated the robot's pose. It adds the pipeline latency, capture latency, and json parsing + * latency. + */ + @Override + public double getLatencySeconds(Limelight limelight) { + return (limelightEstimates[limelight.getId()].latency) / 1000.0; + } + + /** Gets the pose calculated the last time a limelight saw an April Tag */ + @Override + public Pose2d getLastSeenPose() { + return lastSeenPose; + } + + /** + * Gets the average distance between the specified limelight and the April Tags it sees + * + * @param limelight a limelight (SHOOTER, FRONT_LEFT, FRONT_RIGHT). + * @return the average distance between the robot and the April Tag(s) in meters + */ + @Override + public double getLimelightAprilTagDistance(Limelight limelight) { + if (canSeeAprilTags(limelight)) { + return limelightEstimates[limelight.getId()].avgTagDist; + } + // To be safe returns a big distance from the april tags if it can't see any + return Double.MAX_VALUE; + } + + /** + * Sets the heading and heading rate of the robot, this is used for deciding between MegaTag 1 and + * 2 for pose estimation. + * + * @param headingDegrees the angle the robot is facing in degrees (0 degrees facing the red + * alliance) + * @param headingRateDegrees the rate the robot is rotating, CCW positive + */ + @Override + public void setHeadingInfo(double headingDegrees, double headingRateDegrees) { + this.headingDegrees = headingDegrees; + this.headingRateDegreesPerSecond = headingRateDegrees; + } + + public boolean isLimelightConnected(Limelight limelight) { + NetworkTable limelightTable = LimelightHelpers.getLimelightNTTable(limelight.getName()); + return limelightTable.containsKey("tv"); + } + + /** + * This checks is there is new pose detected by a limelight, and if so, updates the pose estimate + * + * @param limelight the limelight number + */ + public void checkAndUpdatePose(Limelight limelight, VisionInputs inputs) { + double last_TX = 0; + double last_TY = 0; + + // Syncronization block to ensure thread safety during the critical section where pose + // information is read and compared. + // This helps prevent race conditions, where one limelight may be updating an object that + // another limelight is reading. + // A race condition could cause unpredictable things to happen. Such as causing a limelight to + // be unable to reference an + // object, as its reference was modified earlier. + synchronized (this) { + try { + double current_TX = LimelightHelpers.getTX(limelight.getName()); + double current_TY = LimelightHelpers.getTY(limelight.getName()); + + // This checks if the limelight reading is new. The reasoning being that if the TX and TY + // are EXACTLY the same, it hasn't updated yet with a new reading. We are doing it this way, + // because to get the timestamp of the reading, you need to parse the JSON dump which can be + // very demanding whereas this only has to get the Network Table entries for TX and TY. + if (current_TX != last_TX || current_TY != last_TY) { + + updatePoseEstimate(limelight, inputs); + latestInputs.set(inputs); + + limelightThreads.computeIfPresent(limelight, (key, value) -> latestInputs); + // // Handle threading for Limelight (start or stop threads if needed) + // Check if this Limelight thread exists in limelightThreads + if (limelightThreads.get(limelight) != null) { + // Update thread inputs or restart the thread if needed + limelightThreads.get(limelight).set(latestInputs.get()); + } + + // This is to keep track of the last valid pose calculated by the limelights + // it is used when the driver resets the robot odometry to the limelight calculated + // position + if (canSeeAprilTags(limelight)) { + lastSeenPose = getMegaTag1PoseEstimate(limelight).pose; + } + } else { + // // Retrieve the AtomicReference for the given limelight number + AtomicReference isThreadRunning = + limelightThreads.getOrDefault(limelight, latestInputs); + // Only stop the thread if it's currently running + if (isThreadRunning.get() != null) { + // stop the thread for the specified limelight + stopLimelightThread(limelight); + } + } + last_TX = current_TX; + last_TY = current_TY; + } catch (Exception e) { + System.err.println( + "Error communicating with the: " + limelight.getName() + ": " + e.getMessage()); + } + } + } + + /** + * Starts a separate thread dedicated to updating the pose estimate for a specified limelight. + * This approach is adopted to prevent loop overruns that would occur if we attempted to parse the + * JSON dump for each limelight sequentially within a single scheduler loop. + * + *

To achieve efficient and safe parallel execution, an ExecutorService is utilized to manage + * the lifecycle of these threads. + * + *

Each thread continuously runs the {@link #checkAndUpdatePose(int)} method as long as the + * corresponding limelight's thread is marked as "running". This ensures that pose estimates are + * updated in real-time, leveraging the parallel processing capabilities of the executor service. + * + * @param limelight the limelight number + */ + public void visionThreadTask(VisionInputs inputs) { // Limelight limelight + try { + synchronized (inputs) { + for (Limelight limelight : Limelight.values()) { + checkAndUpdatePose(limelight, inputs); + } + } + + } catch (Exception e) { + e.printStackTrace(); + } + } + + /** + * Sets the AtomicBoolean 'runningThreads' to false for the specified limelight. Stops the thread + * for the specified limelight. + * + * @param limelight the limelight number + */ + public void stopLimelightThread(Limelight limelight) { + threadManager.stopThread(limelight.getName()); + } + + /** Shuts down all the threads. */ + // @Override + public void endAllThreads() { + threadManager.shutdownAllThreads(); + } +} diff --git a/src/main/java/frc/robot/subsystems/vision/SimulatedVision.java b/src/main/java/frc/robot/subsystems/vision/SimulatedVision.java new file mode 100644 index 00000000..a4e46773 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/vision/SimulatedVision.java @@ -0,0 +1,194 @@ +package frc.robot.subsystems.vision; + +import edu.wpi.first.math.geometry.*; +import edu.wpi.first.networktables.NetworkTable; +import frc.robot.extras.vision.LimelightHelpers; +import frc.robot.subsystems.vision.VisionConstants.Limelight; +import java.util.ArrayList; +import java.util.Arrays; +import java.util.List; +import java.util.function.Supplier; +import org.littletonrobotics.junction.Logger; +import org.photonvision.PhotonCamera; +import org.photonvision.simulation.PhotonCameraSim; +import org.photonvision.simulation.SimCameraProperties; +import org.photonvision.simulation.VisionSystemSim; +import org.photonvision.targeting.PhotonPipelineResult; + +// Simulate the vision system. +// Please see the following link for example code +// https://github.com/PhotonVision/photonvision/blob/2a6fa1b6ac81f239c59d724da5339f608897c510/photonlib-java-examples/swervedriveposeestsim/src/main/java/frc/robot/Vision.java +public class SimulatedVision extends PhysicalVision { + PhotonCameraSim shooterCameraSim; + PhotonCameraSim frontLeftCameraSim; + PhotonCameraSim frontRightCameraSim; + private final VisionSystemSim visionSim; + private final Supplier robotSimulationPose; + private Pose2d lastSeenPose = new Pose2d(); + + private final int kResWidth = 1280; + private final int kResHeight = 800; + + private int[] tagCount = new int[Limelight.values().length]; + private double[] apriltagDist = new double[Limelight.values().length]; + + public SimulatedVision(Supplier robotSimulationPose) { + super(); + this.robotSimulationPose = robotSimulationPose; + // Create the vision system simulation which handles cameras and targets on the + // field. + visionSim = new VisionSystemSim("main"); + + // Add all the AprilTags inside the tag layout as visible targets to this + // simulated field. + visionSim.addAprilTags(VisionConstants.FIELD_LAYOUT); + // visionSim.addVisionTargets(TargetModel.kAprilTag36h11); + + // Create simulated camera properties. These can be set to mimic your actual + // camera. + var cameraProperties = new SimCameraProperties(); + cameraProperties.setCalibration(kResWidth, kResHeight, Rotation2d.fromDegrees(97.7)); + cameraProperties.setCalibError(0.35, 0.10); + cameraProperties.setFPS(15); + cameraProperties.setAvgLatencyMs(20); + cameraProperties.setLatencyStdDevMs(5); + + // Create a PhotonCameraSim which will update the linked PhotonCamera's values + // with visible + // targets. + // Instance variables + shooterCameraSim = + new PhotonCameraSim(getSimulationCamera(Limelight.SHOOTER), cameraProperties); + frontLeftCameraSim = + new PhotonCameraSim(getSimulationCamera(Limelight.FRONT_LEFT), cameraProperties); + frontRightCameraSim = + new PhotonCameraSim(getSimulationCamera(Limelight.FRONT_RIGHT), cameraProperties); + + visionSim.addCamera( + shooterCameraSim, VisionConstants.SHOOTER_TRANSFORM); // check inverse things + visionSim.addCamera(frontLeftCameraSim, VisionConstants.FRONT_LEFT_TRANSFORM); + visionSim.addCamera(frontRightCameraSim, VisionConstants.FRONT_RIGHT_TRANSFORM); + + // Enable the raw and processed streams. (http://localhost:1181 / 1182) + // shooterCameraSim.enableRawStream(true); + // shooterCameraSim.enableProcessedStream(true); + // frontLeftCameraSim.enableRawStream(true); + // frontLeftCameraSim.enableProcessedStream(true); + // frontRightCameraSim.enableRawStream(true); + // frontRightCameraSim.enableProcessedStream(true); + + // // Enable drawing a wireframe visualization of the field to the camera streams. + // // This is extremely resource-intensive and is disabled by default. + // shooterCameraSim.enableDrawWireframe(true); + // frontLeftCameraSim.enableDrawWireframe(true); + // frontRightCameraSim.enableDrawWireframe(true); + } + + @Override + public void updateInputs(VisionInputs inputs) { + // Abuse the updateInputs periodic call to update the sim + + // Move the vision sim robot on the field + if (robotSimulationPose.get() != null) { + visionSim.update(robotSimulationPose.get()); + Logger.recordOutput("Vision/SimIO/updateSimPose", robotSimulationPose.get()); + } + super.updateInputs(inputs); + + for (Limelight limelight : Limelight.values()) { + writeToTable( + getSimulationCamera(limelight).getAllUnreadResults(), + getLimelightTable(limelight), + limelight); + // inputs.limelightTargets[limelight.getId()] = getNumberOfAprilTags(limelight); + // inputs.limelightAprilTagDistance[limelight.getId()] = + // getLimelightAprilTagDistance(limelight); + } + } + + private void writeToTable( + List results, NetworkTable table, Limelight limelight) { + // write to ll table + for (PhotonPipelineResult result : results) { + if (result.getMultiTagResult().isPresent()) { + Transform3d best = result.getMultiTagResult().get().estimatedPose.best; + List pose_data = + new ArrayList<>( + Arrays.asList( + best.getX(), // 0: X + best.getY(), // 1: Y + best.getZ(), // 2: Z, + best.getRotation().getX(), // 3: roll + best.getRotation().getY(), // 4: pitch + best.getRotation().getZ(), // 5: yaw + result.metadata.getLatencyMillis(), // 6: latency ms, + (double) + result.getMultiTagResult().get().fiducialIDsUsed.size(), // 7: tag count + 0.0, // 8: tag span + 0.0, // 9: tag dist + result.getBestTarget().getArea() // 10: tag area + )); + // Add RawFiducials + // This is super inefficient but it's sim only, who cares. + for (var target : result.targets) { + pose_data.add((double) target.getFiducialId()); // 0: id + pose_data.add(target.getYaw()); // 1: txnc + pose_data.add(target.getPitch()); // 2: tync + pose_data.add(0.0); // 3: ta + pose_data.add(0.0); // 4: distToCamera + pose_data.add(0.0); // 5: distToRobot + pose_data.add(0.5); // 6: ambiguity + } + + table + .getEntry("botpose_wpiblue") + .setDoubleArray(pose_data.stream().mapToDouble(Double::doubleValue).toArray()); + table + .getEntry("botpose_orb_wpiblue") + .setDoubleArray(pose_data.stream().mapToDouble(Double::doubleValue).toArray()); + + table.getEntry("tv").setInteger(result.hasTargets() ? 1 : 0); + table.getEntry("cl").setDouble(result.metadata.getLatencyMillis()); + lastSeenPose = + new Pose2d( + result.getMultiTagResult().get().estimatedPose.best.getTranslation().getX(), + result.getMultiTagResult().get().estimatedPose.best.getY(), + new Rotation2d( + result.getMultiTagResult().get().estimatedPose.best.getRotation().getAngle())); + } + } + } + + public PhotonCamera getSimulationCamera(Limelight limelight) { + return switch (limelight) { + case SHOOTER -> VisionConstants.SHOOTER_CAMERA; + case FRONT_LEFT -> VisionConstants.FRONT_LEFT_CAMERA; + case FRONT_RIGHT -> VisionConstants.FRONT_RIGHT_CAMERA; + default -> throw new IllegalArgumentException("Invalid limelight camera " + limelight); + }; + } + + public NetworkTable getLimelightTable(Limelight limelight) { + return switch (limelight) { + case SHOOTER -> LimelightHelpers.getLimelightNTTable(Limelight.SHOOTER.getName()); + case FRONT_LEFT -> LimelightHelpers.getLimelightNTTable(Limelight.FRONT_LEFT.getName()); + case FRONT_RIGHT -> LimelightHelpers.getLimelightNTTable(Limelight.FRONT_RIGHT.getName()); + default -> throw new IllegalArgumentException("Invalid limelight " + limelight); + }; + } + + // @Override + // public boolean canSeeAprilTags(Limelight limelight) { + // table.getEntry("tv").setInteger(result.hasTargets() ? 1 : 0); + + // } + @Override + public void setHeadingInfo(double headingDegrees, double headingRateDegrees) { + super.setHeadingInfo(headingDegrees, headingRateDegrees); + } + + @Override + public Pose2d getLastSeenPose() { + return lastSeenPose; + } +} diff --git a/src/main/java/frc/robot/subsystems/vision/ThreadManager.java b/src/main/java/frc/robot/subsystems/vision/ThreadManager.java new file mode 100644 index 00000000..f0113e80 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/vision/ThreadManager.java @@ -0,0 +1,80 @@ +package frc.robot.subsystems.vision; + +import frc.robot.subsystems.vision.VisionInterface.VisionInputs; +import java.util.Map; +import java.util.concurrent.ConcurrentHashMap; +import java.util.concurrent.ExecutorService; +import java.util.concurrent.Executors; +import java.util.concurrent.Future; +import java.util.concurrent.TimeUnit; + +public class ThreadManager { + private final ExecutorService executorService; + private final Map> threadFutures = new ConcurrentHashMap<>(); + + public ThreadManager(int maxThreads) { + executorService = Executors.newFixedThreadPool(maxThreads); + } + + // Start a thread for a specific task + public void startThread(String threadName, Runnable task) { + if (threadFutures.containsKey(threadName)) { + System.err.println("Thread " + threadName + " is already running."); + return; + } + + Future future = + executorService.submit( + () -> { + try { + task.run(); + } catch (Exception e) { + System.err.println("Error in thread " + threadName + ": " + e.getMessage()); + } + }); + + threadFutures.put(threadName, future); + } + + // Stop a specific thread + public void stopThread(String threadName) { + Future future = threadFutures.get(threadName); + if (future != null) { + future.cancel(true); + threadFutures.remove(threadName); + } else { + System.err.println("Thread " + threadName + " is not running."); + } + } + + // Submit a vision input task + public void startVisionInputTask(String threadName, VisionInputs inputs, Runnable visionTask) { + startThread( + threadName, + () -> { + try { + while (!Thread.currentThread().isInterrupted()) { + visionTask.run(); + Thread.sleep(VisionConstants.THREAD_SLEEP_MS); // Sleep to avoid excessive updates + } + } catch (InterruptedException e) { + Thread.currentThread().interrupt(); + } + }); + } + + // Shut down all threads + public void shutdownAllThreads() { + threadFutures.values().forEach(future -> future.cancel(true)); + threadFutures.clear(); + executorService.shutdown(); + try { + if (!executorService.awaitTermination(5, TimeUnit.SECONDS)) { + executorService.shutdownNow(); + } + } catch (InterruptedException e) { + executorService.shutdownNow(); + Thread.currentThread().interrupt(); + } + } +} diff --git a/src/main/java/frc/robot/subsystems/vision/Vision.java b/src/main/java/frc/robot/subsystems/vision/Vision.java index 72a4df2d..cf714238 100644 --- a/src/main/java/frc/robot/subsystems/vision/Vision.java +++ b/src/main/java/frc/robot/subsystems/vision/Vision.java @@ -1,54 +1,60 @@ +// All praise 254 lib + package frc.robot.subsystems.vision; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.subsystems.vision.VisionConstants.Limelight; import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; public class Vision extends SubsystemBase { - private final VisionIO visionIO; - private final VisionIOInputsAutoLogged inputs = new VisionIOInputsAutoLogged(); + private final VisionInterface visionInterface; + private final VisionInputsAutoLogged inputs = new VisionInputsAutoLogged(); - public Vision(VisionIO visionIO) { + public Vision(VisionInterface visionInterface) { // Initializing Fields - this.visionIO = visionIO; + this.visionInterface = visionInterface; } @Override public void periodic() { // Updates limelight inputs - visionIO.updateInputs(inputs); - Logger.processInputs(visionIO.getLimelightName(0), inputs); + visionInterface.updateInputs(inputs); + Logger.processInputs("Vision/", inputs); } // Add methods to support DriveCommand - public int getNumberOfAprilTags(int limelightNumber) { - return visionIO.getNumberOfAprilTags(limelightNumber); + public int getNumberOfAprilTags(Limelight limelight) { + return inputs.limelightTargets[limelight.getId()]; } - public double getLimelightAprilTagDistance(int limelightNumber) { - return visionIO.getLimelightAprilTagDistance(limelightNumber); + public double getLimelightAprilTagDistance(Limelight limelight) { + return inputs.limelightAprilTagDistance[limelight.getId()]; } - public double getTimeStampSeconds(int limelightNumber) { - return visionIO.getTimeStampSeconds(limelightNumber); + public double getTimeStampSeconds(Limelight limelight) { + return inputs.limelightTimestamp[limelight.getId()]; } - public double getLatencySeconds(int limelightNumber) { - return visionIO.getLatencySeconds(limelightNumber); + public double getLatencySeconds(Limelight limelight) { + return inputs.limelightLatency[limelight.getId()]; } public void setHeadingInfo(double headingDegrees, double headingRateDegrees) { - visionIO.setHeadingInfo(headingDegrees, headingRateDegrees); + visionInterface.setHeadingInfo(headingDegrees, headingRateDegrees); } @AutoLogOutput(key = "Vision/Has Targets") - public boolean canSeeAprilTags(int limelightNumber) { - return visionIO.canSeeAprilTags( - limelightNumber); // Assuming we're checking the shooter limelight + public boolean canSeeAprilTags(Limelight limelight) { + return inputs.limelightSeesAprilTags[limelight.getId()]; + } + + public Pose2d getPoseFromAprilTags(Limelight limelight) { + return inputs.limelightCalculatedPose[limelight.getId()]; } - public Pose2d getPoseFromAprilTags(int limelightNumber) { - return visionIO.getPoseFromAprilTags(limelightNumber); + public Pose2d getLastSeenPose() { + return visionInterface.getLastSeenPose(); } } diff --git a/src/main/java/frc/robot/subsystems/vision/VisionConstants.java b/src/main/java/frc/robot/subsystems/vision/VisionConstants.java index 4cf41ad4..b4ceef69 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionConstants.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionConstants.java @@ -1,9 +1,66 @@ package frc.robot.subsystems.vision; +import edu.wpi.first.apriltag.AprilTagFieldLayout; +import edu.wpi.first.apriltag.AprilTagFields; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.math.util.Units; +import org.photonvision.PhotonCamera; public final class VisionConstants { + public enum Limelight { + SHOOTER(0, VisionConstants.SHOOTER_LIMELIGHT_NAME), + FRONT_LEFT(1, VisionConstants.FRONT_LEFT_LIMELIGHT_NAME), + FRONT_RIGHT(2, VisionConstants.FRONT_RIGHT_LIMELIGHT_NAME); + private final int id; + private final String name; + + Limelight(int id, String name) { + this.id = id; + this.name = name; + } + + public int getId() { + return id; + } + + public String getName() { + return name; + } + + public static Limelight fromId(int id) { + return switch (id) { + case 0 -> SHOOTER; + case 1 -> FRONT_LEFT; + case 2 -> FRONT_RIGHT; + default -> throw new IllegalArgumentException("Invalid Limelight ID: " + id); + }; + } + } + + public static final Transform3d SHOOTER_TRANSFORM = + new Transform3d( + new Translation3d(-0.3119324724, 0.0, 0.1865472012), new Rotation3d(0.0, 35, 180.0)); + public static final Transform3d FRONT_LEFT_TRANSFORM = + new Transform3d( + new Translation3d(0.2749477356, -0.269958439, 0.2318054546), + new Rotation3d(0.0, 25, -35)); + public static final Transform3d FRONT_RIGHT_TRANSFORM = + new Transform3d( + new Translation3d(0.2816630892, 0.2724405524, 0.232156), new Rotation3d(0.0, 25, 35)); + + public static final PhotonCamera SHOOTER_CAMERA = new PhotonCamera(Limelight.SHOOTER.getName()); + public static final PhotonCamera FRONT_LEFT_CAMERA = + new PhotonCamera(Limelight.FRONT_LEFT.getName()); + public static final PhotonCamera FRONT_RIGHT_CAMERA = + new PhotonCamera(Limelight.FRONT_RIGHT.getName()); + + public static final int THREAD_SLEEP_MS = 20; + + public static final AprilTagFieldLayout FIELD_LAYOUT = + AprilTagFieldLayout.loadField(AprilTagFields.kDefaultField); public static final double VISION_X_POS_TRUST = 0.5; // meters public static final double VISION_Y_POS_TRUST = 0.5; // meters public static final double VISION_ANGLE_TRUST = Units.degreesToRadians(50); // radians @@ -17,6 +74,9 @@ public final class VisionConstants { public static final double MEGA_TAG_2_DISTANCE_THRESHOLD = 5; // TODO: Tune + public static final double MEGA_TAG_TRANSLATION_DISCREPANCY_THRESHOLD = 0.5; // TODO: tune + public static final double MEGA_TAG_ROTATION_DISCREPANCY_THREASHOLD = 45; + public static final String SHOOTER_LIMELIGHT_NAME = "limelight-shooter"; public static final int SHOOTER_LIMELIGHT_NUMBER = 0; public static final String FRONT_LEFT_LIMELIGHT_NAME = "limelight-left"; diff --git a/src/main/java/frc/robot/subsystems/vision/VisionIO.java b/src/main/java/frc/robot/subsystems/vision/VisionIO.java deleted file mode 100644 index 668cfeb0..00000000 --- a/src/main/java/frc/robot/subsystems/vision/VisionIO.java +++ /dev/null @@ -1,48 +0,0 @@ -package frc.robot.subsystems.vision; - -import edu.wpi.first.math.geometry.Pose2d; -import org.littletonrobotics.junction.AutoLog; - -public interface VisionIO { - @AutoLog - class VisionIOInputs { - public boolean cameraConnected = false; - public double latency = 0.0; - public double fiducialMarksID = 0.0; - - public int camerasAmount = 0; - public int targetsCount = 0; - } - - default void updateInputs(VisionIOInputs inputs) {} - - default String getLimelightName(int limelightNumber) { - return ""; - } - - default double getLatencySeconds(int limelightNumber) { - return 0.0; - } - - default double getTimeStampSeconds(int limelightNumber) { - return 0.0; - } - - default boolean canSeeAprilTags(int limelightNumber) { - return false; - } - - default double getLimelightAprilTagDistance(int limelightNumber) { - return 0.0; - } - - default int getNumberOfAprilTags(int limelightNumber) { - return 0; - } - - default Pose2d getPoseFromAprilTags(int limelightNumber) { - return null; - } - - default void setHeadingInfo(double headingDegrees, double headingRateDegrees) {} -} diff --git a/src/main/java/frc/robot/subsystems/vision/VisionIOReal.java b/src/main/java/frc/robot/subsystems/vision/VisionIOReal.java deleted file mode 100644 index 971e2613..00000000 --- a/src/main/java/frc/robot/subsystems/vision/VisionIOReal.java +++ /dev/null @@ -1,439 +0,0 @@ -package frc.robot.subsystems.vision; - -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import frc.robot.Constants.FieldConstants; -import frc.robot.extras.vision.LimelightHelpers; -import frc.robot.extras.vision.LimelightHelpers.PoseEstimate; -import java.util.Map; -import java.util.concurrent.ConcurrentHashMap; -import java.util.concurrent.ExecutorService; -import java.util.concurrent.Executors; -import java.util.concurrent.TimeUnit; -import java.util.concurrent.atomic.AtomicBoolean; - -public class VisionIOReal implements VisionIO { - - private Pose2d lastSeenPose = new Pose2d(); - private double headingDegrees = 0; - private double headingRateDegreesPerSecond = 0; - private final Map limelightThreads = new ConcurrentHashMap<>(); - private final ExecutorService executorService = Executors.newFixedThreadPool(3); - - /** - * The pose estimates from the limelights in the following order {shooterLimelight, - * frontLeftLimelight, frontRightLimelight} - */ - private PoseEstimate[] limelightEstimates; - - public VisionIOReal() { - limelightEstimates = new PoseEstimate[3]; - for (int limelightNumber = 0; limelightNumber < limelightEstimates.length; limelightNumber++) { - limelightThreads.put(limelightNumber, new AtomicBoolean(true)); - limelightEstimates[limelightNumber] = new PoseEstimate(); - } - } - - @Override - public void updateInputs(VisionIOInputs inputs) { - inputs.camerasAmount = limelightEstimates.length; - inputs.cameraConnected = true; - - for (int limelightNumber = 0; limelightNumber < limelightEstimates.length; limelightNumber++) { - // Update camera connection status - inputs.cameraConnected = true; - - // Add number of April tags seen by this limelight - inputs.targetsCount += getNumberOfAprilTags(limelightNumber); - - // Add fiducial mark ID - inputs.fiducialMarksID = LimelightHelpers.getFiducialID(getLimelightName(limelightNumber)); - - // Add latency for this limelight - inputs.latency += getLatencySeconds(limelightNumber) / 1000.0; - } - - // Calculate average latency - inputs.latency /= limelightEstimates.length; - - periodic(); - } - - /** - * Checks if the specified limelight can fully see one or more April Tag. - * - * @param limelightNumber the number of the limelight - * @return true if the limelight can fully see one or more April Tag - */ - @Override - public boolean canSeeAprilTags(int limelightNumber) { - // First checks if it can see an april tag, then checks if it is fully in frame - // Different Limelights have different FOVs - if (getNumberOfAprilTags(limelightNumber) >= VisionConstants.MIN_APRIL_TAG_ID - && getNumberOfAprilTags(limelightNumber) <= VisionConstants.MAX_APRIL_TAG_ID) { - if (getLimelightName(limelightNumber).equals(VisionConstants.SHOOTER_LIMELIGHT_NAME)) { - return Math.abs(LimelightHelpers.getTX(getLimelightName(limelightNumber))) - <= VisionConstants.LL3G_FOV_MARGIN_OF_ERROR; - } else { - return Math.abs(LimelightHelpers.getTX(getLimelightName(limelightNumber))) - <= VisionConstants.LL3_FOV_MARGIN_OF_ERROR; - } - } - return false; - } - - /** - * Gets the JSON dump from the specified limelight and puts it into a PoseEstimate object, which - * is then placed into its corresponding spot in the limelightEstimates array. - * - * @param limelightNumber the number of the limelight - */ - public PoseEstimate enabledPoseUpdate(int limelightNumber) { - if (canSeeAprilTags(limelightNumber) && isValidPoseEstimate(limelightNumber)) { - if (isLargeDiscrepancyBetweenMegaTag1And2(limelightNumber) - && getLimelightAprilTagDistance(limelightNumber) - < VisionConstants.MEGA_TAG_2_DISTANCE_THRESHOLD) { - return limelightEstimates[limelightNumber] = getMegaTag1PoseEstimate(limelightNumber); - } else if (headingRateDegreesPerSecond < VisionConstants.MEGA_TAG_2_MAX_HEADING_RATE) { - LimelightHelpers.SetRobotOrientation( - getLimelightName(limelightNumber), headingDegrees, 0, 0, 0, 0, 0); - return limelightEstimates[limelightNumber] = getMegaTag2PoseEstimate(limelightNumber); - } else { - return limelightEstimates[limelightNumber] = getMegaTag1PoseEstimate(limelightNumber); - } - } - return limelightEstimates[limelightNumber] = new PoseEstimate(); - } - - /** - * If the robot is not enabled, update the pose using MegaTag1 and after it is enabled, run {@link - * #enabledPoseUpdate(int)} - * - * @param limelightNumber the number of the limelight - */ - public void updatePoseEstimate(int limelightNumber) { - limelightEstimates[limelightNumber] = - DriverStation.isEnabled() - ? enabledPoseUpdate(limelightNumber) - : getMegaTag1PoseEstimate(limelightNumber); - } - - /** - * Checks if there is a large discrepancy between the MegaTag1 and MegaTag2 estimates. - * - * @param limelightNumber the number of the limelight - * @return true if the discrepancy is larger than the defined threshold, false otherwise - */ - public boolean isLargeDiscrepancyBetweenMegaTag1And2(int limelightNumber) { - PoseEstimate megaTag1Estimate = getMegaTag1PoseEstimate(limelightNumber); - PoseEstimate megaTag2Estimate = getMegaTag2PoseEstimate(limelightNumber); - - // Extract the positions of the two poses - Translation2d megaTag1TranslationMeters = megaTag1Estimate.pose.getTranslation(); - Translation2d megaTag2TranslationMeters = megaTag2Estimate.pose.getTranslation(); - - double megaTag1RotationDegrees = megaTag1Estimate.pose.getRotation().getDegrees(); - double megaTag2RotationDegrees = megaTag2Estimate.pose.getRotation().getDegrees(); - - // Calculate the discrepancy between the two MegaTag translations in meters - double megaTagTranslationDiscrepancyMeters = - megaTag1TranslationMeters.getDistance(megaTag2TranslationMeters); - double megaTagRotationDiscrepancyDegrees = - Math.abs(megaTag1RotationDegrees - megaTag2RotationDegrees); - - // Define a threshold (meters) for what constitutes a "large" discrepancy - // This value should be determined based on your testing - double thresholdMeters = 0.5; - double thresholdDegrees = 45; - - // Check if the discrepancy is larger than the threshold (meters) - return megaTagTranslationDiscrepancyMeters > thresholdMeters - || megaTagRotationDiscrepancyDegrees > thresholdDegrees; - } - - /** - * Gets the MegaTag1 pose of the robot calculated by specified limelight via any April Tags it - * sees - * - * @param limelightNumber the number of the limelight - * @return the MegaTag1 pose of the robot, if the limelight can't see any April Tags, it will - * return 0 for x, y, and theta - */ - public PoseEstimate getMegaTag1PoseEstimate(int limelightNumber) { - return LimelightHelpers.getBotPoseEstimate_wpiBlue(getLimelightName(limelightNumber)); - } - - /** - * Gets the MegaTag2 pose of the robot calculated by specified limelight via any April Tags it - * sees - * - * @param limelightNumber the number of the limelight - * @return the MegaTag2 pose of the robot, if the limelight can't see any April Tags, it will - * return 0 for x, y, and theta - */ - public PoseEstimate getMegaTag2PoseEstimate(int limelightNumber) { - return LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(getLimelightName(limelightNumber)); - } - - /** - * Checks if the MT1 and MT2 pose estimate exists and whether it is within the field - * - * @param limelightNumber the number of the limelight - * @return true if the pose estimate exists within the field and the pose estimate is not null - */ - public boolean isValidPoseEstimate(int limelightNumber) { - PoseEstimate megaTag1Estimate = getMegaTag1PoseEstimate(limelightNumber); - PoseEstimate megaTag2Estimate = getMegaTag2PoseEstimate(limelightNumber); - - return LimelightHelpers.isValidPoseEstimate(megaTag1Estimate) - && LimelightHelpers.isValidPoseEstimate(megaTag2Estimate) - && isWithinFieldBounds(megaTag1Estimate, megaTag2Estimate); - } - - /** - * Checks whether the pose estimate for MT1 and MT2 is within the field - * - * @param megaTag1Estimate the MT1 pose estimate to check - * @param megaTag2Estimate the MT2 pose estimate to check - */ - private boolean isWithinFieldBounds( - PoseEstimate megaTag1Estimate, PoseEstimate megaTag2Estimate) { - return (megaTag1Estimate.pose.getX() > 0 - && megaTag1Estimate.pose.getX() <= FieldConstants.FIELD_WIDTH_METERS) - && (megaTag1Estimate.pose.getY() > 0 - && megaTag1Estimate.pose.getY() <= FieldConstants.FIELD_WIDTH_METERS) - && (megaTag2Estimate.pose.getX() > 0 - && megaTag2Estimate.pose.getX() <= FieldConstants.FIELD_WIDTH_METERS) - && (megaTag2Estimate.pose.getY() > 0 - && megaTag2Estimate.pose.getY() <= FieldConstants.FIELD_WIDTH_METERS); - } - - /** - * Gets the pose of the robot calculated by specified limelight via any April Tags it sees - * - * @param limelightNumber the number of the limelight - * @return the pose of the robot, if the limelight can't see any April Tags, it will return 0 for - * x, y, and theta - */ - @Override - public Pose2d getPoseFromAprilTags(int limelightNumber) { - return limelightEstimates[limelightNumber].pose; - } - - /** Returns how many april tags the limelight that is being used for pose estimation can see. */ - @Override - public int getNumberOfAprilTags(int limelightNumber) { - return limelightEstimates[limelightNumber].tagCount; - } - - /** - * Returns the timestamp for the vision measurement from the limelight that is being used for pose - * estimation. - */ - @Override - public double getTimeStampSeconds(int limelightNumber) { - return limelightEstimates[limelightNumber].timestampSeconds / 1000.0; - } - - /** - * Returns the latency in seconds of when the limelight that is being used for pose estimation - * calculated the robot's pose. It adds the pipeline latency, capture latency, and json parsing - * latency. - */ - @Override - public double getLatencySeconds(int limelightNumber) { - return (limelightEstimates[limelightNumber].latency) / 1000.0; - } - - /** Gets the pose calculated the last time a limelight saw an April Tag */ - public Pose2d getLastSeenPose() { - return lastSeenPose; - } - - /** - * Gets the average distance between the specified limelight and the April Tags it sees - * - * @param limelightNumber the number of the limelight - * @return the average distance between the robot and the April Tag(s) in meters - */ - public double getLimelightAprilTagDistance(int limelightNumber) { - if (canSeeAprilTags(limelightNumber)) { - return limelightEstimates[limelightNumber].avgTagDist; - } - // To be safe returns a big distance from the april tags if it can't see any - return Double.MAX_VALUE; - } - - /** - * Sets the heading and heading rate of the robot, this is used for deciding between MegaTag 1 and - * 2 for pose estimation. - * - * @param headingDegrees the angle the robot is facing in degrees (0 degrees facing the red - * alliance) - * @param headingRateDegrees the rate the robot is rotating, CCW positive - */ - public void setHeadingInfo(double headingDegrees, double headingRateDegrees) { - this.headingDegrees = headingDegrees; - this.headingRateDegreesPerSecond = headingRateDegrees; - } - - /** - * Gets the limelight name associated with the specified limelight number/index - * - * @param limelightNumber the limelight number - * @return 0 = limelight-shooter, 1 = limelight-left, 2 = limelight-right - */ - public String getLimelightName(int limelightNumber) { - return switch (limelightNumber) { - case 0 -> VisionConstants.SHOOTER_LIMELIGHT_NAME; - case 1 -> VisionConstants.FRONT_LEFT_LIMELIGHT_NAME; - case 2 -> VisionConstants.FRONT_RIGHT_LIMELIGHT_NAME; - default -> - throw new IllegalArgumentException("You entered a number for a non-existent limelight"); - }; - } - - /** - * This checks is there is new pose detected by a limelight, and if so, updates the pose estimate - * - * @param limelightNumber the limelight number - */ - public void checkAndUpdatePose(int limelightNumber) { - double last_TX = 0; - double last_TY = 0; - - // Syncronization block to ensure thread safety during the critical section where pose - // information is read and compared. - // This helps prevent race conditions, where one limelight may be updating an object that - // another limelight is reading. - // A race condition could cause unpredictable things to happen. Such as causing a limelight to - // be unable to reference an - // object, as its reference was modified earlier. - synchronized (this) { - try { - double current_TX = LimelightHelpers.getTX(getLimelightName(limelightNumber)); - double current_TY = LimelightHelpers.getTY(getLimelightName(limelightNumber)); - - // This checks if the limelight reading is new. The reasoning being that if the TX and TY - // are EXACTLY the same, it hasn't updated yet with a new reading. We are doing it this way, - // because to get the timestamp of the reading, you need to parse the JSON dump which can be - // very demanding whereas this only has to get the Network Table entries for TX and TY. - if (current_TX != last_TX || current_TY != last_TY) { - updatePoseEstimate(limelightNumber); - limelightThreads.computeIfPresent( - limelightNumber, (key, value) -> new AtomicBoolean(true)); - // This is to keep track of the last valid pose calculated by the limelights - // it is used when the driver resets the robot odometry to the limelight calculated - // position - if (canSeeAprilTags(limelightNumber)) { - lastSeenPose = getMegaTag1PoseEstimate(limelightNumber).pose; - } - } else { - // Retrieve the AtomicBoolean for the given limelight number - AtomicBoolean isThreadRunning = - limelightThreads.getOrDefault(limelightNumber, new AtomicBoolean()); - // Only stop the thread if it's currently running - if (isThreadRunning.get()) { - // stop the thread for the specified limelight - stopThread(limelightNumber); - } - } - last_TX = current_TX; - last_TY = current_TY; - } catch (Exception e) { - System.err.println( - "Error communicating with the: " - + getLimelightName(limelightNumber) - + ": " - + e.getMessage()); - } - } - } - - /** - * Starts a separate thread dedicated to updating the pose estimate for a specified limelight. - * This approach is adopted to prevent loop overruns that would occur if we attempted to parse the - * JSON dump for each limelight sequentially within a single scheduler loop. - * - *

To achieve efficient and safe parallel execution, an ExecutorService is utilized to manage - * the lifecycle of these threads. - * - *

Each thread continuously runs the {@link #checkAndUpdatePose(int)} method as long as the - * corresponding limelight's thread is marked as "running". This ensures that pose estimates are - * updated in real-time, leveraging the parallel processing capabilities of the executor service. - * - * @param limelightNumber the limelight number - */ - public void visionThread(int limelightNumber) { - - executorService.submit( - () -> { - try { - // while (limelightThreads.get(limelightNumber).get()) { - checkAndUpdatePose(limelightNumber); - // } - } catch (Exception e) { - System.err.println( - "Error executing task for the: " - + getLimelightName(limelightNumber) - + ": " - + e.getMessage()); - } - }); - } - - /** - * Sets the AtomicBoolean 'runningThreads' to false for the specified limelight. Stops the thread - * for the specified limelight. - * - * @param limelightNumber the limelight number - */ - public void stopThread(int limelightNumber) { - try { - // Since we can't see an April Tag, set the estimate for the specified limelight to an empty - // PoseEstimate() - limelightEstimates[limelightNumber] = new PoseEstimate(); - limelightThreads.get(limelightNumber).set(false); - } catch (Exception e) { - System.err.println( - "Error stopping thread for the: " - + getLimelightName(limelightNumber) - + ": " - + e.getMessage()); - } - } - - /** Shuts down all the threads. */ - public void endAllThreads() { - // Properly shut down the executor service when the subsystem ends - executorService.shutdown(); // Prevents new tasks from being submitted - try { - // Wait for existing tasks to finish - if (!executorService.awaitTermination(5, TimeUnit.SECONDS)) { - executorService.shutdownNow(); - // Wait a bit longer for tasks to respond to being cancelled - if (!executorService.awaitTermination(5, TimeUnit.SECONDS)) - System.err.println("ExecutorService did not terminate"); - } - } catch (InterruptedException e) { - // (Re-)Cancel if current thread also interrupted - executorService.shutdownNow(); - // Preserve interrupt status - Thread.currentThread().interrupt(); - } - } - - // Override periodic method to start the vision threads at the beginning of each subsystem tick - public void periodic() { - visionThread(VisionConstants.SHOOTER_LIMELIGHT_NUMBER); - visionThread(VisionConstants.FRONT_LEFT_LIMELIGHT_NUMBER); - visionThread(VisionConstants.FRONT_RIGHT_LIMELIGHT_NUMBER); - SmartDashboard.putNumber("april tag dist", getLimelightAprilTagDistance(0)); - SmartDashboard.putString("shooter ll odom", getPoseFromAprilTags(0).toString()); - SmartDashboard.putString("left ll odom", getPoseFromAprilTags(1).toString()); - - SmartDashboard.putString("right ll odom", getPoseFromAprilTags(2).toString()); - } -} diff --git a/src/main/java/frc/robot/subsystems/vision/VisionIOSim.java b/src/main/java/frc/robot/subsystems/vision/VisionIOSim.java deleted file mode 100644 index 343d4b4e..00000000 --- a/src/main/java/frc/robot/subsystems/vision/VisionIOSim.java +++ /dev/null @@ -1,44 +0,0 @@ -// package frc.robot.subsystems.vision; - -// import edu.wpi.first.apriltag.AprilTagFieldLayout; -// import edu.wpi.first.math.geometry.Pose2d; -// import org.photonvision.simulation.PhotonCameraSim; -// import org.photonvision.simulation.VisionSystemSim; - -// import java.util.List; -// import java.util.function.Supplier; - -// public class VisionIOSim extends VisionIOPhoton { -// private final VisionSystemSim visionSystemSim; -// private final PhotonCameraSim[] camerasSim; -// private final Supplier robotActualPoseInSimulationSupplier; -// public VisionIOSim(List cameraProperties, AprilTagFieldLayout -// aprilTagFieldLayout, Supplier robotActualPoseInSimulationSupplier) { -// super(cameraProperties); - -// this.robotActualPoseInSimulationSupplier = robotActualPoseInSimulationSupplier; -// this.visionSystemSim = new VisionSystemSim("main"); -// visionSystemSim.addAprilTags(aprilTagFieldLayout); -// camerasSim = new PhotonCameraSim[cameraProperties.size()]; - -// for (int i = 0; i < cameraProperties.size(); i++) { -// final PhotonCameraSim cameraSim = new PhotonCameraSim( -// super.cameras[i], -// cameraProperties.get(i).getSimulationProperties() -// ); -// cameraSim.enableRawStream(true); -// cameraSim.enableProcessedStream(true); -// cameraSim.enableDrawWireframe(true); -// visionSystemSim.addCamera( -// camerasSim[i] = cameraSim, -// cameraProperties.get(i).robotToCamera -// ); -// } -// } - -// @Override -// public void updateInputs(VisionIOInputs inputs) { -// visionSystemSim.update(robotActualPoseInSimulationSupplier.get()); -// super.updateInputs(inputs); -// } -// } diff --git a/src/main/java/frc/robot/subsystems/vision/VisionInterface.java b/src/main/java/frc/robot/subsystems/vision/VisionInterface.java new file mode 100644 index 00000000..895ff312 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/vision/VisionInterface.java @@ -0,0 +1,57 @@ +package frc.robot.subsystems.vision; + +import edu.wpi.first.math.geometry.Pose2d; +import frc.robot.extras.vision.MegatagPoseEstimate; +import frc.robot.subsystems.vision.VisionConstants.Limelight; +import org.littletonrobotics.junction.AutoLog; + +public interface VisionInterface { + @AutoLog + class VisionInputs { + public boolean[] isLimelightConnected = new boolean[Limelight.values().length]; + + public MegatagPoseEstimate[] limelightMegatagPose = + new MegatagPoseEstimate[Limelight.values().length]; + public double[] limelightLatency = new double[Limelight.values().length]; + public int[] limelightTargets = new int[Limelight.values().length]; + public boolean[] limelightSeesAprilTags = new boolean[Limelight.values().length]; + + public Pose2d[] limelightCalculatedPose = new Pose2d[Limelight.values().length]; + public Pose2d limelightLastSeenPose = new Pose2d(); + public double[] limelightAprilTagDistance = new double[Limelight.values().length]; + + public double[] limelightTimestamp = new double[Limelight.values().length]; + } + + default void updateInputs(VisionInputs inputs) {} + + default double getLatencySeconds(Limelight limelight) { + return 0.0; + } + + default double getTimeStampSeconds(Limelight limelight) { + return 0.0; + } + + default boolean canSeeAprilTags(Limelight limelight) { + return false; + } + + default double getLimelightAprilTagDistance(Limelight limelight) { + return 0.0; + } + + default int getNumberOfAprilTags(Limelight limelight) { + return 0; + } + + default Pose2d getPoseFromAprilTags(Limelight limelight) { + return null; + } + + default void setHeadingInfo(double headingDegrees, double headingRateDegrees) {} + + default Pose2d getLastSeenPose() { + return null; + } +} diff --git a/vendordeps/Phoenix6-25.0.0-beta-2.json b/vendordeps/Phoenix6-25.0.0-beta-4.json similarity index 78% rename from vendordeps/Phoenix6-25.0.0-beta-2.json rename to vendordeps/Phoenix6-25.0.0-beta-4.json index b602fcb2..5a588aab 100644 --- a/vendordeps/Phoenix6-25.0.0-beta-2.json +++ b/vendordeps/Phoenix6-25.0.0-beta-4.json @@ -1,7 +1,7 @@ { - "fileName": "Phoenix6-25.0.0-beta-2.json", + "fileName": "Phoenix6-25.0.0-beta-4.json", "name": "CTRE-Phoenix (v6)", - "version": "25.0.0-beta-2", + "version": "25.0.0-beta-4", "frcYear": "2025", "uuid": "e995de00-2c64-4df5-8831-c1441420ff19", "mavenUrls": [ @@ -9,11 +9,6 @@ ], "jsonUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix6/latest/Phoenix6-frc2025-beta-latest.json", "conflictsWith": [ - { - "uuid": "3fcf3402-e646-4fa6-971e-18afe8173b1a", - "errorMessage": "The combined Phoenix-6-And-5 vendordep is no longer supported. Please remove the vendordep and instead add both the latest Phoenix 6 vendordep and Phoenix 5 vendordep.", - "offlineFileName": "Phoenix6And5.json" - }, { "uuid": "e7900d8d-826f-4dca-a1ff-182f658e98af", "errorMessage": "Users can not have both the replay and regular Phoenix 6 vendordeps in their robot program.", @@ -24,19 +19,20 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-java", - "version": "25.0.0-beta-2" + "version": "25.0.0-beta-4" } ], "jniDependencies": [ { "groupId": "com.ctre.phoenix6", "artifactId": "api-cpp", - "version": "25.0.0-beta-2", + "version": "25.0.0-beta-4", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "linuxathena" ], "simMode": "hwsim" @@ -44,12 +40,13 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "25.0.0-beta-2", + "version": "25.0.0-beta-4", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "linuxathena" ], "simMode": "hwsim" @@ -57,12 +54,13 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "api-cpp-sim", - "version": "25.0.0-beta-2", + "version": "25.0.0-beta-4", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "osxuniversal" ], "simMode": "swsim" @@ -70,12 +68,13 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "25.0.0-beta-2", + "version": "25.0.0-beta-4", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "osxuniversal" ], "simMode": "swsim" @@ -83,12 +82,13 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "25.0.0-beta-2", + "version": "25.0.0-beta-4", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "osxuniversal" ], "simMode": "swsim" @@ -96,12 +96,13 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "25.0.0-beta-2", + "version": "25.0.0-beta-4", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "osxuniversal" ], "simMode": "swsim" @@ -109,12 +110,13 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "25.0.0-beta-2", + "version": "25.0.0-beta-4", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "osxuniversal" ], "simMode": "swsim" @@ -122,12 +124,13 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simCANCoder", - "version": "25.0.0-beta-2", + "version": "25.0.0-beta-4", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "osxuniversal" ], "simMode": "swsim" @@ -135,12 +138,13 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "25.0.0-beta-2", + "version": "25.0.0-beta-4", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "osxuniversal" ], "simMode": "swsim" @@ -148,12 +152,27 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "25.0.0-beta-2", + "version": "25.0.0-beta-4", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANrange", + "version": "25.0.0-beta-4", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "osxuniversal" ], "simMode": "swsim" @@ -161,12 +180,13 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "25.0.0-beta-2", + "version": "25.0.0-beta-4", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "osxuniversal" ], "simMode": "swsim" @@ -176,7 +196,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-cpp", - "version": "25.0.0-beta-2", + "version": "25.0.0-beta-4", "libName": "CTRE_Phoenix6_WPI", "headerClassifier": "headers", "sharedLibrary": true, @@ -184,6 +204,7 @@ "binaryPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "linuxathena" ], "simMode": "hwsim" @@ -191,7 +212,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "25.0.0-beta-2", + "version": "25.0.0-beta-4", "libName": "CTRE_PhoenixTools", "headerClassifier": "headers", "sharedLibrary": true, @@ -199,6 +220,7 @@ "binaryPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "linuxathena" ], "simMode": "hwsim" @@ -206,7 +228,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "wpiapi-cpp-sim", - "version": "25.0.0-beta-2", + "version": "25.0.0-beta-4", "libName": "CTRE_Phoenix6_WPISim", "headerClassifier": "headers", "sharedLibrary": true, @@ -214,6 +236,7 @@ "binaryPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "osxuniversal" ], "simMode": "swsim" @@ -221,7 +244,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "25.0.0-beta-2", + "version": "25.0.0-beta-4", "libName": "CTRE_PhoenixTools_Sim", "headerClassifier": "headers", "sharedLibrary": true, @@ -229,6 +252,7 @@ "binaryPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "osxuniversal" ], "simMode": "swsim" @@ -236,7 +260,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "25.0.0-beta-2", + "version": "25.0.0-beta-4", "libName": "CTRE_SimTalonSRX", "headerClassifier": "headers", "sharedLibrary": true, @@ -244,6 +268,7 @@ "binaryPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "osxuniversal" ], "simMode": "swsim" @@ -251,7 +276,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "25.0.0-beta-2", + "version": "25.0.0-beta-4", "libName": "CTRE_SimVictorSPX", "headerClassifier": "headers", "sharedLibrary": true, @@ -259,6 +284,7 @@ "binaryPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "osxuniversal" ], "simMode": "swsim" @@ -266,7 +292,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "25.0.0-beta-2", + "version": "25.0.0-beta-4", "libName": "CTRE_SimPigeonIMU", "headerClassifier": "headers", "sharedLibrary": true, @@ -274,6 +300,7 @@ "binaryPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "osxuniversal" ], "simMode": "swsim" @@ -281,7 +308,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simCANCoder", - "version": "25.0.0-beta-2", + "version": "25.0.0-beta-4", "libName": "CTRE_SimCANCoder", "headerClassifier": "headers", "sharedLibrary": true, @@ -289,6 +316,7 @@ "binaryPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "osxuniversal" ], "simMode": "swsim" @@ -296,7 +324,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "25.0.0-beta-2", + "version": "25.0.0-beta-4", "libName": "CTRE_SimProTalonFX", "headerClassifier": "headers", "sharedLibrary": true, @@ -304,6 +332,7 @@ "binaryPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "osxuniversal" ], "simMode": "swsim" @@ -311,7 +340,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "25.0.0-beta-2", + "version": "25.0.0-beta-4", "libName": "CTRE_SimProCANcoder", "headerClassifier": "headers", "sharedLibrary": true, @@ -319,6 +348,23 @@ "binaryPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANrange", + "version": "25.0.0-beta-4", + "libName": "CTRE_SimProCANrange", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", "osxuniversal" ], "simMode": "swsim" @@ -326,7 +372,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "25.0.0-beta-2", + "version": "25.0.0-beta-4", "libName": "CTRE_SimProPigeon2", "headerClassifier": "headers", "sharedLibrary": true, @@ -334,6 +380,7 @@ "binaryPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "osxuniversal" ], "simMode": "swsim" diff --git a/vendordeps/Studica-2025.1.1-beta-3.json b/vendordeps/Studica-2025.1.1-beta-4.json similarity index 86% rename from vendordeps/Studica-2025.1.1-beta-3.json rename to vendordeps/Studica-2025.1.1-beta-4.json index 92a57d64..facfc721 100644 --- a/vendordeps/Studica-2025.1.1-beta-3.json +++ b/vendordeps/Studica-2025.1.1-beta-4.json @@ -1,13 +1,13 @@ { - "fileName": "Studica-2025.1.1-beta-3.json", + "fileName": "Studica-2025.1.1-beta-4.json", "name": "Studica", - "version": "2025.1.1-beta-3", + "version": "2025.1.1-beta-4", "uuid": "cb311d09-36e9-4143-a032-55bb2b94443b", "frcYear": "2025", "mavenUrls": [ "https://dev.studica.com/maven/release/2025/" ], - "jsonUrl": "https://dev.studica.com/releases/2025/Studica-2025.1.1-beta-3.json", + "jsonUrl": "https://dev.studica.com/releases/2025/Studica-2025.1.1-beta-4.json", "cppDependencies": [ { "artifactId": "Studica-cpp", @@ -24,7 +24,7 @@ "libName": "Studica", "sharedLibrary": false, "skipInvalidPlatforms": true, - "version": "2025.1.1-beta-3" + "version": "2025.1.1-beta-4" }, { "artifactId": "Studica-driver", @@ -41,14 +41,14 @@ "libName": "StudicaDriver", "sharedLibrary": false, "skipInvalidPlatforms": true, - "version": "2025.1.1-beta-3" + "version": "2025.1.1-beta-4" } ], "javaDependencies": [ { "artifactId": "Studica-java", "groupId": "com.studica.frc", - "version": "2025.1.1-beta-3" + "version": "2025.1.1-beta-4" } ], "jniDependencies": [ @@ -65,7 +65,7 @@ "osxuniversal", "windowsx86-64" ], - "version": "2025.1.1-beta-3" + "version": "2025.1.1-beta-4" } ] } diff --git a/vendordeps/photonlib-v2025.0.0-beta-8.json b/vendordeps/photonlib-v2025.0.0-beta-8.json new file mode 100644 index 00000000..1163429e --- /dev/null +++ b/vendordeps/photonlib-v2025.0.0-beta-8.json @@ -0,0 +1,71 @@ +{ + "fileName": "photonlib.json", + "name": "photonlib", + "version": "v2025.0.0-beta-8", + "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004", + "frcYear": "2025", + "mavenUrls": [ + "https://maven.photonvision.org/repository/internal", + "https://maven.photonvision.org/repository/snapshots" + ], + "jsonUrl": "https://maven.photonvision.org/repository/internal/org/photonvision/photonlib-json/1.0/photonlib-json-1.0.json", + "jniDependencies": [ + { + "groupId": "org.photonvision", + "artifactId": "photontargeting-cpp", + "version": "v2025.0.0-beta-8", + "skipInvalidPlatforms": true, + "isJar": false, + "validPlatforms": [ + "windowsx86-64", + "linuxathena", + "linuxx86-64", + "osxuniversal" + ] + } + ], + "cppDependencies": [ + { + "groupId": "org.photonvision", + "artifactId": "photonlib-cpp", + "version": "v2025.0.0-beta-8", + "libName": "photonlib", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxathena", + "linuxx86-64", + "osxuniversal" + ] + }, + { + "groupId": "org.photonvision", + "artifactId": "photontargeting-cpp", + "version": "v2025.0.0-beta-8", + "libName": "photontargeting", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxathena", + "linuxx86-64", + "osxuniversal" + ] + } + ], + "javaDependencies": [ + { + "groupId": "org.photonvision", + "artifactId": "photonlib-java", + "version": "v2025.0.0-beta-8" + }, + { + "groupId": "org.photonvision", + "artifactId": "photontargeting-java", + "version": "v2025.0.0-beta-8" + } + ] +}