Merge pull request #29 from bcmi-labs/fix-connection-code

Fix: Only ping within the motor connection thread.

Author: aentinger

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;
     }
   }

examples/LCD_Motors/LCD_Motors.ino

@@ -14,24 +14,24 @@ 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;
+      Braccio.pingOn();
       selected_motor = 0;
     }
   }

src/Braccio++.cpp

@@ -20,15 +20,19 @@ BraccioClass::BraccioClass()
 , PD_UFP{PD_LOG_LEVEL_VERBOSE}
 , expander{TCA6424A_ADDRESS_ADDR_HIGH}
 , bl{}
-, _display_thread{}
+, _display_thd{}
+, _is_ping_allowed{true}
+, _is_motor_connected{false}
+, _motors_connected_mtx{}
+, _motors_connected_thd{}
 , runTime{SLOW}
 , _customMenu{nullptr}
 {
 
 }
 
-bool BraccioClass::begin(voidFuncPtr customMenu) {
-
+bool BraccioClass::begin(voidFuncPtr customMenu)
+{
   Wire.begin();
   Serial.begin(115200);
 
@@ -81,6 +85,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);
@@ -122,7 +131,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]()
   {
@@ -156,20 +165,40 @@ 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::motorConnectedThreadFunc));
 
   return true;
 }
 
+void BraccioClass::pingOn()
+{
+  mbed::ScopedLock<rtos::Mutex> lock(_motors_connected_mtx);
+  _is_ping_allowed = true;
+}
+
+void BraccioClass::pingOff()
+{
+  mbed::ScopedLock<rtos::Mutex> lock(_motors_connected_mtx);
+  _is_ping_allowed = false;
+}
+
+bool BraccioClass::connected(int const id)
+{
+  mbed::ScopedLock<rtos::Mutex> lock(_motors_connected_mtx);
+  return _is_motor_connected[SmartServoClass::idToArrayIndex(id)];
+}
+
 MotorsWrapper BraccioClass::move(int const id)
 {
   MotorsWrapper wrapper(servos, 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);
@@ -231,7 +260,7 @@ void BraccioClass::pd_thread() {
   }
 }
 
-void BraccioClass::display_thread()
+void BraccioClass::display_thread_func()
 {
   for(;;)
   {
@@ -285,21 +314,36 @@ 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]);
+bool BraccioClass::isPingAllowed()
+{
+  mbed::ScopedLock<rtos::Mutex> lock(_motors_connected_mtx);
+  return _is_ping_allowed;
+}
+
+void BraccioClass::setMotorConnectionStatus(int const id, bool const is_connected)
+{
+  mbed::ScopedLock<rtos::Mutex> lock(_motors_connected_mtx);
+  _is_motor_connected[SmartServoClass::idToArrayIndex(id)] = is_connected;
+}
+
+void BraccioClass::motorConnectedThreadFunc()
+{
+  for (;;)
+  {
+    if (isPingAllowed())
+    {
+      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 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();
     }

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,18 +48,19 @@ class BraccioClass
          bool begin(voidFuncPtr customMenu);
 
 
-         MotorsWrapper move(int const id);
-  inline MotorsWrapper get(int const id) { return move(id); }
+  void pingOn();
+  void pingOff();
+  bool connected(int const 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);
   void positions(float & a1, float & a2, float & a3, float & a4, float & a5, float & a6);
 
 
-  bool connected(int joint_index) {
-    return _connected[joint_index];
-  }
-
   void speed(speed_grade_t speed_grade) {
     runTime  = speed_grade;
   }
@@ -124,8 +85,6 @@ class BraccioClass
 
   TFT_eSPI gfx = TFT_eSPI();
 
-  bool ping_allowed = true;
-
   static BraccioClass& get_default_instance() {
     static BraccioClass dev;
     return dev;
@@ -151,7 +110,16 @@ 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 _is_ping_allowed;
+  bool _is_motor_connected[SmartServoClass::NUM_MOTORS];
+  rtos::Mutex _motors_connected_mtx;
+  rtos::Thread _motors_connected_thd;
+  bool isPingAllowed();
+  void setMotorConnectionStatus(int const id, bool const is_connected);
+  void motorConnectedThreadFunc();
 
   speed_grade_t runTime; //ms
 
@@ -172,8 +140,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;
@@ -201,14 +167,54 @@ class BraccioClass
   }
 
   void pd_thread();
-  void display_thread();
-  void motors_connected_thread();
 #endif
 
 };
 
 #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;
 };

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      ();