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/examples/Tools/Braccio_Basic/Braccio_Basic.ino b/examples/Tools/Braccio_Basic/Braccio_Basic.ino index ebc00c7..0ea708c 100644 --- a/examples/Tools/Braccio_Basic/Braccio_Basic.ino +++ b/examples/Tools/Braccio_Basic/Braccio_Basic.ino @@ -79,15 +79,16 @@ 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() { if (move_joint) { - Braccio.move(4).to((SmartServoClass::MAX_ANGLE / 2.0f) - 45.0f).in(1s); - delay(1000); - Braccio.move(4).to((SmartServoClass::MAX_ANGLE / 2.0f) + 45.0f).in(1s); - delay(1000); + Braccio.move(4).to((SmartServoClass::MAX_ANGLE / 2.0f) - 45.0f); + delay(2000); + Braccio.move(4).to((SmartServoClass::MAX_ANGLE / 2.0f) + 45.0f); + delay(2000); } } 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++.cpp b/src/Braccio++.cpp index a0ddbfc..82c3776 100644 --- a/src/Braccio++.cpp +++ b/src/Braccio++.cpp @@ -136,7 +136,8 @@ bool BraccioClass::begin(voidFuncPtr custom_menu, bool const wait_for_all_motor_ lvgl_defaultMenu(); _servos.begin(); - _servos.setTime(SLOW); + _servos.setMaxTorque(SmartServoClass::TORQUE_MAX); + _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 6d3d1e6..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 **************************************************************************************/ @@ -83,8 +73,8 @@ 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 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 setAngularVelocity(float const angular_velocity_deg_per_sec) { _servos.setAngularVelocity(angular_velocity_deg_per_sec); } + inline float getAngularVelocity() const { return _servos.getAngularVelocity(); } inline void disengage(int const id = SmartServoClass::BROADCAST) { _servos.disengage(id); } inline void engage (int const id = SmartServoClass::BROADCAST) { _servos.engage(id); } @@ -189,15 +179,12 @@ 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); } 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(); } diff --git a/src/lib/motors/SmartServo.cpp b/src/lib/motors/SmartServo.cpp index 0e6d311..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{} { @@ -198,27 +199,43 @@ 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; - mbed::ScopedLock lock(_mtx); - if (isValidId(id)) + if (!isValidId(id)) + return; + + 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; + uint16_t const limited_runtime_ms = static_cast(limited_runtime_sec * 1000.f); + + 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::RUN_TIME_H), limited_runtime_ms); + writeWordCmd(id, REG(SmartServoRegister::TARGET_POSITION_H), angleToPosition(target_position_deg)); + } + else if (_positionMode == PositionMode::SYNC) + { + _targetSpeed[idToArrayIndex(id)] = limited_runtime_ms; + _targetPosition[idToArrayIndex(id)] = angleToPosition(target_position_deg); + } } 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) @@ -261,16 +278,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); } diff --git a/src/lib/motors/SmartServo.h b/src/lib/motors/SmartServo.h index d42d487..d7186b6 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); - + 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); @@ -97,6 +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 = 20.0f; static uint16_t constexpr TORQUE_MIN = 0; static uint16_t constexpr TORQUE_MAX = 1000; @@ -129,6 +131,7 @@ class SmartServoClass RS485Class& _RS485; int _errors; + float _angular_velocity_deg_per_sec; mbed::Callback _onError; rtos::Mutex _mtx;