From db42ae3426cdf6465f56bb9a592906460c9fa0a1 Mon Sep 17 00:00:00 2001 From: Simon <87281874+simonp17@users.noreply.github.com> Date: Mon, 25 Nov 2024 19:24:23 -0800 Subject: [PATCH] move calibration logging to Limelight.java --- .../java/frc/robot/config/CompConfig.java | 10 ++- .../java/frc/robot/config/RobotConfig.java | 4 +- src/main/java/frc/robot/vision/Limelight.java | 47 +++++++++++++ .../frc/robot/vision/VisionSubsystem.java | 66 +------------------ 4 files changed, 61 insertions(+), 66 deletions(-) diff --git a/src/main/java/frc/robot/config/CompConfig.java b/src/main/java/frc/robot/config/CompConfig.java index fa4d957..f6b5418 100644 --- a/src/main/java/frc/robot/config/CompConfig.java +++ b/src/main/java/frc/robot/config/CompConfig.java @@ -15,6 +15,8 @@ import com.ctre.phoenix6.signals.NeutralModeValue; import edu.wpi.first.math.filter.Debouncer; import edu.wpi.first.math.filter.Debouncer.DebounceType; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.util.Units; import frc.robot.config.RobotConfig.ArmConfig; import frc.robot.config.RobotConfig.IntakeConfig; @@ -216,7 +218,13 @@ class CompConfig { }, -77.0, 87.0), - new VisionConfig(4, 0.4, 0.4, InterpolatedVisionDataset.MADTOWN), + new VisionConfig( + 4, + 0.4, + 0.4, + InterpolatedVisionDataset.MADTOWN, + // right is positive x, up is positive y, forward is positive z + new Pose3d(0, -1.0, -1.5, new Rotation3d(0.0, 0.0, 0.0))), new LightsConfig("rio", 3)); private CompConfig() {} diff --git a/src/main/java/frc/robot/config/RobotConfig.java b/src/main/java/frc/robot/config/RobotConfig.java index 9caba9a..0628300 100644 --- a/src/main/java/frc/robot/config/RobotConfig.java +++ b/src/main/java/frc/robot/config/RobotConfig.java @@ -4,6 +4,7 @@ import com.ctre.phoenix6.mechanisms.swerve.utility.PhoenixPIDController; import com.revrobotics.CANSparkMax; import edu.wpi.first.math.filter.Debouncer; +import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap; import frc.robot.vision.interpolation.InterpolatedVisionDataset; import java.util.function.Consumer; @@ -63,7 +64,8 @@ public record VisionConfig( int translationHistoryArraySize, double xyStdDev, double thetaStdDev, - InterpolatedVisionDataset interpolatedVisionSet) {} + InterpolatedVisionDataset interpolatedVisionSet, + Pose3d robotPoseCalibrationTargetSpace) {} public record LightsConfig(String canBusName, int deviceID) {} diff --git a/src/main/java/frc/robot/vision/Limelight.java b/src/main/java/frc/robot/vision/Limelight.java index 2748a8a..e8f4fcd 100644 --- a/src/main/java/frc/robot/vision/Limelight.java +++ b/src/main/java/frc/robot/vision/Limelight.java @@ -3,7 +3,10 @@ import dev.doglog.DogLog; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.Timer; +import frc.robot.config.RobotConfig; import frc.robot.vision.interpolation.CameraDataset; import frc.robot.vision.interpolation.InterpolatedVision; import java.util.Optional; @@ -49,6 +52,50 @@ public Optional getInterpolatedVisionResult() { return Optional.of(new VisionResult(interpolatedPose, rawResult.get().timestamp())); } + public void logCalibrationValues() { + var leftCameraPoseTargetSpace = 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()); + } + + private Pose3d getRobotRelativeCameraOffset( + Pose3d robotPoseTargetSpace, Pose3d seenCameraPoseTargetSpace) { + // Positive X = Right + var cameraLeftRight = seenCameraPoseTargetSpace.getX(); + // Positive Y = Down, so flipped for common sense + var cameraUpDown = -1 * (seenCameraPoseTargetSpace.getY()); + // 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()); + // Roll rotates around forward backward axis (Z according to LL coordinate systems) + var cameraRoll = Units.degreesToRadians(seenCameraPoseTargetSpace.getRotation().getZ()); + // Yaw rotates around up down axis (y according to LL coordinate systems) + var cameraYaw = Units.degreesToRadians(seenCameraPoseTargetSpace.getRotation().getY()); + + var robotLeftRight = robotPoseTargetSpace.getX(); + var robotUpDown = robotPoseTargetSpace.getY(); + var robotForwardBackward = robotPoseTargetSpace.getZ(); + var robotPitch = robotPoseTargetSpace.getRotation().getY(); + var robotRoll = robotPoseTargetSpace.getRotation().getX(); + var robotYaw = robotPoseTargetSpace.getRotation().getZ(); + + var right = cameraLeftRight - robotLeftRight; + var up = cameraUpDown - robotUpDown; + var forward = cameraForwardBackward - robotForwardBackward; + var roll = cameraRoll - robotRoll; + var pitch = cameraPitch - robotPitch; + var yaw = cameraYaw - robotYaw; + return new Pose3d(right, up, forward, new Rotation3d(roll, pitch, yaw)); + } + private Optional getRawVisionResult() { var estimatePose = LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(limelightTableName); diff --git a/src/main/java/frc/robot/vision/VisionSubsystem.java b/src/main/java/frc/robot/vision/VisionSubsystem.java index a181c1a..0575f3f 100644 --- a/src/main/java/frc/robot/vision/VisionSubsystem.java +++ b/src/main/java/frc/robot/vision/VisionSubsystem.java @@ -2,9 +2,7 @@ import dev.doglog.DogLog; import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.DriverStation; import frc.robot.config.RobotConfig; @@ -26,10 +24,6 @@ public class VisionSubsystem extends StateMachine { private double roll; private double rollRate; - // right is positive x, up is positive y, forward is positive z - private final Pose3d robotPoseCalibrationTargetSpaceMetersRadians = - new Pose3d(0, -1.0, -1.5, new Rotation3d(0.0, 0.0, 0.0)); - public static final Pose2d ORIGINAL_RED_SPEAKER = new Pose2d( Units.inchesToMeters(652.73), Units.inchesToMeters(218.42), Rotation2d.fromDegrees(180)); @@ -77,63 +71,6 @@ public List getInterpolatedVisionResult() { return interpolatedVisionResult; } - private void logCalibrationValues() { - var leftCameraPoseTargetSpace = LimelightHelpers.getCameraPose3d_TargetSpace("limelight"); - var rightCameraPoseTargetSpace = LimelightHelpers.getCameraPose3d_TargetSpace("limelight"); - - var leftCameraRobotRelativePose = - getRobotRelativeCameraOffset( - robotPoseCalibrationTargetSpaceMetersRadians, leftCameraPoseTargetSpace); - var rightCameraRobotRelativePose = - getRobotRelativeCameraOffset( - robotPoseCalibrationTargetSpaceMetersRadians, rightCameraPoseTargetSpace); - - DogLog.log("Calibration/LeftCamera/Right", leftCameraRobotRelativePose.getX()); - DogLog.log("Calibration/LeftCamera/Up", leftCameraRobotRelativePose.getY()); - DogLog.log("Calibration/LeftCamera/Forward", leftCameraRobotRelativePose.getZ()); - DogLog.log("Calibration/LeftCamera/Roll", leftCameraRobotRelativePose.getRotation().getX()); - DogLog.log("Calibration/LeftCamera/Pitch", leftCameraRobotRelativePose.getRotation().getY()); - DogLog.log("Calibration/LeftCamera/Yaw", leftCameraRobotRelativePose.getRotation().getZ()); - - DogLog.log("Calibration/RightCamera/Right", rightCameraRobotRelativePose.getX()); - DogLog.log("Calibration/RightCamera/Up", rightCameraRobotRelativePose.getY()); - DogLog.log("Calibration/RightCamera/Forward", rightCameraRobotRelativePose.getZ()); - DogLog.log("Calibration/RightCamera/Roll", rightCameraRobotRelativePose.getRotation().getX()); - DogLog.log("Calibration/RightCamera/Pitch", rightCameraRobotRelativePose.getRotation().getY()); - DogLog.log("Calibration/RightCamera/Yaw", rightCameraRobotRelativePose.getRotation().getZ()); - } - - private Pose3d getRobotRelativeCameraOffset( - Pose3d robotPoseTargetSpace, Pose3d seenCameraPoseTargetSpace) { - // Positive X = Right - var cameraLeftRight = seenCameraPoseTargetSpace.getX(); - // Positive Y = Down, so flipped for common sense - var cameraUpDown = -1 * (seenCameraPoseTargetSpace.getY()); - // 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()); - // Roll rotates around forward backward axis (Z according to LL coordinate systems) - var cameraRoll = Units.degreesToRadians(seenCameraPoseTargetSpace.getRotation().getZ()); - // Yaw rotates around up down axis (y according to LL coordinate systems) - var cameraYaw = Units.degreesToRadians(seenCameraPoseTargetSpace.getRotation().getY()); - - var robotLeftRight = robotPoseTargetSpace.getX(); - var robotUpDown = robotPoseTargetSpace.getY(); - var robotForwardBackward = robotPoseTargetSpace.getZ(); - var robotPitch = robotPoseTargetSpace.getRotation().getY(); - var robotRoll = robotPoseTargetSpace.getRotation().getX(); - var robotYaw = robotPoseTargetSpace.getRotation().getZ(); - - var right = cameraLeftRight - robotLeftRight; - var up = cameraUpDown - robotUpDown; - var forward = cameraForwardBackward - robotForwardBackward; - var roll = cameraRoll - robotRoll; - var pitch = cameraPitch - robotPitch; - var yaw = cameraYaw - robotYaw; - return new Pose3d(right, up, forward, new Rotation3d(roll, pitch, yaw)); - } - @Override public void robotPeriodic() { super.robotPeriodic(); @@ -148,7 +85,8 @@ public void robotPeriodic() { DogLog.log("Vision/Right/VisionState", rightLimelight.getState()); if (RobotConfig.IS_CALIBRATION) { - logCalibrationValues(); + leftLimelight.logCalibrationValues(); + rightLimelight.logCalibrationValues(); } }