diff --git a/src/Dynamixel2Arduino.cpp b/src/Dynamixel2Arduino.cpp index dc53fdf..2592292 100644 --- a/src/Dynamixel2Arduino.cpp +++ b/src/Dynamixel2Arduino.cpp @@ -60,7 +60,10 @@ const uint16_t model_number_table[] PROGMEM = { PRO_M42P_010_S260_R, 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 + PRO_H54P_100_S500_R, PRO_H54P_200_S500_R, + + 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 }; const uint8_t model_number_table_count = sizeof(model_number_table)/sizeof(model_number_table[0]); @@ -492,6 +495,55 @@ bool Dynamixel2Arduino::setBaudrate(uint8_t id, uint32_t baudrate) } break; + case YM070_210_M001_RH: + case YM070_210_B001_RH: + case YM070_210_R051_RH: + case YM070_210_R099_RH: + case YM070_210_A051_RH: + case YM070_210_A099_RH: + case YM080_230_M001_RH: + case YM080_230_B001_RH: + case YM080_230_R051_RH: + case YM080_230_R099_RH: + case YM080_230_A051_RH: + case YM080_230_A099_RH: + switch(baudrate) + { + case 9600: + baud_idx = 0; + break; + case 57600: + baud_idx = 1; + break; + case 115200: + baud_idx = 2; + break; + case 1000000: + baud_idx = 3; + break; + case 2000000: + baud_idx = 4; + break; + case 3000000: + baud_idx = 5; + break; + case 4000000: + baud_idx = 6; + break; + case 4500000: + baud_idx = 7; + break; + case 6000000: + baud_idx = 8; + break; + case 10500000: + baud_idx = 9; + break; + default: + return false; + } + break; + default: return false; break; @@ -757,7 +809,28 @@ bool Dynamixel2Arduino::setOperatingMode(uint8_t id, uint8_t mode) ret = writeControlTableItem(ControlTableItem::OPERATING_MODE, id, 16); } break; - + + case YM070_210_M001_RH: + case YM070_210_B001_RH: + case YM070_210_R051_RH: + case YM070_210_R099_RH: + case YM070_210_A051_RH: + case YM070_210_A099_RH: + case YM080_230_M001_RH: + case YM080_230_B001_RH: + case YM080_230_R051_RH: + case YM080_230_R099_RH: + case YM080_230_A051_RH: + case YM080_230_A099_RH: + if(mode == OP_POSITION){ + ret = writeControlTableItem(ControlTableItem::OPERATING_MODE, id, 3); + }else if(mode == OP_VELOCITY){ + ret = writeControlTableItem(ControlTableItem::OPERATING_MODE, id, 1); + }else if(mode == OP_CURRENT){ + ret = writeControlTableItem(ControlTableItem::OPERATING_MODE, id, 0); + } + break; + default: break; } @@ -765,8 +838,6 @@ bool Dynamixel2Arduino::setOperatingMode(uint8_t id, uint8_t mode) return ret; } - - bool Dynamixel2Arduino::setGoalPosition(uint8_t id, float value, uint8_t unit) { if(unit != UNIT_RAW && unit != UNIT_DEGREE) @@ -1450,6 +1521,89 @@ const ModelDependencyFuncItemAndRangeInfo_t dependency_pro_ra_plus_h54_200[] PRO {LAST_DUMMY_FUNC, ControlTableItem::LAST_DUMMY_ITEM, UNIT_RAW, 0, 0, 0} }; +const ModelDependencyFuncItemAndRangeInfo_t dependency_ym070_210_000_rh[] PROGMEM = { +#if (ENABLE_ACTUATOR_Y) + {SET_POSITION, GOAL_POSITION, UNIT_DEGREE, -2147483648, 2147483647, 0.0006866455}, + {GET_POSITION, PRESENT_POSITION, UNIT_DEGREE, -2147483648 , 2147483647, 0.0006866455}, + + {SET_VELOCITY, GOAL_VELOCITY, UNIT_RPM, -642200, 642200, 0.01}, + {GET_VELOCITY, PRESENT_VELOCITY, UNIT_RPM, -642200, 642200, 0.01}, + + {SET_CURRENT, GOAL_CURRENT, UNIT_MILLI_AMPERE, -2080, 2080, 0.01}, + {GET_CURRENT, PRESENT_CURRENT, UNIT_MILLI_AMPERE, -2080, 2080, 0.01}, +#endif + {LAST_DUMMY_FUNC, ControlTableItem::LAST_DUMMY_ITEM, UNIT_RAW, 0, 0, 0} +}; + +const ModelDependencyFuncItemAndRangeInfo_t dependency_ym070_210_051_rh[] PROGMEM = { +#if (ENABLE_ACTUATOR_Y) + {SET_POSITION, GOAL_POSITION, UNIT_DEGREE, -2147483648, 2147483647, 0.0006866455}, + {GET_POSITION, PRESENT_POSITION, UNIT_DEGREE, -2147483648 , 2147483647, 0.0006866455}, + + {SET_VELOCITY, GOAL_VELOCITY, UNIT_RPM, -12592, 12592, 0.01}, + {GET_VELOCITY, PRESENT_VELOCITY, UNIT_RPM, -12592, 12592, 0.01}, + + {SET_CURRENT, GOAL_CURRENT, UNIT_MILLI_AMPERE, -2080, 2080, 0.01}, + {GET_CURRENT, PRESENT_CURRENT, UNIT_MILLI_AMPERE, -2080, 2080, 0.01}, +#endif + {LAST_DUMMY_FUNC, ControlTableItem::LAST_DUMMY_ITEM, UNIT_RAW, 0, 0, 0} +}; + +const ModelDependencyFuncItemAndRangeInfo_t dependency_ym070_210_099_rh[] PROGMEM = { +#if (ENABLE_ACTUATOR_Y) + {SET_POSITION, GOAL_POSITION, UNIT_DEGREE, -2147483648, 2147483647, 0.0006866455}, + {GET_POSITION, PRESENT_POSITION, UNIT_DEGREE, -2147483648 , 2147483647, 0.0006866455}, + + {SET_VELOCITY, GOAL_VELOCITY, UNIT_RPM, -6486, 6486, 0.01}, + {GET_VELOCITY, PRESENT_VELOCITY, UNIT_RPM, -6486, 6486, 0.01}, + + {SET_CURRENT, GOAL_CURRENT, UNIT_MILLI_AMPERE, -2080, 2080, 0.01}, + {GET_CURRENT, PRESENT_CURRENT, UNIT_MILLI_AMPERE, -2080, 2080, 0.01}, +#endif + {LAST_DUMMY_FUNC, ControlTableItem::LAST_DUMMY_ITEM, UNIT_RAW, 0, 0, 0} +}; + +const ModelDependencyFuncItemAndRangeInfo_t dependency_ym080_230_000_rh[] PROGMEM = { +#if (ENABLE_ACTUATOR_Y) + {SET_POSITION, GOAL_POSITION, UNIT_DEGREE, -2147483648, 2147483647, 0.0006866455}, + {GET_POSITION, PRESENT_POSITION, UNIT_DEGREE, -2147483648 , 2147483647, 0.0006866455}, + + {SET_VELOCITY, GOAL_VELOCITY, UNIT_RPM, -355600, 355600, 0.01}, + {GET_VELOCITY, PRESENT_VELOCITY, UNIT_RPM, -355600, 355600, 0.01}, + + {SET_CURRENT, GOAL_CURRENT, UNIT_MILLI_AMPERE, -2240, 2240, 0.01}, + {GET_CURRENT, PRESENT_CURRENT, UNIT_MILLI_AMPERE, -2240, 2240, 0.01}, +#endif + {LAST_DUMMY_FUNC, ControlTableItem::LAST_DUMMY_ITEM, UNIT_RAW, 0, 0, 0} +}; + +const ModelDependencyFuncItemAndRangeInfo_t dependency_ym080_230_051_rh[] PROGMEM = { +#if (ENABLE_ACTUATOR_Y) + {SET_POSITION, GOAL_POSITION, UNIT_DEGREE, -2147483648, 2147483647, 0.0006866455}, + {GET_POSITION, PRESENT_POSITION, UNIT_DEGREE, -2147483648 , 2147483647, 0.0006866455}, + + {SET_VELOCITY, GOAL_VELOCITY, UNIT_RPM, -6972, 6972, 0.01}, + {GET_VELOCITY, PRESENT_VELOCITY, UNIT_RPM, -6972, 6972, 0.01}, + + {SET_CURRENT, GOAL_CURRENT, UNIT_MILLI_AMPERE, -2240, 2240, 0.01}, + {GET_CURRENT, PRESENT_CURRENT, UNIT_MILLI_AMPERE, -2240, 2240, 0.01}, +#endif + {LAST_DUMMY_FUNC, ControlTableItem::LAST_DUMMY_ITEM, UNIT_RAW, 0, 0, 0} +}; + +const ModelDependencyFuncItemAndRangeInfo_t dependency_ym080_230_099_rh[] PROGMEM = { +#if (ENABLE_ACTUATOR_Y) + {SET_POSITION, GOAL_POSITION, UNIT_DEGREE, -2147483648, 2147483647, 0.0006866455}, + {GET_POSITION, PRESENT_POSITION, UNIT_DEGREE, -2147483648 , 2147483647, 0.0006866455}, + + {SET_VELOCITY, GOAL_VELOCITY, UNIT_RPM, -3591, 3591, 0.01}, + {GET_VELOCITY, PRESENT_VELOCITY, UNIT_RPM, -3591, 3591, 0.01}, + + {SET_CURRENT, GOAL_CURRENT, UNIT_MILLI_AMPERE, -2240, 2240, 0.01}, + {GET_CURRENT, PRESENT_CURRENT, UNIT_MILLI_AMPERE, -2240, 2240, 0.01}, +#endif + {LAST_DUMMY_FUNC, ControlTableItem::LAST_DUMMY_ITEM, UNIT_RAW, 0, 0, 0} +}; static ItemAndRangeInfo_t getModelDependencyFuncInfo(uint16_t model_num, uint8_t func_num) { @@ -1646,7 +1800,32 @@ static ItemAndRangeInfo_t getModelDependencyFuncInfo(uint16_t model_num, uint8_t p_common_ctable = dependency_ctable_pro_ra_pro_plus_model; p_dep_ctable = dependency_pro_ra_plus_h54_200; break; - + + case YM070_210_M001_RH: + case YM070_210_B001_RH: + p_common_ctable = dependency_ym070_210_000_rh; + break; + case YM070_210_R051_RH: + case YM070_210_A051_RH: + p_common_ctable = dependency_ym070_210_051_rh; + break; + case YM070_210_R099_RH: + case YM070_210_A099_RH: + p_common_ctable = dependency_ym070_210_099_rh; + break; + case YM080_230_M001_RH: + case YM080_230_B001_RH: + p_common_ctable = dependency_ym080_230_000_rh; + break; + case YM080_230_R051_RH: + case YM080_230_A051_RH: + p_common_ctable = dependency_ym080_230_051_rh; + break; + case YM080_230_R099_RH: + case YM080_230_A099_RH: + p_common_ctable = dependency_ym080_230_099_rh; + break; + default: break; } diff --git a/src/actuator.cpp b/src/actuator.cpp index f30348e..59a5319 100644 --- a/src/actuator.cpp +++ b/src/actuator.cpp @@ -416,6 +416,88 @@ const ModelControlTableInfo_t pro_ra_pro_plus_control_table[] PROGMEM = { {ControlTableItem::LAST_DUMMY_ITEM, 0, 0} }; +const ModelControlTableInfo_t y_control_table[] PROGMEM = { +#if (ENABLE_ACTUATOR_Y) + {ControlTableItem::MODEL_NUMBER, 0, 2}, + {ControlTableItem::MODEL_INFORMATION, 2, 4}, + {ControlTableItem::FIRMWARE_VERSION, 6, 1}, + {ControlTableItem::ID, 7, 1}, + {ControlTableItem::BUS_WATCHDOG, 8, 2}, + {ControlTableItem::SECONDARY_ID, 10, 1}, + {ControlTableItem::BAUD_RATE, 12, 1}, + {ControlTableItem::RETURN_DELAY_TIME, 13, 1}, + {ControlTableItem::STATUS_RETURN_LEVEL, 15, 1}, + {ControlTableItem::REGISTERED_INSTRUCTION, 16, 1}, + {ControlTableItem::DRIVE_MODE, 32, 1}, + {ControlTableItem::OPERATING_MODE, 33, 1}, + {ControlTableItem::STARTUP_CONFIGURATION, 34, 1}, + {ControlTableItem::POSITION_LIMIT_THRESHOLD, 38, 2}, + {ControlTableItem::IN_POSITION_THRESHOLD, 40, 4}, + {ControlTableItem::FOLLOWING_ERROR_THRESHOLD, 44, 4}, + {ControlTableItem::MOVING_THRESHOLD, 48, 4}, + {ControlTableItem::HOMING_OFFSET, 52, 4}, + {ControlTableItem::INVERTER_TEMPERATURE_LIMIT, 56, 1}, + {ControlTableItem::MOTOR_TEMPERATURE_LIMIT, 57, 1}, + {ControlTableItem::MAX_VOLTAGE_LIMIT, 60, 2}, + {ControlTableItem::MIN_VOLTAGE_LIMIT, 62, 2}, + {ControlTableItem::PWM_LIMIT, 64, 2}, + {ControlTableItem::CURRENT_LIMIT, 66, 2}, + {ControlTableItem::ACCELERATION_LIMIT, 68, 4}, + {ControlTableItem::VELOCITY_LIMIT, 72, 4}, + {ControlTableItem::MAX_POSITION_LIMIT, 76, 4}, + {ControlTableItem::MIN_POSITION_LIMIT, 84, 4}, + {ControlTableItem::ELECTRONIC_GEAR_RATIO_NUMERATOR, 96, 4}, + {ControlTableItem::ELECTRONIC_GEAR_RATIO_DENOMINATOR, 100, 4}, + {ControlTableItem::Safe_STOP_TIME, 104, 2}, + {ControlTableItem::BRAKE_DELAY, 106, 2}, + {ControlTableItem::GOAL_UPDATE_DELAY, 108, 2}, + {ControlTableItem::OVEREXCITATION_VOLTAGE, 110, 1}, + {ControlTableItem::NORMAL_EXCITATION_VOLTAGE, 111, 1}, + {ControlTableItem::OVEREXCITATION_TIME, 112, 2}, + {ControlTableItem::PRESENT_VELOCITY_LPF_FREQUENCY, 132, 2}, + {ControlTableItem::GOAL_CURRENT_LPF_FREQUENCY, 134, 2}, + {ControlTableItem::POSITION_FF_LPF_TIME, 136, 2}, + {ControlTableItem::VELOCITY_FF_LPF_TIME, 138, 2}, + {ControlTableItem::CONTROLLER_STATE, 152, 1}, + {ControlTableItem::ERROR_CODE, 153, 1}, + {ControlTableItem::ERROR_CODE_HISTORY1, 154, 1}, + + {ControlTableItem::HYBRID_SAVE, 170, 1}, + {ControlTableItem::VELOCITY_I_GAIN, 212, 4}, + {ControlTableItem::VELOCITY_P_GAIN, 216, 4}, + {ControlTableItem::FEEDFORWARD_2ND_GAIN, 220, 4}, // Velocity FF Gain + {ControlTableItem::POSITION_D_GAIN, 224, 4}, + {ControlTableItem::POSITION_I_GAIN, 228, 4}, + {ControlTableItem::POSITION_P_GAIN, 232, 4}, + {ControlTableItem::FEEDFORWARD_1ST_GAIN, 236, 4}, // Position FF Gain + {ControlTableItem::PROFILE_ACCELERATION, 240, 4}, + {ControlTableItem::PROFILE_VELOCITY, 244, 4}, + {ControlTableItem::PROFILE_ACCELERATION_TIME, 248, 4}, + {ControlTableItem::PROFIIE_TIME, 252, 4}, + {ControlTableItem::TORQUE_ENABLE, 512, 1}, + {ControlTableItem::LED, 513, 1}, + {ControlTableItem::PWM_OFFSET, 516, 2}, + {ControlTableItem::CURRENT_OFFSET, 518, 2}, + {ControlTableItem::VELOCITY_OFFSET, 520, 4}, + {ControlTableItem::GOAL_PWM, 524, 2}, + {ControlTableItem::GOAL_CURRENT, 526, 2}, + {ControlTableItem::GOAL_VELOCITY, 528, 4}, + {ControlTableItem::GOAL_POSITION, 532, 4}, + {ControlTableItem::MOVING_STATUS, 541, 1}, + {ControlTableItem::REALTIME_TICK, 542, 2}, + {ControlTableItem::PRESENT_PWM, 544, 2}, + {ControlTableItem::PRESENT_CURRENT, 546, 2}, + {ControlTableItem::PRESENT_VELOCITY, 548, 4}, + {ControlTableItem::PRESENT_POSITION, 552, 4}, + {ControlTableItem::POSITION_TRAJECTORY, 560, 4}, + {ControlTableItem::VELOCITY_TRAJECTORY, 564, 4}, + {ControlTableItem::PRESENT_INPUT_VOLTAGE, 568, 2}, + {ControlTableItem::PRESENT_TEMPERATURE, 570, 1}, // Present Inverter Temperature + {ControlTableItem::PRESENT_MOTOR_TEMPERATURE, 571, 4}, +#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; @@ -549,7 +631,22 @@ ControlTableItemInfo_t DYNAMIXEL::getControlTableItemInfo(uint16_t model_num, ui case PRO_M54P_060_S250_R: p_common_ctable = pro_ra_pro_plus_control_table; break; - + + case YM070_210_M001_RH: + case YM070_210_B001_RH: + case YM070_210_R051_RH: + case YM070_210_R099_RH: + case YM070_210_A051_RH: + case YM070_210_A099_RH: + case YM080_230_M001_RH: + case YM080_230_B001_RH: + case YM080_230_R051_RH: + case YM080_230_R099_RH: + case YM080_230_A051_RH: + case YM080_230_A099_RH: + p_common_ctable = y_control_table; + break; + default: break; } diff --git a/src/actuator.h b/src/actuator.h index dcef381..ef22371 100644 --- a/src/actuator.h +++ b/src/actuator.h @@ -253,6 +253,46 @@ #define PRO_M54P_060_S250_R (uint16_t)2120 #endif +#ifndef YM070_210_M001_RH +#define YM070_210_M001_RH (uint16_t)4000 +#endif +#ifndef YM070_210_B001_RH +#define YM070_210_B001_RH (uint16_t)4010 +#endif +#ifndef YM070_210_R051_RH +#define YM070_210_R051_RH (uint16_t)4020 +#endif +#ifndef YM070_210_R099_RH +#define YM070_210_R099_RH (uint16_t)4030 +#endif +#ifndef YM070_210_A051_RH +#define YM070_210_A051_RH (uint16_t)4040 +#endif +#ifndef YM070_210_A099_RH +#define YM070_210_A099_RH (uint16_t)4050 +#endif + +#ifndef YM080_230_M001_RH +#define YM080_230_M001_RH (uint16_t)4120 +#endif +#ifndef YM080_230_B001_RH +#define YM080_230_B001_RH (uint16_t)4130 +#endif +#ifndef YM080_230_R051_RH +#define YM080_230_R051_RH (uint16_t)4140 +#endif +#ifndef YM080_230_R099_RH +#define YM080_230_R099_RH (uint16_t)4150 +#endif +#ifndef YM080_230_A051_RH +#define YM080_230_A051_RH (uint16_t)4160 +#endif +#ifndef YM080_230_A099_RH +#define YM080_230_A099_RH (uint16_t)4170 +#endif + + + namespace ControlTableItem{ enum ControlTableItemIndex{ MODEL_NUMBER = 0, @@ -347,6 +387,37 @@ namespace ControlTableItem{ EXTERNAL_PORT_DATA_3, EXTERNAL_PORT_DATA_4, + //for DY + STARTUP_CONFIGURATION, + POSITION_LIMIT_THRESHOLD, + IN_POSITION_THRESHOLD, + FOLLOWING_ERROR_THRESHOLD, + INVERTER_TEMPERATURE_LIMIT, + MOTOR_TEMPERATURE_LIMIT, + ELECTRONIC_GEAR_RATIO_NUMERATOR, + ELECTRONIC_GEAR_RATIO_DENOMINATOR, + Safe_STOP_TIME, + BRAKE_DELAY, + GOAL_UPDATE_DELAY, + OVEREXCITATION_VOLTAGE, + NORMAL_EXCITATION_VOLTAGE, + OVEREXCITATION_TIME, + PRESENT_VELOCITY_LPF_FREQUENCY, + GOAL_CURRENT_LPF_FREQUENCY, + POSITION_FF_LPF_TIME, + VELOCITY_FF_LPF_TIME, + CONTROLLER_STATE, + ERROR_CODE, + ERROR_CODE_HISTORY1, + HYBRID_SAVE, + PROFILE_ACCELERATION_TIME, + PROFIIE_TIME, + PWM_OFFSET, + CURRENT_OFFSET, + VELOCITY_OFFSET, + PRESENT_MOTOR_TEMPERATURE, + + LAST_DUMMY_ITEM = 0xFF }; } diff --git a/src/utility/config.h b/src/utility/config.h index a12d246..eac11b0 100644 --- a/src/utility/config.h +++ b/src/utility/config.h @@ -33,6 +33,8 @@ #define ENABLE_ACTUATOR_PRO_RA 1 #define ENABLE_ACTUATOR_PRO_PLUS 1 +#define ENABLE_ACTUATOR_Y 1 + #define DXL_BYTE_STUFF_SAFE_CNT 8