diff --git a/src/main/java/frc/robot/vision/PoseEstimatorSubsystem.java b/src/main/java/frc/robot/vision/PoseEstimatorSubsystem.java index ed71f50..aae66e0 100644 --- a/src/main/java/frc/robot/vision/PoseEstimatorSubsystem.java +++ b/src/main/java/frc/robot/vision/PoseEstimatorSubsystem.java @@ -146,10 +146,10 @@ public void periodic() { // Set the pose on the dashboard var dashboardPose = poseEstimator.getEstimatedPosition(); - // if (originPosition == kRedAllianceWallRightSide) { - // // Flip the pose when red, since the dashboard field photo cannot be rotated - // dashboardPose = flipAlliance(dashboardPose); - // } + if (originPosition == kRedAllianceWallRightSide) { + // Flip the pose when red, since the dashboard field photo cannot be rotated + dashboardPose = flipAlliance(dashboardPose); + } field2d.setRobotPose(dashboardPose); SmartDashboard.putString("Pose Formatted", getFomattedPose()); diff --git a/src/main/java/frc/robot/vision/VisionConfig.java b/src/main/java/frc/robot/vision/VisionConfig.java index a6b5bce..b057031 100644 --- a/src/main/java/frc/robot/vision/VisionConfig.java +++ b/src/main/java/frc/robot/vision/VisionConfig.java @@ -45,12 +45,12 @@ public class VisionConfig { public static final Transform3d[] ROBOT_TO_CAM_TRANSFORMS = new Transform3d[] { //left cam new Transform3d( - new Translation3d(Units.inchesToMeters(11.882),Units.inchesToMeters(11.020),Units.inchesToMeters(6.767)), - new Rotation3d(0,Units.degreesToRadians(15),Units.degreesToRadians(-20))), + new Translation3d(Units.inchesToMeters(-11.882),Units.inchesToMeters(-11.020),Units.inchesToMeters(6.767)), + new Rotation3d(0,Units.degreesToRadians(15),Units.degreesToRadians(160))), //right cam new Transform3d( - new Translation3d(Units.inchesToMeters(11.882),Units.inchesToMeters(-11.020),Units.inchesToMeters(6.767)), - new Rotation3d(0, Units.degreesToRadians(15), Units.degreesToRadians(20))) + new Translation3d(Units.inchesToMeters(-11.882),Units.inchesToMeters(11.020),Units.inchesToMeters(6.767)), + new Rotation3d(0, Units.degreesToRadians(15), Units.degreesToRadians(-160))) }; // Creates field layout for AprilTags