From 1bc38c5faf41e1098e8bf5bad19093fa42d817ec Mon Sep 17 00:00:00 2001 From: Naveen Date: Wed, 22 Feb 2023 13:00:00 +0900 Subject: [PATCH 1/3] Fix: SmartServoClass::getPosition return value This PR fixes the issue #97. --- src/lib/motors/SmartServo.cpp | 13 ++++++++++++- 1 file changed, 12 insertions(+), 1 deletion(-) diff --git a/src/lib/motors/SmartServo.cpp b/src/lib/motors/SmartServo.cpp index 84efd23..1d5f031 100644 --- a/src/lib/motors/SmartServo.cpp +++ b/src/lib/motors/SmartServo.cpp @@ -235,7 +235,18 @@ float SmartServoClass::getPosition(uint8_t const id) return -1.0f; mbed::ScopedLock lock(_mtx); - return positionToAngle(readWordCmd(id, REG(SmartServoRegister::POSITION_H))); + int position = readWordCmd(id, REG(SmartServoRegister::POSITION_H)); + + // retry one more time + if (position < 0) { + delay(1); + position = readWordCmd(id, REG(SmartServoRegister::POSITION_H)); + } + + if (position < 0) + return -1.0f; + + return positionToAngle(position); } void SmartServoClass::center(uint8_t const id, uint16_t const position) From 1217b070f1c67a077dcc000ddf64b9c0d4688d00 Mon Sep 17 00:00:00 2001 From: Naveen Date: Wed, 22 Feb 2023 14:51:31 +0900 Subject: [PATCH 2/3] Update src/lib/motors/SmartServo.cpp Co-authored-by: Alexander Entinger --- src/lib/motors/SmartServo.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/lib/motors/SmartServo.cpp b/src/lib/motors/SmartServo.cpp index 1d5f031..c3cd566 100644 --- a/src/lib/motors/SmartServo.cpp +++ b/src/lib/motors/SmartServo.cpp @@ -235,7 +235,7 @@ float SmartServoClass::getPosition(uint8_t const id) return -1.0f; mbed::ScopedLock lock(_mtx); - int position = readWordCmd(id, REG(SmartServoRegister::POSITION_H)); + int const position = readWordCmd(id, REG(SmartServoRegister::POSITION_H)); // retry one more time if (position < 0) { From e1e6612534cc531cb7e03a7684b0826c942f8fdb Mon Sep 17 00:00:00 2001 From: Alexander Entinger Date: Wed, 22 Feb 2023 07:08:46 +0100 Subject: [PATCH 3/3] Revert "Update src/lib/motors/SmartServo.cpp" - previous commit causes build error a couple of lines down - my bad. This reverts commit 1217b070f1c67a077dcc000ddf64b9c0d4688d00. --- src/lib/motors/SmartServo.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/lib/motors/SmartServo.cpp b/src/lib/motors/SmartServo.cpp index c3cd566..1d5f031 100644 --- a/src/lib/motors/SmartServo.cpp +++ b/src/lib/motors/SmartServo.cpp @@ -235,7 +235,7 @@ float SmartServoClass::getPosition(uint8_t const id) return -1.0f; mbed::ScopedLock lock(_mtx); - int const position = readWordCmd(id, REG(SmartServoRegister::POSITION_H)); + int position = readWordCmd(id, REG(SmartServoRegister::POSITION_H)); // retry one more time if (position < 0) {