Merge pull request #32 from bcmi-labs/unify-speed-api

Unify API for setting the run speed of a servo

Author: aentinger

src/Braccio++.cpp

@@ -25,7 +25,6 @@ BraccioClass::BraccioClass()
 , _is_motor_connected{false}
 , _motors_connected_mtx{}
 , _motors_connected_thd{}
-, runTime{SLOW}
 , _customMenu{nullptr}
 {
 
@@ -163,6 +162,7 @@ bool BraccioClass::begin(voidFuncPtr customMenu)
   }
 
   servos.begin();
+  servos.setTime(SmartServoClass::BROADCAST, SLOW);
   servos.setPositionMode(PositionMode::IMMEDIATE);
 
   _motors_connected_thd.start(mbed::callback(this, &BraccioClass::motorConnectedThreadFunc));
@@ -202,12 +202,12 @@ Servo BraccioClass::get(int const id)
 void BraccioClass::moveTo(float const a1, float const a2, float const a3, float const a4, float const a5, float const a6)
 {
   servos.setPositionMode(PositionMode::SYNC);
-  servos.setPosition(1, a1, runTime);
-  servos.setPosition(2, a2, runTime);
-  servos.setPosition(3, a3, runTime);
-  servos.setPosition(4, a4, runTime);
-  servos.setPosition(5, a5, runTime);
-  servos.setPosition(6, a6, runTime);
+  servos.setPosition(1, a1);
+  servos.setPosition(2, a2);
+  servos.setPosition(3, a3);
+  servos.setPosition(4, a4);
+  servos.setPosition(5, a5);
+  servos.setPosition(6, a6);
   servos.synchronize();
   servos.setPositionMode(PositionMode::IMMEDIATE);
 }

src/Braccio++.h

@@ -60,7 +60,9 @@ 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)           { runTime  = speed_grade; }
+  inline void speed(speed_grade_t const speed_grade) { servos.setTime(SmartServoClass::BROADCAST, 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); }
   inline void engage   (int const id = SmartServoClass::BROADCAST) { servos.engage(id); }
 
@@ -112,8 +114,6 @@ class BraccioClass
   void setMotorConnectionStatus(int const id, bool const is_connected);
   void motorConnectedThreadFunc();
 
-  speed_grade_t runTime; //ms
-
   voidFuncPtr _customMenu;
 
   const int BTN_LEFT = 3;
@@ -175,7 +175,7 @@ class Servo
   inline bool engaged()   { return _servos.isEngaged(_id); }
 
   inline Servo & move()                                    { return *this; }
-  inline Servo & to  (float const angle)                   { _servos.setPosition(_id, angle, _speed); return *this; }
+  inline Servo & to  (float const angle)                   { _servos.setPosition(_id, angle); return *this; }
   inline Servo & in  (std::chrono::milliseconds const len) { _servos.setTime(_id, len.count()); return *this; }
 
   inline float position()            { return _servos.getPosition(_id); }
@@ -189,8 +189,6 @@ class Servo
 
   SmartServoClass & _servos;
   int const _id;
-  int const _speed = 100;
-
 };
 
 struct __callback__container__ {

src/lib/motors/SmartServo.cpp

@@ -175,7 +175,7 @@ void SmartServoClass::begin()
   _positionMode = PositionMode::IMMEDIATE;
 }
 
-void SmartServoClass::setPosition(uint8_t const id, float const angle, uint16_t const speed)
+void SmartServoClass::setPosition(uint8_t const id, float const angle)
 {
   if (!isValidAngle(angle))
     return;
@@ -184,7 +184,6 @@ void SmartServoClass::setPosition(uint8_t const id, float const angle, uint16_t
   if (isValidId(id))
   {
     _targetPosition[idToArrayIndex(id)] = angleToPosition(angle);
-    _targetSpeed[idToArrayIndex(id)] = speed;
     if (_positionMode==PositionMode::IMMEDIATE) {
       writeWordCmd(id, REG(SmartServoRegister::TARGET_POSITION_H), angleToPosition(angle));
     }
@@ -212,15 +211,13 @@ 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] = 4;
+  _txPacket.payload[1] = 2;
   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)];
-    _txPacket.payload[index++] = _targetSpeed[idToArrayIndex(i)]>>8;
-    _txPacket.payload[index++] = _targetSpeed[idToArrayIndex(i)];
   }
   sendPacket();
 }

src/lib/motors/SmartServo.h

@@ -26,7 +26,7 @@ class SmartServoClass
 
   inline void setPositionMode(PositionMode const mode) { _positionMode = mode; }
 
-  void setPosition(uint8_t const id, float const angle, uint16_t const speed);
+  void setPosition(uint8_t const id, float const angle);
 
   float getPosition(uint8_t const id);
 
@@ -120,7 +120,6 @@ class SmartServoClass
   uint8_t _rxBuf[MAX_RX_PAYLOAD_LEN];
   uint8_t _rxLen;
   uint16_t _targetPosition[NUM_MOTORS];
-  uint16_t _targetSpeed[NUM_MOTORS];
   PositionMode _positionMode;
 };