From b83c9477b57f2e1f606134f741a99f0f5379666e Mon Sep 17 00:00:00 2001 From: Jonghee Son Date: Tue, 13 May 2025 11:14:20 +0900 Subject: [PATCH 1/4] [Feature] Added API for MovingStatus Signed-off-by: Jonghee Son --- src/Dynamixel2Arduino.cpp | 5 +++++ src/Dynamixel2Arduino.h | 41 +++++++++++++++++++++++++++++++++++++++ 2 files changed, 46 insertions(+) diff --git a/src/Dynamixel2Arduino.cpp b/src/Dynamixel2Arduino.cpp index 2592292..ece4d6e 100644 --- a/src/Dynamixel2Arduino.cpp +++ b/src/Dynamixel2Arduino.cpp @@ -915,6 +915,11 @@ bool Dynamixel2Arduino::getTorqueEnableStat(uint8_t id) return ret; } +uint8_t Dynamixel2Arduino::getMovingStatus(uint8_t id) +{ + return readControlTableItem(ControlTableItem::MOVING_STATUS, id); +} + int32_t Dynamixel2Arduino::readControlTableItem(uint8_t item_idx, uint8_t id, uint32_t timeout) { int32_t ret = 0; diff --git a/src/Dynamixel2Arduino.h b/src/Dynamixel2Arduino.h index 29a6b52..a44af6f 100644 --- a/src/Dynamixel2Arduino.h +++ b/src/Dynamixel2Arduino.h @@ -48,6 +48,14 @@ enum D2ALibErrorCode D2A_LIB_ERROR_UNKNOWN_MODEL_NUMBER }; +enum MovingStatus +{ + IN_POSITION = 0x80, + PROFILE_ONGOING = 0x40, + FOLLOWING_ERROR = 0x10, + VELOCITY_PROFILE = 0x0C +}; + class Dynamixel2Arduino : public DYNAMIXEL::Master { public: @@ -387,6 +395,39 @@ class Dynamixel2Arduino : public DYNAMIXEL::Master */ bool getTorqueEnableStat(uint8_t id); + /** + * @brief It is API for getting the moving status of DYNAMIXEL. + * @code + * const int DXL_DIR_PIN = 2; + * Dynamixel2Arduino dxl(Serial1, DXL_DIR_PIN); + * status = dxl.getMovingStatus(1); + * if (status & IN_POSITION == 1) { + * Serial.print("Arrived"); + * } + * if (status & PROFILE_ONGOING == 1) { + * Serial.print("Profile is in progress"); + * } + * if (status & FOLLOWING_ERROR == 1) { + * Serial.print("Not following desired position trajectory"); + * } + * if (status & VELOCITY_PROFILE == 0xC0) { + * Serial.print("Using trapezoidal profile"); + * } + * else if (status & VELOCITY_PROFILE == 0x80) { + * Serial.print("Using triangular profile"); + * } + * else if (status & VELOCITY_PROFILE == 0x40) { + * Serial.print("Using rectangular profile"); + * } + * else { + * Serial.print("Not using any profile (step)"); + * } + * @endcode + * @param id DYNAMIXEL Actuator's ID. + * @return It returns the data read from DXL control table item. + */ + uint8_t getMovingStatus(uint8_t id); + /** * @brief It is API for getting data of a DYNAMIXEL control table item. * @code From 27faec4a67ddb198081d83b2ab8d744c86ad979d Mon Sep 17 00:00:00 2001 From: Jonghee Son Date: Tue, 13 May 2025 11:27:36 +0900 Subject: [PATCH 2/4] [Fix] Added model identification logic for getMovingStatus Signed-off-by: Jonghee Son --- src/Dynamixel2Arduino.cpp | 24 +++++++++++++++++++++++- src/Dynamixel2Arduino.h | 1 + 2 files changed, 24 insertions(+), 1 deletion(-) diff --git a/src/Dynamixel2Arduino.cpp b/src/Dynamixel2Arduino.cpp index ece4d6e..a1c00fc 100644 --- a/src/Dynamixel2Arduino.cpp +++ b/src/Dynamixel2Arduino.cpp @@ -917,7 +917,29 @@ bool Dynamixel2Arduino::getTorqueEnableStat(uint8_t id) uint8_t Dynamixel2Arduino::getMovingStatus(uint8_t id) { - return readControlTableItem(ControlTableItem::MOVING_STATUS, id); + uint16_t model_num = getModelNumberFromTable(id); + uint8_t ret = 0; + + if(model_num == UNREGISTERED_MODEL){ + if(setModelNumber(id, getModelNumber(id)) == true){ + model_num = getModelNumberFromTable(id); + } + } + + if(model_num == AX12A || model_num == AX12W || model_num == AX18A || model_num == DX113 || model_num == DX116 || model_num == DX117 || model_num == RX10 || model_num == RX24F || model_num == RX28 || model_num == RX64 || model_num == EX106 || model_num == MX12W || model_num == MX28 || model_num == MX64 || model_num == MX106 || model_num == XL320) + { + setLastLibErrCode(DXL_LIB_ERROR_NOT_SUPPORTED); + } + else if(model_num == YM070_210_M001_RH || model_num == YM070_210_B001_RH || model_num == YM070_210_R051_RH || model_num == YM070_210_R099_RH || model_num == YM070_210_A051_RH || model_num == YM070_210_A099_RH || model_num == YM080_230_M001_RH || model_num == YM080_230_B001_RH || model_num == YM080_230_R051_RH || model_num == YM080_230_R099_RH || model_num == YM080_230_A051_RH || model_num == YM080_230_A099_RH) + { + setLastLibErrCode(DXL_LIB_ERROR_NOT_SUPPORTED); + } + else + { + ret = (uint8_t)readControlTableItem(ControlTableItem::MOVING_STATUS, id); + } + + return ret; } int32_t Dynamixel2Arduino::readControlTableItem(uint8_t item_idx, uint8_t id, uint32_t timeout) diff --git a/src/Dynamixel2Arduino.h b/src/Dynamixel2Arduino.h index a44af6f..b4dffb1 100644 --- a/src/Dynamixel2Arduino.h +++ b/src/Dynamixel2Arduino.h @@ -425,6 +425,7 @@ class Dynamixel2Arduino : public DYNAMIXEL::Master * @endcode * @param id DYNAMIXEL Actuator's ID. * @return It returns the data read from DXL control table item. + * If the read fails, 0 is returned. Whether or not this is an actual value can be confirmed with @getLastLibErrCode(). */ uint8_t getMovingStatus(uint8_t id); From 2f3c03d0f39c2228a46651cb6d095136a37b122d Mon Sep 17 00:00:00 2001 From: Jonghee Son Date: Tue, 13 May 2025 12:34:20 +0900 Subject: [PATCH 3/4] [Fix] Fixed bit orientation for MovingStatus identifier Signed-off-by: Jonghee Son --- src/Dynamixel2Arduino.h | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/Dynamixel2Arduino.h b/src/Dynamixel2Arduino.h index b4dffb1..2985c74 100644 --- a/src/Dynamixel2Arduino.h +++ b/src/Dynamixel2Arduino.h @@ -50,10 +50,10 @@ enum D2ALibErrorCode enum MovingStatus { - IN_POSITION = 0x80, - PROFILE_ONGOING = 0x40, - FOLLOWING_ERROR = 0x10, - VELOCITY_PROFILE = 0x0C + IN_POSITION = 0x01, + PROFILE_ONGOING = 0x02, + FOLLOWING_ERROR = 0x08, + VELOCITY_PROFILE = 0x30 }; class Dynamixel2Arduino : public DYNAMIXEL::Master From c0555728cf1542f5e0c51a8189e5827392f285ae Mon Sep 17 00:00:00 2001 From: Jonghee Son Date: Tue, 13 May 2025 12:47:07 +0900 Subject: [PATCH 4/4] [Fix] Fixed example code sniffet for getMovingStatus Signed-off-by: Jonghee Son --- src/Dynamixel2Arduino.h | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/Dynamixel2Arduino.h b/src/Dynamixel2Arduino.h index 2985c74..fb3f2c3 100644 --- a/src/Dynamixel2Arduino.h +++ b/src/Dynamixel2Arduino.h @@ -401,22 +401,22 @@ class Dynamixel2Arduino : public DYNAMIXEL::Master * const int DXL_DIR_PIN = 2; * Dynamixel2Arduino dxl(Serial1, DXL_DIR_PIN); * status = dxl.getMovingStatus(1); - * if (status & IN_POSITION == 1) { + * if ((status & IN_POSITION) == IN_POSITION) { * Serial.print("Arrived"); * } - * if (status & PROFILE_ONGOING == 1) { + * if ((status & PROFILE_ONGOING) == PROFILE_ONGOING) { * Serial.print("Profile is in progress"); * } - * if (status & FOLLOWING_ERROR == 1) { + * if ((status & FOLLOWING_ERROR) == FOLLOWING_ERROR) { * Serial.print("Not following desired position trajectory"); * } - * if (status & VELOCITY_PROFILE == 0xC0) { + * if ((status & VELOCITY_PROFILE) == VELOCITY_PROFILE) { * Serial.print("Using trapezoidal profile"); * } - * else if (status & VELOCITY_PROFILE == 0x80) { + * else if ((status & VELOCITY_PROFILE) == VELOCITY_PROFILE) { * Serial.print("Using triangular profile"); * } - * else if (status & VELOCITY_PROFILE == 0x40) { + * else if ((status & VELOCITY_PROFILE) == VELOCITY_PROFILE) { * Serial.print("Using rectangular profile"); * } * else {