diff --git a/src/Dynamixel2Arduino.cpp b/src/Dynamixel2Arduino.cpp index 2592292..a1c00fc 100644 --- a/src/Dynamixel2Arduino.cpp +++ b/src/Dynamixel2Arduino.cpp @@ -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); + } + } + + 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) { int32_t ret = 0; diff --git a/src/Dynamixel2Arduino.h b/src/Dynamixel2Arduino.h index 29a6b52..fb3f2c3 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 = 0x01, + PROFILE_ONGOING = 0x02, + FOLLOWING_ERROR = 0x08, + VELOCITY_PROFILE = 0x30 +}; + class Dynamixel2Arduino : public DYNAMIXEL::Master { public: @@ -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) == IN_POSITION) { + * Serial.print("Arrived"); + * } + * if ((status & PROFILE_ONGOING) == PROFILE_ONGOING) { + * Serial.print("Profile is in progress"); + * } + * if ((status & FOLLOWING_ERROR) == FOLLOWING_ERROR) { + * Serial.print("Not following desired position trajectory"); + * } + * if ((status & VELOCITY_PROFILE) == VELOCITY_PROFILE) { + * Serial.print("Using trapezoidal profile"); + * } + * else if ((status & VELOCITY_PROFILE) == VELOCITY_PROFILE) { + * Serial.print("Using triangular profile"); + * } + * else if ((status & VELOCITY_PROFILE) == VELOCITY_PROFILE) { + * 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. + * 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