From f109c2fb0db77837a850dd60ab0ab7f3beb1aa31 Mon Sep 17 00:00:00 2001 From: Alexander Entinger Date: Mon, 22 Aug 2022 08:15:15 +0200 Subject: [PATCH 1/4] Bugfix: No need to increment the ID when setting the max torque, this is a leftover error from earlier code. --- 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 397e960..dcd656a 100644 --- a/src/lib/motors/SmartServo.cpp +++ b/src/lib/motors/SmartServo.cpp @@ -283,7 +283,7 @@ void SmartServoClass::setMaxTorque(uint16_t const torque) void SmartServoClass::setMaxTorque(uint8_t const id, uint16_t const torque) { mbed::ScopedLock lock(_mtx); - writeWordCmd(id+1, REG(SmartServoRegister::MAX_TORQUE_H), torque); + writeWordCmd(id, REG(SmartServoRegister::MAX_TORQUE_H), torque); } void SmartServoClass::setID(uint8_t const id) From f9969af39a79a1dd21a7aab28eb1542e9eb1f7b8 Mon Sep 17 00:00:00 2001 From: Alexander Entinger Date: Mon, 22 Aug 2022 08:30:00 +0200 Subject: [PATCH 2/4] Provide API to set the max allowed torque on a per-servo basis. --- src/Braccio++.cpp | 8 ++++++++ src/Braccio++.h | 3 +++ src/lib/motors/SmartServo.h | 3 +++ 3 files changed, 14 insertions(+) diff --git a/src/Braccio++.cpp b/src/Braccio++.cpp index a0ddbfc..1091dfb 100644 --- a/src/Braccio++.cpp +++ b/src/Braccio++.cpp @@ -215,6 +215,14 @@ void BraccioClass::positions(float & a1, float & a2, float & a3, float & a4, flo a6 = _servos.getPosition(6); } +void BraccioClass::setMaxTorque(int const id, uint16_t const max_torque) +{ + if (max_torque > SmartServoClass::TORQUE_MAX) + _servos.setMaxTorque(SmartServoClass::TORQUE_MAX); + else + _servos.setMaxTorque(max_torque); +} + int BraccioClass::getKey() { if (isJoystickPressed_LEFT()) { return 1; diff --git a/src/Braccio++.h b/src/Braccio++.h index 5ba6db9..37620aa 100644 --- a/src/Braccio++.h +++ b/src/Braccio++.h @@ -80,6 +80,9 @@ class BraccioClass void positions(float * buffer); void positions(float & a1, float & a2, float & a3, float & a4, float & a5, float & a6); + void setMaxTorque(int const id, uint16_t const max_torque); + inline void setMaxTorque(uint16_t const max_torque) { setMaxTorque(SmartServoClass::BROADCAST, max_torque); } + inline void speed(speed_grade_t const speed_grade) { _servos.setTime(speed_grade); } inline void speed(int const id, speed_grade_t const speed_grade) { _servos.setTime(id, speed_grade); } diff --git a/src/lib/motors/SmartServo.h b/src/lib/motors/SmartServo.h index 46d6927..266ecd8 100644 --- a/src/lib/motors/SmartServo.h +++ b/src/lib/motors/SmartServo.h @@ -98,6 +98,9 @@ class SmartServoClass static int constexpr NUM_MOTORS = 6; static float constexpr MAX_ANGLE = 315.0f; + static uint16_t constexpr TORQUE_MIN = 0; + static uint16_t constexpr TORQUE_MAX = 1000; + static size_t idToArrayIndex(size_t const id) { return (id - 1); } private: From 6188939cd00f2639622816f7f5afcbdbc0b73473 Mon Sep 17 00:00:00 2001 From: Alexander Entinger Date: Mon, 22 Aug 2022 08:32:22 +0200 Subject: [PATCH 3/4] Also make the setMaxTorque API available from the Servo abstraction. --- src/Braccio++.h | 1 + 1 file changed, 1 insertion(+) diff --git a/src/Braccio++.h b/src/Braccio++.h index 37620aa..f288270 100644 --- a/src/Braccio++.h +++ b/src/Braccio++.h @@ -187,6 +187,7 @@ class Servo inline void disengage() { _servos.disengage(_id); } inline void engage() { _servos.engage(_id); } inline bool engaged() { return _servos.isEngaged(_id); } + inline void setMaxTorque(uint16_t const max_torque) { _servos.setMaxTorque(_id, max_torque); } inline Servo & move() { return *this; } inline Servo & to (float const angle) { _servos.setPosition(_id, angle); return *this; } From 135893f164a018ba4ff780835b47ff29093f198c Mon Sep 17 00:00:00 2001 From: Alexander Entinger Date: Mon, 22 Aug 2022 08:39:42 +0200 Subject: [PATCH 4/4] Move check of torque limits inside of SmartServoClass. --- src/Braccio++.cpp | 8 -------- src/Braccio++.h | 4 ++-- src/lib/motors/SmartServo.cpp | 12 +++++------- src/lib/motors/SmartServo.h | 5 ++--- 4 files changed, 9 insertions(+), 20 deletions(-) diff --git a/src/Braccio++.cpp b/src/Braccio++.cpp index 1091dfb..a0ddbfc 100644 --- a/src/Braccio++.cpp +++ b/src/Braccio++.cpp @@ -215,14 +215,6 @@ void BraccioClass::positions(float & a1, float & a2, float & a3, float & a4, flo a6 = _servos.getPosition(6); } -void BraccioClass::setMaxTorque(int const id, uint16_t const max_torque) -{ - if (max_torque > SmartServoClass::TORQUE_MAX) - _servos.setMaxTorque(SmartServoClass::TORQUE_MAX); - else - _servos.setMaxTorque(max_torque); -} - int BraccioClass::getKey() { if (isJoystickPressed_LEFT()) { return 1; diff --git a/src/Braccio++.h b/src/Braccio++.h index f288270..ad0b014 100644 --- a/src/Braccio++.h +++ b/src/Braccio++.h @@ -80,8 +80,8 @@ class BraccioClass void positions(float * buffer); void positions(float & a1, float & a2, float & a3, float & a4, float & a5, float & a6); - void setMaxTorque(int const id, uint16_t const max_torque); - inline void setMaxTorque(uint16_t const max_torque) { setMaxTorque(SmartServoClass::BROADCAST, max_torque); } + inline void setMaxTorque(uint16_t const max_torque) { _servos.setMaxTorque(max_torque); } + inline void setMaxTorque(int const id, uint16_t const max_torque) { _servos.setMaxTorque(id, max_torque); } inline void speed(speed_grade_t const speed_grade) { _servos.setTime(speed_grade); } inline void speed(int const id, speed_grade_t const speed_grade) { _servos.setTime(id, speed_grade); } diff --git a/src/lib/motors/SmartServo.cpp b/src/lib/motors/SmartServo.cpp index dcd656a..8f8e1f7 100644 --- a/src/lib/motors/SmartServo.cpp +++ b/src/lib/motors/SmartServo.cpp @@ -274,16 +274,14 @@ void SmartServoClass::setTime(uint8_t const id, uint16_t const time) writeWordCmd(id, REG(SmartServoRegister::RUN_TIME_H), time); } -void SmartServoClass::setMaxTorque(uint16_t const torque) +void SmartServoClass::setMaxTorque(uint8_t const id, uint16_t const max_torque) { mbed::ScopedLock lock(_mtx); - writeWordCmd(BROADCAST, REG(SmartServoRegister::MAX_TORQUE_H), torque); -} -void SmartServoClass::setMaxTorque(uint8_t const id, uint16_t const torque) -{ - mbed::ScopedLock lock(_mtx); - writeWordCmd(id, REG(SmartServoRegister::MAX_TORQUE_H), torque); + if (max_torque > TORQUE_MAX) + writeWordCmd(id, REG(SmartServoRegister::MAX_TORQUE_H), TORQUE_MAX); + else + writeWordCmd(id, REG(SmartServoRegister::MAX_TORQUE_H), max_torque); } void SmartServoClass::setID(uint8_t const id) diff --git a/src/lib/motors/SmartServo.h b/src/lib/motors/SmartServo.h index 266ecd8..43d38bb 100644 --- a/src/lib/motors/SmartServo.h +++ b/src/lib/motors/SmartServo.h @@ -56,9 +56,8 @@ class SmartServoClass void setTorque(uint8_t const id, bool const torque); - void setMaxTorque(uint16_t const torque); - - void setMaxTorque(uint8_t const id, uint16_t const torque); + void setMaxTorque(uint8_t const id, uint16_t const max_torque); + inline void setMaxTorque(uint16_t const max_torque) { setMaxTorque(BROADCAST, max_torque); } void setID(uint8_t const id);