diff --git a/components/shooter.py b/components/shooter.py index 526ec0ea..d7ce48d8 100644 --- a/components/shooter.py +++ b/components/shooter.py @@ -39,13 +39,13 @@ 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, @@ -53,9 +53,9 @@ class ShooterComponent: 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 index 98a71bc1..55d0d1d2 100644 --- 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)