Skip to content

Commit

Permalink
move calibration logging to Limelight.java
Browse files Browse the repository at this point in the history
  • Loading branch information
@simonstoryparker
simonstoryparker committed Nov 26, 2024
1 parent 0aef558 commit db42ae3
Show file tree
Hide file tree
Showing 4 changed files with 61 additions and 66 deletions.
10 changes: 9 additions & 1 deletion src/main/java/frc/robot/config/CompConfig.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -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() {}
Expand Down
4 changes: 3 additions & 1 deletion src/main/java/frc/robot/config/RobotConfig.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -63,7 +64,8 @@ public record VisionConfig(
int translationHistoryArraySize,
double xyStdDev,
double thetaStdDev,
InterpolatedVisionDataset interpolatedVisionSet) {}
InterpolatedVisionDataset interpolatedVisionSet,
Pose3d robotPoseCalibrationTargetSpace) {}

This comment has been minimized.

Sign in to view

Copy link
@jonahsnider

jonahsnider Nov 26, 2024 •
edited
Loading

Member

can we add a comment to this guy that explains what this field is?

      /** The pose of the robot (center of the belly pan, with a height of 0 from the floor) relative to the calibration rig AprilTag (center of the tag). */
      Pose3d robotPoseCalibrationTargetSpace) {}

public record LightsConfig(String canBusName, int deviceID) {}

Expand Down
47 changes: 47 additions & 0 deletions src/main/java/frc/robot/vision/Limelight.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -49,6 +52,50 @@ public Optional<VisionResult> getInterpolatedVisionResult() {
return Optional.of(new VisionResult(interpolatedPose, rawResult.get().timestamp()));
}

public void logCalibrationValues() {
var leftCameraPoseTargetSpace = LimelightHelpers.getCameraPose3d_TargetSpace(name);

This comment has been minimized.

Sign in to view

Copy link
@jonahsnider

jonahsnider Nov 26, 2024

Member

variable name here should be updated
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)

This comment has been minimized.

Sign in to view

Copy link
@jonahsnider

jonahsnider Nov 26, 2024

Member

getX()/getY()/getZ() already return a value in radians
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();

This comment has been minimized.

Sign in to view

Copy link
@jonahsnider

jonahsnider Nov 26, 2024

Member

these variables are good. even though they're just aliases for the getters on the Pose3d object, their names have significance & greatly improve readability

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<VisionResult> getRawVisionResult() {
var estimatePose = LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(limelightTableName);

Expand Down
66 changes: 2 additions & 64 deletions src/main/java/frc/robot/vision/VisionSubsystem.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand All @@ -26,10 +24,6 @@ public class VisionSubsystem extends StateMachine<VisionState> {
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));
Expand Down Expand Up @@ -77,63 +71,6 @@ public List<VisionResult> 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();
Expand All @@ -148,7 +85,8 @@ public void robotPeriodic() {
DogLog.log("Vision/Right/VisionState", rightLimelight.getState());

if (RobotConfig.IS_CALIBRATION) {
logCalibrationValues();
leftLimelight.logCalibrationValues();
rightLimelight.logCalibrationValues();
}
}

Expand Down

0 comments on commit db42ae3

Please sign in to comment.