diff --git a/src/main/java/frc/robot/subsystems/vision/AprilTagVision.java b/src/main/java/frc/robot/subsystems/vision/AprilTagVision.java index 348c61d..474ee06 100644 --- a/src/main/java/frc/robot/subsystems/vision/AprilTagVision.java +++ b/src/main/java/frc/robot/subsystems/vision/AprilTagVision.java @@ -83,7 +83,7 @@ public void addVisionEstimateConsumer( timestampRobotPoseEstimateConsumers.add(timestampRobotPoseEstimateConsumer); } - public Stream cameras() { + private Stream cameras() { return Arrays.stream(cameras); } diff --git a/src/main/java/frc/robot/subsystems/vision/Camera.java b/src/main/java/frc/robot/subsystems/vision/Camera.java index 96b76b7..c687b61 100644 --- a/src/main/java/frc/robot/subsystems/vision/Camera.java +++ b/src/main/java/frc/robot/subsystems/vision/Camera.java @@ -25,10 +25,10 @@ public class Camera { private static final LoggedTunableNumberGroup group = new LoggedTunableNumberGroup("VisionResultsStatus"); - private static final LoggedTunableNumber thetaStdDevCoefficient = - group.build("thetaStdDevCoefficient", 0.075); private static final LoggedTunableNumber xyStdDevCoefficient = group.build("xyStdDevCoefficient", 0.075); + private static final LoggedTunableNumber thetaStdDevCoefficient = + group.build("thetaStdDevCoefficient", 0.085); private static final LoggedTunableNumber zHeightToleranceMeters = group.build("zHeightToleranceMeters", 0.5); diff --git a/src/main/java/frc/robot/subsystems/vision/VisionConstants.java b/src/main/java/frc/robot/subsystems/vision/VisionConstants.java index 8cd6db9..5634b1d 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionConstants.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionConstants.java @@ -9,10 +9,9 @@ public class VisionConstants { public record CameraConfig(String cameraName, Transform3d robotToCamera) {} - // TODO find actual values here, as well as in Asset config public static final CameraConfig FRONT_CAMERA = new CameraConfig( "frontCam", new Transform3d( - new Translation3d(0.805, 0, 0.0762), new Rotation3d(0, 0, Math.toRadians(30)))); + new Translation3d(0.805, 0, 0), new Rotation3d(0, Math.toRadians(-30), 0))); }