diff --git a/components/intake.py b/components/intake.py index 2a380171..d2be4040 100644 --- 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 index 55d0d1d2..1acf639f 100644 --- 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: