Define ID to array index function for better convenience.

Author: aentinger

src/lib/motors/SmartServo.cpp

@@ -219,10 +219,10 @@ void SmartServoClass::synchronize() {
 
   for (int i = MIN_MOTOR_ID; i <= MAX_MOTOR_ID; i++) {
     _txPacket.payload[index++] = i;
-    _txPacket.payload[index++] = _targetPosition[i-1] >>8;
-    _txPacket.payload[index++] = _targetPosition[i-1];
-    _txPacket.payload[index++] = _targetSpeed[i-1]>>8;
-    _txPacket.payload[index++] = _targetSpeed[i-1];
+    _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();
   mutex.unlock();

src/lib/motors/SmartServo.h

@@ -89,6 +89,7 @@ class SmartServoClass
   static int constexpr MAX_MOTOR_ID = 6;
 
   inline bool isValidId(int const id) const { return ((id >= MIN_MOTOR_ID) && (id <= MAX_MOTOR_ID)); }
+  inline int  idToArrayIndex(int const id) const { return (id - 1); }
 
   int      calcChecksum    ();
   void     sendPacket      ();