Fix: Also write run time when doing sync action (not only position). (#48)

Author: aentinger

src/Braccio++.h

@@ -80,7 +80,7 @@ class BraccioClass
   void positions(float * buffer);
   void positions(float & a1, float & a2, float & a3, float & a4, float & a5, float & a6);
 
-  inline void speed(speed_grade_t const speed_grade) { _servos.setTime(SmartServoClass::BROADCAST, speed_grade); }
+  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); }
 
   inline void disengage(int const id = SmartServoClass::BROADCAST) { _servos.disengage(id); }

src/lib/motors/SmartServo.cpp

@@ -193,6 +193,9 @@ void SmartServoClass::begin()
   writeByteCmd(BROADCAST, REG(SmartServoRegister::SERVO_MOTOR_MODE), 1);
   writeByteCmd(BROADCAST, REG(SmartServoRegister::TORQUE_SWITCH) ,1);
   _positionMode = PositionMode::IMMEDIATE;
+
+  for (int i = MIN_MOTOR_ID; i <= MAX_MOTOR_ID; i++)
+    _targetSpeed[idToArrayIndex(i)] = 1000;
 }
 
 void SmartServoClass::setPosition(uint8_t const id, float const angle)
@@ -231,13 +234,15 @@ void SmartServoClass::synchronize()
   _txPacket.length = MAX_TX_PAYLOAD_LEN;
   _txPacket.instruction = CMD(SmartServoOperation::SYNC_WRITE);
   _txPacket.payload[0] = REG(SmartServoRegister::TARGET_POSITION_H);
-  _txPacket.payload[1] = 2;
+  _txPacket.payload[1] = 4;
   int index = 2;
 
   for (int i = MIN_MOTOR_ID; i <= MAX_MOTOR_ID; i++) {
     _txPacket.payload[index++] = i;
-    _txPacket.payload[index++] = _targetPosition[idToArrayIndex(i)] >>8;
+    _txPacket.payload[index++] = _targetPosition[idToArrayIndex(i)]>>8;
     _txPacket.payload[index++] = _targetPosition[idToArrayIndex(i)];
+    _txPacket.payload[index++] = _targetSpeed[idToArrayIndex(i)]>>8;
+    _txPacket.payload[index++] = _targetSpeed[idToArrayIndex(i)];
   }
   sendPacket();
 }
@@ -254,9 +259,18 @@ void SmartServoClass::setTorque(uint8_t const id, bool const torque)
   writeByteCmd(id, REG(SmartServoRegister::TORQUE_SWITCH), torque ? 1 : 0);
 }
 
+void SmartServoClass::setTime(uint16_t const time)
+{
+  mbed::ScopedLock<rtos::Mutex> lock(_mtx);
+  for (int i = MIN_MOTOR_ID; i <= MAX_MOTOR_ID; i++)
+    _targetSpeed[idToArrayIndex(i)] = time;
+  writeWordCmd(BROADCAST, REG(SmartServoRegister::RUN_TIME_H), time);
+}
+
 void SmartServoClass::setTime(uint8_t const id, uint16_t const time)
 {
   mbed::ScopedLock<rtos::Mutex> lock(_mtx);
+  if ((id >= MIN_MOTOR_ID) && (id <= MAX_MOTOR_ID)) _targetSpeed[idToArrayIndex(id)] = time;
   writeWordCmd(id, REG(SmartServoRegister::RUN_TIME_H), time);
 }
 

src/lib/motors/SmartServo.h

@@ -75,6 +75,7 @@ class SmartServoClass
   void setMaxAngle(uint16_t const max_angle);
   void setMaxAngle(uint8_t const id, uint16_t const max_angle);
 
+  void setTime(uint16_t const time);
   void setTime(uint8_t const id, uint16_t const time);
 
   void center(uint8_t const id, uint16_t const position);
@@ -97,7 +98,7 @@ class SmartServoClass
   static int   constexpr NUM_MOTORS = 6;
   static float constexpr MAX_ANGLE = 315.0f;
 
-  static int idToArrayIndex(int const id) { return (id - 1); }
+  static size_t idToArrayIndex(size_t const id) { return (id - 1); }
 
 private:
 
@@ -140,6 +141,8 @@ class SmartServoClass
   uint8_t _rxBuf[MAX_RX_PAYLOAD_LEN];
   uint8_t _rxLen;
   uint16_t _targetPosition[NUM_MOTORS];
+  uint16_t _targetSpeed[NUM_MOTORS];
+
   PositionMode _positionMode;
 };