diff --git a/furuta/controls/controllers.py b/furuta/controls/controllers.py
index ad3160c..c6e772c 100644
--- a/furuta/controls/controllers.py
+++ b/furuta/controls/controllers.py
@@ -1,5 +1,6 @@
 from abc import ABC, abstractmethod
 
+import numpy as np
 from simple_pid import PID
 
 
@@ -30,7 +31,7 @@ def __init__(self, parameters):
                 Kp=parameters["Kp"],
                 Ki=parameters["Ki"],
                 Kd=parameters["Kd"],
-                setpoint=parameters["setpoint"],
+                setpoint=np.deg2rad(parameters["setpoint"]),
                 sample_time=sample_time,
             )
         except KeyError:
diff --git a/scripts/control.py b/scripts/control.py
index bf39ad0..df349f9 100644
--- a/scripts/control.py
+++ b/scripts/control.py
@@ -7,15 +7,13 @@
 from furuta.robot import Robot
 
 
-def read_parameters_file():
-    with open("scripts/configs/parameters.json") as f:
+def read_parameters_file(path: str):
+    with open(path) as f:
         parameters = json.load(f)
     return parameters
 
 
-def has_pendulum_fallen(pendulum_angle: float, parameters: dict):
-    setpoint = parameters["pendulum_controller"]["setpoint"]
-    angle_threshold = parameters["angle_threshold"]
+def has_pendulum_fallen(pendulum_angle: float, setpoint: float, angle_threshold: float):
     return np.abs(pendulum_angle - setpoint) > angle_threshold
 
 
@@ -49,13 +47,11 @@ def plot_data(actions, motor_angles, pendulum_angles):
 
 if __name__ == "__main__":
     # Read parameters from the .json file, angles are in degrees
-    parameters = read_parameters_file()
+    parameters = read_parameters_file("scripts/configs/parameters.json")
 
     # 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"])
+    SETPOINT = np.deg2rad(parameters["pendulum_controller"]["setpoint"])
+    ANGLE_THRESHOLD = np.deg2rad(parameters["angle_threshold"])
 
     robot = Robot(parameters["device"])
 
@@ -105,7 +101,7 @@ def plot_data(actions, motor_angles, pendulum_angles):
         pendulum_angles.append(pendulum_angle)
 
         # Check if pendulum fell
-        if has_pendulum_fallen(pendulum_angle, parameters):
+        if has_pendulum_fallen(pendulum_angle, setpoint=SETPOINT, angle_threshold=ANGLE_THRESHOLD):
             print("Pendulum fell!")
             break
 
diff --git a/tests/interactive_robot_selftest.py b/tests/interactive_robot_selftest.py
index e158f97..0b7c8e8 100644
--- a/tests/interactive_robot_selftest.py
+++ b/tests/interactive_robot_selftest.py
@@ -1,6 +1,9 @@
 import time
 
-from furuta_gym.robot import Robot  # replace with your actual module and class
+import numpy as np
+
+from furuta.robot import Robot
+from scripts.control import has_pendulum_fallen
 
 # not using pytest here bc couldn't get user input during test session
 
@@ -16,10 +19,9 @@ def test_control_freq(device):
     elapsed_time = time.time() - start_time
     mean_loop_time = elapsed_time / nb_iterations
     max_control_freq = round(1 / mean_loop_time)
-    print(mean_loop_time)
-    print("max_control_freq: ", max_control_freq)
     robot.close()
     assert max_control_freq > 100, "Control frequency weirdly low."
+    return f"max_control_freq: {max_control_freq}"
 
 
 def test_motor_stop(device):
@@ -32,6 +34,7 @@ def test_motor_stop(device):
     user_response = input("Has the motor stopped? (yes/no): ")
     robot.close()
     assert user_response.lower() == "yes", "The motor did not stop after 500ms"
+    return ""
 
 
 def test_motor_direction(device):
@@ -42,6 +45,22 @@ def test_motor_direction(device):
     user_response = input("Was the motor spinning clockwise? (yes/no): ")
     robot.close()
     assert user_response.lower() == "yes", "Motor spinning backwards, swap motor wires."
+    return ""
+
+
+def test_has_pendulum_fallen(device):
+    robot = Robot(device)
+    robot.reset_encoders()
+
+    input("Lift the pendulum and press enter, then let the pendulum fall")
+    time.sleep(3.0)
+
+    _, pendulum_angle = robot.step(0.0)
+    robot.close()
+    assert has_pendulum_fallen(
+        pendulum_angle, setpoint=np.pi, angle_threshold=np.deg2rad(60.0)
+    ), "The fall was not detcected"
+    return ""
 
 
 def print_report(results):
@@ -54,8 +73,8 @@ def run_tests(tests, device):
 
     for test in tests:
         try:
-            test(device)
-            results.append(f"{test.__name__}: OK")
+            result = test(device)
+            results.append(f"{test.__name__}: OK\n{str(result)}")
         except Exception as e:
             results.append(f"{test.__name__}: FAIL\n{str(e)}")