Merge pull request #215 from thedropbears/rolling_comp

thedropbears · Jun 25, 2024 · 36f6b97 · 36f6b97
2 parents 08be232 + 345d773
commit 36f6b97
Show file tree

Hide file tree

Showing 2 changed files with 15 additions and 9 deletions.
diff --git a/components/shooter.py b/components/shooter.py
@@ -39,23 +39,23 @@ class ShooterComponent:
     INCLINATOR_VELOCITY_CONVERSION_FACTOR = (
         INCLINATOR_POSITION_CONVERSION_FACTOR / 60
     )  # rpm -> radians/s
-    INCLINATOR_JETTISON_ANGLE = 1
+    INCLINATOR_JETTISON_ANGLE = 1.03
 
     # Add extra point outside our range to ramp speed down to zero
     FLYWHEEL_DISTANCE_LOOKUP = (0, 1.3, 2.0, 3.0, 4.0, 5.0, 7.0)
     FLYWHEEL_SPEED_LOOKUP = (
-        60,
-        60,
+        54,
+        54,
         60,
         64,
         65,
         65,
         0,
     )
     FLYWHEEL_ANGLE_LOOKUP = (
-        0.95,
-        0.95,
-        0.68,
+        0.93,
+        0.93,
+        0.77,
         0.57,
         0.49,
         0.46,
@@ -120,7 +120,7 @@ def __init__(self) -> None:
         flywheel_right_config.apply(flywheel_pid)
         flywheel_right_config.apply(flywheel_gear_ratio)
 
-        self.inclinator_controller = PIDController(0.6, 0, 0)
+        self.inclinator_controller = PIDController(3.0, 0, 0)
         self.inclinator_controller.setTolerance(ShooterComponent.INCLINATOR_TOLERANCE)
         SmartDashboard.putData(self.inclinator_controller)
 

diff --git a/components/vision.py b/components/vision.py
@@ -4,7 +4,7 @@
 
 import wpilib
 import wpiutil.log
-from magicbot import tunable
+from magicbot import tunable, feedback
 from photonlibpy.photonCamera import PhotonCamera
 from photonlibpy.photonTrackedTarget import PhotonTrackedTarget
 from wpimath import objectToRobotPose
@@ -27,7 +27,7 @@ class VisualLocalizer:
     TIMEOUT = 1.0  # s
 
     add_to_estimator = tunable(True)
-    should_log = tunable(False)
+    should_log = tunable(True)
 
     last_pose_z = tunable(0.0, writeDefault=False)
 
@@ -59,6 +59,11 @@ def __init__(
         )
 
         self.chassis = chassis
+        self.current_reproj = 0.0
+
+    @feedback
+    def reproj(self) -> float:
+        return self.current_reproj
 
     def execute(self) -> None:
         # stop warnings in simulation
@@ -82,6 +87,7 @@ def execute(self) -> None:
             p = results.multiTagResult.estimatedPose
             pose = (Pose3d() + p.best + self.camera_to_robot).toPose2d()
             reprojectionErr = p.bestReprojError
+            self.current_reproj = reprojectionErr
 
             self.field_pos_obj.setPose(pose)