diff --git a/src/Dynamixel2Arduino.cpp b/src/Dynamixel2Arduino.cpp index dc53fdf..fb83bde 100644 --- a/src/Dynamixel2Arduino.cpp +++ b/src/Dynamixel2Arduino.cpp @@ -61,6 +61,9 @@ const uint16_t model_number_table[] PROGMEM = { PRO_M54P_040_S250_R, PRO_M54P_060_S250_R, PRO_H42P_020_S300_R, PRO_H54P_100_S500_R, PRO_H54P_200_S500_R + + RH_P12_RN, + RH_P12_RNA }; const uint8_t model_number_table_count = sizeof(model_number_table)/sizeof(model_number_table[0]); @@ -311,6 +314,7 @@ bool Dynamixel2Arduino::setBaudrate(uint8_t id, uint32_t baudrate) case XC330_T181: case XL330_M288: case XL330_M077: + case RH_P12_RN: switch(baudrate) { case 9600: @@ -455,6 +459,7 @@ bool Dynamixel2Arduino::setBaudrate(uint8_t id, uint32_t baudrate) case PRO_M42P_010_S260_R: case PRO_M54P_040_S250_R: case PRO_M54P_060_S250_R: + case RH_P12_RNA: switch(baudrate) { case 9600: @@ -757,7 +762,17 @@ bool Dynamixel2Arduino::setOperatingMode(uint8_t id, uint8_t mode) ret = writeControlTableItem(ControlTableItem::OPERATING_MODE, id, 16); } break; - + + case RH_P12_RN: + case RH_P12_RNA: + if (mode == OP_CURRENT) { + ret = writeControlTableItem(ControlTableItem::OPERATING_MODE, id, 0); + } + else if (mode == OP_CURRENT_BASED_POSITION) { + ret = writeControlTableItem(ControlTableItem::OPERATING_MODE, id, 5); + } + break; + default: break; } diff --git a/src/actuator.cpp b/src/actuator.cpp index f30348e..fd3c602 100644 --- a/src/actuator.cpp +++ b/src/actuator.cpp @@ -349,7 +349,8 @@ const ModelControlTableInfo_t pro_r_control_table[] PROGMEM = { const ModelControlTableInfo_t pro_ra_pro_plus_control_table[] PROGMEM = { #if (ENABLE_ACTUATOR_PRO_RA\ - || ENABLE_ACTUATOR_PRO_PLUS) + || ENABLE_ACTUATOR_PRO_PLUS\ + || ENABLE_ACTUATOR_RH_P12_RNA) {ControlTableItem::MODEL_NUMBER, 0, 2}, {ControlTableItem::MODEL_INFORMATION, 2, 4}, {ControlTableItem::FIRMWARE_VERSION, 6, 1}, @@ -416,6 +417,58 @@ const ModelControlTableInfo_t pro_ra_pro_plus_control_table[] PROGMEM = { {ControlTableItem::LAST_DUMMY_ITEM, 0, 0} }; +const ModelControlTableInfo_t rh_p12_rn_control_table[] PROGMEM = { +#if (ENABLE_ACTUATOR_RH_P12_RN) + {ControlTableItem::MODEL_NUMBER, 0, 2}, + {ControlTableItem::MODEL_INFORMATION, 2, 4}, + {ControlTableItem::FIRMWARE_VERSION, 6, 1}, + {ControlTableItem::ID, 7, 1}, + {ControlTableItem::BAUD_RATE, 8, 1}, + {ControlTableItem::RETURN_DELAY_TIME, 9, 1}, + {ControlTableItem::OPERATING_MODE, 11, 1}, + {ControlTableItem::MOVING_THRESHOLD, 17, 4}, + {ControlTableItem::TEMPERATURE_LIMIT, 21, 1}, + {ControlTableItem::MAX_VOLTAGE_LIMIT, 22, 2}, + {ControlTableItem::MIN_VOLTAGE_LIMIT, 24, 2}, + {ControlTableItem::ACCELERATION_LIMIT, 26, 4}, + {ControlTableItem::CURRENT_LIMIT, 30, 2}, + {ControlTableItem::VELOCITY_LIMIT, 32, 4}, + {ControlTableItem::MAX_POSITION_LIMIT, 36, 4}, + {ControlTableItem::MIN_POSITION_LIMIT, 40, 4}, + {ControlTableItem::EXTERNAL_PORT_MODE_1, 44, 1}, + {ControlTableItem::EXTERNAL_PORT_MODE_2, 45, 1}, + {ControlTableItem::EXTERNAL_PORT_MODE_3, 46, 1}, + {ControlTableItem::EXTERNAL_PORT_MODE_4, 47, 1}, + {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::POSITION_D_GAIN, 590, 2}, + {ControlTableItem::POSITION_I_GAIN, 592, 2}, + {ControlTableItem::POSITION_P_GAIN, 594, 2}, + {ControlTableItem::GOAL_POSITION, 596, 4}, + {ControlTableItem::GOAL_VELOCITY, 600, 4}, + {ControlTableItem::GOAL_CURRENT, 604, 2}, + {ControlTableItem::GOAL_ACCELERATION, 606, 4}, + {ControlTableItem::MOVING, 610, 1}, + {ControlTableItem::PRESENT_POSITION, 611, 4}, + {ControlTableItem::PRESENT_VELOCITY, 615, 4}, + {ControlTableItem::PRESENT_CURRENT, 621, 2}, + {ControlTableItem::PRESENT_INPUT_VOLTAGE, 623, 2}, + {ControlTableItem::PRESENT_TEMPERATURE, 625, 1}, + {ControlTableItem::EXTERNAL_PORT_DATA_1, 600, 2}, + {ControlTableItem::EXTERNAL_PORT_DATA_2, 602, 2}, + {ControlTableItem::EXTERNAL_PORT_DATA_3, 604, 2}, + {ControlTableItem::EXTERNAL_PORT_DATA_4, 606, 2}, + {ControlTableItem::STATUS_RETURN_LEVEL, 891, 1}, + {ControlTableItem::HARDWARE_ERROR_STATUS, 892, 1}, + +#endif + {ControlTableItem::LAST_DUMMY_ITEM, 0, 0} +}; + ControlTableItemInfo_t DYNAMIXEL::getControlTableItemInfo(uint16_t model_num, uint8_t control_item) { uint8_t item_idx, i = 0; @@ -547,9 +600,14 @@ ControlTableItemInfo_t DYNAMIXEL::getControlTableItemInfo(uint16_t model_num, ui case PRO_M42P_010_S260_R: case PRO_M54P_040_S250_R: case PRO_M54P_060_S250_R: + case RH_P12_RNA: p_common_ctable = pro_ra_pro_plus_control_table; break; - + + case RH_P12_RN: + p_common_ctable = rh_p12_rn_control_table; + break; + default: break; } diff --git a/src/actuator.h b/src/actuator.h index dcef381..fc23b0d 100644 --- a/src/actuator.h +++ b/src/actuator.h @@ -180,6 +180,14 @@ #ifndef PRO_L42_10_S300_R #define PRO_L42_10_S300_R (uint16_t)35072 #endif + +#ifndef RH_P12_RN +#define RH_P12_RN (uint16_t)35073 +#endif +#ifndef RH_P12_RNA +#define RH_P12_RNA (uint16_t)35074 +#endif + #ifndef PRO_L54_30_S400_R #define PRO_L54_30_S400_R (uint16_t)37928 #endif diff --git a/src/utility/config.h b/src/utility/config.h index a12d246..fc17eae 100644 --- a/src/utility/config.h +++ b/src/utility/config.h @@ -33,6 +33,9 @@ #define ENABLE_ACTUATOR_PRO_RA 1 #define ENABLE_ACTUATOR_PRO_PLUS 1 + +#define ENABLE_ACTUATOR_RH_P12_RN 1 +#define ENABLE_ACTUATOR_RH_P12_RNA 1 #define DXL_BYTE_STUFF_SAFE_CNT 8