Merge pull request #241 from thedropbears/lookup-table-stuff

Set hopper surface speed in ballistics

Author: SyntaxBreak

components/ballistics.py

@@ -9,6 +9,7 @@
 from wpimath.kinematics import ChassisSpeeds
 
 from components.chassis import ChassisComponent
+from components.hopper import HopperComponent
 from components.leds import LEDComponent
 from components.shooter import ShooterComponent
 from components.turret import TurretComponent
@@ -28,7 +29,9 @@
 TIME_LOOKUP_PASS =     np.array([1.139, 1.293, 1.376, 1.431], dtype=float)
 # fmt: on
 
-type ForcedSolution = tuple[units.turns_per_second, units.radians, units.radians]
+type ForcedSolution = tuple[
+    units.turns_per_second, units.radians, units.radians, units.meters_per_second
+]
 
 
 @dataclass
@@ -47,6 +50,7 @@ class LookupTable:
     The flight time for a "pass shot" should be once its hit the ground
     """
     hood_angle: units.radians
+    hopper_surface_speed: units.meters_per_second
     name: str
 
     def is_within_range(self, distance: units.meters) -> bool:
@@ -64,6 +68,7 @@ class BallisticsComponent:
     shooter: ShooterComponent
     turret: TurretComponent
     leds: LEDComponent
+    hopper: HopperComponent
 
     forced_solution = will_reset_to[ForcedSolution | None](None)
     should_energise_flywheels = will_reset_to(False)
@@ -83,20 +88,23 @@ def __init__(self) -> None:
                 SPEED_LOOKUP_25,
                 TIME_LOOKUP_25,
                 math.radians(25),
+                10,
                 "Score Table 25",
             ),
             LookupTable(
                 DISTANCE_LOOKUP_45,
                 SPEED_LOOKUP_45,
                 TIME_LOOKUP_45,
                 math.radians(45),
+                12,
                 "Score Table 45",
             ),
             LookupTable(
                 DISTANCE_LOOKUP_PASS,
                 SPEED_LOOKUP_PASS,
                 TIME_LOOKUP_PASS,
                 math.radians(54),
+                12,
                 "Pass Table",
             ),
         )
@@ -121,11 +129,13 @@ def force_solution(
         desired_flywheel_speed: units.turns_per_second,
         desired_turret_bearing: units.radians,
         desired_hood_angle: units.radians,
+        desired_hopper_surface_speed: units.meters_per_second,
     ) -> None:
         self.forced_solution = (
             desired_flywheel_speed,
             desired_turret_bearing,
             desired_hood_angle,
+            desired_hopper_surface_speed,
         )
 
     def solve_moving_shot(self, current_pose: Pose2d, current_velocity: ChassisSpeeds):
@@ -194,11 +204,15 @@ def execute(self) -> None:
             target_flywheel_speed: units.turns_per_second = self.active_table.speed_for(
                 distance_to_target
             )
+            target_hopper_surface_speed = self.active_table.hopper_surface_speed
 
         else:
-            target_flywheel_speed, target_turret_angle, target_hood_angle = (
-                self.forced_solution
-            )
+            (
+                target_flywheel_speed,
+                target_turret_angle,
+                target_hood_angle,
+                target_hopper_surface_speed,
+            ) = self.forced_solution
 
         if self.should_energise_flywheels:
             self.shooter.set_flywheel(target_flywheel_speed)
@@ -212,3 +226,4 @@ def execute(self) -> None:
 
         if self.is_driving_faster_than_max_shoot_speed():
             self.leds.driving_faster_than_shoot_speed()
+        self.hopper.set_desired_surface_speed(target_hopper_surface_speed)

components/hopper.py

@@ -1,6 +1,6 @@
 from math import pi, tau
 
-from magicbot import feedback, tunable, will_reset_to
+from magicbot import feedback, will_reset_to
 from phoenix6.configs import (
     FeedbackConfigs,
     MotorOutputConfigs,
@@ -19,7 +19,7 @@
 class HopperComponent:
     leds: LEDComponent
 
-    desired_hopper_surface_speed = tunable(12.0)  # meters / sec
+    desired_surface_speed: units.meters_per_second = 12.0
 
     INJECTOR_WHEEL_DIAMETER: units.meters = 0.05
     INDEXER_WHEEL_DIAMETER: units.meters = 0.137
@@ -79,6 +79,9 @@ def __init__(self) -> None:
             .with_slot0(injector_gains_config)
         )
 
+    def set_desired_surface_speed(self, desired_speed: units.meters_per_second) -> None:
+        self.desired_surface_speed = desired_speed
+
     @feedback
     def get_target_indexer_rps(self) -> float:
         return self.target_indexer_rps
@@ -111,10 +114,10 @@ def is_jammed(self) -> bool:
 
     def feed(self) -> None:
 
-        self.target_indexer_rps = self.desired_hopper_surface_speed / (
+        self.target_indexer_rps = self.desired_surface_speed / (
             pi * self.INDEXER_WHEEL_DIAMETER
         )
-        self.target_injector_rps = self.desired_hopper_surface_speed / (
+        self.target_injector_rps = self.desired_surface_speed / (
             pi * self.INJECTOR_WHEEL_DIAMETER
         )
 

robot.py

@@ -54,6 +54,7 @@ class MyRobot(magicbot.MagicRobot):
     test_flywheel_speed = tunable(0.0)  # rotations/s
     test_turret_angle = tunable(0.0)  # degrees
     test_hood_angle = tunable(45.0)  # degrees
+    test_hopper_surface_speed = tunable(12.0)  # metres/s
 
     START_POS_TOLERANCE = 0.2
 
@@ -262,12 +263,16 @@ def testPeriodic(self) -> None:
         self.chassis.execute()
         self.targeter.execute()
 
-        if self.gamepad.getRightTriggerAxis() > 0.5:
-            self.ballistics.force_solution(
-                self.test_flywheel_speed,
-                math.radians(self.test_turret_angle),
-                math.radians(self.test_hood_angle),
-            )
+        if self.gamepad.getRightTriggerAxis() > 0.5 or self.gamepad.getXButton():
+            if self.gamepad.getRightTriggerAxis() > 0.5:
+                self.ballistics.solve_for(self.targeter.get_target())
+            else:
+                self.ballistics.force_solution(
+                    self.test_flywheel_speed,
+                    math.radians(self.test_turret_angle),
+                    math.radians(self.test_hood_angle),
+                    self.test_hopper_surface_speed,
+                )
 
             self.ballistics.energise_flywheels()
             self.ballistics.execute()