Move check of torque limits inside of SmartServoClass.

Author: aentinger

src/Braccio++.cpp

@@ -215,14 +215,6 @@ void BraccioClass::positions(float & a1, float & a2, float & a3, float & a4, flo
   a6 = _servos.getPosition(6);
 }
 
-void BraccioClass::setMaxTorque(int const id, uint16_t const max_torque)
-{
-  if (max_torque > SmartServoClass::TORQUE_MAX)
-    _servos.setMaxTorque(SmartServoClass::TORQUE_MAX);
-  else
-    _servos.setMaxTorque(max_torque);
-}
-
 int BraccioClass::getKey() {
   if (isJoystickPressed_LEFT()) {
     return 1;

src/Braccio++.h

@@ -80,8 +80,8 @@ class BraccioClass
   void positions(float * buffer);
   void positions(float & a1, float & a2, float & a3, float & a4, float & a5, float & a6);
 
-         void setMaxTorque(int const id, uint16_t const max_torque);
-  inline void setMaxTorque(uint16_t const max_torque) { setMaxTorque(SmartServoClass::BROADCAST, max_torque); }
+  inline void setMaxTorque(uint16_t const max_torque) { _servos.setMaxTorque(max_torque); }
+  inline void setMaxTorque(int const id, uint16_t const max_torque) { _servos.setMaxTorque(id, max_torque); }
 
   inline void speed(speed_grade_t const speed_grade) { _servos.setTime(speed_grade); }
   inline void speed(int const id, speed_grade_t const speed_grade) { _servos.setTime(id, speed_grade); }

src/lib/motors/SmartServo.cpp

@@ -274,16 +274,14 @@ void SmartServoClass::setTime(uint8_t const id, uint16_t const time)
   writeWordCmd(id, REG(SmartServoRegister::RUN_TIME_H), time);
 }
 
-void SmartServoClass::setMaxTorque(uint16_t const torque)
+void SmartServoClass::setMaxTorque(uint8_t const id, uint16_t const max_torque)
 {
   mbed::ScopedLock<rtos::Mutex> lock(_mtx);
-  writeWordCmd(BROADCAST, REG(SmartServoRegister::MAX_TORQUE_H), torque);
-}
 
-void SmartServoClass::setMaxTorque(uint8_t const id, uint16_t const torque)
-{
-  mbed::ScopedLock<rtos::Mutex> lock(_mtx);
-  writeWordCmd(id, REG(SmartServoRegister::MAX_TORQUE_H), torque);
+  if (max_torque > TORQUE_MAX)
+    writeWordCmd(id, REG(SmartServoRegister::MAX_TORQUE_H), TORQUE_MAX);
+  else
+    writeWordCmd(id, REG(SmartServoRegister::MAX_TORQUE_H), max_torque);
 }
 
 void SmartServoClass::setID(uint8_t const id)

src/lib/motors/SmartServo.h

@@ -56,9 +56,8 @@ class SmartServoClass
 
   void setTorque(uint8_t const id, bool const torque);
 
-  void setMaxTorque(uint16_t const torque);
-
-  void setMaxTorque(uint8_t const id, uint16_t const torque);
+         void setMaxTorque(uint8_t const id, uint16_t const max_torque);
+  inline void setMaxTorque(uint16_t const max_torque) { setMaxTorque(BROADCAST, max_torque); }
 
   void setID(uint8_t const id);