From b628c8e01f851c1141000b1aae41fc9551c59fe7 Mon Sep 17 00:00:00 2001 From: Alexander Entinger Date: Wed, 9 Feb 2022 17:07:47 +0100 Subject: [PATCH] Fix: Wait for all motors to be connected before allowing the program flow to continue. Otherwise we risk sending commands when servos are not yet conected. If a timeout occurs, begin returns false. --- src/Braccio++.cpp | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/src/Braccio++.cpp b/src/Braccio++.cpp index 3fca34f..29ec05f 100644 --- a/src/Braccio++.cpp +++ b/src/Braccio++.cpp @@ -140,6 +140,15 @@ bool BraccioClass::begin(voidFuncPtr custom_menu) _motors_connected_thd.start(mbed::callback(this, &BraccioClass::motorConnectedThreadFunc)); + /* Wait for all motors to be actually connected. */ + for (int id = SmartServoClass::MIN_MOTOR_ID; id <= SmartServoClass::MAX_MOTOR_ID; id++) + { + auto const start = millis(); + auto isTimeout = [start]() -> bool { return ((millis() - start) > 5000); }; + for(; !isTimeout() && !connected(id); delay(100)) { } + if (isTimeout()) return false; + } + return true; }