seems to work okay.

TechnoTitans · Oct 11, 2024 · f8ed1c2 · f8ed1c2
1 parent 95a93be
commit f8ed1c2
Show file tree

Hide file tree

Showing 3 changed files with 4 additions and 5 deletions.
diff --git a/src/main/java/frc/robot/constants/Constants.java b/src/main/java/frc/robot/constants/Constants.java
@@ -10,7 +10,7 @@
 import org.photonvision.PhotonPoseEstimator;
 
 public interface Constants {
-    RobotMode CURRENT_MODE = RobotMode.SIM;
+    RobotMode CURRENT_MODE = RobotMode.REAL;
     CompetitionType CURRENT_COMPETITION_TYPE = CompetitionType.TESTING;
     double LOOP_PERIOD_SECONDS = 0.02;
 
@@ -50,7 +50,7 @@ interface Vision {
         );
         Transform3d ROBOT_TO_REAR_NOTE = new Transform3d(
                 new Translation3d(Units.inchesToMeters(-16), Units.inchesToMeters(0), Units.inchesToMeters(14)),
-                new Rotation3d(0, Units.degreesToRadians(21), Units.degreesToRadians(180))
+                new Rotation3d(0, Units.degreesToRadians(22), Units.degreesToRadians(180))
         );
 
         double NOTE_HEIGHT_Z = Units.inchesToMeters(2);

diff --git a/src/main/java/frc/robot/subsystems/vision/PhotonVision.java b/src/main/java/frc/robot/subsystems/vision/PhotonVision.java
@@ -302,7 +302,7 @@ private void update() {
             if (noteTrackingResult != null) {
                 Logger.recordOutput(logKey + "/HasTargets", noteTrackingResult.hasTargets);
                 final Optional<Pose2d> optionalBestNotePose = noteTrackingResult
-                        .getBestNotePose(timestamp -> Optional.of(swerve.getPose()));
+                        .getBestNotePose(swerve::getPose);
 
                 Logger.recordOutput(logKey + "/HasBestNotePose", optionalBestNotePose.isPresent());
                 Logger.recordOutput(

diff --git a/src/main/java/frc/robot/subsystems/vision/result/NoteTrackingResult.java b/src/main/java/frc/robot/subsystems/vision/result/NoteTrackingResult.java
@@ -12,7 +12,6 @@
 import java.util.function.Function;
 
 public class NoteTrackingResult {
-    private static final double NoteTargetLocationHeight = Units.inchesToMeters(0); //Bounding box Offset (Selected in PV)
     private static final double DistanceOffsetMeters = Units.inchesToMeters(0); //Bounding box Offset (Selected in PV)
     private static final double AllowableTimestampTimeout = 1;
 
@@ -93,7 +92,7 @@ public static Rotation2d getTargetPitch(final PhotonTrackedTarget trackedTarget)
     public static double getNoteDistance(final Transform3d robotToCamera, final PhotonTrackedTarget trackedTarget) {
         return PhotonUtils.calculateDistanceToTargetMeters(
                 robotToCamera.getZ(),
-                NoteTargetLocationHeight,
+                Constants.Vision.NOTE_HEIGHT_Z,
                 -robotToCamera.getRotation().getY(), //Convert CCW+ to CW+
                 Units.degreesToRadians(trackedTarget.getPitch())
         ) / Math.cos(Units.degreesToRadians(trackedTarget.getYaw())) + DistanceOffsetMeters;