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..62334fe 100644 --- a/examples/LCD_Motors/LCD_Motors.ino +++ b/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; } } diff --git a/src/Braccio++.cpp b/src/Braccio++.cpp index 88a0db9..8902cd3 100644 --- a/src/Braccio++.cpp +++ b/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 lock(_motors_connected_mtx); + _is_ping_allowed = true; +} + +void BraccioClass::pingOff() +{ + mbed::ScopedLock lock(_motors_connected_mtx); + _is_ping_allowed = false; +} + +bool BraccioClass::connected(int const id) +{ + mbed::ScopedLock 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 lock(_motors_connected_mtx); + return _is_ping_allowed; +} + +void BraccioClass::setMotorConnectionStatus(int const id, bool const is_connected) +{ + mbed::ScopedLock 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(); } diff --git a/src/Braccio++.h b/src/Braccio++.h index 8589add..23a276a 100644 --- a/src/Braccio++.h +++ b/src/Braccio++.h @@ -36,47 +36,7 @@ enum speed_grade_t { #include 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 fn; }; 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 ();