From 84faad46c47488f5cd64123d01a232dbc892253e Mon Sep 17 00:00:00 2001 From: Michael Mikovsky <77305074+Astatin3@users.noreply.github.com> Date: Sat, 11 Jan 2025 11:26:10 -0700 Subject: [PATCH] Working thing --- src/main/java/frc4388/robot/Constants.java | 4 +--- src/main/java/frc4388/robot/subsystems/Vision.java | 6 ++++-- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 34cceba..4e0c599 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -279,9 +279,7 @@ public final class Constants { public static final class VisionConstants { public static final String CAMERA_NAME = "Camera_Module_v1"; - public static final Rotation2d ROTATE_BY = Rotation2d.fromDegrees(180); - - public static final Transform3d CAMERA_POS = new Transform3d(new Translation3d(.3048, .2413, .2794), new Rotation3d(0,0.52333,0)); + public static final Transform3d CAMERA_POS = new Transform3d(new Translation3d(.3048, 0.2413, .2794), new Rotation3d(0,0.52333,Math.PI)); public static final AprilTagFieldLayout kTagLayout = AprilTagFields.k2024Crescendo.loadAprilTagLayoutField(); diff --git a/src/main/java/frc4388/robot/subsystems/Vision.java b/src/main/java/frc4388/robot/subsystems/Vision.java index 2eebceb..21f3500 100644 --- a/src/main/java/frc4388/robot/subsystems/Vision.java +++ b/src/main/java/frc4388/robot/subsystems/Vision.java @@ -106,12 +106,14 @@ public class Vision extends Subsystem { } lastVisionPose = EstimatedRobotPose.get().estimatedPose.toPose2d(); + // lastVisionPose.rotateBy(lastVisionPose.getRotation().minus(lastPhysOdomPose.getRotation())); // lastVisionPose = new Pose2d( - // new Translation2d(lastVisionPose.getY(), lastVisionPose.getX()), - // lastVisionPose.getRotation() + // lastVisionPose.getTranslation(), + // lastPhysOdomPose.getRotation() // ); + field.setRobotPose(getPose2d());