From 7ce2ba4433c043f5c82b6ae26bbaaec98d1813d3 Mon Sep 17 00:00:00 2001 From: Alexander Entinger Date: Wed, 27 Apr 2022 06:47:46 +0200 Subject: [PATCH 1/4] Instead of pinging every servo each second ping them sequentially. --- src/Braccio++.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/Braccio++.cpp b/src/Braccio++.cpp index d81f682..443a224 100644 --- a/src/Braccio++.cpp +++ b/src/Braccio++.cpp @@ -342,15 +342,15 @@ void BraccioClass::setMotorConnectionStatus(int const id, bool const is_connecte void BraccioClass::motorConnectedThreadFunc() { + int next_id_to_be_pinged = SmartServoClass::MIN_MOTOR_ID; + 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); - } + bool const is_connected = (_servos.ping(next_id_to_be_pinged) == 0); + setMotorConnectionStatus(next_id_to_be_pinged, is_connected); + next_id_to_be_pinged = (next_id_to_be_pinged < SmartServoClass::MAX_MOTOR_ID) ? next_id_to_be_pinged + 1 : SmartServoClass::MIN_MOTOR_ID; for (int id = SmartServoClass::MIN_MOTOR_ID; id <= SmartServoClass::MAX_MOTOR_ID; id++) { if (connected(id)) From 47df1485ae07218a83e9af98ccaa5124195f0af0 Mon Sep 17 00:00:00 2001 From: Alexander Entinger Date: Wed, 27 Apr 2022 06:54:15 +0200 Subject: [PATCH 2/4] Only signal missing servo after three consecutive lost pings. --- src/Braccio++.cpp | 13 ++++++++++++- 1 file changed, 12 insertions(+), 1 deletion(-) diff --git a/src/Braccio++.cpp b/src/Braccio++.cpp index 443a224..fc61bdc 100644 --- a/src/Braccio++.cpp +++ b/src/Braccio++.cpp @@ -343,13 +343,24 @@ void BraccioClass::setMotorConnectionStatus(int const id, bool const is_connecte void BraccioClass::motorConnectedThreadFunc() { int next_id_to_be_pinged = SmartServoClass::MIN_MOTOR_ID; + int servo_missed_ping_cnt[SmartServoClass::NUM_MOTORS] = {0}; + + static int const MAX_MISSED_PING_CNT = 3; for (;;) { if (isPingAllowed()) { bool const is_connected = (_servos.ping(next_id_to_be_pinged) == 0); - setMotorConnectionStatus(next_id_to_be_pinged, is_connected); + + if (!is_connected) servo_missed_ping_cnt[SmartServoClass::idToArrayIndex(next_id_to_be_pinged)]++; + else servo_missed_ping_cnt[SmartServoClass::idToArrayIndex(next_id_to_be_pinged)] = 0; + + if (servo_missed_ping_cnt[SmartServoClass::idToArrayIndex(next_id_to_be_pinged)] > MAX_MISSED_PING_CNT) + setMotorConnectionStatus(next_id_to_be_pinged, false); + else + setMotorConnectionStatus(next_id_to_be_pinged, true); + next_id_to_be_pinged = (next_id_to_be_pinged < SmartServoClass::MAX_MOTOR_ID) ? next_id_to_be_pinged + 1 : SmartServoClass::MIN_MOTOR_ID; for (int id = SmartServoClass::MIN_MOTOR_ID; id <= SmartServoClass::MAX_MOTOR_ID; id++) { From 8b27240f61b881390ee565411a59cfd0e50cb0f9 Mon Sep 17 00:00:00 2001 From: Alexander Entinger Date: Wed, 27 Apr 2022 06:56:50 +0200 Subject: [PATCH 3/4] Run thread every 500 ms instead of every 1 second to account for increased delay due to staggered polling. --- src/Braccio++.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/Braccio++.cpp b/src/Braccio++.cpp index fc61bdc..6b16e45 100644 --- a/src/Braccio++.cpp +++ b/src/Braccio++.cpp @@ -370,7 +370,7 @@ void BraccioClass::motorConnectedThreadFunc() expander_setRed(id); } } - delay(1000); + delay(500); } } From 37c8b3c07edb84f6415444902ef270abc0a3a11d Mon Sep 17 00:00:00 2001 From: Alexander Entinger Date: Wed, 27 Apr 2022 07:00:52 +0200 Subject: [PATCH 4/4] After two consecutive pings fail, we flag the issue with the user via LED. This is a trade off to prevent too long wait times. --- src/Braccio++.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/Braccio++.cpp b/src/Braccio++.cpp index 6b16e45..a0ddbfc 100644 --- a/src/Braccio++.cpp +++ b/src/Braccio++.cpp @@ -345,7 +345,7 @@ void BraccioClass::motorConnectedThreadFunc() int next_id_to_be_pinged = SmartServoClass::MIN_MOTOR_ID; int servo_missed_ping_cnt[SmartServoClass::NUM_MOTORS] = {0}; - static int const MAX_MISSED_PING_CNT = 3; + static int const MAX_MISSED_PING_CNT = 2; for (;;) { @@ -356,7 +356,7 @@ void BraccioClass::motorConnectedThreadFunc() if (!is_connected) servo_missed_ping_cnt[SmartServoClass::idToArrayIndex(next_id_to_be_pinged)]++; else servo_missed_ping_cnt[SmartServoClass::idToArrayIndex(next_id_to_be_pinged)] = 0; - if (servo_missed_ping_cnt[SmartServoClass::idToArrayIndex(next_id_to_be_pinged)] > MAX_MISSED_PING_CNT) + if (servo_missed_ping_cnt[SmartServoClass::idToArrayIndex(next_id_to_be_pinged)] >= MAX_MISSED_PING_CNT) setMotorConnectionStatus(next_id_to_be_pinged, false); else setMotorConnectionStatus(next_id_to_be_pinged, true);