diff --git a/src/Dynamixel2Arduino.cpp b/src/Dynamixel2Arduino.cpp index 2592292..bd36e5c 100644 --- a/src/Dynamixel2Arduino.cpp +++ b/src/Dynamixel2Arduino.cpp @@ -610,14 +610,14 @@ bool Dynamixel2Arduino::setLedState(uint8_t id, bool state) case PRO_M54P_040_S250_R: case PRO_M54P_060_S250_R: if (state == false) { - writeControlTableItem(ControlTableItem::LED_GREEN, id, state); - writeControlTableItem(ControlTableItem::LED_BLUE, id, state); + writeControlTableItem(ControlTableItem::DXL_LED_GREEN, id, state); + writeControlTableItem(ControlTableItem::DXL_LED_BLUE, id, state); } - ret = writeControlTableItem(ControlTableItem::LED_RED, id, state); + ret = writeControlTableItem(ControlTableItem::DXL_LED_RED, id, state); break; default: - ret = writeControlTableItem(ControlTableItem::LED, id, state); + ret = writeControlTableItem(ControlTableItem::DXL_LED, id, state); break; } @@ -915,6 +915,33 @@ bool Dynamixel2Arduino::getTorqueEnableStat(uint8_t id) return ret; } +uint8_t Dynamixel2Arduino::getHardwareError(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) + { + ret = (uint8_t)readControlTableItem(ControlTableItem::ERROR_CODE, id); + } + else + { + ret = (uint8_t)readControlTableItem(ControlTableItem::HARDWARE_ERROR_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..45bfbb0 100644 --- a/src/Dynamixel2Arduino.h +++ b/src/Dynamixel2Arduino.h @@ -48,6 +48,37 @@ enum D2ALibErrorCode D2A_LIB_ERROR_UNKNOWN_MODEL_NUMBER }; +enum HardwareErrorCode +{ + INPUT_VOLTAGE_ERROR = 0x01, + OVERHEATING_ERROR = 0x04, + MOTOR_ENCODER_ERROR = 0x08, + ELECTRICAL_SHOCK_ERROR = 0x10, + OVERLOAD_ERROR = 0x20 +}; + +enum DYErrorCode +{ + DY_NO_ERROR = 0, + DY_OVER_VOLTAGE_ERROR, + DY_LOW_VOLTAGE_ERROR, + DY_INVERTER_OVERHEATING_ERROR, + DY_MOTOR_OVERHEATING_ERROR, + DY_OVERLOAD_ERROR, + DY_INVERTER_ERROR, + DY_BATTERY_WARNING, + DY_BATTERY_ERROR, + DY_MAGNET_ERROR, + DY_MULTI_TURN_ERROR, + DY_ENCODER_ERROR, + DY_HALL_SENSOR_ERROR, + DY_CALIBRATION_ERROR, + DY_FOLLOWING_ERROR, + DY_BUS_WATCHDOG_ERROR, + DY_OVER_SPEED_ERROR, + DY_POSITION_LIMIT_REACHED_ERROR +}; + class Dynamixel2Arduino : public DYNAMIXEL::Master { public: @@ -385,7 +416,22 @@ class Dynamixel2Arduino : public DYNAMIXEL::Master * @return It returns the Torque Enable data read from DXL control table item. * If the Torque is On, true(1) is returned. Otherwise false(0) is returned. */ - bool getTorqueEnableStat(uint8_t id); + bool getTorqueEnableStat(uint8_t id); + + /** + * @brief It is API for getting hardware error status of DYNAMIXEL. + * @code + * const int DXL_DIR_PIN = 2; + * Dynamixel2Arduino dxl(Serial1, DXL_DIR_PIN); + * if ((dxl.getHardwareError(1) & INPUT_VOLTAGE_ERROR) == INPUT_VOLTAGE_ERROR) { + * Serial.println("Input Voltage Error"); + * } + * @endcode + * @param id DYNAMIXEL Actuator's ID. + * @return It returns 0 on no error, value any other than 0 on hardware error. + * If the read fails, 0 is returned. Whether or not this is an actual value can be confirmed with @getLastLibErrCode(). + */ + uint8_t getHardwareError(uint8_t id); /** * @brief It is API for getting data of a DYNAMIXEL control table item. @@ -441,8 +487,6 @@ class Dynamixel2Arduino : public DYNAMIXEL::Master bool setPositionPIDGain(uint8_t id, uint16_t p_gain, uint16_t i_gain, uint16_t d_gain); bool setVelocityPIGain(uint8_t id, uint16_t p_gain, uint16_t i_gain); bool setFeedForwardGain(uint8_t id, uint16_t fisrt_gain, uint16_t second_gain); - - uint8_t getHardwareError(uint8_t id); // https://github.com/ROBOTIS-GIT/Dynamixel2Arduino/issues/73 uint8_t getOperatingMode(uint8_t id); diff --git a/src/actuator.cpp b/src/actuator.cpp index 59a5319..7cd1dbc 100644 --- a/src/actuator.cpp +++ b/src/actuator.cpp @@ -33,7 +33,7 @@ const ModelControlTableInfo_t control_table_1_0[] PROGMEM = { {ControlTableItem::SHUTDOWN, 18, 1}, {ControlTableItem::TORQUE_ENABLE, 24, 1}, - {ControlTableItem::LED, 25, 1}, + {ControlTableItem::DXL_LED, 25, 1}, {ControlTableItem::CW_COMPLIANCE_MARGIN, 26, 1}, {ControlTableItem::CCW_COMPLIANCE_MARGIN, 27, 1}, {ControlTableItem::CW_COMPLIANCE_SLOPE, 28, 1}, @@ -85,7 +85,7 @@ const ModelControlTableInfo_t control_table_1_1[] PROGMEM = { {ControlTableItem::RESOLUTION_DIVIDER, 22, 1}, {ControlTableItem::TORQUE_ENABLE, 24, 1}, - {ControlTableItem::LED, 25, 1}, + {ControlTableItem::DXL_LED, 25, 1}, {ControlTableItem::D_GAIN, 26, 1}, {ControlTableItem::I_GAIN, 27, 1}, {ControlTableItem::P_GAIN, 28, 1}, @@ -144,7 +144,7 @@ const ModelControlTableInfo_t xl320_control_table[] PROGMEM = { {ControlTableItem::SHUTDOWN, 18, 1}, {ControlTableItem::TORQUE_ENABLE, 24, 1}, - {ControlTableItem::LED, 25, 1}, + {ControlTableItem::DXL_LED, 25, 1}, {ControlTableItem::D_GAIN, 27, 1}, {ControlTableItem::I_GAIN, 28, 1}, {ControlTableItem::P_GAIN, 29, 1}, @@ -197,7 +197,7 @@ const ModelControlTableInfo_t control_table_2_0[] PROGMEM = { {ControlTableItem::SHUTDOWN, 63, 1}, {ControlTableItem::TORQUE_ENABLE, 64, 1}, - {ControlTableItem::LED, 65, 1}, + {ControlTableItem::DXL_LED, 65, 1}, {ControlTableItem::STATUS_RETURN_LEVEL, 68, 1}, {ControlTableItem::REGISTERED_INSTRUCTION, 69, 1}, {ControlTableItem::HARDWARE_ERROR_STATUS, 70, 1}, @@ -319,9 +319,9 @@ const ModelControlTableInfo_t pro_r_control_table[] PROGMEM = { {ControlTableItem::SHUTDOWN, 48, 1}, {ControlTableItem::TORQUE_ENABLE, 562, 1}, - {ControlTableItem::LED_RED, 563, 1}, - {ControlTableItem::LED_GREEN, 564, 1}, - {ControlTableItem::LED_BLUE, 565, 1}, + {ControlTableItem::DXL_LED_RED, 563, 1}, + {ControlTableItem::DXL_LED_GREEN, 564, 1}, + {ControlTableItem::DXL_LED_BLUE, 565, 1}, {ControlTableItem::VELOCITY_I_GAIN, 586, 2}, {ControlTableItem::VELOCITY_P_GAIN, 588, 2}, {ControlTableItem::POSITION_P_GAIN, 594, 2}, @@ -377,9 +377,9 @@ const ModelControlTableInfo_t pro_ra_pro_plus_control_table[] PROGMEM = { {ControlTableItem::SHUTDOWN, 63, 1}, {ControlTableItem::TORQUE_ENABLE, 512, 1}, - {ControlTableItem::LED_RED, 513, 1}, - {ControlTableItem::LED_GREEN, 514, 1}, - {ControlTableItem::LED_BLUE, 515, 1}, + {ControlTableItem::DXL_LED_RED, 513, 1}, + {ControlTableItem::DXL_LED_GREEN, 514, 1}, + {ControlTableItem::DXL_LED_BLUE, 515, 1}, {ControlTableItem::STATUS_RETURN_LEVEL, 516, 1}, {ControlTableItem::REGISTERED_INSTRUCTION, 517, 1}, {ControlTableItem::HARDWARE_ERROR_STATUS, 518, 1}, @@ -475,7 +475,7 @@ const ModelControlTableInfo_t y_control_table[] PROGMEM = { {ControlTableItem::PROFILE_ACCELERATION_TIME, 248, 4}, {ControlTableItem::PROFIIE_TIME, 252, 4}, {ControlTableItem::TORQUE_ENABLE, 512, 1}, - {ControlTableItem::LED, 513, 1}, + {ControlTableItem::DXL_LED, 513, 1}, {ControlTableItem::PWM_OFFSET, 516, 2}, {ControlTableItem::CURRENT_OFFSET, 518, 2}, {ControlTableItem::VELOCITY_OFFSET, 520, 4}, diff --git a/src/actuator.h b/src/actuator.h index ef22371..be37d48 100644 --- a/src/actuator.h +++ b/src/actuator.h @@ -331,10 +331,10 @@ namespace ControlTableItem{ SHUTDOWN, TORQUE_ENABLE, - LED, - LED_RED, - LED_GREEN, - LED_BLUE, + DXL_LED, + DXL_LED_RED, + DXL_LED_GREEN, + DXL_LED_BLUE, REGISTERED_INSTRUCTION, HARDWARE_ERROR_STATUS, VELOCITY_P_GAIN,