Fill in Shooter StateMachine

components/shooter.py

@@ -1,3 +1,4 @@
+import math
 from magicbot import tunable, feedback
 from rev import CANSparkMax
 from ids import SparkMaxIds, TalonIds, DioChannels
@@ -9,7 +10,6 @@
 from wpilib import DigitalInput, DutyCycle, SmartDashboard
 from wpimath.controller import PIDController
 
-import math
 from utilities.functions import clamp
 
 
@@ -25,7 +25,7 @@ class ShooterComponent:
 
     desired_inclinator_angle = tunable((MAX_INCLINE_ANGLE + MIN_INCLINE_ANGLE) / 2)
     desired_flywheel_speed = tunable(0.0)
-    inject_speed = tunable(0.0)
+    inject_speed = tunable(0.3)
 
     def __init__(self) -> None:
         self.inclinator = CANSparkMax(
@@ -78,6 +78,9 @@ def shoot(self) -> None:
     def on_enable(self) -> None:
         self.inclinator_controller.reset()
 
+    def set_flywheel_target(self, target_speed: float) -> None:
+        self.desired_flywheel_speed = target_speed
+
     @feedback
     def is_ready(self) -> bool:
         """Is the shooter ready to fire?"""
@@ -108,9 +111,15 @@ def _inclination_angle(self) -> float:
     def _flywheel_velocity(self) -> float:
         return self.flywheel.get_velocity().value
 
+    @feedback
+    def is_flywheel_at_speed(self) -> bool:
+        return (
+            abs(self.desired_flywheel_speed - self.flywheel.get_velocity().value)
+            < self.FLYWHEEL_TOLERANCE
+        )
+
     def execute(self) -> None:
         """This gets called at the end of the control loop"""
-
         inclinator_speed = self.inclinator_controller.calculate(
             self._inclination_angle(),
             clamp(
@@ -121,7 +130,7 @@ def execute(self) -> None:
         )
         self.inclinator.set(inclinator_speed)
 
-        if self.should_inject and self.at_inclination():
+        if self.should_inject:
             self.injector.set(self.inject_speed)
         else:
             self.injector.set(0.0)

controllers/shooter.py

@@ -1,36 +1,80 @@
-from magicbot import StateMachine, state
+from math import atan2
+from magicbot import StateMachine, state, timed_state, default_state, will_reset_to
 from components.chassis import ChassisComponent
-
+from components.shooter import ShooterComponent
 from utilities.game import get_goal_speaker_position
 
-from math import atan2
-
 
 class Shooter(StateMachine):
     chassis: ChassisComponent
+    shooter_component: ShooterComponent
+    should_fire = will_reset_to(False)
+    INJECTION_DURATION = 1  # seconds
+
+    def __init__(self) -> None:
+        self.just_fired = False
+
+    def has_just_fired(self) -> bool:
+        return self.just_fired
+
+    def shoot(self) -> None:
+        """Engage the state machine to attempt a shot, needs to be called repeatedly"""
+        self.should_fire = True
+        self.engage()
 
-    def setup(self) -> None:
-        pass
+    def update_ranging(self) -> None:
+        dist = (
+            self.chassis.get_pose()
+            .translation()
+            .distance(get_goal_speaker_position().toTranslation2d())
+        )
+        # TODO get flywheel speeds and inclination angle from lookup table
+        flywheel_target = dist
+        inclination_target = 0
+        self.shooter_component.set_flywheel_target(flywheel_target)
+        self.shooter_component.set_inclination(inclination_target)
+
+    @default_state
+    def idle(self) -> None:
+        """Run ranging whenever we are not doing anything else"""
+        self.just_fired = False
+        self.update_ranging()
+        if self.should_fire:
+            self.next_state("acquiring")
 
     @state(first=True)
     def acquiring(self) -> None:
-        # determine heading required for goal
+        # NOTE Turret can't rotate, instead we face the chassis towards the goal
+        # Determine heading required for goal
         translation_to_goal = (
             get_goal_speaker_position().toTranslation2d()
             - self.chassis.get_pose().translation()
         )
         bearing_to_speaker = atan2(translation_to_goal.y, translation_to_goal.x)
-        # set to appropriate heading
+
+        # Set to appropriate heading
         self.chassis.snap_to_heading(bearing_to_speaker)
 
-        # progress state machine if within tolerance
+        # Update ranging but don't check for tolerance yet
+        self.update_ranging()
+
+        # Progress state machine if within tolerance
         if self.chassis.at_desired_heading():
+            self.next_state("ranging")
+
+    @state
+    def ranging(self) -> None:
+        # Now check ranging tolerances
+        self.update_ranging()
+        if self.shooter_component.is_ready():
             self.next_state("shooting")
 
-    @state(must_finish=True)
+    @timed_state(must_finish=True, next_state="resetting", duration=INJECTION_DURATION)
     def shooting(self) -> None:
-        # commence shooting action
-        self.done()
+        self.shooter_component.shoot()
 
-    def shoot(self) -> None:
-        self.engage()
+    @state
+    def resetting(self) -> None:
+        self.chassis.stop_snapping()
+        self.just_fired = True
+        self.done()

robot.py

@@ -107,7 +107,7 @@ def testInit(self) -> None:
     def testPeriodic(self) -> None:
         # injecting
         if self.gamepad.getBButton():
-            self.shooter.shoot()
+            self.shooter_component.shoot()
 
         if self.gamepad.getXButton():
             self.intake.intake()