diff --git a/src/main/java/frc/robot/subsystems/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/DriveSubsystem.java index e2700c2..1db3eeb 100644 --- a/src/main/java/frc/robot/subsystems/DriveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/DriveSubsystem.java @@ -200,42 +200,43 @@ public class DriveSubsystem extends TestableSubsystem { LimelightHelpers.SetRobotOrientation(Constants.CameraConstanst.FRONT_CAMERA_NAME, driveSystem.getRobotPose2d().getRotation().getDegrees(), 0, 0, 0, 0, 0); LimelightHelpers.SetRobotOrientation(Constants.CameraConstanst.BACK_CAMERA_NAME, driveSystem.getRobotPose2d().getRotation().getDegrees(), 0, 0, 0, 0, 0); + // DO NOT USE EVER, PERIOD // MegaTag 2 impl - if(RobotState.isDisabled()){ - LimelightHelpers.SetIMUMode(Constants.CameraConstanst.FRONT_CAMERA_NAME, 1); - LimelightHelpers.SetIMUMode(Constants.CameraConstanst.BACK_CAMERA_NAME, 1); - }else{ - LimelightHelpers.SetIMUMode(Constants.CameraConstanst.FRONT_CAMERA_NAME, 4); - LimelightHelpers.SetIMUMode(Constants.CameraConstanst.BACK_CAMERA_NAME, 4); - } - - LimelightHelpers.PoseEstimate frontTag = LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(Constants.CameraConstanst.FRONT_CAMERA_NAME); - LimelightHelpers.PoseEstimate backTag = LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(Constants.CameraConstanst.BACK_CAMERA_NAME); - - if(frontTag.tagCount >= 2){ - driveSystem.getPoseEstimator().addVisionMeasurement(new Pose3d(frontTag.pose), frontTag.timestampSeconds); - Logger.recordOutput("FrontPose-Estimate", frontTag.pose); - Logger.recordOutput("FrontPose-IsMegaTag2", frontTag.isMegaTag2); - } - - if(backTag.tagCount >= 2){ - driveSystem.getPoseEstimator().addVisionMeasurement(new Pose3d(backTag.pose), backTag.timestampSeconds); - Logger.recordOutput("BackPose-Estimate", backTag.pose); - Logger.recordOutput("BackPose-IsMegaTag2", frontTag.isMegaTag2); - } - - - // MegaTag 1 Impl - // LimelightHelpers.PoseEstimate frontCameraPose = LimelightHelpers.getBotPoseEstimate_wpiBlue(Constants.CameraConstanst.FRONT_CAMERA_NAME); - // if (frontCameraPose.tagCount >= 2) { - // driveSystem.getPoseEstimator().addVisionMeasurement(new Pose3d(frontCameraPose.pose), frontCameraPose.timestampSeconds); - // Logger.recordOutput("FrontPoseEstimate", frontCameraPose.pose); + // if(RobotState.isDisabled()){ + // LimelightHelpers.SetIMUMode(Constants.CameraConstanst.FRONT_CAMERA_NAME, 1); + // LimelightHelpers.SetIMUMode(Constants.CameraConstanst.BACK_CAMERA_NAME, 1); + // }else{ + // LimelightHelpers.SetIMUMode(Constants.CameraConstanst.FRONT_CAMERA_NAME, 4); + // LimelightHelpers.SetIMUMode(Constants.CameraConstanst.BACK_CAMERA_NAME, 4); // } - // LimelightHelpers.PoseEstimate backCameraPose = LimelightHelpers.getBotPoseEstimate_wpiBlue(Constants.CameraConstanst.BACK_CAMERA_NAME); - // if (backCameraPose.tagCount >= 2) { - // driveSystem.getPoseEstimator().addVisionMeasurement(new Pose3d(backCameraPose.pose), backCameraPose.timestampSeconds); - // Logger.recordOutput("BackPoseEstimate", backCameraPose.pose); + + // LimelightHelpers.PoseEstimate frontTag = LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(Constants.CameraConstanst.FRONT_CAMERA_NAME); + // LimelightHelpers.PoseEstimate backTag = LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(Constants.CameraConstanst.BACK_CAMERA_NAME); + + // if(frontTag.tagCount >= 2){ + // driveSystem.getPoseEstimator().addVisionMeasurement(new Pose3d(frontTag.pose), frontTag.timestampSeconds); + // Logger.recordOutput("FrontPose-Estimate", frontTag.pose); + // Logger.recordOutput("FrontPose-IsMegaTag2", frontTag.isMegaTag2); // } + + // if(backTag.tagCount >= 2){ + // driveSystem.getPoseEstimator().addVisionMeasurement(new Pose3d(backTag.pose), backTag.timestampSeconds); + // Logger.recordOutput("BackPose-Estimate", backTag.pose); + // Logger.recordOutput("BackPose-IsMegaTag2", frontTag.isMegaTag2); + // } + + + //MegaTag 1 Impl + LimelightHelpers.PoseEstimate frontCameraPose = LimelightHelpers.getBotPoseEstimate_wpiBlue(Constants.CameraConstanst.FRONT_CAMERA_NAME); + if (frontCameraPose.tagCount >= 2) { + driveSystem.getPoseEstimator().addVisionMeasurement(new Pose3d(frontCameraPose.pose), frontCameraPose.timestampSeconds); + Logger.recordOutput("FrontPoseEstimate", frontCameraPose.pose); + } + LimelightHelpers.PoseEstimate backCameraPose = LimelightHelpers.getBotPoseEstimate_wpiBlue(Constants.CameraConstanst.BACK_CAMERA_NAME); + if (backCameraPose.tagCount >= 2) { + driveSystem.getPoseEstimator().addVisionMeasurement(new Pose3d(backCameraPose.pose), backCameraPose.timestampSeconds); + Logger.recordOutput("BackPoseEstimate", backCameraPose.pose); + } } @Override