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());