Provide API to set the max allowed torque on a per-servo basis.

Author: aentinger

src/Braccio++.cpp

@@ -215,6 +215,14 @@ 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,6 +80,9 @@ 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 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.h

@@ -98,6 +98,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: