1+ // Copyright (c) FIRST and other WPILib contributors.
2+ // Open Source Software; you can modify and/or share it under the terms of
3+ // the WPILib BSD license file in the root directory of this project.
4+
5+ package frc .robot .subsystems ;
6+
7+ import com .frcteam3255 .utils .LimelightHelpers ;
8+ import com .frcteam3255 .utils .LimelightHelpers .PoseEstimate ;
9+ import java .util .Optional ;
10+
11+ import edu .wpi .first .epilogue .Logged ;
12+ import edu .wpi .first .epilogue .NotLogged ;
13+ import edu .wpi .first .math .geometry .Pose2d ;
14+ import edu .wpi .first .units .measure .AngularVelocity ;
15+
16+ import edu .wpi .first .wpilibj2 .command .SubsystemBase ;
17+ import frc .robot .Constants .constVision ;
18+
19+ @ Logged
20+ public class Vision extends SubsystemBase {
21+ PoseEstimate lastEstimateRight = new PoseEstimate ();
22+ PoseEstimate lastEstimateLeft = new PoseEstimate ();
23+ PoseEstimate lastEstimateBack = new PoseEstimate ();
24+
25+ // Not logged, as they turn to false immediately after being read
26+ @ NotLogged
27+ boolean newRightEstimate = false ;
28+ @ NotLogged
29+ boolean newLeftEstimate = false ;
30+ @ NotLogged
31+ boolean newBackEstimate = false ;
32+
33+ Pose2d rightPose = new Pose2d ();
34+ Pose2d leftPose = new Pose2d ();
35+ Pose2d backPose = new Pose2d ();
36+
37+ private boolean useMegaTag2 = false ;
38+
39+ public Vision () {
40+ }
41+
42+ public PoseEstimate [] getLastPoseEstimates () {
43+ return new PoseEstimate [] { lastEstimateRight , lastEstimateLeft , lastEstimateBack };
44+ }
45+
46+ public void setMegaTag2 (boolean useMegaTag2 ) {
47+ this .useMegaTag2 = useMegaTag2 ;
48+ }
49+
50+ /**
51+ * Determines if a given pose estimate should be rejected.
52+ *
53+ *
54+ * @param poseEstimate The pose estimate to check
55+ * @param gyroRate The current rate of rotation observed by our gyro.
56+ *
57+ * @return True if the estimate should be rejected
58+ */
59+
60+ public boolean rejectUpdate (PoseEstimate poseEstimate , AngularVelocity gyroRate , double areaThreshold ) {
61+ // Angular velocity is too high to have accurate vision
62+ if (gyroRate .compareTo (constVision .MAX_ANGULAR_VELOCITY ) > 0 ) {
63+ return true ;
64+ }
65+
66+ // No tags :<
67+ if (poseEstimate .tagCount == 0 ) {
68+ return true ;
69+ }
70+
71+ // 1 Tag with a large area
72+ if (poseEstimate .tagCount == 1 && poseEstimate .avgTagArea > areaThreshold ) {
73+ return false ;
74+ // 2 tags or more
75+ } else if (poseEstimate .tagCount > 1 ) {
76+ return false ;
77+ }
78+
79+ return true ;
80+ }
81+
82+ /**
83+ * Updates the current pose estimates for the left and right of the robot using
84+ * data from Limelight cameras.
85+ *
86+ * @param gyroRate The current angular velocity of the robot, used to validate
87+ * the pose estimates.
88+ *
89+ * This method retrieves pose estimates from two Limelight
90+ * cameras (left and right) and updates the
91+ * corresponding pose estimates if they are valid. The method
92+ * supports two modes of operation:
93+ * one using MegaTag2 and one without. The appropriate pose
94+ * estimate retrieval method is chosen
95+ * based on the value of the `useMegaTag2` flag.
96+ *
97+ * If the retrieved pose estimates are valid and not rejected
98+ * based on the current angular velocity,
99+ * the method updates the last known estimates and sets flags
100+ * indicating new estimates are available.
101+ */
102+ public void setCurrentEstimates (AngularVelocity gyroRate ) {
103+ PoseEstimate currentEstimateRight = new PoseEstimate ();
104+ PoseEstimate currentEstimateLeft = new PoseEstimate ();
105+ PoseEstimate currentEstimateBack = new PoseEstimate ();
106+
107+ if (useMegaTag2 ) {
108+ currentEstimateRight = LimelightHelpers .getBotPoseEstimate_wpiBlue_MegaTag2 (constVision .LIMELIGHT_NAMES [0 ]);
109+ currentEstimateLeft = LimelightHelpers .getBotPoseEstimate_wpiBlue_MegaTag2 (constVision .LIMELIGHT_NAMES [1 ]);
110+ currentEstimateBack = LimelightHelpers .getBotPoseEstimate_wpiBlue_MegaTag2 (constVision .LIMELIGHT_NAMES [2 ]);
111+ } else {
112+ currentEstimateRight = LimelightHelpers .getBotPoseEstimate_wpiBlue (constVision .LIMELIGHT_NAMES [0 ]);
113+ currentEstimateLeft = LimelightHelpers .getBotPoseEstimate_wpiBlue (constVision .LIMELIGHT_NAMES [1 ]);
114+ currentEstimateBack = LimelightHelpers .getBotPoseEstimate_wpiBlue (constVision .LIMELIGHT_NAMES [2 ]);
115+ }
116+
117+ if (currentEstimateRight != null
118+ && !rejectUpdate (currentEstimateRight , gyroRate , constVision .AREA_THRESHOLD_FRONT )) {
119+ lastEstimateRight = currentEstimateRight ;
120+ rightPose = currentEstimateRight .pose ;
121+ newRightEstimate = true ;
122+ }
123+ if (currentEstimateLeft != null && !rejectUpdate (currentEstimateLeft , gyroRate , constVision .AREA_THRESHOLD_FRONT )) {
124+ lastEstimateLeft = currentEstimateLeft ;
125+ leftPose = currentEstimateLeft .pose ;
126+ newLeftEstimate = true ;
127+ }
128+ if (currentEstimateBack != null && !rejectUpdate (currentEstimateBack , gyroRate , constVision .AREA_THRESHOLD_BACK )) {
129+ lastEstimateBack = currentEstimateBack ;
130+ backPose = currentEstimateBack .pose ;
131+ newBackEstimate = true ;
132+ }
133+ }
134+
135+ public Optional <PoseEstimate > determinePoseEstimate (AngularVelocity gyroRate ) {
136+ setCurrentEstimates (gyroRate );
137+
138+ // No valid pose estimates :(
139+ if (!newRightEstimate && !newLeftEstimate && !newBackEstimate ) {
140+ return Optional .empty ();
141+
142+ } else if (newRightEstimate && !newLeftEstimate && !newBackEstimate ) {
143+ // One valid pose estimate (right)
144+ newRightEstimate = false ;
145+ return Optional .of (lastEstimateRight );
146+
147+ } else if (!newRightEstimate && newLeftEstimate && !newBackEstimate ) {
148+ // One valid pose estimate (left)
149+ newLeftEstimate = false ;
150+ return Optional .of (lastEstimateLeft );
151+
152+ } else if (!newRightEstimate && !newLeftEstimate && newBackEstimate ) {
153+ // One valid pose estimate (back)
154+ newLeftEstimate = false ;
155+ return Optional .of (lastEstimateBack );
156+
157+ } else {
158+ // More than one valid pose estimate, use the closest one
159+ newRightEstimate = false ;
160+ newLeftEstimate = false ;
161+ newBackEstimate = false ;
162+ if (lastEstimateRight .avgTagDist < lastEstimateLeft .avgTagDist
163+ && lastEstimateRight .avgTagDist < lastEstimateBack .avgTagDist ) {
164+ return Optional .of (lastEstimateRight );
165+ } else if (lastEstimateLeft .avgTagDist < lastEstimateRight .avgTagDist
166+ && lastEstimateLeft .avgTagDist < lastEstimateBack .avgTagDist ) {
167+ return Optional .of (lastEstimateLeft );
168+ } else {
169+ return Optional .of (lastEstimateBack );
170+ }
171+ }
172+ }
173+
174+ @ Override
175+ public void periodic () {
176+ }
177+ }
0 commit comments