im gonna need to rewrite this shit - vision sim works but is high key…

… incredibly jank
TitaniumTigers4829 · Jan 4, 2025 · c1d909f · c1d909f
1 parent bf661f9
commit c1d909f
Show file tree

Hide file tree

Showing 4 changed files with 30 additions and 39 deletions.
diff --git 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
@@ -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
@@ -24,6 +24,7 @@ public class SimulatedVision extends PhysicalVision {
   PhotonCameraSim frontRightCameraSim;
   private final VisionSystemSim visionSim;
   private final Supplier<Pose2d> robotSimulationPose;
+  private Pose2d lastSeenPose = new Pose2d();
 
   private final int kResWidth = 1280;
   private final int kResHeight = 800;
@@ -63,9 +64,10 @@ public SimulatedVision(Supplier<Pose2d> 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
@@ -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();
   }
 }