Merge pull request #293 from thedropbears/make_shooter_check_reasonable

ease requirement for stationary chassis and fix sim

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, abs_tol=0.1)
+            and math.isclose(velocity.vy, 0.0, abs_tol=0.1)
+            and math.isclose(velocity.omega, 0.0, abs_tol=math.radians(3))
+        )
+
     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)
 

physics.py

@@ -31,6 +31,22 @@
     from robot import MyRobot
 
 
+class RollingBuffer:
+    def __init__(self, max_length: int):
+        self.max_length = max_length
+
+        self.buffer_: list[float] = []
+
+    def average(self) -> float:
+        return sum(self.buffer_) / len(self.buffer_) if len(self.buffer_) > 0 else 0.0
+
+    def add_sample(self, sample: float):
+        self.buffer_.append(sample)
+
+        if len(self.buffer_) > self.max_length:
+            self.buffer_.pop(0)
+
+
 class SimpleTalonFXMotorSim:
     def __init__(
         self, motor: phoenix6.hardware.TalonFX, units_per_rev: float, kV: float
@@ -39,9 +55,16 @@ def __init__(
         self.sim_state.set_supply_voltage(12.0)
         self.kV = kV  # volt seconds per unit
         self.units_per_rev = units_per_rev
+        self.voltage_buffer = RollingBuffer(10)
 
     def update(self, dt: float) -> None:
         voltage = self.sim_state.motor_voltage
+
+        self.voltage_buffer.add_sample(voltage)
+
+        if math.isclose(self.voltage_buffer.average(), 0.0, abs_tol=0.1):
+            voltage = 0.0
+
         velocity = voltage / self.kV  # units per second
         velocity_rps = velocity * self.units_per_rev
         self.sim_state.set_rotor_velocity(velocity_rps)