Cleanup Python code

RotaryInvertedPendulum-python/requirements.txt

@@ -0,0 +1,2 @@
+pygame
+pyserial

RotaryInvertedPendulum-python/src/gamepad_control.py

@@ -1,82 +1,47 @@
 #!/usr/bin/env python
 
-import pygame
 import time
-import serial
-
-# Constants
-BAUD_RATE = 115200  # Baud rate of the serial connection
-CONTROL_FREQUENCY = 100  # Frequency of the control loop in Hz
-
-# Command constants
-COMMAND_CHECK_READY = "CHECK_READY"
-COMMAND_GET_POSITION = "GET_POSITION"
-COMMAND_SET_TARGET = "SET_TARGET"
-
-
-def check_ready(arduino):
-    # Send the CHECK_READY command to the Arduino
-    arduino.write(f"{COMMAND_CHECK_READY}\n".encode())
-
-    # Wait for the response
-    response = arduino.readline().decode().strip()
-
-    # Check if the Arduino is ready
-    return response == "READY"
-
-
-def get_position(arduino):
-    """
-    Get the current position of the stepper motor from the Arduino.
-    """
-    # Send the GET_POSITION command to the Arduino
-    arduino.write(f"{COMMAND_GET_POSITION}\n".encode())
+from enum import Enum
 
-    # Read the response from the Arduino
-    response = arduino.readline().decode().strip()
-
-    # Print the response
-    print("Response from Arduino:", response)
-
-    # Return the position
-    return float(response)
-
-
-def set_target(arduino, target_position: int):
-    """
-    Set the target position of the stepper motor on the Arduino.
-    """
-    # Send the SET_TARGET command to the Arduino
-    arduino.write(f"{COMMAND_SET_TARGET} {target_position}\n".encode())
+import pygame
+import serial
 
-    # Print the target position
-    print(f"Sent target position: {target_position}")
+from pendulum_controller import Pendulum
 
 
 def main():
-    # Initialize motor position
+    # Relevant parameters
+    control_frequency = 100
+    motor_multiplier = 50.0
     actual_position_motor = 0
     target_position_motor = 0
 
     # Initialize pygame
     pygame.init()
 
+    if not pygame.joystick.get_count():
+        print("No joysticks available")
+        exit()
+
     # Set up the Xbox controller
     joystick = None
     for i in range(pygame.joystick.get_count()):
         if "Xbox" in pygame.joystick.Joystick(i).get_name():
             joystick = pygame.joystick.Joystick(i)
             joystick.init()
 
-    # Set up the serial connection
-    ser = serial.Serial("/dev/cu.usbserial-110", BAUD_RATE, timeout=1)
+    # Create the Pendulum object and initialize the connection
+    pendulum = Pendulum(serial_path="/dev/cu.usbserial-110")
+
+    if not pendulum.initialized:
+        exit()
 
-    # Wait for Arduino to be ready
-    while not check_ready(ser):
-        print("Arduino is not ready. Retrying...")
+    # Wait for the Pendulum to initialize
+    while not pendulum.check_ready():
+        print("Pendulum is not ready. Retrying...")
         time.sleep(0.1)  # Wait for 100 ms before trying again
 
-    print("Arduino is ready to receive commands.")
+    print("Pendulum is ready to receive commands.")
 
     # Initialize variables for tracking time
     last_update_time = time.time()
@@ -96,11 +61,8 @@ def main():
         if abs(x_axis) < 0.1:
             x_axis = 0.0
 
-        # Set the motor speed multiplier
-        multiplier = 50.0
-
         # Adjust motor position based on joystick input
-        target_position_motor += int(x_axis * multiplier)
+        target_position_motor += int(x_axis * motor_multiplier)
 
         # Clamp motor position between 0.0 and 1.0
         # target_position_motor = max(0.0, min(1.0, target_position_motor))
@@ -109,12 +71,16 @@ def main():
         current_time = time.time()
 
         # Check if it is time to update the motor
-        if current_time - last_update_time >= 1.0 / CONTROL_FREQUENCY:
+        if current_time - last_update_time >= 1.0 / control_frequency:
             # Get the current position of the motor
-            actual_position_motor = get_position(ser)
+            actual_position_motor = pendulum.get_position()
+
+            print(
+                f"Current position: {actual_position_motor}, Target Position: {target_position_motor}"
+            )
 
             # Set the target position of the motor
-            set_target(ser, target_position_motor)
+            pendulum.set_target(target_position_motor)
 
             # Update last update time
             last_update_time = current_time
@@ -123,7 +89,7 @@ def main():
         time.sleep(0.01)  # 10 ms
 
     # Close the serial connection
-    ser.close()
+    pendulum.close()
 
     # Quit pygame
     pygame.quit()

RotaryInvertedPendulum-python/src/pendulum_controller.py

@@ -0,0 +1,67 @@
+import time
+from enum import Enum
+
+from serial import Serial, SerialException
+
+
+class Command(Enum):
+    GET_POSITION = "GET_POSITION"
+    CHECK_READY = "CHECK_READY"
+    SET_TARGET = "SET_TARGET"
+
+
+class Pendulum:
+    def __init__(self, serial_path: str, baud_rate: int = 115200) -> None:
+        self.baud_rate = baud_rate
+
+        self.arduino = None
+        self.initialized = False
+
+        # Set up the serial connection
+        try:
+            self.arduino = Serial(serial_path, baud_rate, timeout=1)
+        except SerialException as e:
+            print(f"Warning: Failed to initialize serial connection: {e}")
+
+        if self.arduino and self.arduino.is_open:
+            self.initialized = True
+
+    def __del__(self) -> None:
+        self.close()
+
+    def check_ready(self) -> bool:
+        # Send the CHECK_READY command to the Arduino
+        self.arduino.write(f"{Command.CHECK_READY}\n".encode())
+
+        # Wait for the response
+        response = self.arduino.readline().decode().strip()
+
+        # Check if the Arduino is ready
+        return response == "READY"
+
+    def get_position(self) -> float:
+        """
+        Get the current position of the stepper motor from the Arduino.
+        """
+        # Send the GET_POSITION command to the Arduino
+        self.arduino.write(f"{Command.GET_POSITION}\n".encode())
+
+        # Read the response from the Arduino
+        response = self.arduino.readline().decode().strip()
+
+        # Return the position
+        return float(response)
+
+    def set_target(self, target_position: int) -> None:
+        """
+        Set the target position of the stepper motor on the Arduino.
+        """
+        # Send the SET_TARGET command to the Arduino
+        self.arduino.write(f"{Command.SET_TARGET} {target_position}\n".encode())
+
+        # Print the target position
+        print(f"Sent target position: {target_position}")
+
+    def close(self) -> None:
+        if self.arduino:
+            self.arduino.close()

RotaryInvertedPendulum-python/test/gamepad_test.py

@@ -6,6 +6,10 @@
 def main():
     pygame.init()
 
+    if not pygame.joystick.get_count():
+        print("No joysticks available")
+        exit()
+
     # Set up the Xbox controller
     joystick = None
     for i in range(pygame.joystick.get_count()):

RotaryInvertedPendulum-python/test/serial_test.py

@@ -1,65 +1,38 @@
-import serial
-import time
-
-# Command constants
-CHECK_READY_COMMAND = "CHECK_READY"
-GET_POSITION_COMMAND = "GET_POSITION"
-SET_TARGET_COMMAND = "SET_TARGET"
-
-
-def check_ready(arduino):
-    # Send the CHECK_READY command to the Arduino
-    arduino.write(f"{CHECK_READY_COMMAND}\n".encode())
-
-    # Wait for the response
-    response = arduino.readline().decode().strip()
-
-    # Check if the Arduino is ready
-    return response == "READY"
-
-
-def test_get_position(arduino):
-    # Send the GET_POSITION command to the Arduino
-    arduino.write(f"{GET_POSITION_COMMAND}\n".encode())
-
-    # Read the response from the Arduino
-    response = arduino.readline().decode().strip()
-
-    # Print the response
-    print("Response from Arduino:", response)
+#!/usr/bin/env python3
 
+import time
 
-def test_set_target(arduino, target_position):
-    # Send the SET_TARGET command to the Arduino
-    arduino.write(f"{SET_TARGET_COMMAND} {target_position}\n".encode())
+import serial
 
-    # Print the target position
-    print(f"Sent target position: {target_position}")
+from src.pendulum_controller import Pendulum
 
 
 def main():
-    # Set up the serial connection
-    arduino = serial.Serial("/dev/cu.usbserial-110", 9600, timeout=1)
+    # Create the Pendulum object and initialize the connection
+    pendulum = Pendulum(serial_path="/dev/cu.usbserial-110", baud_rate=9600)
+
+    if not pendulum.initialized:
+        exit()
 
     # Check if the Arduino is ready to receive commands
-    while not check_ready(arduino):
-        print("Arduino is not ready. Retrying...")
+    while not pendulum.check_ready():
+        print("Pendulum is not ready. Retrying...")
         time.sleep(0.1)  # Wait for 100 ms before trying again
 
-    print("Arduino is ready to receive commands.")
+    print("Pendulum is ready to receive commands.")
 
     for i in range(10):
-        test_get_position(arduino)
+        pendulum.get_position()
         time.sleep(0.1)
 
-    test_set_target(arduino, 1600)
+    pendulum.set_target(1600)
 
     for i in range(10):
-        test_get_position(arduino)
+        pendulum.get_position()
         time.sleep(0.1)
 
     # Close the serial connection
-    arduino.close()
+    pendulum.close()
 
 
 if __name__ == "__main__":