From 942dcf512f5fd90109b605a8982fd962d7c3f488 Mon Sep 17 00:00:00 2001
From: Alexander Entinger <cto@lxrobotics.com>
Date: Wed, 19 Jan 2022 11:33:56 +0100
Subject: [PATCH 01/12] Turn BraccioClass::connected into an inline function
 call.

---
 src/Braccio++.h | 4 +---
 1 file changed, 1 insertion(+), 3 deletions(-)

diff --git a/src/Braccio++.h b/src/Braccio++.h
index 8589add..7cd6029 100644
--- a/src/Braccio++.h
+++ b/src/Braccio++.h
@@ -96,9 +96,7 @@ class BraccioClass
   void positions(float & a1, float & a2, float & a3, float & a4, float & a5, float & a6);
 
 
-  bool connected(int joint_index) {
-    return _connected[joint_index];
-  }
+  bool connected(int const id) const { return _connected[id]; }
 
   void speed(speed_grade_t speed_grade) {
     runTime  = speed_grade;

From d1296b7cc795f6adb71a58f47169afdb5819c749 Mon Sep 17 00:00:00 2001
From: Alexander Entinger <cto@lxrobotics.com>
Date: Wed, 19 Jan 2022 11:40:15 +0100
Subject: [PATCH 02/12] Move MotorsWrapper to end of include file, allows using
 Braccio.connected - otherwise redundant calls to 'ping'.

---
 src/Braccio++.cpp |  5 +++
 src/Braccio++.h   | 88 ++++++++++++++++++++++++-----------------------
 2 files changed, 50 insertions(+), 43 deletions(-)

diff --git a/src/Braccio++.cpp b/src/Braccio++.cpp
index 88a0db9..3c31146 100644
--- a/src/Braccio++.cpp
+++ b/src/Braccio++.cpp
@@ -170,6 +170,11 @@ MotorsWrapper BraccioClass::move(int const id)
   return wrapper;
 }
 
+MotorsWrapper BraccioClass::get(int const id)
+{
+  return move(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);
diff --git a/src/Braccio++.h b/src/Braccio++.h
index 7cd6029..50a8b00 100644
--- a/src/Braccio++.h
+++ b/src/Braccio++.h
@@ -36,47 +36,7 @@ enum speed_grade_t {
 #include <chrono>
 using namespace std::chrono;
 
-class MotorsWrapper {
-public:
-  MotorsWrapper(SmartServoClass & servos, int idx) : _servos(servos), _idx(idx) {}
-  MotorsWrapper& to(float angle) {
-    _servos.setPosition(_idx, angle, _speed);
-    return *this;
-  }
-  MotorsWrapper& in(std::chrono::milliseconds len) {
-    _servos.setTime(_idx, len.count());
-    return *this;
-  }
-  MotorsWrapper& move() {
-    return *this;
-  }
-  float position() {
-    return _servos.getPosition(_idx);
-  }
-  bool connected() {
-    return _servos.ping(_idx) == 0;
-  }
-  operator bool() {
-    return connected();
-  }
-  void info(Stream& stream) {
-    _servos.getInfo(stream, _idx);
-  }
-  void disengage() {
-    _servos.disengage(_idx);
-  }
-  void engage() {
-    _servos.engage(_idx);
-  }
-  bool engaged() {
-    return _servos.isEngaged(_idx);
-  }
-
-private:
-  SmartServoClass & _servos;
-  int _idx;
-  int _speed = 100;
-};
+class MotorsWrapper;
 
 class BraccioClass
 {
@@ -88,8 +48,8 @@ class BraccioClass
          bool begin(voidFuncPtr customMenu);
 
 
-         MotorsWrapper move(int const id);
-  inline MotorsWrapper get(int const id) { return move(id); }
+  MotorsWrapper move(int const id);
+  MotorsWrapper get (int const id);
 
   void moveTo(float const a1, float const a2, float const a3, float const a4, float const a5, float const a6);
   void positions(float * buffer);
@@ -207,6 +167,48 @@ class BraccioClass
 
 #define Braccio BraccioClass::get_default_instance()
 
+class MotorsWrapper {
+public:
+  MotorsWrapper(SmartServoClass & servos, int idx) : _servos(servos), _idx(idx) {}
+  MotorsWrapper& to(float angle) {
+    _servos.setPosition(_idx, angle, _speed);
+    return *this;
+  }
+  MotorsWrapper& in(std::chrono::milliseconds len) {
+    _servos.setTime(_idx, len.count());
+    return *this;
+  }
+  MotorsWrapper& move() {
+    return *this;
+  }
+  float position() {
+    return _servos.getPosition(_idx);
+  }
+
+  inline bool connected() { return Braccio.connected(_idx); }
+
+  operator bool() {
+    return connected();
+  }
+  void info(Stream& stream) {
+    _servos.getInfo(stream, _idx);
+  }
+  void disengage() {
+    _servos.disengage(_idx);
+  }
+  void engage() {
+    _servos.engage(_idx);
+  }
+  bool engaged() {
+    return _servos.isEngaged(_idx);
+  }
+
+private:
+  SmartServoClass & _servos;
+  int _idx;
+  int _speed = 100;
+};
+
 struct __callback__container__ {
   mbed::Callback<void()> fn;
 };

From 8c47529d8a88c31d3983ea98437bc728d011600f Mon Sep 17 00:00:00 2001
From: Alexander Entinger <cto@lxrobotics.com>
Date: Wed, 19 Jan 2022 11:50:03 +0100
Subject: [PATCH 03/12] Replace direct access to member variable 'ping_allowed'
 with pingOn()/pingOff() API.

---
 .../Braccio__Template/Braccio__Template.ino   |  4 +--
 examples/LCD_Motors/LCD_Motors.ino            | 12 ++++----
 src/Braccio++.cpp                             | 29 ++++++++++---------
 src/Braccio++.h                               |  8 ++---
 4 files changed, 28 insertions(+), 25 deletions(-)

diff --git a/examples/Braccio__Template/Braccio__Template.ino b/examples/Braccio__Template/Braccio__Template.ino
index 786567c..e3c2725 100644
--- a/examples/Braccio__Template/Braccio__Template.ino
+++ b/examples/Braccio__Template/Braccio__Template.ino
@@ -14,9 +14,9 @@ static void event_handler(lv_event_t * e)
     Serial.println(txt);
     if (strcmp(txt, "Demo") == 0) {
       demo_mode = 1;
-      Braccio.ping_allowed = false;
+      Braccio.pingOff();
     } else {
-      Braccio.ping_allowed = true;
+      Braccio.pingOn();
       demo_mode = 0;
     }
   }
diff --git a/examples/LCD_Motors/LCD_Motors.ino b/examples/LCD_Motors/LCD_Motors.ino
index 98b270e..dedd1cd 100644
--- a/examples/LCD_Motors/LCD_Motors.ino
+++ b/examples/LCD_Motors/LCD_Motors.ino
@@ -14,22 +14,22 @@ static void event_handler(lv_event_t * e)
     Serial.println(txt);
     if (strcmp(txt, "Motor 1") == 0) {
       selected_motor = 1;
-      Braccio.ping_allowed = false;
+      Braccio.pingOff();
     } else if (strcmp(txt, "Motor 2") == 0) {
       selected_motor = 2;
-      Braccio.ping_allowed = false;
+      Braccio.pingOff();
     } else if (strcmp(txt, "Motor 3") == 0) {
       selected_motor = 3;
-      Braccio.ping_allowed = false;
+      Braccio.pingOff();
     } else if (strcmp(txt, "Motor 4") == 0) {
       selected_motor = 4;
-      Braccio.ping_allowed = false;
+      Braccio.pingOff();
     } else if (strcmp(txt, "Motor 5") == 0) {
       selected_motor = 5;
-      Braccio.ping_allowed = false;
+      Braccio.pingOff();
     } else if (strcmp(txt, "Motor 6") == 0) {
       selected_motor = 6;
-      Braccio.ping_allowed = false;
+      Braccio.pingOff();
     } else {
       Braccio.ping_allowed = true;
       selected_motor = 0;
diff --git a/src/Braccio++.cpp b/src/Braccio++.cpp
index 3c31146..8f388b8 100644
--- a/src/Braccio++.cpp
+++ b/src/Braccio++.cpp
@@ -21,6 +21,8 @@ BraccioClass::BraccioClass()
 , expander{TCA6424A_ADDRESS_ADDR_HIGH}
 , bl{}
 , _display_thread{}
+, _ping_allowed{true}
+, _connected{false}
 , runTime{SLOW}
 , _customMenu{nullptr}
 {
@@ -290,21 +292,22 @@ void BraccioClass::defaultMenu()
   lv_obj_set_pos(label1, 0, 0);
 }
 
-void BraccioClass::motors_connected_thread() {
-  while (1) {
-    if (ping_allowed) {
-      for (int i = 1; i < 7; i++) {
-        _connected[i] = (servos.ping(i) == 0);
-        //Serial.print(String(i) + ": ");
-        //Serial.println(_connected[i]);
+void BraccioClass::motors_connected_thread()
+{
+  for (;;)
+  {
+    if (_ping_allowed)
+    {
+      for (int id = SmartServoClass::MIN_MOTOR_ID; id <= SmartServoClass::MAX_MOTOR_ID; id++) {
+        _connected[id] = (servos.ping(id) == 0);
       }
+
       i2c_mutex.lock();
-      for (int i = 1; i < 7; i++) {
-        if (_connected[i]) {
-          setGreen(i);
-        } else {
-          setRed(i);
-        }
+      for (int id = SmartServoClass::MIN_MOTOR_ID; id <= SmartServoClass::MAX_MOTOR_ID; id++) {
+        if (_connected[id])
+          setGreen(id);
+        else
+          setRed(id);
       }
       i2c_mutex.unlock();
     }
diff --git a/src/Braccio++.h b/src/Braccio++.h
index 50a8b00..8a149b7 100644
--- a/src/Braccio++.h
+++ b/src/Braccio++.h
@@ -47,6 +47,8 @@ class BraccioClass
   inline bool begin() { return begin(nullptr); }
          bool begin(voidFuncPtr customMenu);
 
+  inline void pingOn()  { _ping_allowed = true;  }
+  inline void pingOff() { _ping_allowed = false; }
 
   MotorsWrapper move(int const id);
   MotorsWrapper get (int const id);
@@ -82,8 +84,6 @@ class BraccioClass
 
   TFT_eSPI gfx = TFT_eSPI();
 
-  bool ping_allowed = true;
-
   static BraccioClass& get_default_instance() {
     static BraccioClass dev;
     return dev;
@@ -110,6 +110,8 @@ class BraccioClass
   TCA6424A expander;
   Backlight bl;
   rtos::Thread _display_thread;
+  bool _ping_allowed;
+  bool _connected[8];
 
   speed_grade_t runTime; //ms
 
@@ -130,8 +132,6 @@ class BraccioClass
   lv_indev_t *kb_indev;
   lv_style_t _lv_style;
 
-  bool _connected[8];
-
 #ifdef __MBED__
   rtos::EventFlags pd_events;
   rtos::Mutex i2c_mutex;

From e18b5c04cfce190cd7b5f3f96b77e28d8567074f Mon Sep 17 00:00:00 2001
From: Alexander Entinger <cto@lxrobotics.com>
Date: Wed, 19 Jan 2022 11:54:05 +0100
Subject: [PATCH 04/12] Set all motor status LEDs to 'red' immediately during
 initialisation.

---
 src/Braccio++.cpp | 9 +++++++--
 1 file changed, 7 insertions(+), 2 deletions(-)

diff --git a/src/Braccio++.cpp b/src/Braccio++.cpp
index 8f388b8..41e1fc1 100644
--- a/src/Braccio++.cpp
+++ b/src/Braccio++.cpp
@@ -29,8 +29,8 @@ BraccioClass::BraccioClass()
 
 }
 
-bool BraccioClass::begin(voidFuncPtr customMenu) {
-
+bool BraccioClass::begin(voidFuncPtr customMenu)
+{
   Wire.begin();
   Serial.begin(115200);
 
@@ -83,6 +83,11 @@ bool BraccioClass::begin(voidFuncPtr customMenu) {
   expander.setPinDirection(18, 0); // P22 = 8 * 2 + 2
   expander.writePin(18, 0); // reset LCD
   expander.writePin(18, 1); // LCD out of reset
+
+  /* Set all motor status LEDs to red. */
+  for (int id = SmartServoClass::MIN_MOTOR_ID; id <= SmartServoClass::MAX_MOTOR_ID; id++) {
+    setRed(id);
+  }
   i2c_mutex.unlock();
 
   pinMode(BTN_LEFT, INPUT_PULLUP);

From cb7010c65e6c02862ac186546b4d16464fef156e Mon Sep 17 00:00:00 2001
From: Alexander Entinger <cto@lxrobotics.com>
Date: Wed, 19 Jan 2022 11:59:07 +0100
Subject: [PATCH 05/12] Grouping variables and functions related to motor
 connection code together within code.

---
 src/Braccio++.cpp | 8 +++-----
 src/Braccio++.h   | 4 +++-
 2 files changed, 6 insertions(+), 6 deletions(-)

diff --git a/src/Braccio++.cpp b/src/Braccio++.cpp
index 41e1fc1..293d8cc 100644
--- a/src/Braccio++.cpp
+++ b/src/Braccio++.cpp
@@ -23,6 +23,7 @@ BraccioClass::BraccioClass()
 , _display_thread{}
 , _ping_allowed{true}
 , _connected{false}
+, _motors_connected_thd{}
 , runTime{SLOW}
 , _customMenu{nullptr}
 {
@@ -163,10 +164,7 @@ bool BraccioClass::begin(voidFuncPtr customMenu)
   servos.begin();
   servos.setPositionMode(PositionMode::IMMEDIATE);
 
-#ifdef __MBED__
-  static rtos::Thread connected_th;
-  connected_th.start(mbed::callback(this, &BraccioClass::motors_connected_thread));
-#endif
+  _motors_connected_thd.start(mbed::callback(this, &BraccioClass::motors_connected_thread_func));
 
   return true;
 }
@@ -297,7 +295,7 @@ void BraccioClass::defaultMenu()
   lv_obj_set_pos(label1, 0, 0);
 }
 
-void BraccioClass::motors_connected_thread()
+void BraccioClass::motors_connected_thread_func()
 {
   for (;;)
   {
diff --git a/src/Braccio++.h b/src/Braccio++.h
index 8a149b7..d19eba6 100644
--- a/src/Braccio++.h
+++ b/src/Braccio++.h
@@ -110,8 +110,11 @@ class BraccioClass
   TCA6424A expander;
   Backlight bl;
   rtos::Thread _display_thread;
+
   bool _ping_allowed;
   bool _connected[8];
+  rtos::Thread _motors_connected_thd;
+  void motors_connected_thread_func();
 
   speed_grade_t runTime; //ms
 
@@ -160,7 +163,6 @@ class BraccioClass
 
   void pd_thread();
   void display_thread();
-  void motors_connected_thread();
 #endif
 
 };

From afbd59a17fc076a3e2406dab22442f45feb8b712 Mon Sep 17 00:00:00 2001
From: Alexander Entinger <cto@lxrobotics.com>
Date: Wed, 19 Jan 2022 12:01:43 +0100
Subject: [PATCH 06/12] In related news group display thread with display
 member variables.

---
 src/Braccio++.cpp | 6 +++---
 src/Braccio++.h   | 4 ++--
 2 files changed, 5 insertions(+), 5 deletions(-)

diff --git a/src/Braccio++.cpp b/src/Braccio++.cpp
index 293d8cc..1ce90ed 100644
--- a/src/Braccio++.cpp
+++ b/src/Braccio++.cpp
@@ -20,7 +20,7 @@ BraccioClass::BraccioClass()
 , PD_UFP{PD_LOG_LEVEL_VERBOSE}
 , expander{TCA6424A_ADDRESS_ADDR_HIGH}
 , bl{}
-, _display_thread{}
+, _display_thd{}
 , _ping_allowed{true}
 , _connected{false}
 , _motors_connected_thd{}
@@ -130,7 +130,7 @@ bool BraccioClass::begin(voidFuncPtr customMenu)
   p_objGroup = lv_group_create();
   lv_group_set_default(p_objGroup);
 
-  _display_thread.start(mbed::callback(this, &BraccioClass::display_thread));
+  _display_thd.start(mbed::callback(this, &BraccioClass::display_thread_func));
 
   auto check_power_func = [this]()
   {
@@ -241,7 +241,7 @@ void BraccioClass::pd_thread() {
   }
 }
 
-void BraccioClass::display_thread()
+void BraccioClass::display_thread_func()
 {
   for(;;)
   {
diff --git a/src/Braccio++.h b/src/Braccio++.h
index d19eba6..c99914b 100644
--- a/src/Braccio++.h
+++ b/src/Braccio++.h
@@ -109,7 +109,8 @@ class BraccioClass
   PD_UFP_log_c PD_UFP;
   TCA6424A expander;
   Backlight bl;
-  rtos::Thread _display_thread;
+  rtos::Thread _display_thd;
+  void display_thread_func();
 
   bool _ping_allowed;
   bool _connected[8];
@@ -162,7 +163,6 @@ class BraccioClass
   }
 
   void pd_thread();
-  void display_thread();
 #endif
 
 };

From d9c2c4ab984694c61183f1385d9ff8708104f4c9 Mon Sep 17 00:00:00 2001
From: Alexander Entinger <cto@lxrobotics.com>
Date: Wed, 19 Jan 2022 12:43:30 +0100
Subject: [PATCH 07/12] Fix direct access to member variable 'ping_allowed'.

---
 examples/LCD_Motors/LCD_Motors.ino | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/examples/LCD_Motors/LCD_Motors.ino b/examples/LCD_Motors/LCD_Motors.ino
index dedd1cd..62334fe 100644
--- a/examples/LCD_Motors/LCD_Motors.ino
+++ b/examples/LCD_Motors/LCD_Motors.ino
@@ -31,7 +31,7 @@ static void event_handler(lv_event_t * e)
       selected_motor = 6;
       Braccio.pingOff();
     } else {
-      Braccio.ping_allowed = true;
+      Braccio.pingOn();
       selected_motor = 0;
     }
   }

From bdd0d65e6fc63ff9824f150d04879f4eafedc3fd Mon Sep 17 00:00:00 2001
From: Alexander Entinger <cto@lxrobotics.com>
Date: Wed, 19 Jan 2022 12:55:24 +0100
Subject: [PATCH 08/12] Ensure that there are no race conditions, since
 '_connected' is updated within a separate thread and '_ping_allowed' is set
 from another thread than the motor connection thread check.

---
 src/Braccio++.cpp | 41 +++++++++++++++++++++++++++++++++++++----
 src/Braccio++.h   | 12 ++++++++----
 2 files changed, 45 insertions(+), 8 deletions(-)

diff --git a/src/Braccio++.cpp b/src/Braccio++.cpp
index 1ce90ed..5e76afb 100644
--- a/src/Braccio++.cpp
+++ b/src/Braccio++.cpp
@@ -23,6 +23,7 @@ BraccioClass::BraccioClass()
 , _display_thd{}
 , _ping_allowed{true}
 , _connected{false}
+, _motors_connected_mtx{}
 , _motors_connected_thd{}
 , runTime{SLOW}
 , _customMenu{nullptr}
@@ -169,6 +170,24 @@ bool BraccioClass::begin(voidFuncPtr customMenu)
   return true;
 }
 
+void BraccioClass::pingOn()
+{
+  mbed::ScopedLock<rtos::Mutex> lock(_motors_connected_mtx);
+  _ping_allowed = true;
+}
+
+void BraccioClass::pingOff()
+{
+  mbed::ScopedLock<rtos::Mutex> lock(_motors_connected_mtx);
+  _ping_allowed = false;
+}
+
+bool BraccioClass::connected(int const id)
+{
+  mbed::ScopedLock<rtos::Mutex> lock(_motors_connected_mtx);
+  return _connected[id];
+}
+
 MotorsWrapper BraccioClass::move(int const id)
 {
   MotorsWrapper wrapper(servos, id);
@@ -295,19 +314,33 @@ void BraccioClass::defaultMenu()
   lv_obj_set_pos(label1, 0, 0);
 }
 
+bool BraccioClass::isPingAllowed()
+{
+  mbed::ScopedLock<rtos::Mutex> lock(_motors_connected_mtx);
+  return _ping_allowed;
+}
+
+void BraccioClass::setMotorConnectionStatus(int const id, bool const is_connected)
+{
+  mbed::ScopedLock<rtos::Mutex> lock(_motors_connected_mtx);
+  _connected[id] = is_connected;
+}
+
 void BraccioClass::motors_connected_thread_func()
 {
   for (;;)
   {
-    if (_ping_allowed)
+    if (isPingAllowed())
     {
-      for (int id = SmartServoClass::MIN_MOTOR_ID; id <= SmartServoClass::MAX_MOTOR_ID; id++) {
-        _connected[id] = (servos.ping(id) == 0);
+      for (int id = SmartServoClass::MIN_MOTOR_ID; id <= SmartServoClass::MAX_MOTOR_ID; id++)
+      {
+        bool const is_connected = (servos.ping(id) == 0);
+        setMotorConnectionStatus(id, is_connected);
       }
 
       i2c_mutex.lock();
       for (int id = SmartServoClass::MIN_MOTOR_ID; id <= SmartServoClass::MAX_MOTOR_ID; id++) {
-        if (_connected[id])
+        if (connected(id))
           setGreen(id);
         else
           setRed(id);
diff --git a/src/Braccio++.h b/src/Braccio++.h
index c99914b..feede8b 100644
--- a/src/Braccio++.h
+++ b/src/Braccio++.h
@@ -47,8 +47,11 @@ class BraccioClass
   inline bool begin() { return begin(nullptr); }
          bool begin(voidFuncPtr customMenu);
 
-  inline void pingOn()  { _ping_allowed = true;  }
-  inline void pingOff() { _ping_allowed = false; }
+
+  void pingOn();
+  void pingOff();
+  bool connected(int const id);
+
 
   MotorsWrapper move(int const id);
   MotorsWrapper get (int const id);
@@ -58,8 +61,6 @@ class BraccioClass
   void positions(float & a1, float & a2, float & a3, float & a4, float & a5, float & a6);
 
 
-  bool connected(int const id) const { return _connected[id]; }
-
   void speed(speed_grade_t speed_grade) {
     runTime  = speed_grade;
   }
@@ -114,7 +115,10 @@ class BraccioClass
 
   bool _ping_allowed;
   bool _connected[8];
+  rtos::Mutex _motors_connected_mtx;
   rtos::Thread _motors_connected_thd;
+  bool isPingAllowed();
+  void setMotorConnectionStatus(int const id, bool const is_connected);
   void motors_connected_thread_func();
 
   speed_grade_t runTime; //ms

From 9251dcc9a141e348dac17786f891934c3621d1c0 Mon Sep 17 00:00:00 2001
From: Alexander Entinger <cto@lxrobotics.com>
Date: Wed, 19 Jan 2022 12:56:44 +0100
Subject: [PATCH 09/12] CamelCase for consistency.

---
 src/Braccio++.cpp | 4 ++--
 src/Braccio++.h   | 2 +-
 2 files changed, 3 insertions(+), 3 deletions(-)

diff --git a/src/Braccio++.cpp b/src/Braccio++.cpp
index 5e76afb..79f1b5d 100644
--- a/src/Braccio++.cpp
+++ b/src/Braccio++.cpp
@@ -165,7 +165,7 @@ bool BraccioClass::begin(voidFuncPtr customMenu)
   servos.begin();
   servos.setPositionMode(PositionMode::IMMEDIATE);
 
-  _motors_connected_thd.start(mbed::callback(this, &BraccioClass::motors_connected_thread_func));
+  _motors_connected_thd.start(mbed::callback(this, &BraccioClass::motorConnectedThreadFunc));
 
   return true;
 }
@@ -326,7 +326,7 @@ void BraccioClass::setMotorConnectionStatus(int const id, bool const is_connecte
   _connected[id] = is_connected;
 }
 
-void BraccioClass::motors_connected_thread_func()
+void BraccioClass::motorConnectedThreadFunc()
 {
   for (;;)
   {
diff --git a/src/Braccio++.h b/src/Braccio++.h
index feede8b..37286fc 100644
--- a/src/Braccio++.h
+++ b/src/Braccio++.h
@@ -119,7 +119,7 @@ class BraccioClass
   rtos::Thread _motors_connected_thd;
   bool isPingAllowed();
   void setMotorConnectionStatus(int const id, bool const is_connected);
-  void motors_connected_thread_func();
+  void motorConnectedThreadFunc();
 
   speed_grade_t runTime; //ms
 

From 66d2e92c48801663eb91e136c8d3a501a36cf0b5 Mon Sep 17 00:00:00 2001
From: Alexander Entinger <cto@lxrobotics.com>
Date: Wed, 19 Jan 2022 12:57:39 +0100
Subject: [PATCH 10/12] Prefixing boolean member with '_is' which makes it
 easier to read.

---
 src/Braccio++.cpp | 8 ++++----
 src/Braccio++.h   | 2 +-
 2 files changed, 5 insertions(+), 5 deletions(-)

diff --git a/src/Braccio++.cpp b/src/Braccio++.cpp
index 79f1b5d..668ea8f 100644
--- a/src/Braccio++.cpp
+++ b/src/Braccio++.cpp
@@ -21,7 +21,7 @@ BraccioClass::BraccioClass()
 , expander{TCA6424A_ADDRESS_ADDR_HIGH}
 , bl{}
 , _display_thd{}
-, _ping_allowed{true}
+, _is_ping_allowed{true}
 , _connected{false}
 , _motors_connected_mtx{}
 , _motors_connected_thd{}
@@ -173,13 +173,13 @@ bool BraccioClass::begin(voidFuncPtr customMenu)
 void BraccioClass::pingOn()
 {
   mbed::ScopedLock<rtos::Mutex> lock(_motors_connected_mtx);
-  _ping_allowed = true;
+  _is_ping_allowed = true;
 }
 
 void BraccioClass::pingOff()
 {
   mbed::ScopedLock<rtos::Mutex> lock(_motors_connected_mtx);
-  _ping_allowed = false;
+  _is_ping_allowed = false;
 }
 
 bool BraccioClass::connected(int const id)
@@ -317,7 +317,7 @@ void BraccioClass::defaultMenu()
 bool BraccioClass::isPingAllowed()
 {
   mbed::ScopedLock<rtos::Mutex> lock(_motors_connected_mtx);
-  return _ping_allowed;
+  return _is_ping_allowed;
 }
 
 void BraccioClass::setMotorConnectionStatus(int const id, bool const is_connected)
diff --git a/src/Braccio++.h b/src/Braccio++.h
index 37286fc..3262ce3 100644
--- a/src/Braccio++.h
+++ b/src/Braccio++.h
@@ -113,7 +113,7 @@ class BraccioClass
   rtos::Thread _display_thd;
   void display_thread_func();
 
-  bool _ping_allowed;
+  bool _is_ping_allowed;
   bool _connected[8];
   rtos::Mutex _motors_connected_mtx;
   rtos::Thread _motors_connected_thd;

From b3ddf4c730d1de7162e7fe4dee70c5941f9c8e98 Mon Sep 17 00:00:00 2001
From: Alexander Entinger <cto@lxrobotics.com>
Date: Wed, 19 Jan 2022 13:00:10 +0100
Subject: [PATCH 11/12] Prefixing boolean member with '_is' which makes it
 easier to read.

---
 src/Braccio++.cpp | 6 +++---
 src/Braccio++.h   | 2 +-
 2 files changed, 4 insertions(+), 4 deletions(-)

diff --git a/src/Braccio++.cpp b/src/Braccio++.cpp
index 668ea8f..133770c 100644
--- a/src/Braccio++.cpp
+++ b/src/Braccio++.cpp
@@ -22,7 +22,7 @@ BraccioClass::BraccioClass()
 , bl{}
 , _display_thd{}
 , _is_ping_allowed{true}
-, _connected{false}
+, _is_motor_connected{false}
 , _motors_connected_mtx{}
 , _motors_connected_thd{}
 , runTime{SLOW}
@@ -185,7 +185,7 @@ void BraccioClass::pingOff()
 bool BraccioClass::connected(int const id)
 {
   mbed::ScopedLock<rtos::Mutex> lock(_motors_connected_mtx);
-  return _connected[id];
+  return _is_motor_connected[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);
-  _connected[id] = is_connected;
+  _is_motor_connected[id] = is_connected;
 }
 
 void BraccioClass::motorConnectedThreadFunc()
diff --git a/src/Braccio++.h b/src/Braccio++.h
index 3262ce3..6d6e6a0 100644
--- a/src/Braccio++.h
+++ b/src/Braccio++.h
@@ -114,7 +114,7 @@ class BraccioClass
   void display_thread_func();
 
   bool _is_ping_allowed;
-  bool _connected[8];
+  bool _is_motor_connected[8];
   rtos::Mutex _motors_connected_mtx;
   rtos::Thread _motors_connected_thd;
   bool isPingAllowed();

From cac386f4979fcde058a47f3fb3678593af3ca521 Mon Sep 17 00:00:00 2001
From: Alexander Entinger <cto@lxrobotics.com>
Date: Wed, 19 Jan 2022 13:04:02 +0100
Subject: [PATCH 12/12] Use only the maximum number of array elements
 necessary.

---
 src/Braccio++.cpp           | 4 ++--
 src/Braccio++.h             | 2 +-
 src/lib/motors/SmartServo.h | 5 +++--
 3 files changed, 6 insertions(+), 5 deletions(-)

diff --git a/src/Braccio++.cpp b/src/Braccio++.cpp
index 133770c..8902cd3 100644
--- a/src/Braccio++.cpp
+++ b/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()
diff --git a/src/Braccio++.h b/src/Braccio++.h
index 6d6e6a0..23a276a 100644
--- a/src/Braccio++.h
+++ b/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();
diff --git a/src/lib/motors/SmartServo.h b/src/lib/motors/SmartServo.h
index 63914af..8dbe81c 100644
--- a/src/lib/motors/SmartServo.h
+++ b/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      ();