rework of the control script

.pre-commit-config.yaml

@@ -26,7 +26,7 @@ repos:
 
   # python import sorting
   - repo: https://github.com/PyCQA/isort
-    rev: 5.10.1
+    rev: 5.12.0
     hooks:
       - id: isort
         args: ["--profile", "black", "--filter-files"]
@@ -40,7 +40,7 @@ repos:
 
   # python docstring formatting
   - repo: https://github.com/myint/docformatter
-    rev: v1.4
+    rev: v1.7.5
     hooks:
       - id: docformatter
         args: [--in-place, --wrap-summaries=99, --wrap-descriptions=99]

robot/software/controllers.py

@@ -0,0 +1,40 @@
+from abc import ABC, abstractmethod
+
+from simple_pid import PID
+
+
+class Controller(ABC):
+    @abstractmethod
+    def compute_command(self, position: float):
+        pass
+
+    @staticmethod
+    def build_controller(parameters: dict):
+        controller_type = parameters["controller_type"]
+        match controller_type:
+            case "PIDController":
+                return PIDController(parameters)
+            case _:
+                raise ValueError(f"Invalid controller type: {controller_type}")
+
+
+class PIDController(Controller):
+    def __init__(self, parameters):
+        try:
+            sample_time = 1 / parameters["control_frequency"]
+        except KeyError:
+            sample_time = None
+
+        try:
+            self.pid = PID(
+                Kp=parameters["Kp"],
+                Ki=parameters["Ki"],
+                Kd=parameters["Kd"],
+                setpoint=parameters["setpoint"],
+                sample_time=sample_time,
+            )
+        except KeyError:
+            raise ValueError("Invalid PID parameters")
+
+    def compute_command(self, position: float):
+        return self.pid(position)

scripts/configs/parameters.json

@@ -0,0 +1,19 @@
+{
+    "device": "/dev/ttyACM0",
+    "pendulum_controller":{
+        "controller_type": "PIDController",
+        "control_frequency": 2500,
+        "Kp": 1.0,
+        "Ki": 20.0,
+        "Kd": 0.01,
+        "setpoint": 180.0
+    },
+    "motor_controller":{
+        "controller_type": "PIDController",
+        "Kp": 1.0,
+        "Ki": 0.0,
+        "Kd": 0.0,
+        "setpoint": 0.0
+    },
+    "angle_threshold": 30.0
+}

scripts/control.py

@@ -0,0 +1,121 @@
+import json
+
+import matplotlib
+
+# Tkinter needs to be installed for matplotlib to work
+# sudo apt-get install python3-tk
+matplotlib.use("TkAgg")
+
+import matplotlib.pyplot as plt
+import numpy as np
+
+from furuta_gym.robot import Robot
+from robot.software.controllers import Controller
+
+
+def read_parameters_file():
+    with open("scripts/configs/parameters.json") as f:
+        parameters = json.load(f)
+    return parameters
+
+
+def has_pendulum_fallen(pendulum_angle: float, parameters: dict):
+    # TODO: Implement in firware.ino for better safety?
+    setpoint = parameters["pendulum_controller"]["setpoint"]
+    angle_threshold = parameters["angle_threshold"]
+    return np.abs(pendulum_angle - setpoint) > angle_threshold
+
+
+def plot_data(actions, motor_angles, pendulum_angles):
+    # plot actions
+    plt.figure(1)
+    plt.plot(actions)
+    plt.title("Actions Over Time")
+    plt.xlabel("Time")
+    plt.ylabel("Action Value")
+    plt.grid(True)
+
+    # plot motor angles
+    plt.figure(2)
+    plt.plot(motor_angles)
+    plt.title("Motor Angles Over Time")
+    plt.xlabel("Time")
+    plt.ylabel("Motor Angle (in radians)")
+    plt.grid(True)
+
+    # plot pendulum angles
+    plt.figure(3)
+    plt.plot(pendulum_angles)
+    plt.title("Pendulum Angles Over Time")
+    plt.xlabel("Time")
+    plt.ylabel("Pendulum Angle (in radians)")
+    plt.grid(True)
+
+    plt.show()
+
+
+if __name__ == "__main__":
+    # Read parameters from the .json file, angles are in degrees
+    parameters = read_parameters_file()
+
+    # Convert the setpoint and the angle threshold to radians
+    parameters["pendulum_controller"]["setpoint"] = np.deg2rad(
+        parameters["pendulum_controller"]["setpoint"]
+    )
+    parameters["angle_threshold"] = np.deg2rad(parameters["angle_threshold"])
+
+    robot = Robot(parameters["device"])
+
+    # Init data lists
+    actions = []
+    motor_angles = []
+    pendulum_angles = []
+
+    # Init pendulum controller
+    pendulum_controller = Controller.build_controller(parameters["pendulum_controller"])
+
+    # Init motor controller
+    motor_controller = Controller.build_controller(parameters["motor_controller"])
+
+    # Reset encoders
+    robot.reset_encoders()
+    print("GO")
+
+    # Wait for user input to start the control loop
+    input()
+
+    # Get the initial motor and pendulum angles
+    motor_angle, pendulum_angle = robot.step(0)
+
+    # Control loop
+    while True:
+        # Compute the motor command from pendulum controller
+        action = pendulum_controller.compute_command(pendulum_angle)
+
+        # Add the motor command from motor controller
+        action += motor_controller.compute_command(motor_angle)
+
+        # Clip the command between -1 and 1
+        action = np.clip(action, -1, 1)
+
+        # Call the step function and get the motor and pendulum angles
+        motor_angle, pendulum_angle = robot.step(action)
+
+        # Take the modulus of the pendulum angle between 0 and 2pi
+        pendulum_angle = np.mod(pendulum_angle, 2 * np.pi)
+
+        # Append the data to the lists
+        actions.append(action)
+        motor_angles.append(motor_angle)
+        pendulum_angles.append(pendulum_angle)
+
+        # Check if pendulum fell
+        if has_pendulum_fallen(pendulum_angle, parameters):
+            print("Pendulum fell!")
+            break
+
+    # Close the serial connection
+    robot.close()
+
+    # Plot data
+    plot_data(actions, motor_angles, pendulum_angles)