move stationary check to chassis and add rotational velocity

Author: LucienMorey

components/chassis.py

@@ -390,6 +390,15 @@ def stop_snapping(self) -> None:
         """stops the heading_controller"""
         self.snapping_to_heading = False
 
+    @feedback
+    def is_stationary(self) -> bool:
+        velocity = self.get_velocity()
+        return (
+            math.isclose(velocity.vx, 0.0)
+            and math.isclose(velocity.vy, 0.0)
+            and math.isclose(velocity.omega, 0.0)
+        )
+
     def execute(self) -> None:
         # rotate desired velocity to compensate for skew caused by discretization
         # see https://www.chiefdelphi.com/t/field-relative-swervedrive-drift-even-with-simulated-perfect-modules/413892/

controllers/algae_shooter.py

@@ -61,8 +61,7 @@ def preparing(self):
             self.wrist.at_setpoint()
             and self.shooter_component.top_flywheels_up_to_speed()
             and self.shooter_component.bottom_flywheels_up_to_speed()
-            and math.isclose(self.chassis.get_velocity().vx, 0)
-            and math.isclose(self.chassis.get_velocity().vy, 0)
+            and self.chassis.is_stationary()
         ):
             self.next_state(self.shooting)