Merge pull request #84 from arduino-libraries/torque-api

Provide API to limit the maximum torque of each servo.

Author: aentinger

src/Braccio++.h

@@ -80,6 +80,9 @@ class BraccioClass
   void positions(float * buffer);
   void positions(float & a1, float & a2, float & a3, float & a4, float & a5, float & a6);
 
+  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); }
 
@@ -184,6 +187,7 @@ class Servo
   inline void disengage() { _servos.disengage(_id); }
   inline void engage()    { _servos.engage(_id); }
   inline bool engaged()   { return _servos.isEngaged(_id); }
+  inline void setMaxTorque(uint16_t const max_torque) { _servos.setMaxTorque(_id, max_torque); }
 
   inline Servo & move()                                    { return *this; }
   inline Servo & to  (float const angle)                   { _servos.setPosition(_id, angle); return *this; }

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+1, 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);
 
@@ -98,6 +97,9 @@ class SmartServoClass
   static int   constexpr NUM_MOTORS = 6;
   static float constexpr MAX_ANGLE = 315.0f;
 
+  static uint16_t constexpr TORQUE_MIN =    0;
+  static uint16_t constexpr TORQUE_MAX = 1000;
+
   static size_t idToArrayIndex(size_t const id) { return (id - 1); }
 
 private: