Improve example Limelight integration

CrossTheRoadElec · Jan 10, 2025 · 4ef80ce · 4ef80ce
1 parent daa6af2
commit 4ef80ce
Show file tree

Hide file tree

Showing 5 changed files with 797 additions and 128 deletions.
diff --git a/cpp/SwerveWithPathPlanner/src/main/cpp/Robot.cpp b/cpp/SwerveWithPathPlanner/src/main/cpp/Robot.cpp
@@ -21,8 +21,13 @@ void Robot::RobotPeriodic() {
    * of how to use vision should be tuned per-robot and to the team's specification.
    */
   if (kUseLimelight) {
-    auto llMeasurement = LimelightHelpers::getBotPoseEstimate_wpiBlue("limelight");
-    if (llMeasurement) {
+    auto const driveState = m_container.drivetrain.GetState();
+    auto const heading = driveState.Pose.Rotation().Degrees();
+    auto const omega = driveState.Speeds.omega;
+
+    LimelightHelpers::SetRobotOrientation("limelight", heading.value(), 0, 0, 0, 0, 0);
+    auto llMeasurement = LimelightHelpers::getBotPoseEstimate_wpiBlue_MegaTag2("limelight");
+    if (llMeasurement && llMeasurement->tagCount > 0 && omega < 2_tps) {
       m_container.drivetrain.AddVisionMeasurement(llMeasurement->pose, utils::FPGAToCurrentTime(llMeasurement->timestampSeconds));
     }
   }