From 279a155e671f07c4155413b08575fd1706d724c4 Mon Sep 17 00:00:00 2001 From: Alexander Entinger Date: Wed, 24 Aug 2022 07:47:58 +0200 Subject: [PATCH 01/16] Rewrite setPosition in a better structured way (exit early, lock only where necessary). --- src/lib/motors/SmartServo.cpp | 16 +++++++++------- 1 file changed, 9 insertions(+), 7 deletions(-) diff --git a/src/lib/motors/SmartServo.cpp b/src/lib/motors/SmartServo.cpp index 0e6d311..6c81d96 100644 --- a/src/lib/motors/SmartServo.cpp +++ b/src/lib/motors/SmartServo.cpp @@ -203,14 +203,16 @@ void SmartServoClass::setPosition(uint8_t const id, float const angle) if (!isValidAngle(angle)) return; - mbed::ScopedLock lock(_mtx); - if (isValidId(id)) + if (!isValidId(id)) + return; + + _targetPosition[idToArrayIndex(id)] = angleToPosition(angle); + + if (_positionMode == PositionMode::IMMEDIATE) { - _targetPosition[idToArrayIndex(id)] = angleToPosition(angle); - if (_positionMode==PositionMode::IMMEDIATE) { - writeWordCmd(id, REG(SmartServoRegister::TARGET_POSITION_H), angleToPosition(angle)); - } - } + mbed::ScopedLock lock(_mtx); + writeWordCmd(id, REG(SmartServoRegister::TARGET_POSITION_H), angleToPosition(angle)); + } } float SmartServoClass::getPosition(uint8_t const id) From 7bb9bcbab26349a182615679898593ff18896588 Mon Sep 17 00:00:00 2001 From: Alexander Entinger Date: Wed, 24 Aug 2022 07:53:58 +0200 Subject: [PATCH 02/16] Rewrite getPosition in a better structure (exit early, always a valid return code). --- src/lib/motors/SmartServo.cpp | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/src/lib/motors/SmartServo.cpp b/src/lib/motors/SmartServo.cpp index 6c81d96..9258082 100644 --- a/src/lib/motors/SmartServo.cpp +++ b/src/lib/motors/SmartServo.cpp @@ -217,10 +217,11 @@ void SmartServoClass::setPosition(uint8_t const id, float const angle) float SmartServoClass::getPosition(uint8_t const id) { + if (!isValidId(id)) + return -1.0f; + mbed::ScopedLock lock(_mtx); - float ret = -1; - if (isValidId(id)) - return positionToAngle(readWordCmd(id, REG(SmartServoRegister::POSITION_H))); + return positionToAngle(readWordCmd(id, REG(SmartServoRegister::POSITION_H))); } void SmartServoClass::center(uint8_t const id, uint16_t const position) From 6e15737315316c449785042100ee8fb16f6cb935 Mon Sep 17 00:00:00 2001 From: Alexander Entinger Date: Wed, 24 Aug 2022 08:13:52 +0200 Subject: [PATCH 03/16] =?UTF-8?q?Limit=20angular=20velocity=20to=2010?= =?UTF-8?q?=C2=B0/s.?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/lib/motors/SmartServo.cpp | 34 ++++++++++++++++++++++++++++++++-- 1 file changed, 32 insertions(+), 2 deletions(-) diff --git a/src/lib/motors/SmartServo.cpp b/src/lib/motors/SmartServo.cpp index 9258082..3a475bd 100644 --- a/src/lib/motors/SmartServo.cpp +++ b/src/lib/motors/SmartServo.cpp @@ -206,12 +206,42 @@ void SmartServoClass::setPosition(uint8_t const id, float const angle) if (!isValidId(id)) return; - _targetPosition[idToArrayIndex(id)] = angleToPosition(angle); + float const MAX_ANGULAR_VELOCITY_deg_per_sec = 10.0f; + + float const target_position_deg = angleToPosition(angle); + float const actual_position_deg = getPosition(id); + if (actual_position_deg < 0.0f) + { + char msg[64] = {0}; + snprintf(msg, sizeof(msg), "error obtaining position for servo %d", (int)id); + Serial.println(msg); + return; + } + + float const abs_position_diff_deg = fabs(target_position_deg - actual_position_deg); + float const runtime_sec = static_cast(getTime(id)) / 1000.0f; + float const angular_velocity_deg_per_sec = abs_position_diff_deg / runtime_sec; + + /* Check whether or not the maximum allowed angular velocity is exceeded, + * if it is indeed exceeded increase the runtime accordingly. + */ + if (angular_velocity_deg_per_sec > MAX_ANGULAR_VELOCITY_deg_per_sec) + { + float const limited_runtime_sec = abs_position_diff_deg / MAX_ANGULAR_VELOCITY_deg_per_sec; + setTime(id, static_cast(limited_runtime_sec)); + + char msg[64] = {0}; + snprintf(msg, sizeof(msg), "targed = %0.2f, actual = %0.2f, diff = %0.2f, omega_vel = %0.2f", + target_position_deg, actual_position_deg, abs_position_diff_deg, angular_velocity_deg_per_sec); + Serial.println(msg); + } + + _targetPosition[idToArrayIndex(id)] = target_position_deg; if (_positionMode == PositionMode::IMMEDIATE) { mbed::ScopedLock lock(_mtx); - writeWordCmd(id, REG(SmartServoRegister::TARGET_POSITION_H), angleToPosition(angle)); + writeWordCmd(id, REG(SmartServoRegister::TARGET_POSITION_H), target_position_deg); } } From 99242d9d1be0c873e2c494c8ba462ea142fac88e Mon Sep 17 00:00:00 2001 From: Alexander Entinger Date: Wed, 24 Aug 2022 08:16:57 +0200 Subject: [PATCH 04/16] Only lock actual writing to the servos. --- src/lib/motors/SmartServo.cpp | 12 ++++++++++-- 1 file changed, 10 insertions(+), 2 deletions(-) diff --git a/src/lib/motors/SmartServo.cpp b/src/lib/motors/SmartServo.cpp index 3a475bd..6cfe01c 100644 --- a/src/lib/motors/SmartServo.cpp +++ b/src/lib/motors/SmartServo.cpp @@ -294,16 +294,24 @@ void SmartServoClass::setTorque(uint8_t const id, bool const torque) void SmartServoClass::setTime(uint16_t const time) { - mbed::ScopedLock lock(_mtx); for (int i = MIN_MOTOR_ID; i <= MAX_MOTOR_ID; i++) _targetSpeed[idToArrayIndex(i)] = time; + + mbed::ScopedLock lock(_mtx); writeWordCmd(BROADCAST, REG(SmartServoRegister::RUN_TIME_H), time); } void SmartServoClass::setTime(uint8_t const id, uint16_t const time) { + if (!isValidId(id)) + return; + + if (id == BROADCAST) + return; + + _targetSpeed[idToArrayIndex(id)] = time; + mbed::ScopedLock lock(_mtx); - if ((id >= MIN_MOTOR_ID) && (id <= MAX_MOTOR_ID)) _targetSpeed[idToArrayIndex(id)] = time; writeWordCmd(id, REG(SmartServoRegister::RUN_TIME_H), time); } From 7acbf71a049960c36a7e28364637bc5731fec498 Mon Sep 17 00:00:00 2001 From: Alexander Entinger Date: Wed, 24 Aug 2022 08:22:34 +0200 Subject: [PATCH 05/16] Only update position array when we are in synched mode (otherwise this variable is not needed). --- src/lib/motors/SmartServo.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/lib/motors/SmartServo.cpp b/src/lib/motors/SmartServo.cpp index 6cfe01c..d716d34 100644 --- a/src/lib/motors/SmartServo.cpp +++ b/src/lib/motors/SmartServo.cpp @@ -236,13 +236,15 @@ void SmartServoClass::setPosition(uint8_t const id, float const angle) Serial.println(msg); } - _targetPosition[idToArrayIndex(id)] = target_position_deg; - if (_positionMode == PositionMode::IMMEDIATE) { mbed::ScopedLock lock(_mtx); writeWordCmd(id, REG(SmartServoRegister::TARGET_POSITION_H), target_position_deg); } + else if (_positionMode == PositionMode::SYNC) + { + _targetPosition[idToArrayIndex(id)] = target_position_deg; + } } float SmartServoClass::getPosition(uint8_t const id) From cb87a59128f5136743cb0d98af02ce7bb1bc7cb3 Mon Sep 17 00:00:00 2001 From: Alexander Entinger Date: Wed, 24 Aug 2022 09:13:00 +0200 Subject: [PATCH 06/16] Bugfix: Runtime is in milliseconds, not seconds. --- src/lib/motors/SmartServo.cpp | 15 +++++++++++++-- 1 file changed, 13 insertions(+), 2 deletions(-) diff --git a/src/lib/motors/SmartServo.cpp b/src/lib/motors/SmartServo.cpp index d716d34..41294a0 100644 --- a/src/lib/motors/SmartServo.cpp +++ b/src/lib/motors/SmartServo.cpp @@ -227,8 +227,19 @@ void SmartServoClass::setPosition(uint8_t const id, float const angle) */ if (angular_velocity_deg_per_sec > MAX_ANGULAR_VELOCITY_deg_per_sec) { - float const limited_runtime_sec = abs_position_diff_deg / MAX_ANGULAR_VELOCITY_deg_per_sec; - setTime(id, static_cast(limited_runtime_sec)); + float const limited_runtime_sec = abs_position_diff_deg / MAX_ANGULAR_VELOCITY_deg_per_sec; + float const limited_runtime_ms = limited_runtime_sec * 1000.f; + uint16_t const limited_runtime_ms_param = max(5000, static_cast(limited_runtime_ms)); + + if (_positionMode == PositionMode::IMMEDIATE) + { + mbed::ScopedLock lock(_mtx); + writeWordCmd(id, REG(SmartServoRegister::RUN_TIME_H), limited_runtime_ms_param); + } + else if (_positionMode == PositionMode::SYNC) + { + _targetSpeed[idToArrayIndex(id)] = limited_runtime_ms; + } char msg[64] = {0}; snprintf(msg, sizeof(msg), "targed = %0.2f, actual = %0.2f, diff = %0.2f, omega_vel = %0.2f", From e49c5d04e6f65974182ad4ef38c51ee5ca9aa2ec Mon Sep 17 00:00:00 2001 From: Alexander Entinger Date: Wed, 24 Aug 2022 09:23:03 +0200 Subject: [PATCH 07/16] From a pre-defined max angular velocity calculate the individual runtime. --- src/lib/motors/SmartServo.cpp | 50 ++++++++--------------------------- src/lib/motors/SmartServo.h | 2 +- 2 files changed, 12 insertions(+), 40 deletions(-) diff --git a/src/lib/motors/SmartServo.cpp b/src/lib/motors/SmartServo.cpp index 41294a0..931b44d 100644 --- a/src/lib/motors/SmartServo.cpp +++ b/src/lib/motors/SmartServo.cpp @@ -198,63 +198,35 @@ void SmartServoClass::begin() _targetSpeed[idToArrayIndex(i)] = 1000; } -void SmartServoClass::setPosition(uint8_t const id, float const angle) +void SmartServoClass::setPosition(uint8_t const id, float const angle_deg) { - if (!isValidAngle(angle)) + if (!isValidAngle(angle_deg)) return; if (!isValidId(id)) return; - float const MAX_ANGULAR_VELOCITY_deg_per_sec = 10.0f; + float const ANGULAR_VELOCITY_deg_per_sec = 10.0f; - float const target_position_deg = angleToPosition(angle); + float const target_position_deg = angle_deg; float const actual_position_deg = getPosition(id); if (actual_position_deg < 0.0f) - { - char msg[64] = {0}; - snprintf(msg, sizeof(msg), "error obtaining position for servo %d", (int)id); - Serial.println(msg); return; - } - - float const abs_position_diff_deg = fabs(target_position_deg - actual_position_deg); - float const runtime_sec = static_cast(getTime(id)) / 1000.0f; - float const angular_velocity_deg_per_sec = abs_position_diff_deg / runtime_sec; - - /* Check whether or not the maximum allowed angular velocity is exceeded, - * if it is indeed exceeded increase the runtime accordingly. - */ - if (angular_velocity_deg_per_sec > MAX_ANGULAR_VELOCITY_deg_per_sec) - { - float const limited_runtime_sec = abs_position_diff_deg / MAX_ANGULAR_VELOCITY_deg_per_sec; - float const limited_runtime_ms = limited_runtime_sec * 1000.f; - uint16_t const limited_runtime_ms_param = max(5000, static_cast(limited_runtime_ms)); - - if (_positionMode == PositionMode::IMMEDIATE) - { - mbed::ScopedLock lock(_mtx); - writeWordCmd(id, REG(SmartServoRegister::RUN_TIME_H), limited_runtime_ms_param); - } - else if (_positionMode == PositionMode::SYNC) - { - _targetSpeed[idToArrayIndex(id)] = limited_runtime_ms; - } - char msg[64] = {0}; - snprintf(msg, sizeof(msg), "targed = %0.2f, actual = %0.2f, diff = %0.2f, omega_vel = %0.2f", - target_position_deg, actual_position_deg, abs_position_diff_deg, angular_velocity_deg_per_sec); - Serial.println(msg); - } + float const abs_position_diff_deg = fabs(target_position_deg - actual_position_deg); + float const limited_runtime_sec = abs_position_diff_deg / ANGULAR_VELOCITY_deg_per_sec; + uint16_t const limited_runtime_ms = static_cast(limited_runtime_sec * 1000.f); if (_positionMode == PositionMode::IMMEDIATE) { mbed::ScopedLock lock(_mtx); - writeWordCmd(id, REG(SmartServoRegister::TARGET_POSITION_H), target_position_deg); + writeWordCmd(id, REG(SmartServoRegister::RUN_TIME_H), limited_runtime_ms); + writeWordCmd(id, REG(SmartServoRegister::TARGET_POSITION_H), angleToPosition(target_position_deg)); } else if (_positionMode == PositionMode::SYNC) { - _targetPosition[idToArrayIndex(id)] = target_position_deg; + _targetSpeed[idToArrayIndex(id)] = limited_runtime_ms; + _targetPosition[idToArrayIndex(id)] = angleToPosition(target_position_deg); } } diff --git a/src/lib/motors/SmartServo.h b/src/lib/motors/SmartServo.h index d42d487..ab516f3 100644 --- a/src/lib/motors/SmartServo.h +++ b/src/lib/motors/SmartServo.h @@ -46,7 +46,7 @@ class SmartServoClass inline void setPositionMode(PositionMode const mode) { _positionMode = mode; } - void setPosition(uint8_t const id, float const angle); + void setPosition(uint8_t const id, float const angle_deg); float getPosition(uint8_t const id); From 672628a9f9b98801f6bd9ec389de4911590425de Mon Sep 17 00:00:00 2001 From: Alexander Entinger Date: Wed, 24 Aug 2022 09:29:33 +0200 Subject: [PATCH 08/16] Turn compile time constant into member variable. --- src/lib/motors/SmartServo.cpp | 5 ++--- src/lib/motors/SmartServo.h | 2 ++ 2 files changed, 4 insertions(+), 3 deletions(-) diff --git a/src/lib/motors/SmartServo.cpp b/src/lib/motors/SmartServo.cpp index 931b44d..84efd23 100644 --- a/src/lib/motors/SmartServo.cpp +++ b/src/lib/motors/SmartServo.cpp @@ -31,6 +31,7 @@ SmartServoClass::SmartServoClass(RS485Class & RS485) : _RS485{RS485} , _errors{0} +, _angular_velocity_deg_per_sec{DEFAULT_ANGULAR_VELOCITY_deg_per_sec} , _onError{} , _mtx{} { @@ -206,15 +207,13 @@ void SmartServoClass::setPosition(uint8_t const id, float const angle_deg) if (!isValidId(id)) return; - float const ANGULAR_VELOCITY_deg_per_sec = 10.0f; - float const target_position_deg = angle_deg; float const actual_position_deg = getPosition(id); if (actual_position_deg < 0.0f) return; float const abs_position_diff_deg = fabs(target_position_deg - actual_position_deg); - float const limited_runtime_sec = abs_position_diff_deg / ANGULAR_VELOCITY_deg_per_sec; + float const limited_runtime_sec = abs_position_diff_deg / _angular_velocity_deg_per_sec; uint16_t const limited_runtime_ms = static_cast(limited_runtime_sec * 1000.f); if (_positionMode == PositionMode::IMMEDIATE) diff --git a/src/lib/motors/SmartServo.h b/src/lib/motors/SmartServo.h index ab516f3..7b87885 100644 --- a/src/lib/motors/SmartServo.h +++ b/src/lib/motors/SmartServo.h @@ -97,6 +97,7 @@ class SmartServoClass static int constexpr MAX_MOTOR_ID = 6; static int constexpr NUM_MOTORS = 6; static float constexpr MAX_ANGLE = 315.0f; + static float constexpr DEFAULT_ANGULAR_VELOCITY_deg_per_sec = 10.0f; static uint16_t constexpr TORQUE_MIN = 0; static uint16_t constexpr TORQUE_MAX = 1000; @@ -129,6 +130,7 @@ class SmartServoClass RS485Class& _RS485; int _errors; + float _angular_velocity_deg_per_sec; mbed::Callback _onError; rtos::Mutex _mtx; From 14c3c7a3cfb8f771f3b0e17abefded7d8b50db42 Mon Sep 17 00:00:00 2001 From: Alexander Entinger Date: Wed, 24 Aug 2022 09:33:19 +0200 Subject: [PATCH 09/16] Provide getter/setters for angular velocity. --- src/Braccio++.h | 3 +++ src/lib/motors/SmartServo.h | 7 ++++--- 2 files changed, 7 insertions(+), 3 deletions(-) diff --git a/src/Braccio++.h b/src/Braccio++.h index 6d3d1e6..c895b56 100644 --- a/src/Braccio++.h +++ b/src/Braccio++.h @@ -83,6 +83,9 @@ class BraccioClass 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 setAngularVelocity(float const angular_velocity_deg_per_sec) { _servos.setAngularVelocity(angular_velocity_deg_per_sec); } + inline float getAngularVelocity() const { return _servos.getAngularVelocity(); } + 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 7b87885..4c634da 100644 --- a/src/lib/motors/SmartServo.h +++ b/src/lib/motors/SmartServo.h @@ -46,14 +46,15 @@ class SmartServoClass inline void setPositionMode(PositionMode const mode) { _positionMode = mode; } - void setPosition(uint8_t const id, float const angle_deg); - + void setPosition(uint8_t const id, float const angle_deg); float getPosition(uint8_t const id); + inline void setAngularVelocity(float const angular_velocity_deg_per_sec) { _angular_velocity_deg_per_sec = angular_velocity_deg_per_sec; } + inline float getAngularVelocity() const { return _angular_velocity_deg_per_sec; } + void synchronize(); void setTorque(bool const torque); - void setTorque(uint8_t const id, bool const torque); void setMaxTorque(uint8_t const id, uint16_t const max_torque); From bc001ef75a2ae341420050e9b1ffd57710b388cf Mon Sep 17 00:00:00 2001 From: Alexander Entinger Date: Wed, 24 Aug 2022 09:34:32 +0200 Subject: [PATCH 10/16] Drop runtime/getRuntime API. --- src/Braccio++.h | 2 -- 1 file changed, 2 deletions(-) diff --git a/src/Braccio++.h b/src/Braccio++.h index c895b56..c3a3f2b 100644 --- a/src/Braccio++.h +++ b/src/Braccio++.h @@ -199,8 +199,6 @@ class Servo inline float position() { return _servos.getPosition(_id); } inline bool connected() { return Braccio.connected(_id); } inline void info(Stream & stream) { _servos.getInfo(stream, _id); } - inline uint16_t runtime() { return _servos.getTime(_id); } - inline void setRuntime(uint16_t const time) { _servos.setTime(_id, time); } operator bool() { return connected(); } From 517570821a3308670ef516504932d3918c788675 Mon Sep 17 00:00:00 2001 From: Alexander Entinger Date: Wed, 24 Aug 2022 09:47:53 +0200 Subject: [PATCH 11/16] Eliminate speed API alltogether. --- .../Braccio_Record_and_Replay/Braccio_Record_and_Replay.ino | 5 ++++- .../Controlling_Manually_Braccio.ino | 2 -- .../01_aligning_braccio/01_aligning_braccio.ino | 3 --- .../02_waving_with_Braccio/02_waving_with_Braccio.ino | 3 --- .../03_moving_challenge/03_moving_challenge.ino | 3 --- .../01_Controlling_Manually_Braccio.ino | 2 -- .../02_Manual_Control_Challenge.ino | 2 -- .../01_Braccio_Record_and_Replay.ino | 2 ++ src/Braccio++.cpp | 2 +- src/Braccio++.h | 3 --- 10 files changed, 7 insertions(+), 20 deletions(-) diff --git a/examples/Braccio_Record_and_Replay/Braccio_Record_and_Replay.ino b/examples/Braccio_Record_and_Replay/Braccio_Record_and_Replay.ino index 8e0439d..88e6b06 100644 --- a/examples/Braccio_Record_and_Replay/Braccio_Record_and_Replay.ino +++ b/examples/Braccio_Record_and_Replay/Braccio_Record_and_Replay.ino @@ -18,8 +18,11 @@ RecordAndReplayApp app; void setup() { - if (Braccio.begin(custom_main_menu)) { + if (Braccio.begin(custom_main_menu)) + { app.enableButtons(); + /* Allow greater angular velocity than the default one. */ + Braccio.setAngularVelocity(45.0f); } } diff --git a/examples/Controlling_Manually_Braccio/Controlling_Manually_Braccio.ino b/examples/Controlling_Manually_Braccio/Controlling_Manually_Braccio.ino index 4d2f5ea..679fd73 100644 --- a/examples/Controlling_Manually_Braccio/Controlling_Manually_Braccio.ino +++ b/examples/Controlling_Manually_Braccio/Controlling_Manually_Braccio.ino @@ -54,8 +54,6 @@ void setup() if (Braccio.begin(directionScreen)) { - /* Configure Braccio. */ - Braccio.speed(speed_grade_t(120)/*MEDIUM*/); /* Move to home position. */ Braccio.moveTo(HOME_POS[0], HOME_POS[1], HOME_POS[2], HOME_POS[3], HOME_POS[4], HOME_POS[5]); delay(500); diff --git a/examples/Platform_Tutorials/projects/p01-moving-braccio/01_aligning_braccio/01_aligning_braccio.ino b/examples/Platform_Tutorials/projects/p01-moving-braccio/01_aligning_braccio/01_aligning_braccio.ino index bd0c162..b06b096 100644 --- a/examples/Platform_Tutorials/projects/p01-moving-braccio/01_aligning_braccio/01_aligning_braccio.ino +++ b/examples/Platform_Tutorials/projects/p01-moving-braccio/01_aligning_braccio/01_aligning_braccio.ino @@ -18,9 +18,6 @@ void setup() { Braccio.begin(); delay(500); // Waits for the Braccio initialization - // You can choose the speed beforehand with - Braccio.speed(SLOW); // could be FAST or MEDIUM or SLOW - // Send motors initial angle gripper.move().to(initialGripper); delay(100); diff --git a/examples/Platform_Tutorials/projects/p01-moving-braccio/02_waving_with_Braccio/02_waving_with_Braccio.ino b/examples/Platform_Tutorials/projects/p01-moving-braccio/02_waving_with_Braccio/02_waving_with_Braccio.ino index 2bda3b1..f837495 100644 --- a/examples/Platform_Tutorials/projects/p01-moving-braccio/02_waving_with_Braccio/02_waving_with_Braccio.ino +++ b/examples/Platform_Tutorials/projects/p01-moving-braccio/02_waving_with_Braccio/02_waving_with_Braccio.ino @@ -21,9 +21,6 @@ void setup() { Braccio.begin(); delay(500); // Waits for the Braccio initialization - // You can choose the speed beforehand with - Braccio.speed(SLOW); // could be FAST or MEDIUM or SLOW - // Set motors initial angle // Should move all the motors at once Braccio.moveTo(homePos[0], homePos[1], homePos[2], homePos[3], homePos[4], homePos[5]); diff --git a/examples/Platform_Tutorials/projects/p01-moving-braccio/03_moving_challenge/03_moving_challenge.ino b/examples/Platform_Tutorials/projects/p01-moving-braccio/03_moving_challenge/03_moving_challenge.ino index 2999342..138d7d7 100644 --- a/examples/Platform_Tutorials/projects/p01-moving-braccio/03_moving_challenge/03_moving_challenge.ino +++ b/examples/Platform_Tutorials/projects/p01-moving-braccio/03_moving_challenge/03_moving_challenge.ino @@ -20,9 +20,6 @@ void setup() { Braccio.begin(); delay(500); // Waits for the Braccio initialization - // You can choose the speed beforehand with - Braccio.speed(SLOW); // could be FAST or MEDIUM or SLOW - // Set motors initial angle // Should move all the motors at once Braccio.moveTo(homePos[0], homePos[1], homePos[2], homePos[3], homePos[4], homePos[5]); diff --git a/examples/Platform_Tutorials/projects/p02-controlling-braccio-manually/01_Controlling_Manually_Braccio/01_Controlling_Manually_Braccio.ino b/examples/Platform_Tutorials/projects/p02-controlling-braccio-manually/01_Controlling_Manually_Braccio/01_Controlling_Manually_Braccio.ino index 4d2f5ea..679fd73 100644 --- a/examples/Platform_Tutorials/projects/p02-controlling-braccio-manually/01_Controlling_Manually_Braccio/01_Controlling_Manually_Braccio.ino +++ b/examples/Platform_Tutorials/projects/p02-controlling-braccio-manually/01_Controlling_Manually_Braccio/01_Controlling_Manually_Braccio.ino @@ -54,8 +54,6 @@ void setup() if (Braccio.begin(directionScreen)) { - /* Configure Braccio. */ - Braccio.speed(speed_grade_t(120)/*MEDIUM*/); /* Move to home position. */ Braccio.moveTo(HOME_POS[0], HOME_POS[1], HOME_POS[2], HOME_POS[3], HOME_POS[4], HOME_POS[5]); delay(500); diff --git a/examples/Platform_Tutorials/projects/p02-controlling-braccio-manually/02_Manual_Control_Challenge/02_Manual_Control_Challenge.ino b/examples/Platform_Tutorials/projects/p02-controlling-braccio-manually/02_Manual_Control_Challenge/02_Manual_Control_Challenge.ino index 4d2f5ea..679fd73 100644 --- a/examples/Platform_Tutorials/projects/p02-controlling-braccio-manually/02_Manual_Control_Challenge/02_Manual_Control_Challenge.ino +++ b/examples/Platform_Tutorials/projects/p02-controlling-braccio-manually/02_Manual_Control_Challenge/02_Manual_Control_Challenge.ino @@ -54,8 +54,6 @@ void setup() if (Braccio.begin(directionScreen)) { - /* Configure Braccio. */ - Braccio.speed(speed_grade_t(120)/*MEDIUM*/); /* Move to home position. */ Braccio.moveTo(HOME_POS[0], HOME_POS[1], HOME_POS[2], HOME_POS[3], HOME_POS[4], HOME_POS[5]); delay(500); diff --git a/examples/Platform_Tutorials/projects/p03-record-replay-mode/01_Braccio_Record_and_Replay/01_Braccio_Record_and_Replay.ino b/examples/Platform_Tutorials/projects/p03-record-replay-mode/01_Braccio_Record_and_Replay/01_Braccio_Record_and_Replay.ino index c5180cf..5456ea5 100644 --- a/examples/Platform_Tutorials/projects/p03-record-replay-mode/01_Braccio_Record_and_Replay/01_Braccio_Record_and_Replay.ino +++ b/examples/Platform_Tutorials/projects/p03-record-replay-mode/01_Braccio_Record_and_Replay/01_Braccio_Record_and_Replay.ino @@ -20,6 +20,8 @@ void setup() { if (Braccio.begin(custom_main_menu)) { app.enableButtons(); + /* Allow greater angular velocity than the default one. */ + Braccio.setAngularVelocity(45.0f); } } diff --git a/src/Braccio++.cpp b/src/Braccio++.cpp index a0ddbfc..d5d7d9c 100644 --- a/src/Braccio++.cpp +++ b/src/Braccio++.cpp @@ -136,7 +136,7 @@ bool BraccioClass::begin(voidFuncPtr custom_menu, bool const wait_for_all_motor_ lvgl_defaultMenu(); _servos.begin(); - _servos.setTime(SLOW); + _servos.setAngularVelocity(SmartServoClass::DEFAULT_ANGULAR_VELOCITY_deg_per_sec); _servos.setPositionMode(PositionMode::IMMEDIATE); _motors_connected_thd.start(mbed::callback(this, &BraccioClass::motorConnectedThreadFunc)); diff --git a/src/Braccio++.h b/src/Braccio++.h index c3a3f2b..9fe3145 100644 --- a/src/Braccio++.h +++ b/src/Braccio++.h @@ -86,9 +86,6 @@ class BraccioClass inline void setAngularVelocity(float const angular_velocity_deg_per_sec) { _servos.setAngularVelocity(angular_velocity_deg_per_sec); } inline float getAngularVelocity() const { return _servos.getAngularVelocity(); } - 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); } - inline void disengage(int const id = SmartServoClass::BROADCAST) { _servos.disengage(id); } inline void engage (int const id = SmartServoClass::BROADCAST) { _servos.engage(id); } From 81e83b0ff1273c97632e453b5d4d119a2b2b35ac Mon Sep 17 00:00:00 2001 From: Alexander Entinger Date: Wed, 24 Aug 2022 10:00:11 +0200 Subject: [PATCH 12/16] =?UTF-8?q?Increase=20default=20angular=20velocity?= =?UTF-8?q?=20to=2020.0=20=C2=B0/s.=20This=20seems=20save=20enough.?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- 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 4c634da..d7186b6 100644 --- a/src/lib/motors/SmartServo.h +++ b/src/lib/motors/SmartServo.h @@ -98,7 +98,7 @@ class SmartServoClass static int constexpr MAX_MOTOR_ID = 6; static int constexpr NUM_MOTORS = 6; static float constexpr MAX_ANGLE = 315.0f; - static float constexpr DEFAULT_ANGULAR_VELOCITY_deg_per_sec = 10.0f; + static float constexpr DEFAULT_ANGULAR_VELOCITY_deg_per_sec = 20.0f; static uint16_t constexpr TORQUE_MIN = 0; static uint16_t constexpr TORQUE_MAX = 1000; From e5a0e6adfac9e17264cd3b4ad0c020a85f3b4240 Mon Sep 17 00:00:00 2001 From: Alexander Entinger Date: Wed, 24 Aug 2022 10:04:58 +0200 Subject: [PATCH 13/16] Eliminate 'in' API. --- examples/Tools/Braccio_Basic/Braccio_Basic.ino | 4 ++-- .../Factory_Set_Initial_Servo_Position.ino | 2 +- .../Test_Motor_Angular_Control.ino | 4 ++-- src/Braccio++.h | 5 ++--- 4 files changed, 7 insertions(+), 8 deletions(-) diff --git a/examples/Tools/Braccio_Basic/Braccio_Basic.ino b/examples/Tools/Braccio_Basic/Braccio_Basic.ino index ebc00c7..547c1c0 100644 --- a/examples/Tools/Braccio_Basic/Braccio_Basic.ino +++ b/examples/Tools/Braccio_Basic/Braccio_Basic.ino @@ -85,9 +85,9 @@ void loop() { if (move_joint) { - Braccio.move(4).to((SmartServoClass::MAX_ANGLE / 2.0f) - 45.0f).in(1s); + Braccio.move(4).to((SmartServoClass::MAX_ANGLE / 2.0f) - 45.0f); delay(1000); - Braccio.move(4).to((SmartServoClass::MAX_ANGLE / 2.0f) + 45.0f).in(1s); + Braccio.move(4).to((SmartServoClass::MAX_ANGLE / 2.0f) + 45.0f); delay(1000); } } diff --git a/examples/Tools/Factory_Set_Initial_Servo_Position/Factory_Set_Initial_Servo_Position.ino b/examples/Tools/Factory_Set_Initial_Servo_Position/Factory_Set_Initial_Servo_Position.ino index f833ffd..64ff66b 100644 --- a/examples/Tools/Factory_Set_Initial_Servo_Position/Factory_Set_Initial_Servo_Position.ino +++ b/examples/Tools/Factory_Set_Initial_Servo_Position/Factory_Set_Initial_Servo_Position.ino @@ -85,7 +85,7 @@ bool set_initial_servo_position(int const id, float const target_angle) for ( float current_angle = Braccio.get(id).position(); !isTargetAngleReached(EPSILON) && !isTimeout(start);) { - Braccio.get(id).move().to(target_angle).in(200ms); + Braccio.get(id).move().to(target_angle); delay(250); char msg[64] = {0}; diff --git a/examples/Tools/Test_Motor_Angular_Control/Test_Motor_Angular_Control.ino b/examples/Tools/Test_Motor_Angular_Control/Test_Motor_Angular_Control.ino index bba4fd5..cb97616 100644 --- a/examples/Tools/Test_Motor_Angular_Control/Test_Motor_Angular_Control.ino +++ b/examples/Tools/Test_Motor_Angular_Control/Test_Motor_Angular_Control.ino @@ -31,13 +31,13 @@ void test_motor(int const id) delay(500); Serial.print("Drive to start . "); - Braccio.get(id).move().to(0.0f).in(1s); + Braccio.get(id).move().to(0.0f); Serial.println("OK."); delay(1500); for (float target_angle = 0.0f; target_angle < SmartServoClass::MAX_ANGLE; target_angle += 1.0f) { - Braccio.get(id).move().to(target_angle).in(200ms); + Braccio.get(id).move().to(target_angle); delay(250); char msg[64] = {0}; diff --git a/src/Braccio++.h b/src/Braccio++.h index 9fe3145..07651b5 100644 --- a/src/Braccio++.h +++ b/src/Braccio++.h @@ -189,9 +189,8 @@ class Servo 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; } - inline Servo & in (std::chrono::milliseconds const len) { _servos.setTime(_id, len.count()); return *this; } + inline Servo & move() { return *this; } + inline Servo & to (float const angle) { _servos.setPosition(_id, angle); return *this; } inline float position() { return _servos.getPosition(_id); } inline bool connected() { return Braccio.connected(_id); } From a5820a628ce9ec4e81637b762363111df200370e Mon Sep 17 00:00:00 2001 From: Alexander Entinger Date: Wed, 24 Aug 2022 10:13:25 +0200 Subject: [PATCH 14/16] Fix example after removing in() API. --- examples/Tools/Braccio_Basic/Braccio_Basic.ino | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/examples/Tools/Braccio_Basic/Braccio_Basic.ino b/examples/Tools/Braccio_Basic/Braccio_Basic.ino index 547c1c0..0ea708c 100644 --- a/examples/Tools/Braccio_Basic/Braccio_Basic.ino +++ b/examples/Tools/Braccio_Basic/Braccio_Basic.ino @@ -79,6 +79,7 @@ void setup() Braccio.moveTo(home_position[0], home_position[1], home_position[2], home_position[3], home_position[4], home_position[5]); delay(1000); + Braccio.setAngularVelocity(45.0f); /* 45 deg/sec */ } void loop() @@ -86,8 +87,8 @@ void loop() if (move_joint) { Braccio.move(4).to((SmartServoClass::MAX_ANGLE / 2.0f) - 45.0f); - delay(1000); + delay(2000); Braccio.move(4).to((SmartServoClass::MAX_ANGLE / 2.0f) + 45.0f); - delay(1000); + delay(2000); } } From 5f81b5396ee5ee1dfc867c19e4f4f283b349bf83 Mon Sep 17 00:00:00 2001 From: Alexander Entinger Date: Wed, 24 Aug 2022 10:13:42 +0200 Subject: [PATCH 15/16] Enable max. torque which is save now due to velocity limiting. --- src/Braccio++.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/Braccio++.cpp b/src/Braccio++.cpp index d5d7d9c..82c3776 100644 --- a/src/Braccio++.cpp +++ b/src/Braccio++.cpp @@ -136,6 +136,7 @@ bool BraccioClass::begin(voidFuncPtr custom_menu, bool const wait_for_all_motor_ lvgl_defaultMenu(); _servos.begin(); + _servos.setMaxTorque(SmartServoClass::TORQUE_MAX); _servos.setAngularVelocity(SmartServoClass::DEFAULT_ANGULAR_VELOCITY_deg_per_sec); _servos.setPositionMode(PositionMode::IMMEDIATE); From 8a5cc9472d49eb98c31a4f67a06792070839f646 Mon Sep 17 00:00:00 2001 From: Alexander Entinger Date: Wed, 24 Aug 2022 10:15:28 +0200 Subject: [PATCH 16/16] Eliminate no-longer used speed_grade_t constant. --- src/Braccio++.h | 10 ---------- 1 file changed, 10 deletions(-) diff --git a/src/Braccio++.h b/src/Braccio++.h index 07651b5..565b9ab 100644 --- a/src/Braccio++.h +++ b/src/Braccio++.h @@ -38,16 +38,6 @@ #include using namespace std::chrono; -/************************************************************************************** - * TYPEDEF - **************************************************************************************/ - -enum speed_grade_t { - FAST = 10, - MEDIUM = 100, - SLOW = 1000, -}; - /************************************************************************************** * FORWARD DECLARATION **************************************************************************************/