Skip to content

Add API for getting moving status #147

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

Closed
jonghee-son wants to merge 4 commits into from
Closed

Add API for getting moving status #147

Show file tree
Hide file tree
Changes from 3 commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
b83c947
[Feature] Added API for MovingStatus
jonghee-son May 13, 2025
27faec4
[Fix] Added model identification logic for getMovingStatus
jonghee-son May 13, 2025
2f3c03d
[Fix] Fixed bit orientation for MovingStatus identifier
jonghee-son May 13, 2025
c055572
[Fix] Fixed example code sniffet for getMovingStatus
jonghee-son May 13, 2025
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
27 changes: 27 additions & 0 deletions src/Dynamixel2Arduino.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -915,6 +915,33 @@ bool Dynamixel2Arduino::getTorqueEnableStat(uint8_t id)
return ret;
}

uint8_t Dynamixel2Arduino::getMovingStatus(uint8_t 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);
}
}
Comment on lines +923 to +927
Copy link
Copy Markdown

@gemini-code-assist gemini-code-assist Bot May 13, 2025

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

medium

This block attempts to set the model number if it's unregistered. However, if setModelNumber fails, model_num will remain UNREGISTERED_MODEL, and the subsequent checks will be performed with an invalid model number. It would be better to check the return value of setModelNumber and skip the unsupported model check if it fails. Also, the setModelNumber function already sets the last lib error code if it fails, so there is no need to set it again in the else block.

Suggested change
if(model_num == UNREGISTERED_MODEL){
if(setModelNumber(id, getModelNumber(id)) == true){
model_num = getModelNumberFromTable(id);
}
}
if(model_num == UNREGISTERED_MODEL){
if(setModelNumber(id, getModelNumber(id)) == true){
model_num = getModelNumberFromTable(id);
} else {
return 0; // Or some other appropriate error value
}
}


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);
Comment on lines +929 to +935
Copy link
Copy Markdown

@gemini-code-assist gemini-code-assist Bot May 13, 2025

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

medium

This block checks for a long list of specific model numbers to determine if the MOVING_STATUS control table item is supported. This approach is not scalable and makes the code harder to maintain. Consider using a data structure (e.g., an array or a hash table) to store the unsupported model numbers and then check against that data structure. This would make the code more readable and easier to update in the future. Also, consider consolidating the two if statements into one, since they both perform the same action.

Could this be simplified by using a switch statement or a lookup table?

Suggested change
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);
const uint16_t unsupported_models[] = {
AX12A, AX12W, AX18A, DX113, DX116, DX117, RX10, RX24F, RX28, RX64, EX106, MX12W, MX28, MX64, MX106, XL320,
YM070_210_M001_RH, YM070_210_B001_RH, YM070_210_R051_RH, YM070_210_R099_RH, YM070_210_A051_RH, YM070_210_A099_RH,
YM080_230_M001_RH, YM080_230_B001_RH, YM080_230_R051_RH, YM080_230_R099_RH, YM080_230_A051_RH, YM080_230_A099_RH
};
bool is_supported = true;
for (size_t i = 0; i < sizeof(unsupported_models) / sizeof(unsupported_models[0]); ++i) {
if (model_num == unsupported_models[i]) {
is_supported = false;
break;
}
}
if (!is_supported) {
setLastLibErrCode(DXL_LIB_ERROR_NOT_SUPPORTED);
} else {
ret = (uint8_t)readControlTableItem(ControlTableItem::MOVING_STATUS, id);
}

}
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)
{
int32_t ret = 0;
Expand Down
42 changes: 42 additions & 0 deletions src/Dynamixel2Arduino.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -48,6 +48,14 @@ enum D2ALibErrorCode
D2A_LIB_ERROR_UNKNOWN_MODEL_NUMBER
};

enum MovingStatus
{
IN_POSITION = 0x01,
PROFILE_ONGOING = 0x02,
FOLLOWING_ERROR = 0x08,
VELOCITY_PROFILE = 0x30
};

class Dynamixel2Arduino : public DYNAMIXEL::Master
{
public:
Expand Down Expand Up @@ -387,6 +395,40 @@ 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)");
* }
Comment on lines +403 to +424
Copy link
Copy Markdown

@gemini-code-assist gemini-code-assist Bot May 13, 2025

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

high

The example code in the documentation uses status & IN_POSITION == 1. This is incorrect. Bitwise AND has lower precedence than equality. The code should use (status & IN_POSITION) == IN_POSITION or status & IN_POSITION. Please correct the example code to avoid confusion.

Suggested change
* 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)");
* }
* if (status & IN_POSITION) {
* Serial.print("Arrived");
* }

* @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);

/**
* @brief It is API for getting data of a DYNAMIXEL control table item.
* @code
Expand Down