From e6677c983308fb66d93139cba7dc0bf2f7e7c8b3 Mon Sep 17 00:00:00 2001 From: Alexander Entinger Date: Wed, 19 Jan 2022 13:21:45 +0100 Subject: [PATCH 1/4] Shorten private member 'mutex' to 'mtx' and prefix with '_'. --- src/lib/motors/SmartServo.cpp | 95 ++++++++++++++++++----------------- src/lib/motors/SmartServo.h | 3 +- 2 files changed, 49 insertions(+), 49 deletions(-) diff --git a/src/lib/motors/SmartServo.cpp b/src/lib/motors/SmartServo.cpp index 76dd225..3b99add 100644 --- a/src/lib/motors/SmartServo.cpp +++ b/src/lib/motors/SmartServo.cpp @@ -12,6 +12,7 @@ SmartServoClass::SmartServoClass(RS485Class & RS485) : _RS485{RS485} , _errors{0} , _onError{} +, _mtx{} { } @@ -130,7 +131,7 @@ int SmartServoClass::readByteCmd(uint8_t const id, uint8_t const address) { **************************************************************************************/ int SmartServoClass::ping(uint8_t const id) { - mutex.lock(); + _mtx.lock(); writeCmd(id, SmartServoOperation::PING); // TODO: check return receiveResponse(6); @@ -140,10 +141,10 @@ int SmartServoClass::ping(uint8_t const id) { _rxBuf[2]==id && _rxBuf[3]==2) { - mutex.unlock(); + _mtx.unlock(); return _rxBuf[4]; } - mutex.unlock(); + _mtx.unlock(); _errors++; if (_onError) _onError(); return -1; @@ -153,16 +154,16 @@ int SmartServoClass::ping(uint8_t const id) { // ATTENTION: RESET also changes the ID of the motor void SmartServoClass::reset(uint8_t const id) { - mutex.lock(); + _mtx.lock(); writeCmd(id, SmartServoOperation::RESET); - mutex.unlock(); + _mtx.unlock(); } */ void SmartServoClass::action(uint8_t const id) { - mutex.lock(); + _mtx.lock(); writeCmd(id, SmartServoOperation::ACTION); - mutex.unlock(); + _mtx.unlock(); } int SmartServoClass::begin() { @@ -185,7 +186,7 @@ void SmartServoClass::setPosition(uint8_t const id, float const angle, uint16_t if (!isValidAngle(angle)) return; - mutex.lock(); + _mtx.lock(); if (isValidId(id)) { _targetPosition[id-1] = angleToPosition(angle); @@ -194,26 +195,26 @@ void SmartServoClass::setPosition(uint8_t const id, float const angle, uint16_t writeWordCmd(id, REG(SmartServoRegister::TARGET_POSITION_H), angleToPosition(angle)); } } - mutex.unlock(); + _mtx.unlock(); } float SmartServoClass::getPosition(uint8_t const id) { - mutex.lock(); + _mtx.lock(); float ret = -1; if (isValidId(id)) ret = positionToAngle(readWordCmd(id, REG(SmartServoRegister::POSITION_H))); - mutex.unlock(); + _mtx.unlock(); return ret; } void SmartServoClass::center(uint8_t const id, uint16_t const position) { - mutex.lock(); + _mtx.lock(); writeWordCmd(id, REG(SmartServoRegister::CENTER_POINT_ADJ_H), position); - mutex.unlock(); + _mtx.unlock(); } void SmartServoClass::synchronize() { - mutex.lock(); + _mtx.lock(); _txPacket.id = 0xFE; _txPacket.length = MAX_TX_PAYLOAD_LEN; _txPacket.instruction = CMD(SmartServoOperation::SYNC_WRITE); @@ -229,109 +230,109 @@ void SmartServoClass::synchronize() { _txPacket.payload[index++] = _targetSpeed[idToArrayIndex(i)]; } sendPacket(); - mutex.unlock(); + _mtx.unlock(); } void SmartServoClass::setTorque(bool const torque) { - mutex.lock(); + _mtx.lock(); writeByteCmd(BROADCAST, REG(SmartServoRegister::TORQUE_SWITCH), torque ? 1 : 0); - mutex.unlock(); + _mtx.unlock(); } void SmartServoClass::setTorque(uint8_t const id, bool const torque) { - mutex.lock(); + _mtx.lock(); writeByteCmd(id, REG(SmartServoRegister::TORQUE_SWITCH), torque ? 1 : 0); - mutex.unlock(); + _mtx.unlock(); } void SmartServoClass::setTime(uint8_t const id, uint16_t const time) { - mutex.lock(); + _mtx.lock(); writeWordCmd(id, REG(SmartServoRegister::RUN_TIME_H), time); - mutex.unlock(); + _mtx.unlock(); } void SmartServoClass::setMaxTorque(uint16_t const torque) { - mutex.lock(); + _mtx.lock(); writeWordCmd(BROADCAST, REG(SmartServoRegister::MAX_TORQUE_H), torque); - mutex.unlock(); + _mtx.unlock(); } void SmartServoClass::setMaxTorque(uint8_t const id, uint16_t const torque) { - mutex.lock(); + _mtx.lock(); writeWordCmd(id+1, REG(SmartServoRegister::MAX_TORQUE_H), torque); - mutex.unlock(); + _mtx.unlock(); } void SmartServoClass::setID(uint8_t const id) { - mutex.lock(); + _mtx.lock(); writeByteCmd(BROADCAST, REG(SmartServoRegister::ID), id); - mutex.unlock(); + _mtx.unlock(); } void SmartServoClass::engage(uint8_t const id) { - mutex.lock(); + _mtx.lock(); writeByteCmd(id, REG(SmartServoRegister::TORQUE_SWITCH), 0x1); - mutex.unlock(); + _mtx.unlock(); } void SmartServoClass::disengage(uint8_t const id) { - mutex.lock(); + _mtx.lock(); writeByteCmd(id, REG(SmartServoRegister::TORQUE_SWITCH), 0); - mutex.unlock(); + _mtx.unlock(); } bool SmartServoClass::isEngaged(uint8_t const id) { - mutex.lock(); + _mtx.lock(); int ret = readByteCmd(id, REG(SmartServoRegister::TORQUE_SWITCH)); - mutex.unlock(); + _mtx.unlock(); return ret != 0; } void SmartServoClass::setStallProtectionTime(uint8_t const time) { - mutex.lock(); + _mtx.lock(); writeByteCmd(BROADCAST, REG(SmartServoRegister::STALL_PROTECTION_TIME), time); - mutex.unlock(); + _mtx.unlock(); } void SmartServoClass::setStallProtectionTime(uint8_t const id, uint8_t const time) { - mutex.lock(); + _mtx.lock(); writeByteCmd(id, REG(SmartServoRegister::STALL_PROTECTION_TIME), time); - mutex.unlock(); + _mtx.unlock(); } void SmartServoClass::setMinAngle(uint16_t const min_angle) { - mutex.lock(); + _mtx.lock(); writeWordCmd(BROADCAST, REG(SmartServoRegister::MIN_ANGLE_LIMIT_H), min_angle); - mutex.unlock(); + _mtx.unlock(); } void SmartServoClass::setMinAngle(uint8_t const id, uint16_t const min_angle) { - mutex.lock(); + _mtx.lock(); writeWordCmd(id, REG(SmartServoRegister::MIN_ANGLE_LIMIT_H), min_angle); - mutex.unlock(); + _mtx.unlock(); } void SmartServoClass::setMaxAngle(uint16_t const max_angle) { - mutex.lock(); + _mtx.lock(); writeWordCmd(BROADCAST, REG(SmartServoRegister::MAX_ANGLE_LIMIT_H), max_angle); - mutex.unlock(); + _mtx.unlock(); } void SmartServoClass::setMaxAngle(uint8_t const id, uint16_t const max_angle) { - mutex.lock(); + _mtx.lock(); writeWordCmd(id, REG(SmartServoRegister::MAX_ANGLE_LIMIT_H), max_angle); - mutex.unlock(); + _mtx.unlock(); } void SmartServoClass::getInfo(Stream & stream, uint8_t const id) { uint8_t regs[65]; memset(regs, 0x55, sizeof(regs)); int i = 0; - mutex.lock(); + _mtx.lock(); while (i < sizeof(regs)) { if ((i > 29 && i < 40) || (i > 48 && i < 56)) { i++; @@ -339,7 +340,7 @@ void SmartServoClass::getInfo(Stream & stream, uint8_t const id) { } regs[i++] = readByteCmd(id, i); } - mutex.unlock(); + _mtx.unlock(); stream.println("regs map:"); for (i = 0; i < sizeof(regs); i++) { stream.println(String(i, HEX) + " : " + String(regs[i], HEX)); diff --git a/src/lib/motors/SmartServo.h b/src/lib/motors/SmartServo.h index 8dbe81c..166262b 100644 --- a/src/lib/motors/SmartServo.h +++ b/src/lib/motors/SmartServo.h @@ -106,6 +106,7 @@ class SmartServoClass RS485Class& _RS485; int _errors; mbed::Callback _onError; + rtos::Mutex _mtx; struct __attribute__((packed)) { @@ -121,8 +122,6 @@ class SmartServoClass uint16_t _targetPosition[NUM_MOTORS]; uint16_t _targetSpeed[NUM_MOTORS]; PositionMode _positionMode; - - rtos::Mutex mutex; }; #endif // _SMARTMOTOR_H_ From 8309a1634435a3326fe837e7ec541f4a19be4604 Mon Sep 17 00:00:00 2001 From: Alexander Entinger Date: Wed, 19 Jan 2022 13:27:01 +0100 Subject: [PATCH 2/4] Never forget to unlock() by using a ScopedLock ;). --- src/lib/motors/SmartServo.cpp | 145 +++++++++++++++++----------------- 1 file changed, 71 insertions(+), 74 deletions(-) diff --git a/src/lib/motors/SmartServo.cpp b/src/lib/motors/SmartServo.cpp index 3b99add..23fed74 100644 --- a/src/lib/motors/SmartServo.cpp +++ b/src/lib/motors/SmartServo.cpp @@ -130,8 +130,9 @@ int SmartServoClass::readByteCmd(uint8_t const id, uint8_t const address) { * PUBLIC MEMBER FUNCTIONS **************************************************************************************/ -int SmartServoClass::ping(uint8_t const id) { - _mtx.lock(); +int SmartServoClass::ping(uint8_t const id) +{ + mbed::ScopedLock lock(_mtx); writeCmd(id, SmartServoOperation::PING); // TODO: check return receiveResponse(6); @@ -140,11 +141,8 @@ int SmartServoClass::ping(uint8_t const id) { _rxBuf[1]==0xf5 && _rxBuf[2]==id && _rxBuf[3]==2) { - - _mtx.unlock(); return _rxBuf[4]; } - _mtx.unlock(); _errors++; if (_onError) _onError(); return -1; @@ -153,20 +151,21 @@ int SmartServoClass::ping(uint8_t const id) { /* // ATTENTION: RESET also changes the ID of the motor -void SmartServoClass::reset(uint8_t const id) { - _mtx.lock(); +void SmartServoClass::reset(uint8_t const id) +{ + mbed::ScopedLock lock(_mtx); writeCmd(id, SmartServoOperation::RESET); - _mtx.unlock(); } */ -void SmartServoClass::action(uint8_t const id) { - _mtx.lock(); +void SmartServoClass::action(uint8_t const id) +{ + mbed::ScopedLock lock(_mtx); writeCmd(id, SmartServoOperation::ACTION); - _mtx.unlock(); } -int SmartServoClass::begin() { +int SmartServoClass::begin() +{ if (_RS485) { _txPacket.header[0] = 0xff; _txPacket.header[1] = 0xff; @@ -186,7 +185,7 @@ void SmartServoClass::setPosition(uint8_t const id, float const angle, uint16_t if (!isValidAngle(angle)) return; - _mtx.lock(); + mbed::ScopedLock lock(_mtx); if (isValidId(id)) { _targetPosition[id-1] = angleToPosition(angle); @@ -195,26 +194,25 @@ void SmartServoClass::setPosition(uint8_t const id, float const angle, uint16_t writeWordCmd(id, REG(SmartServoRegister::TARGET_POSITION_H), angleToPosition(angle)); } } - _mtx.unlock(); } -float SmartServoClass::getPosition(uint8_t const id) { - _mtx.lock(); +float SmartServoClass::getPosition(uint8_t const id) +{ + mbed::ScopedLock lock(_mtx); float ret = -1; if (isValidId(id)) - ret = positionToAngle(readWordCmd(id, REG(SmartServoRegister::POSITION_H))); - _mtx.unlock(); - return ret; + return positionToAngle(readWordCmd(id, REG(SmartServoRegister::POSITION_H))); } -void SmartServoClass::center(uint8_t const id, uint16_t const position) { - _mtx.lock(); +void SmartServoClass::center(uint8_t const id, uint16_t const position) +{ + mbed::ScopedLock lock(_mtx); writeWordCmd(id, REG(SmartServoRegister::CENTER_POINT_ADJ_H), position); - _mtx.unlock(); } -void SmartServoClass::synchronize() { - _mtx.lock(); +void SmartServoClass::synchronize() +{ + mbed::ScopedLock lock(_mtx); _txPacket.id = 0xFE; _txPacket.length = MAX_TX_PAYLOAD_LEN; _txPacket.instruction = CMD(SmartServoOperation::SYNC_WRITE); @@ -230,117 +228,116 @@ void SmartServoClass::synchronize() { _txPacket.payload[index++] = _targetSpeed[idToArrayIndex(i)]; } sendPacket(); - _mtx.unlock(); } -void SmartServoClass::setTorque(bool const torque) { - _mtx.lock(); +void SmartServoClass::setTorque(bool const torque) +{ + mbed::ScopedLock lock(_mtx); writeByteCmd(BROADCAST, REG(SmartServoRegister::TORQUE_SWITCH), torque ? 1 : 0); - _mtx.unlock(); } -void SmartServoClass::setTorque(uint8_t const id, bool const torque) { - _mtx.lock(); +void SmartServoClass::setTorque(uint8_t const id, bool const torque) +{ + mbed::ScopedLock lock(_mtx); writeByteCmd(id, REG(SmartServoRegister::TORQUE_SWITCH), torque ? 1 : 0); - _mtx.unlock(); } -void SmartServoClass::setTime(uint8_t const id, uint16_t const time) { - _mtx.lock(); +void SmartServoClass::setTime(uint8_t const id, uint16_t const time) +{ + mbed::ScopedLock lock(_mtx); writeWordCmd(id, REG(SmartServoRegister::RUN_TIME_H), time); - _mtx.unlock(); } -void SmartServoClass::setMaxTorque(uint16_t const torque) { - _mtx.lock(); +void SmartServoClass::setMaxTorque(uint16_t const torque) +{ + mbed::ScopedLock lock(_mtx); writeWordCmd(BROADCAST, REG(SmartServoRegister::MAX_TORQUE_H), torque); - _mtx.unlock(); } -void SmartServoClass::setMaxTorque(uint8_t const id, uint16_t const torque) { - _mtx.lock(); +void SmartServoClass::setMaxTorque(uint8_t const id, uint16_t const torque) +{ + mbed::ScopedLock lock(_mtx); writeWordCmd(id+1, REG(SmartServoRegister::MAX_TORQUE_H), torque); - _mtx.unlock(); } -void SmartServoClass::setID(uint8_t const id) { - _mtx.lock(); +void SmartServoClass::setID(uint8_t const id) +{ + mbed::ScopedLock lock(_mtx); writeByteCmd(BROADCAST, REG(SmartServoRegister::ID), id); - _mtx.unlock(); } -void SmartServoClass::engage(uint8_t const id) { - _mtx.lock(); +void SmartServoClass::engage(uint8_t const id) +{ + mbed::ScopedLock lock(_mtx); writeByteCmd(id, REG(SmartServoRegister::TORQUE_SWITCH), 0x1); - _mtx.unlock(); } -void SmartServoClass::disengage(uint8_t const id) { - _mtx.lock(); +void SmartServoClass::disengage(uint8_t const id) +{ + mbed::ScopedLock lock(_mtx); writeByteCmd(id, REG(SmartServoRegister::TORQUE_SWITCH), 0); - _mtx.unlock(); } -bool SmartServoClass::isEngaged(uint8_t const id) { - _mtx.lock(); +bool SmartServoClass::isEngaged(uint8_t const id) +{ + mbed::ScopedLock lock(_mtx); int ret = readByteCmd(id, REG(SmartServoRegister::TORQUE_SWITCH)); - _mtx.unlock(); return ret != 0; } -void SmartServoClass::setStallProtectionTime(uint8_t const time) { - _mtx.lock(); +void SmartServoClass::setStallProtectionTime(uint8_t const time) +{ + mbed::ScopedLock lock(_mtx); writeByteCmd(BROADCAST, REG(SmartServoRegister::STALL_PROTECTION_TIME), time); - _mtx.unlock(); } -void SmartServoClass::setStallProtectionTime(uint8_t const id, uint8_t const time) { - _mtx.lock(); +void SmartServoClass::setStallProtectionTime(uint8_t const id, uint8_t const time) +{ + mbed::ScopedLock lock(_mtx); writeByteCmd(id, REG(SmartServoRegister::STALL_PROTECTION_TIME), time); - _mtx.unlock(); } void SmartServoClass::setMinAngle(uint16_t const min_angle) { - _mtx.lock(); + mbed::ScopedLock lock(_mtx); writeWordCmd(BROADCAST, REG(SmartServoRegister::MIN_ANGLE_LIMIT_H), min_angle); - _mtx.unlock(); } void SmartServoClass::setMinAngle(uint8_t const id, uint16_t const min_angle) { - _mtx.lock(); + mbed::ScopedLock lock(_mtx); writeWordCmd(id, REG(SmartServoRegister::MIN_ANGLE_LIMIT_H), min_angle); - _mtx.unlock(); } void SmartServoClass::setMaxAngle(uint16_t const max_angle) { - _mtx.lock(); + mbed::ScopedLock lock(_mtx); writeWordCmd(BROADCAST, REG(SmartServoRegister::MAX_ANGLE_LIMIT_H), max_angle); - _mtx.unlock(); } void SmartServoClass::setMaxAngle(uint8_t const id, uint16_t const max_angle) { - _mtx.lock(); + mbed::ScopedLock lock(_mtx); writeWordCmd(id, REG(SmartServoRegister::MAX_ANGLE_LIMIT_H), max_angle); - _mtx.unlock(); } -void SmartServoClass::getInfo(Stream & stream, uint8_t const id) { +void SmartServoClass::getInfo(Stream & stream, uint8_t const id) +{ uint8_t regs[65]; memset(regs, 0x55, sizeof(regs)); int i = 0; - _mtx.lock(); - while (i < sizeof(regs)) { - if ((i > 29 && i < 40) || (i > 48 && i < 56)) { - i++; - continue; + + { + mbed::ScopedLock lock(_mtx); + while (i < sizeof(regs)) { + if ((i > 29 && i < 40) || (i > 48 && i < 56)) { + i++; + continue; + } + regs[i++] = readByteCmd(id, i); } - regs[i++] = readByteCmd(id, i); } - _mtx.unlock(); + stream.println("regs map:"); for (i = 0; i < sizeof(regs); i++) { stream.println(String(i, HEX) + " : " + String(regs[i], HEX)); From d85eaadc6e950395736f8478102834c6dff0c697 Mon Sep 17 00:00:00 2001 From: Alexander Entinger Date: Wed, 19 Jan 2022 13:31:22 +0100 Subject: [PATCH 3/4] Fix: Broadcast is also valid id. --- src/lib/motors/SmartServo.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/lib/motors/SmartServo.h b/src/lib/motors/SmartServo.h index 166262b..1c202dd 100644 --- a/src/lib/motors/SmartServo.h +++ b/src/lib/motors/SmartServo.h @@ -86,7 +86,7 @@ class SmartServoClass 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 bool isValidId(int const id) const { return ((id >= MIN_MOTOR_ID) && (id <= MAX_MOTOR_ID)) || (id == BROADCAST); } int calcChecksum (); void sendPacket (); From c53ffa9419105f2d3a91885477985485d20959cc Mon Sep 17 00:00:00 2001 From: Alexander Entinger Date: Thu, 20 Jan 2022 06:35:29 +0100 Subject: [PATCH 4/4] RS485 is passed by reference, can not be zero (compile time check). --- src/lib/motors/SmartServo.cpp | 25 ++++++++++--------------- src/lib/motors/SmartServo.h | 2 +- 2 files changed, 11 insertions(+), 16 deletions(-) diff --git a/src/lib/motors/SmartServo.cpp b/src/lib/motors/SmartServo.cpp index 23fed74..d6c20ab 100644 --- a/src/lib/motors/SmartServo.cpp +++ b/src/lib/motors/SmartServo.cpp @@ -164,20 +164,15 @@ void SmartServoClass::action(uint8_t const id) writeCmd(id, SmartServoOperation::ACTION); } -int SmartServoClass::begin() +void SmartServoClass::begin() { - if (_RS485) { - _txPacket.header[0] = 0xff; - _txPacket.header[1] = 0xff; - _RS485.begin(115200, 0, 90); - _RS485.receive(); - writeByteCmd(BROADCAST, REG(SmartServoRegister::SERVO_MOTOR_MODE), 1); - writeByteCmd(BROADCAST, REG(SmartServoRegister::TORQUE_SWITCH) ,1); - _positionMode = PositionMode::IMMEDIATE; - return 0; - } else { - return -1; - } + _txPacket.header[0] = 0xff; + _txPacket.header[1] = 0xff; + _RS485.begin(115200, 0, 90); + _RS485.receive(); + writeByteCmd(BROADCAST, REG(SmartServoRegister::SERVO_MOTOR_MODE), 1); + writeByteCmd(BROADCAST, REG(SmartServoRegister::TORQUE_SWITCH) ,1); + _positionMode = PositionMode::IMMEDIATE; } void SmartServoClass::setPosition(uint8_t const id, float const angle, uint16_t const speed) @@ -188,8 +183,8 @@ void SmartServoClass::setPosition(uint8_t const id, float const angle, uint16_t mbed::ScopedLock lock(_mtx); if (isValidId(id)) { - _targetPosition[id-1] = angleToPosition(angle); - _targetSpeed[id-1] = speed; + _targetPosition[idToArrayIndex(id)] = angleToPosition(angle); + _targetSpeed[idToArrayIndex(id)] = speed; if (_positionMode==PositionMode::IMMEDIATE) { writeWordCmd(id, REG(SmartServoRegister::TARGET_POSITION_H), angleToPosition(angle)); } diff --git a/src/lib/motors/SmartServo.h b/src/lib/motors/SmartServo.h index 1c202dd..25cd347 100644 --- a/src/lib/motors/SmartServo.h +++ b/src/lib/motors/SmartServo.h @@ -21,7 +21,7 @@ class SmartServoClass SmartServoClass(RS485Class & RS485); - int begin(); + void begin(); void end(); inline void setPositionMode(PositionMode const mode) { _positionMode = mode; }