fix visionConstants error

src/main/java/frc/robot/Constants.java

@@ -1,6 +1,8 @@
 package frc.robot;
 
 import com.pathplanner.lib.util.PIDConstants;
+import edu.wpi.first.apriltag.AprilTagFieldLayout;
+import edu.wpi.first.apriltag.AprilTagFields;
 import edu.wpi.first.math.util.Units;
 import swervelib.math.SwerveMath;
 
@@ -100,6 +102,16 @@ public static final class IOConstants {
  public static final double DEADBAND = 0.3;
  }
 
+ public static final class VisionConstants {
+ public static final double CAMERA_HEIGHT = 0.0;
+ public static final double APRILTAG_RED_SHOOTER_HEIGHT = 0.0;
+ public static final double CAMERA_PITCH = 0.0;
+ public static final String CAMERA_NAME = "photonvision";
+
+ public static final AprilTagFieldLayout APRIL_TAG_FIELD_LAYOUT =
+ AprilTagFields.k2024Crescendo.loadAprilTagLayoutField();
+ }
+
  public static final class TrapConstants {
  // FIXME: should say ID instead of PORT because it's a CAN ID
  public static final int VICTOR_MOTOR_PORT = 0;

src/main/java/frc/robot/subsystems/VisionSubsystem.java

@@ -0,0 +1,65 @@
+package frc.robot.subsystems;
+
+import edu.wpi.first.math.geometry.Pose3d;
+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 edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.Constants.VisionConstants;
+import java.util.List;
+import java.util.Optional;
+import org.photonvision.EstimatedRobotPose;
+import org.photonvision.PhotonCamera;
+import org.photonvision.PhotonPoseEstimator;
+import org.photonvision.PhotonPoseEstimator.PoseStrategy;
+import org.photonvision.PhotonUtils;
+import org.photonvision.targeting.PhotonPipelineResult;
+import org.photonvision.targeting.PhotonTrackedTarget;
+
+public class VisionSubsystem extends SubsystemBase {
+ private static PhotonCamera camera = new PhotonCamera(VisionConstants.CAMERA_NAME);
+ private Transform3d robotToCam =
+ new Transform3d(new Translation3d(0.0, 0.0, 0.0), new Rotation3d(0, 0, 0));
+
+ private PhotonPoseEstimator photonPoseEstimator =
+ new PhotonPoseEstimator(
+ VisionConstants.APRIL_TAG_FIELD_LAYOUT,
+ PoseStrategy.CLOSEST_TO_REFERENCE_POSE,
+ camera,
+ robotToCam);
+
+ public PhotonPipelineResult getResult() {
+ return camera.getLatestResult();
+ }
+
+ public boolean hasTarget() {
+ return getResult().hasTargets();
+ }
+
+ public List<PhotonTrackedTarget> getTargets() {
+ return getResult().getTargets();
+ }
+
+ public double getPoseFromTarget() {
+ PhotonTrackedTarget target = getTargets().get(0);
+ double range =
+ PhotonUtils.calculateDistanceToTargetMeters(
+ VisionConstants.CAMERA_HEIGHT,
+ VisionConstants.APRILTAG_RED_SHOOTER_HEIGHT,
+ VisionConstants.CAMERA_PITCH,
+ Units.degreesToRadians(target.getPitch()));
+
+ return range;
+ }
+
+ double getRangeMeters() {
+ double range = PhotonUtils.calculateDistanceToTargetMeters(0, 0, 0, 0);
+ return range;
+ }
+
+ public Pose3d getRobotPose() {
+ Optional<EstimatedRobotPose> pose = photonPoseEstimator.update();
+ return pose.get().estimatedPose;
+ }
+}