diff --git a/src/main/java/frc/robot/config/RobotConfig.java b/src/main/java/frc/robot/config/RobotConfig.java index 0628300..07ae185 100644 --- a/src/main/java/frc/robot/config/RobotConfig.java +++ b/src/main/java/frc/robot/config/RobotConfig.java @@ -65,6 +65,8 @@ public record VisionConfig( double xyStdDev, double thetaStdDev, InterpolatedVisionDataset interpolatedVisionSet, + // Pose of robot (center of bottom of bellypan) relative to the center of callibration rig + // AprilTag Pose3d robotPoseCalibrationTargetSpace) {} public record LightsConfig(String canBusName, int deviceID) {} @@ -72,7 +74,7 @@ public record LightsConfig(String canBusName, int deviceID) {} // TODO: Change this to false during events public static final boolean IS_DEVELOPMENT = true; - public static final boolean IS_CALIBRATION = false; + public static final boolean IS_CAMERA_POSITION_CALIBRATION = false; public static final String SERIAL_NUMBER = System.getenv("serialnum"); diff --git a/src/main/java/frc/robot/vision/Limelight.java b/src/main/java/frc/robot/vision/Limelight.java index e8f4fcd..53970c1 100644 --- a/src/main/java/frc/robot/vision/Limelight.java +++ b/src/main/java/frc/robot/vision/Limelight.java @@ -52,20 +52,27 @@ public Optional getInterpolatedVisionResult() { return Optional.of(new VisionResult(interpolatedPose, rawResult.get().timestamp())); } - public void logCalibrationValues() { - var leftCameraPoseTargetSpace = LimelightHelpers.getCameraPose3d_TargetSpace(name); + public void logCameraPositionCalibrationValues() { + var cameraPoseTargetSpace = LimelightHelpers.getCameraPose3d_TargetSpace(name); var robotPoseTargetSpace = RobotConfig.get().vision().robotPoseCalibrationTargetSpace(); var leftCameraRobotRelativePose = - getRobotRelativeCameraOffset(robotPoseTargetSpace, leftCameraPoseTargetSpace); - DogLog.log("Calibration/" + name + "/Right", leftCameraRobotRelativePose.getX()); - DogLog.log("Calibration/" + name + "/Up", leftCameraRobotRelativePose.getY()); - DogLog.log("Calibration/" + name + "/Forward", leftCameraRobotRelativePose.getZ()); - DogLog.log("Calibration/" + name + "/Roll", leftCameraRobotRelativePose.getRotation().getX()); - DogLog.log("Calibration/" + name + "/Pitch", leftCameraRobotRelativePose.getRotation().getY()); - DogLog.log("Calibration/" + name + "/Yaw", leftCameraRobotRelativePose.getRotation().getZ()); + getRobotRelativeCameraPosition(robotPoseTargetSpace, cameraPoseTargetSpace); + DogLog.log("CameraPositionCalibration/" + name + "/Right", leftCameraRobotRelativePose.getX()); + DogLog.log("CameraPositionCalibration/" + name + "/Up", leftCameraRobotRelativePose.getY()); + DogLog.log( + "CameraPositionCalibration/" + name + "/Forward", leftCameraRobotRelativePose.getZ()); + DogLog.log( + "CameraPositionCalibration/" + name + "/Roll", + Units.radiansToDegrees(leftCameraRobotRelativePose.getRotation().getX())); + DogLog.log( + "CameraPositionCalibration/" + name + "/Pitch", + Units.radiansToDegrees(leftCameraRobotRelativePose.getRotation().getY())); + DogLog.log( + "CameraPositionCalibration/" + name + "/Yaw", + Units.radiansToDegrees(leftCameraRobotRelativePose.getRotation().getZ())); } - private Pose3d getRobotRelativeCameraOffset( + private Pose3d getRobotRelativeCameraPosition( Pose3d robotPoseTargetSpace, Pose3d seenCameraPoseTargetSpace) { // Positive X = Right var cameraLeftRight = seenCameraPoseTargetSpace.getX(); @@ -74,11 +81,11 @@ private Pose3d getRobotRelativeCameraOffset( // Positive Z = Backward, so flipped for common sense var cameraForwardBackward = -1 * (seenCameraPoseTargetSpace.getZ()); // Pitch rotates around left right axis (x according to LL coordinate systems) - var cameraPitch = Units.degreesToRadians(seenCameraPoseTargetSpace.getRotation().getX()); + var cameraPitch = seenCameraPoseTargetSpace.getRotation().getX(); // Roll rotates around forward backward axis (Z according to LL coordinate systems) - var cameraRoll = Units.degreesToRadians(seenCameraPoseTargetSpace.getRotation().getZ()); + var cameraRoll = seenCameraPoseTargetSpace.getRotation().getZ(); // Yaw rotates around up down axis (y according to LL coordinate systems) - var cameraYaw = Units.degreesToRadians(seenCameraPoseTargetSpace.getRotation().getY()); + var cameraYaw = seenCameraPoseTargetSpace.getRotation().getY(); var robotLeftRight = robotPoseTargetSpace.getX(); var robotUpDown = robotPoseTargetSpace.getY(); diff --git a/src/main/java/frc/robot/vision/VisionSubsystem.java b/src/main/java/frc/robot/vision/VisionSubsystem.java index 0575f3f..dabfdbd 100644 --- a/src/main/java/frc/robot/vision/VisionSubsystem.java +++ b/src/main/java/frc/robot/vision/VisionSubsystem.java @@ -84,9 +84,9 @@ public void robotPeriodic() { DogLog.log("Vision/Left/VisionState", leftLimelight.getState()); DogLog.log("Vision/Right/VisionState", rightLimelight.getState()); - if (RobotConfig.IS_CALIBRATION) { - leftLimelight.logCalibrationValues(); - rightLimelight.logCalibrationValues(); + if (RobotConfig.IS_CAMERA_POSITION_CALIBRATION) { + leftLimelight.logCameraPositionCalibrationValues(); + rightLimelight.logCameraPositionCalibrationValues(); } }