From fa1cf2553fa5611fcbbd2ede5d58538303ab20fc Mon Sep 17 00:00:00 2001
From: PAJohnson <pajohns4@ncsu.edu>
Date: Tue, 23 Jun 2020 03:46:45 +0100
Subject: [PATCH] Added test for torque limit - TestTorqueLimit()

---
 tools/odrive/tests/closed_loop_test.py | 84 +++++++++++++++++++++++++-
 1 file changed, 83 insertions(+), 1 deletion(-)

diff --git a/tools/odrive/tests/closed_loop_test.py b/tools/odrive/tests/closed_loop_test.py
index d900e6a4..31adce5d 100644
--- a/tools/odrive/tests/closed_loop_test.py
+++ b/tools/odrive/tests/closed_loop_test.py
@@ -294,11 +294,93 @@ class TestVelLimitInTorqueControl(TestClosedLoopControlBase):
             test_curve_fit(dataA[:,(0,3)], dataA[:,4], max_mean_err=0.02, inlier_range=0.05, max_outliers=len(dataA[:,0]*0.01))
             test_curve_fit(dataB[:,(0,3)], dataB[:,4], max_mean_err=0.1, inlier_range=0.2, max_outliers=len(dataB[:,0])*0.01)
 
+class TestTorqueLimit(TestClosedLoopControlBase):
+    """
+    Checks that the torque limit is respected in position, velocity, and torque control modes
+    """
+    def run_test(self, axis_ctx: ODriveAxisComponent, motor_ctx: MotorComponent, enc_ctx: EncoderComponent, logger: Logger):
+        with self.prepare(axis_ctx, motor_ctx, enc_ctx, logger):
+            max_rps = 15.0
+            max_vel = max_rps * float(enc_ctx.yaml['cpr'])
+            max_current = 30.0
+            max_torque = 0.1 # must be less than max_current * torque_constant.
+            torque_constant = axis_ctx.handle.motor.config.torque_constant
 
+            test_pos = 5 * float(enc_ctx.yaml['cpr'])
+            test_vel = 10 * float(enc_ctx.yaml['cpr'])
+            test_torque = 0.5
+
+            axis_ctx.handle.controller.config.vel_limit = max_vel
+            axis_ctx.handle.motor.config.current_lim = max_current
+            axis_ctx.handle.motor.config.torque_lim = inf #disable torque limit
+            axis_ctx.handle.controller.config.control_mode = CONTROL_MODE_VELOCITY_CONTROL
+
+            def data_getter():
+                current_setpoint = axis_ctx.handle.motor.current_control.Iq_setpoint
+                torque_setpoint = current_setpoint * torque_constant
+                torque_limit = axis_ctx.handle.motor.config.torque_lim
+                # Abort immediately if the absolute limits are exceeded
+                test_assert_within(current_setpoint, -max_current, max_current)
+                test_assert_within(torque_setpoint, -torque_limit, torque_limit)
+                return max_current, current_setpoint, torque_limit, torque_setpoint
+
+            # begin test
+            axis_ctx.handle.motor.config.torque_lim = max_torque
+            request_state(axis_ctx, AXIS_STATE_CLOSED_LOOP_CONTROL)
+
+            # step input positions
+            logger.debug('input_pos step test')
+            axis_ctx.handle.controller.config.control_mode = CONTROL_MODE_POSITION_CONTROL
+            axis_ctx.handle.controller.input_pos = test_pos
+            dataPos = record_log(data_getter, duration=1.0)
+            axis_ctx.handle.controller.input_pos = -test_pos
+            dataPos = np.concatenate([dataPos, record_log(data_getter, duration=1.0)])
+            axis_ctx.handle.controller.input_pos = test_pos
+            dataPos = np.concatenate([dataPos, record_log(data_getter, duration=1.0)])
+            axis_ctx.handle.controller.input_pos = -test_pos
+            dataPos = np.concatenate([dataPos, record_log(data_getter, duration=1.0)])
+            time.sleep(0.5)
+
+            test_assert_no_error(axis_ctx)
+
+            # step input velocities
+            logger.debug('input_vel step test')
+            axis_ctx.handle.controller.config.control_mode = CONTROL_MODE_VELOCITY_CONTROL
+            axis_ctx.handle.controller.input_vel = test_vel
+            dataVel = record_log(data_getter, duration=1.0)
+            axis_ctx.handle.controller.input_vel = -test_vel
+            dataVel = np.concatenate([dataVel, record_log(data_getter, duration=1.0)])
+            axis_ctx.handle.controller.input_vel = test_vel
+            dataVel = np.concatenate([dataVel, record_log(data_getter, duration=1.0)])
+            axis_ctx.handle.controller.input_vel = -test_vel
+            dataVel = np.concatenate([dataVel, record_log(data_getter, duration=1.0)])
+            axis_ctx.handle.controller.input_vel = 0
+            time.sleep(0.5)
+
+            # step input torques
+            logger.debug('input_torque step test')
+            axis_ctx.handle.controller.config.control_mode = CONTROL_MODE_TORQUE_CONTROL
+            axis_ctx.handle.controller.input_torque = test_torque
+            dataTq = record_log(data_getter, duration=1.0)
+            axis_ctx.handle.controller.input_torque = -test_torque
+            dataTq = np.concatenate([dataTq, record_log(data_getter, duration=1.0)])
+            axis_ctx.handle.controller.input_torque = test_torque
+            dataTq = np.concatenate([dataTq, record_log(data_getter, duration=1.0)])
+            axis_ctx.handle.controller.input_torque = -test_torque
+            dataTq = np.concatenate([dataTq, record_log(data_getter, duration=1.0)])
+            axis_ctx.handle.controller.input_torque = 0
+            time.sleep(0.5)
+
+            # did we pass?
+
+            test_assert_no_error(axis_ctx)
+
+            axis_ctx.handle.requested_state=1
 
 if __name__ == '__main__':
     test_runner.run([
         TestClosedLoopControl(),
         TestRegenProtection(),
-        TestVelLimitInTorqueControl()
+        TestVelLimitInTorqueControl(),
+        TestTorqueLimit()
     ])