Skip to content

Use scoped lock for simple & error-free locking within SmartServoClass #30

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
merged 4 commits into from
Jan 20, 2022
Merged

Use scoped lock for simple & error-free locking within SmartServoClass #30

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
e6677c9
Shorten private member 'mutex' to 'mtx' and prefix with '_'.
aentinger Jan 19, 2022
8309a16
Never forget to unlock() by using a ScopedLock ;).
aentinger Jan 19, 2022
d85eaad
Fix: Broadcast is also valid id.
aentinger Jan 19, 2022
c53ffa9
RS485 is passed by reference, can not be zero (compile time check).
aentinger Jan 20, 2022
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
173 changes: 83 additions & 90 deletions src/lib/motors/SmartServo.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -12,6 +12,7 @@ SmartServoClass::SmartServoClass(RS485Class & RS485)
: _RS485{RS485}
, _errors{0}
, _onError{}
, _mtx{}
{

}
Expand Down Expand Up @@ -129,8 +130,9 @@ int SmartServoClass::readByteCmd(uint8_t const id, uint8_t const address) {
* PUBLIC MEMBER FUNCTIONS
**************************************************************************************/

int SmartServoClass::ping(uint8_t const id) {
mutex.lock();
int SmartServoClass::ping(uint8_t const id)
{
mbed::ScopedLock<rtos::Mutex> lock(_mtx);
writeCmd(id, SmartServoOperation::PING);
// TODO: check return
receiveResponse(6);
Expand All @@ -139,11 +141,8 @@ int SmartServoClass::ping(uint8_t const id) {
_rxBuf[1]==0xf5 &&
_rxBuf[2]==id &&
_rxBuf[3]==2) {

mutex.unlock();
return _rxBuf[4];
}
mutex.unlock();
_errors++;
if (_onError) _onError();
return -1;
Expand All @@ -152,68 +151,63 @@ 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();
void SmartServoClass::reset(uint8_t const id)
{
mbed::ScopedLock<rtos::Mutex> lock(_mtx);
writeCmd(id, SmartServoOperation::RESET);
mutex.unlock();
}
*/

void SmartServoClass::action(uint8_t const id) {
mutex.lock();
void SmartServoClass::action(uint8_t const id)
{
mbed::ScopedLock<rtos::Mutex> lock(_mtx);
writeCmd(id, SmartServoOperation::ACTION);
mutex.unlock();
}

int 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;
}
}

void SmartServoClass::begin()
{
_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)
{
if (!isValidAngle(angle))
return;

mutex.lock();
mbed::ScopedLock<rtos::Mutex> 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));
}
}
mutex.unlock();
}

float SmartServoClass::getPosition(uint8_t const id) {
mutex.lock();
float SmartServoClass::getPosition(uint8_t const id)
{
mbed::ScopedLock<rtos::Mutex> lock(_mtx);
float ret = -1;
if (isValidId(id))
ret = positionToAngle(readWordCmd(id, REG(SmartServoRegister::POSITION_H)));
mutex.unlock();
return ret;
return positionToAngle(readWordCmd(id, REG(SmartServoRegister::POSITION_H)));
}

void SmartServoClass::center(uint8_t const id, uint16_t const position) {
mutex.lock();
void SmartServoClass::center(uint8_t const id, uint16_t const position)
{
mbed::ScopedLock<rtos::Mutex> lock(_mtx);
writeWordCmd(id, REG(SmartServoRegister::CENTER_POINT_ADJ_H), position);
mutex.unlock();
}

void SmartServoClass::synchronize() {
mutex.lock();
void SmartServoClass::synchronize()
{
mbed::ScopedLock<rtos::Mutex> lock(_mtx);
_txPacket.id = 0xFE;
_txPacket.length = MAX_TX_PAYLOAD_LEN;
_txPacket.instruction = CMD(SmartServoOperation::SYNC_WRITE);
Expand All @@ -229,117 +223,116 @@ void SmartServoClass::synchronize() {
_txPacket.payload[index++] = _targetSpeed[idToArrayIndex(i)];
}
sendPacket();
mutex.unlock();
}

void SmartServoClass::setTorque(bool const torque) {
mutex.lock();
void SmartServoClass::setTorque(bool const torque)
{
mbed::ScopedLock<rtos::Mutex> lock(_mtx);
writeByteCmd(BROADCAST, REG(SmartServoRegister::TORQUE_SWITCH), torque ? 1 : 0);
mutex.unlock();
}

void SmartServoClass::setTorque(uint8_t const id, bool const torque) {
mutex.lock();
void SmartServoClass::setTorque(uint8_t const id, bool const torque)
{
mbed::ScopedLock<rtos::Mutex> lock(_mtx);
writeByteCmd(id, REG(SmartServoRegister::TORQUE_SWITCH), torque ? 1 : 0);
mutex.unlock();
}

void SmartServoClass::setTime(uint8_t const id, uint16_t const time) {
mutex.lock();
void SmartServoClass::setTime(uint8_t const id, uint16_t const time)
{
mbed::ScopedLock<rtos::Mutex> lock(_mtx);
writeWordCmd(id, REG(SmartServoRegister::RUN_TIME_H), time);
mutex.unlock();
}

void SmartServoClass::setMaxTorque(uint16_t const torque) {
mutex.lock();
void SmartServoClass::setMaxTorque(uint16_t const torque)
{
mbed::ScopedLock<rtos::Mutex> lock(_mtx);
writeWordCmd(BROADCAST, REG(SmartServoRegister::MAX_TORQUE_H), torque);
mutex.unlock();
}

void SmartServoClass::setMaxTorque(uint8_t const id, uint16_t const torque) {
mutex.lock();
void SmartServoClass::setMaxTorque(uint8_t const id, uint16_t const torque)
{
mbed::ScopedLock<rtos::Mutex> lock(_mtx);
writeWordCmd(id+1, REG(SmartServoRegister::MAX_TORQUE_H), torque);
mutex.unlock();
}

void SmartServoClass::setID(uint8_t const id) {
mutex.lock();
void SmartServoClass::setID(uint8_t const id)
{
mbed::ScopedLock<rtos::Mutex> lock(_mtx);
writeByteCmd(BROADCAST, REG(SmartServoRegister::ID), id);
mutex.unlock();
}

void SmartServoClass::engage(uint8_t const id) {
mutex.lock();
void SmartServoClass::engage(uint8_t const id)
{
mbed::ScopedLock<rtos::Mutex> lock(_mtx);
writeByteCmd(id, REG(SmartServoRegister::TORQUE_SWITCH), 0x1);
mutex.unlock();
}

void SmartServoClass::disengage(uint8_t const id) {
mutex.lock();
void SmartServoClass::disengage(uint8_t const id)
{
mbed::ScopedLock<rtos::Mutex> lock(_mtx);
writeByteCmd(id, REG(SmartServoRegister::TORQUE_SWITCH), 0);
mutex.unlock();
}

bool SmartServoClass::isEngaged(uint8_t const id) {
mutex.lock();
bool SmartServoClass::isEngaged(uint8_t const id)
{
mbed::ScopedLock<rtos::Mutex> lock(_mtx);
int ret = readByteCmd(id, REG(SmartServoRegister::TORQUE_SWITCH));
mutex.unlock();
return ret != 0;
}

void SmartServoClass::setStallProtectionTime(uint8_t const time) {
mutex.lock();
void SmartServoClass::setStallProtectionTime(uint8_t const time)
{
mbed::ScopedLock<rtos::Mutex> lock(_mtx);
writeByteCmd(BROADCAST, REG(SmartServoRegister::STALL_PROTECTION_TIME), time);
mutex.unlock();
}

void SmartServoClass::setStallProtectionTime(uint8_t const id, uint8_t const time) {
mutex.lock();
void SmartServoClass::setStallProtectionTime(uint8_t const id, uint8_t const time)
{
mbed::ScopedLock<rtos::Mutex> lock(_mtx);
writeByteCmd(id, REG(SmartServoRegister::STALL_PROTECTION_TIME), time);
mutex.unlock();
}

void SmartServoClass::setMinAngle(uint16_t const min_angle)
{
mutex.lock();
mbed::ScopedLock<rtos::Mutex> lock(_mtx);
writeWordCmd(BROADCAST, REG(SmartServoRegister::MIN_ANGLE_LIMIT_H), min_angle);
mutex.unlock();
}

void SmartServoClass::setMinAngle(uint8_t const id, uint16_t const min_angle)
{
mutex.lock();
mbed::ScopedLock<rtos::Mutex> lock(_mtx);
writeWordCmd(id, REG(SmartServoRegister::MIN_ANGLE_LIMIT_H), min_angle);
mutex.unlock();
}

void SmartServoClass::setMaxAngle(uint16_t const max_angle)
{
mutex.lock();
mbed::ScopedLock<rtos::Mutex> lock(_mtx);
writeWordCmd(BROADCAST, REG(SmartServoRegister::MAX_ANGLE_LIMIT_H), max_angle);
mutex.unlock();
}

void SmartServoClass::setMaxAngle(uint8_t const id, uint16_t const max_angle)
{
mutex.lock();
mbed::ScopedLock<rtos::Mutex> lock(_mtx);
writeWordCmd(id, REG(SmartServoRegister::MAX_ANGLE_LIMIT_H), max_angle);
mutex.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;
mutex.lock();
while (i < sizeof(regs)) {
if ((i > 29 && i < 40) || (i > 48 && i < 56)) {
i++;
continue;

{
mbed::ScopedLock<rtos::Mutex> 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);
}
mutex.unlock();

stream.println("regs map:");
for (i = 0; i < sizeof(regs); i++) {
stream.println(String(i, HEX) + " : " + String(regs[i], HEX));
Expand Down
7 changes: 3 additions & 4 deletions src/lib/motors/SmartServo.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -21,7 +21,7 @@ class SmartServoClass

SmartServoClass(RS485Class & RS485);

int begin();
void begin();
void end();

inline void setPositionMode(PositionMode const mode) { _positionMode = mode; }
Expand Down Expand Up @@ -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 ();
Expand All @@ -106,6 +106,7 @@ class SmartServoClass
RS485Class& _RS485;
int _errors;
mbed::Callback<void()> _onError;
rtos::Mutex _mtx;


struct __attribute__((packed)) {
Expand All @@ -121,8 +122,6 @@ class SmartServoClass
uint16_t _targetPosition[NUM_MOTORS];
uint16_t _targetSpeed[NUM_MOTORS];
PositionMode _positionMode;

rtos::Mutex mutex;
};

#endif // _SMARTMOTOR_H_