Skip to content

Commit

Permalink
Merge pull request #137 from ROBOTIS-GIT/feature_dy
Browse files Browse the repository at this point in the history 
Added DYNAMIXEL-Y series.
  • Loading branch information
@sookyungson
sookyungson authored Jan 21, 2025
2 parents 84b29f1 + 2143d05 commit f8a4f2a
Show file tree
Hide file tree
Showing 4 changed files with 355 additions and 6 deletions.
189 changes: 184 additions & 5 deletions src/Dynamixel2Arduino.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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]);
Expand Down Expand Up @@ -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;
Expand Down Expand Up @@ -757,16 +809,35 @@ 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;
}

return ret;
}



bool Dynamixel2Arduino::setGoalPosition(uint8_t id, float value, uint8_t unit)
{
if(unit != UNIT_RAW && unit != UNIT_DEGREE)
Expand Down Expand Up @@ -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)
{
Expand Down Expand Up @@ -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;
}
Expand Down
99 changes: 98 additions & 1 deletion src/actuator.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -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;
}
Expand Down
Loading

0 comments on commit f8a4f2a

Please sign in to comment.