Skip to content

Commit

Permalink
Update VisionSubsystem.java
Browse files Browse the repository at this point in the history
add some docstrings
  • Loading branch information
Ishan1522 authored Oct 4, 2024
1 parent b3d4d1f commit 400b5ef
Showing 1 changed file with 12 additions and 0 deletions.
12 changes: 12 additions & 0 deletions src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java
Original file line number Diff line number Diff line change
Expand Up @@ -153,6 +153,12 @@ public PoseEstimate getMegaTag2PoseEstimate(int limelightNumber) {
return LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(getLimelightName(limelightNumber));
}

/**
* Checks if the MT1 and MT2 pose estimate exists and whether it is within the field
*
* @param limelightNumber the number of the limelight
* @return true if the pose estimate exists within the field and the pose estimate is not null
*/
public boolean isValidPoseEstimate(int limelightNumber) {
PoseEstimate megaTag1Estimate = getMegaTag1PoseEstimate(limelightNumber);
PoseEstimate megaTag2Estimate = getMegaTag2PoseEstimate(limelightNumber);
Expand All @@ -162,6 +168,12 @@ public boolean isValidPoseEstimate(int limelightNumber) {
&& isWithinFieldBounds(megaTag1Estimate, megaTag2Estimate);
}

/**
* Checks whether the pose estimate for MT1 and MT2 is within the field
*
* @param megaTag1Estimate the MT1 pose estimate to check
* @param megaTag2Estimate the MT2 pose estimate to check
*/
private boolean isWithinFieldBounds(
PoseEstimate megaTag1Estimate, PoseEstimate megaTag2Estimate) {
return (megaTag1Estimate.pose.getX() > 0
Expand Down

0 comments on commit 400b5ef

Please sign in to comment.