Use only the maximum number of array elements necessary.

Author: aentinger

src/Braccio++.cpp

@@ -185,7 +185,7 @@ void BraccioClass::pingOff()
 bool BraccioClass::connected(int const id)
 {
   mbed::ScopedLock<rtos::Mutex> lock(_motors_connected_mtx);
-  return _is_motor_connected[id];
+  return _is_motor_connected[SmartServoClass::idToArrayIndex(id)];
 }
 
 MotorsWrapper BraccioClass::move(int const id)
@@ -323,7 +323,7 @@ bool BraccioClass::isPingAllowed()
 void BraccioClass::setMotorConnectionStatus(int const id, bool const is_connected)
 {
   mbed::ScopedLock<rtos::Mutex> lock(_motors_connected_mtx);
-  _is_motor_connected[id] = is_connected;
+  _is_motor_connected[SmartServoClass::idToArrayIndex(id)] = is_connected;
 }
 
 void BraccioClass::motorConnectedThreadFunc()

src/Braccio++.h

@@ -114,7 +114,7 @@ class BraccioClass
   void display_thread_func();
 
   bool _is_ping_allowed;
-  bool _is_motor_connected[8];
+  bool _is_motor_connected[SmartServoClass::NUM_MOTORS];
   rtos::Mutex _motors_connected_mtx;
   rtos::Thread _motors_connected_thd;
   bool isPingAllowed();

src/lib/motors/SmartServo.h

@@ -74,18 +74,19 @@ class SmartServoClass
   static int   constexpr BROADCAST = 0xFE;
   static int   constexpr MIN_MOTOR_ID = 1;
   static int   constexpr MAX_MOTOR_ID = 6;
+  static int   constexpr NUM_MOTORS = 6;
   static float constexpr MAX_ANGLE = 315.0f;
 
+  static int idToArrayIndex(int const id) { return (id - 1); }
+
 private:
 
-  static int constexpr NUM_MOTORS = 6;
   static int constexpr MAX_TX_PAYLOAD_LEN = (5*NUM_MOTORS+4);
   static int constexpr MAX_RX_PAYLOAD_LEN = 10;
   static int constexpr MAX_POSITION = 4000;
 
   inline bool isValidAngle(float const angle) { return ((angle >= 0.0f) && (angle <= MAX_ANGLE)); }
   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      ();