diff --git a/src/main/java/frc/robot/BuildConstants.java b/src/main/java/frc/robot/BuildConstants.java index 8cbe822..fcb85e8 100644 --- a/src/main/java/frc/robot/BuildConstants.java +++ b/src/main/java/frc/robot/BuildConstants.java @@ -5,12 +5,12 @@ 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 = 533; - public static final String GIT_SHA = "79a886e543767e4320fa8f9f31ac613afa9377c3"; - public static final String GIT_DATE = "2025-01-02 19:18:09 EST"; + 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-03 21:17:04 EST"; - public static final long BUILD_UNIX_TIME = 1735957024697L; + 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/commands/drive/DriveCommandBase.java b/src/main/java/frc/robot/commands/drive/DriveCommandBase.java index 73ae452..efadafa 100644 --- a/src/main/java/frc/robot/commands/drive/DriveCommandBase.java +++ b/src/main/java/frc/robot/commands/drive/DriveCommandBase.java @@ -42,8 +42,8 @@ public void execute() { vision.setHeadingInfo( swerveDrive.getPose().getRotation().getDegrees(), swerveDrive.getGyroRate()); calculatePoseFromLimelight(Limelight.SHOOTER); - calculatePoseFromLimelight(Limelight.FRONT_LEFT); - calculatePoseFromLimelight(Limelight.FRONT_RIGHT); + // calculatePoseFromLimelight(Limelight.FRONT_LEFT); + // calculatePoseFromLimelight(Limelight.FRONT_RIGHT); } public void calculatePoseFromLimelight(Limelight limelight) { diff --git a/src/main/java/frc/robot/subsystems/vision/SimulatedVision.java b/src/main/java/frc/robot/subsystems/vision/SimulatedVision.java index f2c5bd2..a4e4677 100644 --- a/src/main/java/frc/robot/subsystems/vision/SimulatedVision.java +++ b/src/main/java/frc/robot/subsystems/vision/SimulatedVision.java @@ -24,6 +24,7 @@ public class SimulatedVision extends PhysicalVision { 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; @@ -63,9 +64,10 @@ public SimulatedVision(Supplier robotSimulationPose) { frontRightCameraSim = new PhotonCameraSim(getSimulationCamera(Limelight.FRONT_RIGHT), cameraProperties); - visionSim.addCamera(shooterCameraSim, VisionConstants.SHOOTER_TRANSFORM.inverse()); - visionSim.addCamera(frontLeftCameraSim, VisionConstants.FRONT_LEFT_TRANSFORM.inverse()); - visionSim.addCamera(frontRightCameraSim, VisionConstants.FRONT_RIGHT_TRANSFORM.inverse()); + 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); @@ -91,6 +93,7 @@ public void updateInputs(VisionInputs inputs) { visionSim.update(robotSimulationPose.get()); Logger.recordOutput("Vision/SimIO/updateSimPose", robotSimulationPose.get()); } + super.updateInputs(inputs); for (Limelight limelight : Limelight.values()) { writeToTable( @@ -101,7 +104,6 @@ public void updateInputs(VisionInputs inputs) { // inputs.limelightAprilTagDistance[limelight.getId()] = // getLimelightAprilTagDistance(limelight); } - super.updateInputs(inputs); } private void writeToTable( @@ -144,19 +146,16 @@ private void writeToTable( table .getEntry("botpose_orb_wpiblue") .setDoubleArray(pose_data.stream().mapToDouble(Double::doubleValue).toArray()); - tagCount[limelight.getId()] = result.getMultiTagResult().get().fiducialIDsUsed.size(); - apriltagDist[limelight.getId()] = - result - .getMultiTagResult() - .get() - .estimatedPose - .best - .getTranslation() - .getDistance(result.getBestTarget().bestCameraToTarget.getTranslation()); - } - table.getEntry("tv").setInteger(result.hasTargets() ? 1 : 0); - table.getEntry("cl").setDouble(result.metadata.getLatencyMillis()); + 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())); + } } } @@ -178,26 +177,18 @@ public NetworkTable getLimelightTable(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 int getNumberOfAprilTags(Limelight limelight) { - // TODO Auto-generated method stub - return tagCount[limelight.getId()]; - } - - @Override - public double getLimelightAprilTagDistance(Limelight limelight) { - // TODO Auto-generated method stub - return apriltagDist[limelight.getId()]; - } - @Override public Pose2d getLastSeenPose() { - // TODO Auto-generated method stub - return super.getLastSeenPose(); + return lastSeenPose; } } diff --git a/src/main/java/frc/robot/subsystems/vision/Vision.java b/src/main/java/frc/robot/subsystems/vision/Vision.java index 4f34c45..cf71423 100644 --- a/src/main/java/frc/robot/subsystems/vision/Vision.java +++ b/src/main/java/frc/robot/subsystems/vision/Vision.java @@ -26,7 +26,7 @@ public void periodic() { // Add methods to support DriveCommand public int getNumberOfAprilTags(Limelight limelight) { - return visionInterface.getNumberOfAprilTags(limelight); + return inputs.limelightTargets[limelight.getId()]; } public double getLimelightAprilTagDistance(Limelight limelight) { @@ -55,6 +55,6 @@ public Pose2d getPoseFromAprilTags(Limelight limelight) { } public Pose2d getLastSeenPose() { - return inputs.limelightLastSeenPose; + return visionInterface.getLastSeenPose(); } }