FairportRobotics
/
Castor2026
mirror of https://github.com/FairportRobotics/Castor2026.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

FUCK MEGATAG 2

This commit is contained in:
TheRealGreydon 2026-10-13 19:50:16 -04:00
parent 3e2aefdc6f
commit 024804150a
1 changed files with 34 additions and 33 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

59
src/main/java/frc/robot/subsystems/DriveSubsystem.java
View File

@ -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);
}
// 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);
// 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(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);
}
// 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);
// }
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