Repository: NCSU-High-Powered-Rocketry-Club/AirbrakesV2

Issue:
Fix controls logic
If we are doing an actual control flight, and our target altitude is quite close to predicted apogee, then our controls logic would subtly fail because of race conditions. Consider this case, when our target altitude is 800m:

1. Predicted apogee is 801m:
- Airbrakes extend to `MAX`. Thread to extend to `MAX_NO_BUZZ` is set to execute in 0.3 seconds.

2. Predicted apogee is 799m, before 0.3 seconds have passed:
- Airbrakes extend to `MIN`. Thread to extend to `MIN_NO_BUZZ` is set to execute in 0.3 seconds.

3. 0.3 seconds from the first extension has passed, airbrakes go to `MAX_NO_BUZZ` (incorrect!)

4. A little bit later, the airbrakes are then extended to `MIN_NO_BUZZ`, as they should have been.


### Proposed Fix

We should use [Timer](https://docs.python.org/3.12/library/threading.html#threading.Timer) objects and cancel the execution of the NO_BUZZ code whenever we are setting the opposite extension. 

Tests for the servo are also long overdue.  


Discussion:

Patch:
diff --git a/airbrakes/hardware/servo.py b/airbrakes/hardware/servo.py
index cad697c..07bb2e1 100644
--- a/airbrakes/hardware/servo.py
+++ b/airbrakes/hardware/servo.py
@@ -2,7 +2,6 @@
 the airbrakes."""
 
 import threading
-import time
 import warnings
 
 import gpiozero
@@ -17,7 +16,7 @@ class Servo:
     controlling GPIO pins on the Raspberry Pi.
     """
 
-    __slots__ = ("current_extension", "servo")
+    __slots__ = ("_go_to_max_no_buzz", "_go_to_min_no_buzz", "current_extension", "servo")
 
     def __init__(self, gpio_pin_number: int, pin_factory=None) -> None:
         """
@@ -40,41 +39,53 @@ class Servo:
             gpiozero.Device.pin_factory = pin_factory
 
         self.servo = gpiozero.Servo(gpio_pin_number)
+        # We have to use threading to avoid blocking the main thread because our extension methods
+        # need to run at a specific time.
+        self._go_to_max_no_buzz = threading.Timer(SERVO_DELAY, self._set_max_no_buzz)
+        self._go_to_min_no_buzz = threading.Timer(SERVO_DELAY, self._set_min_no_buzz)
 
     def set_extended(self) -> None:
         """
-        Extends the servo to the maximum extension.
+        Extends the servo to the maximum extension. Starts a timer to stop the buzzing after the
+        servo reaches the maximum extension.
         """
-        # We have to use threading to avoid blocking the main thread because our extension methods
-        # sleep
-        thread = threading.Thread(target=self._extend_then_no_buzz)
-        thread.start()
+        # If we are already going to the minimum extension, we cancel that operation before
+        # extending the servo.
+        self._go_to_min_no_buzz.cancel()
+
+        self._set_extension(ServoExtension.MAX_EXTENSION)
+
+        # Creates a timer to stop the buzzing after the servo reaches the maximum extension
+        self._go_to_max_no_buzz = threading.Timer(SERVO_DELAY, self._set_max_no_buzz)
+        self._go_to_max_no_buzz.start()
 
     def set_retracted(self) -> None:
         """
-        Retracts the servo to the minimum extension.
+        Retracts the servo to the minimum extension. Starts a timer to stop the buzzing after the
+        servo reaches the minimum extension.
         """
-        thread = threading.Thread(target=self._retract_then_no_buzz)
-        thread.start()
+        # If we are already going to the maximum extension, we cancel that operation before
+        # retracting the servo.
+        self._go_to_max_no_buzz.cancel()
 
-    def _extend_then_no_buzz(self) -> None:
+        self._set_extension(ServoExtension.MIN_EXTENSION)
+
+        # Creates a timer to stop the buzzing after the servo reaches the minimum extension
+        self._go_to_min_no_buzz = threading.Timer(SERVO_DELAY, self._set_min_no_buzz)
+        self._go_to_min_no_buzz.start()
+
+    def _set_max_no_buzz(self) -> None:
         """
-        Extends the servo then stops buzzing. This extends the servo to the maximum extension,
-        waits for the servo to reach the physical end of the air brakes, and then sets its
-        extension to its actual extension.
+        Extends the servo to the stop buzz position. This extends the servo such that it reaches
+        the physical end of the airbrakes, and then sets its extension to its actual extension.
         """
-        self._set_extension(ServoExtension.MAX_EXTENSION)
-        time.sleep(SERVO_DELAY)
         self._set_extension(ServoExtension.MAX_NO_BUZZ)
 
-    def _retract_then_no_buzz(self) -> None:
+    def _set_min_no_buzz(self) -> None:
         """
-        Retracts the servo then stops buzzing. This retracts the servo to the minimum extension,
-        waits for the servo to reach the physical end of the air brakes, and then sets its
-        extension to its actual extension.
+        Retracts the servo to the stop buzz position. This retracts the servo such that it reaches
+        the physical start of the airbrakes, and then sets its extension to its actual extension.
         """
-        self._set_extension(ServoExtension.MIN_EXTENSION)
-        time.sleep(SERVO_DELAY)
         self._set_extension(ServoExtension.MIN_NO_BUZZ)
 
     def _set_extension(self, extension: ServoExtension) -> None:


Test patch:
diff --git a/tests/conftest.py b/tests/conftest.py
index 57d3a8d..b75ca92 100644
--- a/tests/conftest.py
+++ b/tests/conftest.py
@@ -62,11 +62,13 @@ def airbrakes(imu, logger, servo, data_processor, apogee_predictor):
 
 @pytest.fixture
 def random_data_mock_imu():
+    # A mock IMU that outputs random data packets
     return RandomDataIMU(port=PORT, frequency=FREQUENCY)
 
 
 @pytest.fixture
 def idle_mock_imu():
+    # A sleeping IMU that doesn't output any data packets
     return IdleIMU(port=PORT, frequency=FREQUENCY)
 
 
diff --git a/tests/test_servo.py b/tests/test_servo.py
new file mode 100644
index 0000000..8dfc394
--- /dev/null
+++ b/tests/test_servo.py
@@ -0,0 +1,90 @@
+import threading
+import time
+
+import gpiozero
+
+from airbrakes.hardware.servo import Servo
+from constants import SERVO_DELAY, ServoExtension
+
+
+class TestServo:
+    """Tests the Servo class, which controls the servo that extends and retracts the airbrakes."""
+
+    def test_slots(self, servo):
+        inst = servo
+        for attr in inst.__slots__:
+            assert getattr(inst, attr, "err") != "err", f"got extra slot '{attr}'"
+
+    def test_init(self, servo):
+        assert isinstance(servo, Servo)
+        assert isinstance(servo.current_extension, ServoExtension)
+        assert servo.current_extension == ServoExtension.MIN_NO_BUZZ
+        assert isinstance(servo.servo, gpiozero.Servo)
+        assert isinstance(servo._go_to_max_no_buzz, threading.Timer)
+        assert isinstance(servo._go_to_min_no_buzz, threading.Timer)
+
+    def test_set_extension(self, servo):
+        servo._set_extension(ServoExtension.MAX_EXTENSION)
+        assert servo.current_extension == ServoExtension.MAX_EXTENSION
+        assert servo.servo.value == ServoExtension.MAX_EXTENSION.value
+        servo._set_extension(ServoExtension.MIN_EXTENSION)
+        assert servo.current_extension == ServoExtension.MIN_EXTENSION
+        assert servo.servo.value == ServoExtension.MIN_EXTENSION.value
+
+    def test_set_extended(self, servo):
+        """Tests that the servo extends to the maximum extension, and that threading is handled
+        correctly."""
+        assert servo.current_extension == ServoExtension.MIN_NO_BUZZ
+        servo.set_extended()
+        assert servo.current_extension == ServoExtension.MAX_EXTENSION
+        assert servo.servo.value == ServoExtension.MAX_EXTENSION.value
+        time.sleep(SERVO_DELAY + 0.05)
+        assert servo.current_extension == ServoExtension.MAX_NO_BUZZ
+        assert servo.servo.value == ServoExtension.MAX_NO_BUZZ.value
+
+    def test_set_retracted(self, servo):
+        """Tests that the servo retracts to the minimum extension, and that threading is handled
+        correctly."""
+        assert servo.current_extension == ServoExtension.MIN_NO_BUZZ
+        servo.set_retracted()
+        assert servo.current_extension == ServoExtension.MIN_EXTENSION
+        assert servo.servo.value == ServoExtension.MIN_EXTENSION.value
+        time.sleep(SERVO_DELAY + 0.05)
+        assert servo.current_extension == ServoExtension.MIN_NO_BUZZ
+        assert servo.servo.value == ServoExtension.MIN_NO_BUZZ.value
+
+    def test_repeated_extension_retraction(self, servo):
+        """Tests that repeatedly extending and retracting the servo works as expected, and has
+        no race conditions with threads."""
+
+        assert servo.current_extension == ServoExtension.MIN_NO_BUZZ
+
+        servo.set_extended()
+        assert servo.current_extension == ServoExtension.MAX_EXTENSION
+        assert servo.servo.value == ServoExtension.MAX_EXTENSION.value
+        # Assert that going to min no buzz was cancelled:
+        assert servo._go_to_min_no_buzz.finished.is_set()
+        # Assert that the thread to tell the servo to go to max no buzz has started:
+        assert servo._go_to_max_no_buzz._started.is_set()
+
+        time_taken = SERVO_DELAY / 2  # At 0.15s
+        time.sleep(time_taken)
+        servo.set_retracted()
+        assert servo.current_extension == ServoExtension.MIN_EXTENSION
+        assert servo.servo.value == ServoExtension.MIN_EXTENSION.value
+        # Assert that going to max no buzz was cancelled:
+        assert servo._go_to_max_no_buzz.finished.is_set()
+        # Assert that the thread to tell the servo to go to min no buzz has started:
+        assert servo._go_to_min_no_buzz._started.is_set()
+
+        # At 0.32s, make sure the servo will *not* go to max_no_buzz
+        time_taken = SERVO_DELAY / 2 + 0.02  # The 0.02 is to give the code time to execute:
+        time.sleep(time_taken)
+        assert servo.current_extension == ServoExtension.MIN_EXTENSION
+        assert servo.servo.value == ServoExtension.MIN_EXTENSION.value
+
+        # At 0.45s, make sure the servo will go to min_no_buzz:
+        time_taken = SERVO_DELAY / 2
+        time.sleep(time_taken)
+        assert servo.current_extension == ServoExtension.MIN_NO_BUZZ
+        assert servo.servo.value == ServoExtension.MIN_NO_BUZZ.value
diff --git a/tests/test_state.py b/tests/test_state.py
index 3677f1d..1e1f807 100644
--- a/tests/test_state.py
+++ b/tests/test_state.py
@@ -1,4 +1,3 @@
-import time
 from abc import ABC
 
 import pytest
@@ -18,7 +17,6 @@ from constants import (
     LOG_BUFFER_SIZE,
     MAX_FREE_FALL_LENGTH,
     MAX_VELOCITY_THRESHOLD,
-    SERVO_DELAY,
     ServoExtension,
 )
 
@@ -40,7 +38,7 @@ def state(airbrakes):
 
 
 @pytest.fixture
-def stand_by_state(airbrakes):
+def standby_state(airbrakes):
     return StandbyState(airbrakes)
 
 
@@ -83,8 +81,6 @@ class TestState:
     def test_init(self, state, airbrakes):
         assert state.context == airbrakes
         assert airbrakes.servo.current_extension == ServoExtension.MIN_EXTENSION
-        time.sleep(SERVO_DELAY + 0.2)  # wait for servo to extend
-        assert airbrakes.servo.current_extension == ServoExtension.MIN_NO_BUZZ
         assert issubclass(state.__class__, ABC)
 
     def test_name(self, state):
@@ -94,20 +90,17 @@ class TestState:
 class TestStandbyState:
     """Tests the StandbyState class"""
 
-    def test_slots(self, stand_by_state):
-        inst = stand_by_state
+    def test_slots(self, standby_state):
+        inst = standby_state
         for attr in inst.__slots__:
             assert getattr(inst, attr, "err") != "err", f"got extra slot '{attr}'"
 
-    def test_init(self, stand_by_state, airbrakes):
-        assert stand_by_state.context == airbrakes
-        assert airbrakes.servo.current_extension == ServoExtension.MIN_EXTENSION
-        time.sleep(SERVO_DELAY + 0.2)  # wait for servo to extend
-        assert airbrakes.servo.current_extension == ServoExtension.MIN_NO_BUZZ
-        assert issubclass(stand_by_state.__class__, State)
+    def test_init(self, standby_state, airbrakes):
+        assert standby_state.context == airbrakes
+        assert issubclass(standby_state.__class__, State)
 
-    def test_name(self, stand_by_state):
-        assert stand_by_state.name == "StandbyState"
+    def test_name(self, standby_state):
+        assert standby_state.name == "StandbyState"
 
     @pytest.mark.parametrize(
         ("current_velocity", "current_altitude", "expected_state"),
@@ -126,11 +119,11 @@ class TestStandbyState:
             "high_velocity",
         ],
     )
-    def test_update(self, stand_by_state, current_velocity, current_altitude, expected_state):
-        stand_by_state.context.data_processor._vertical_velocities = [current_velocity]
-        stand_by_state.context.data_processor._current_altitudes = [current_altitude]
-        stand_by_state.update()
-        assert isinstance(stand_by_state.context.state, expected_state)
+    def test_update(self, standby_state, current_velocity, current_altitude, expected_state):
+        standby_state.context.data_processor._vertical_velocities = [current_velocity]
+        standby_state.context.data_processor._current_altitudes = [current_altitude]
+        standby_state.update()
+        assert isinstance(standby_state.context.state, expected_state)
 
 
 class TestMotorBurnState:
@@ -143,9 +136,6 @@ class TestMotorBurnState:
 
     def test_init(self, motor_burn_state, airbrakes):
         assert motor_burn_state.context == airbrakes
-        assert airbrakes.servo.current_extension == ServoExtension.MIN_EXTENSION
-        time.sleep(SERVO_DELAY + 0.1)  # wait for servo to extend
-        assert airbrakes.servo.current_extension == ServoExtension.MIN_NO_BUZZ
         assert issubclass(motor_burn_state.__class__, State)
         assert motor_burn_state.start_time_ns == 0