Merge pull request #216 from thedropbears/comp_tuning

thedropbears · Jul 24, 2024 · 2d6231b · 2d6231b
2 parents 36f6b97 + cf8847f
commit 2d6231b
Show file tree

Hide file tree

Showing 2 changed files with 14 additions and 4 deletions.
diff --git a/components/intake.py b/components/intake.py
@@ -16,8 +16,8 @@
 
 
 class IntakeComponent:
-    motor_speed = tunable(0.8)
-    inject_intake_speed = tunable(0.3)
+    motor_speed = tunable(0.7)
+    inject_intake_speed = tunable(0.5)
     inject_shoot_speed = tunable(1.0)
 
     INTAKE_GEAR_RATIO = 2

diff --git a/components/vision.py b/components/vision.py
@@ -30,6 +30,9 @@ class VisualLocalizer:
     should_log = tunable(True)
 
     last_pose_z = tunable(0.0, writeDefault=False)
+    linear_vision_uncertainty = tunable(0.04)
+    rotation_vision_uncertainty = tunable(0.03)
+    reproj_error_threshold = 1
 
     def __init__(
         self,
@@ -91,11 +94,18 @@ def execute(self) -> None:
 
             self.field_pos_obj.setPose(pose)
 
-            if self.add_to_estimator:
+            if (
+                self.add_to_estimator
+                and self.current_reproj < self.reproj_error_threshold
+            ):
                 self.chassis.estimator.addVisionMeasurement(
                     pose,
                     timestamp,
-                    (reprojectionErr * 1.5, reprojectionErr * 1.5, reprojectionErr / 2),
+                    (
+                        self.linear_vision_uncertainty,
+                        self.linear_vision_uncertainty,
+                        self.rotation_vision_uncertainty,
+                    ),
                 )
 
             if self.should_log: