From 111bdf8eaafba194ff97451460ede0a55b174a1a Mon Sep 17 00:00:00 2001
From: Nicolas Rabault <nicolas.rabault@luos.io>
Date: Thu, 1 Aug 2024 14:49:48 +0200
Subject: [PATCH] Add torque management on servomotors

---
 pyluos/services/servoMotor.py | 30 ++++++++++++++++++++++++++++++
 1 file changed, 30 insertions(+)

diff --git a/pyluos/services/servoMotor.py b/pyluos/services/servoMotor.py
index 29a2efa..292c1fe 100644
--- a/pyluos/services/servoMotor.py
+++ b/pyluos/services/servoMotor.py
@@ -62,6 +62,7 @@ def __init__(self, id, alias, device):
         self._target_rot_position = 0.0
         self._target_trans_speed = 0.0
         self._target_trans_position = 0.0
+        self._target_torque = 0.0
 
         # report modes
         self._rot_position = 0.0
@@ -390,6 +391,35 @@ def target_trans_position(self, s):
         else :
             self._push_value("target_trans_position", s)
 
+    # torque
+    @property
+    def target_torque(self):
+        if (self._config[ServoMotor._MODE_TORQUE] != True):
+            print("torque mode could be not enabled in the service please use 'device.service.torque_mode = True' to enable it")
+        return self._target_torque
+
+    @target_torque.setter
+    def target_torque(self, s):
+        self._target_torque = s
+        self._push_value("target_torque", s)
+
+    @property
+    def torque_mode(self):
+        return self._config[ServoMotor._MODE_TORQUE]
+
+    @torque_mode.setter
+    def torque_mode(self, enable):
+        self._config[ServoMotor._MODE_TORQUE] = True if enable != 0  else False
+        if (enable == True) :
+            self._config[ServoMotor._MODE_LINEAR_POSITION] = False
+            self._config[ServoMotor._MODE_POWER] = False
+            self._config[ServoMotor._MODE_ANGULAR_POSITION] = False
+            self._config[ServoMotor._MODE_ANGULAR_SPEED ] = False
+            self._config[ServoMotor._MODE_LINEAR_SPEED] = False
+        self._push_value('parameters', self._convert_config())
+        time.sleep(0.01)
+
+
     @property
     def trans_position_mode(self):
         return self._config[ServoMotor._MODE_LINEAR_POSITION]