Skip to content

Commit 2143d05

Browse files
sookyungsonsookyungson
committed
Added DYNAMIXEL-Y series.
1 parent 84b29f1 commit 2143d05

File tree

4 files changed

+355
-6
lines changed

4 files changed

+355
-6
lines changed

‎src/Dynamixel2Arduino.cpp

+184-5
Original file line numberDiff line numberDiff line change
@@ -60,7 +60,10 @@ const uint16_t model_number_table[] PROGMEM = {
6060
PRO_M42P_010_S260_R,
6161
PRO_M54P_040_S250_R, PRO_M54P_060_S250_R,
6262
PRO_H42P_020_S300_R,
63-
PRO_H54P_100_S500_R, PRO_H54P_200_S500_R
63+
PRO_H54P_100_S500_R, PRO_H54P_200_S500_R,
64+
65+
YM070_210_M001_RH, YM070_210_B001_RH, YM070_210_R051_RH, YM070_210_R099_RH, YM070_210_A051_RH, YM070_210_A099_RH,
66+
YM080_230_M001_RH, YM080_230_B001_RH, YM080_230_R051_RH, YM080_230_R099_RH, YM080_230_A051_RH, YM080_230_A099_RH
6467
};
6568

6669
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)
492495
}
493496
break;
494497

498+
case YM070_210_M001_RH:
499+
case YM070_210_B001_RH:
500+
case YM070_210_R051_RH:
501+
case YM070_210_R099_RH:
502+
case YM070_210_A051_RH:
503+
case YM070_210_A099_RH:
504+
case YM080_230_M001_RH:
505+
case YM080_230_B001_RH:
506+
case YM080_230_R051_RH:
507+
case YM080_230_R099_RH:
508+
case YM080_230_A051_RH:
509+
case YM080_230_A099_RH:
510+
switch(baudrate)
511+
{
512+
case 9600:
513+
baud_idx = 0;
514+
break;
515+
case 57600:
516+
baud_idx = 1;
517+
break;
518+
case 115200:
519+
baud_idx = 2;
520+
break;
521+
case 1000000:
522+
baud_idx = 3;
523+
break;
524+
case 2000000:
525+
baud_idx = 4;
526+
break;
527+
case 3000000:
528+
baud_idx = 5;
529+
break;
530+
case 4000000:
531+
baud_idx = 6;
532+
break;
533+
case 4500000:
534+
baud_idx = 7;
535+
break;
536+
case 6000000:
537+
baud_idx = 8;
538+
break;
539+
case 10500000:
540+
baud_idx = 9;
541+
break;
542+
default:
543+
return false;
544+
}
545+
break;
546+
495547
default:
496548
return false;
497549
break;
@@ -757,16 +809,35 @@ bool Dynamixel2Arduino::setOperatingMode(uint8_t id, uint8_t mode)
757809
ret = writeControlTableItem(ControlTableItem::OPERATING_MODE, id, 16);
758810
}
759811
break;
760-
812+
813+
case YM070_210_M001_RH:
814+
case YM070_210_B001_RH:
815+
case YM070_210_R051_RH:
816+
case YM070_210_R099_RH:
817+
case YM070_210_A051_RH:
818+
case YM070_210_A099_RH:
819+
case YM080_230_M001_RH:
820+
case YM080_230_B001_RH:
821+
case YM080_230_R051_RH:
822+
case YM080_230_R099_RH:
823+
case YM080_230_A051_RH:
824+
case YM080_230_A099_RH:
825+
if(mode == OP_POSITION){
826+
ret = writeControlTableItem(ControlTableItem::OPERATING_MODE, id, 3);
827+
}else if(mode == OP_VELOCITY){
828+
ret = writeControlTableItem(ControlTableItem::OPERATING_MODE, id, 1);
829+
}else if(mode == OP_CURRENT){
830+
ret = writeControlTableItem(ControlTableItem::OPERATING_MODE, id, 0);
831+
}
832+
break;
833+
761834
default:
762835
break;
763836
}
764837

765838
return ret;
766839
}
767840

768-
769-
770841
bool Dynamixel2Arduino::setGoalPosition(uint8_t id, float value, uint8_t unit)
771842
{
772843
if(unit != UNIT_RAW && unit != UNIT_DEGREE)
@@ -1450,6 +1521,89 @@ const ModelDependencyFuncItemAndRangeInfo_t dependency_pro_ra_plus_h54_200[] PRO
14501521
{LAST_DUMMY_FUNC, ControlTableItem::LAST_DUMMY_ITEM, UNIT_RAW, 0, 0, 0}
14511522
};
14521523

1524+
const ModelDependencyFuncItemAndRangeInfo_t dependency_ym070_210_000_rh[] PROGMEM = {
1525+
#if (ENABLE_ACTUATOR_Y)
1526+
{SET_POSITION, GOAL_POSITION, UNIT_DEGREE, -2147483648, 2147483647, 0.0006866455},
1527+
{GET_POSITION, PRESENT_POSITION, UNIT_DEGREE, -2147483648 , 2147483647, 0.0006866455},
1528+
1529+
{SET_VELOCITY, GOAL_VELOCITY, UNIT_RPM, -642200, 642200, 0.01},
1530+
{GET_VELOCITY, PRESENT_VELOCITY, UNIT_RPM, -642200, 642200, 0.01},
1531+
1532+
{SET_CURRENT, GOAL_CURRENT, UNIT_MILLI_AMPERE, -2080, 2080, 0.01},
1533+
{GET_CURRENT, PRESENT_CURRENT, UNIT_MILLI_AMPERE, -2080, 2080, 0.01},
1534+
#endif
1535+
{LAST_DUMMY_FUNC, ControlTableItem::LAST_DUMMY_ITEM, UNIT_RAW, 0, 0, 0}
1536+
};
1537+
1538+
const ModelDependencyFuncItemAndRangeInfo_t dependency_ym070_210_051_rh[] PROGMEM = {
1539+
#if (ENABLE_ACTUATOR_Y)
1540+
{SET_POSITION, GOAL_POSITION, UNIT_DEGREE, -2147483648, 2147483647, 0.0006866455},
1541+
{GET_POSITION, PRESENT_POSITION, UNIT_DEGREE, -2147483648 , 2147483647, 0.0006866455},
1542+
1543+
{SET_VELOCITY, GOAL_VELOCITY, UNIT_RPM, -12592, 12592, 0.01},
1544+
{GET_VELOCITY, PRESENT_VELOCITY, UNIT_RPM, -12592, 12592, 0.01},
1545+
1546+
{SET_CURRENT, GOAL_CURRENT, UNIT_MILLI_AMPERE, -2080, 2080, 0.01},
1547+
{GET_CURRENT, PRESENT_CURRENT, UNIT_MILLI_AMPERE, -2080, 2080, 0.01},
1548+
#endif
1549+
{LAST_DUMMY_FUNC, ControlTableItem::LAST_DUMMY_ITEM, UNIT_RAW, 0, 0, 0}
1550+
};
1551+
1552+
const ModelDependencyFuncItemAndRangeInfo_t dependency_ym070_210_099_rh[] PROGMEM = {
1553+
#if (ENABLE_ACTUATOR_Y)
1554+
{SET_POSITION, GOAL_POSITION, UNIT_DEGREE, -2147483648, 2147483647, 0.0006866455},
1555+
{GET_POSITION, PRESENT_POSITION, UNIT_DEGREE, -2147483648 , 2147483647, 0.0006866455},
1556+
1557+
{SET_VELOCITY, GOAL_VELOCITY, UNIT_RPM, -6486, 6486, 0.01},
1558+
{GET_VELOCITY, PRESENT_VELOCITY, UNIT_RPM, -6486, 6486, 0.01},
1559+
1560+
{SET_CURRENT, GOAL_CURRENT, UNIT_MILLI_AMPERE, -2080, 2080, 0.01},
1561+
{GET_CURRENT, PRESENT_CURRENT, UNIT_MILLI_AMPERE, -2080, 2080, 0.01},
1562+
#endif
1563+
{LAST_DUMMY_FUNC, ControlTableItem::LAST_DUMMY_ITEM, UNIT_RAW, 0, 0, 0}
1564+
};
1565+
1566+
const ModelDependencyFuncItemAndRangeInfo_t dependency_ym080_230_000_rh[] PROGMEM = {
1567+
#if (ENABLE_ACTUATOR_Y)
1568+
{SET_POSITION, GOAL_POSITION, UNIT_DEGREE, -2147483648, 2147483647, 0.0006866455},
1569+
{GET_POSITION, PRESENT_POSITION, UNIT_DEGREE, -2147483648 , 2147483647, 0.0006866455},
1570+
1571+
{SET_VELOCITY, GOAL_VELOCITY, UNIT_RPM, -355600, 355600, 0.01},
1572+
{GET_VELOCITY, PRESENT_VELOCITY, UNIT_RPM, -355600, 355600, 0.01},
1573+
1574+
{SET_CURRENT, GOAL_CURRENT, UNIT_MILLI_AMPERE, -2240, 2240, 0.01},
1575+
{GET_CURRENT, PRESENT_CURRENT, UNIT_MILLI_AMPERE, -2240, 2240, 0.01},
1576+
#endif
1577+
{LAST_DUMMY_FUNC, ControlTableItem::LAST_DUMMY_ITEM, UNIT_RAW, 0, 0, 0}
1578+
};
1579+
1580+
const ModelDependencyFuncItemAndRangeInfo_t dependency_ym080_230_051_rh[] PROGMEM = {
1581+
#if (ENABLE_ACTUATOR_Y)
1582+
{SET_POSITION, GOAL_POSITION, UNIT_DEGREE, -2147483648, 2147483647, 0.0006866455},
1583+
{GET_POSITION, PRESENT_POSITION, UNIT_DEGREE, -2147483648 , 2147483647, 0.0006866455},
1584+
1585+
{SET_VELOCITY, GOAL_VELOCITY, UNIT_RPM, -6972, 6972, 0.01},
1586+
{GET_VELOCITY, PRESENT_VELOCITY, UNIT_RPM, -6972, 6972, 0.01},
1587+
1588+
{SET_CURRENT, GOAL_CURRENT, UNIT_MILLI_AMPERE, -2240, 2240, 0.01},
1589+
{GET_CURRENT, PRESENT_CURRENT, UNIT_MILLI_AMPERE, -2240, 2240, 0.01},
1590+
#endif
1591+
{LAST_DUMMY_FUNC, ControlTableItem::LAST_DUMMY_ITEM, UNIT_RAW, 0, 0, 0}
1592+
};
1593+
1594+
const ModelDependencyFuncItemAndRangeInfo_t dependency_ym080_230_099_rh[] PROGMEM = {
1595+
#if (ENABLE_ACTUATOR_Y)
1596+
{SET_POSITION, GOAL_POSITION, UNIT_DEGREE, -2147483648, 2147483647, 0.0006866455},
1597+
{GET_POSITION, PRESENT_POSITION, UNIT_DEGREE, -2147483648 , 2147483647, 0.0006866455},
1598+
1599+
{SET_VELOCITY, GOAL_VELOCITY, UNIT_RPM, -3591, 3591, 0.01},
1600+
{GET_VELOCITY, PRESENT_VELOCITY, UNIT_RPM, -3591, 3591, 0.01},
1601+
1602+
{SET_CURRENT, GOAL_CURRENT, UNIT_MILLI_AMPERE, -2240, 2240, 0.01},
1603+
{GET_CURRENT, PRESENT_CURRENT, UNIT_MILLI_AMPERE, -2240, 2240, 0.01},
1604+
#endif
1605+
{LAST_DUMMY_FUNC, ControlTableItem::LAST_DUMMY_ITEM, UNIT_RAW, 0, 0, 0}
1606+
};
14531607

14541608
static ItemAndRangeInfo_t getModelDependencyFuncInfo(uint16_t model_num, uint8_t func_num)
14551609
{
@@ -1646,7 +1800,32 @@ static ItemAndRangeInfo_t getModelDependencyFuncInfo(uint16_t model_num, uint8_t
16461800
p_common_ctable = dependency_ctable_pro_ra_pro_plus_model;
16471801
p_dep_ctable = dependency_pro_ra_plus_h54_200;
16481802
break;
1649-
1803+
1804+
case YM070_210_M001_RH:
1805+
case YM070_210_B001_RH:
1806+
p_common_ctable = dependency_ym070_210_000_rh;
1807+
break;
1808+
case YM070_210_R051_RH:
1809+
case YM070_210_A051_RH:
1810+
p_common_ctable = dependency_ym070_210_051_rh;
1811+
break;
1812+
case YM070_210_R099_RH:
1813+
case YM070_210_A099_RH:
1814+
p_common_ctable = dependency_ym070_210_099_rh;
1815+
break;
1816+
case YM080_230_M001_RH:
1817+
case YM080_230_B001_RH:
1818+
p_common_ctable = dependency_ym080_230_000_rh;
1819+
break;
1820+
case YM080_230_R051_RH:
1821+
case YM080_230_A051_RH:
1822+
p_common_ctable = dependency_ym080_230_051_rh;
1823+
break;
1824+
case YM080_230_R099_RH:
1825+
case YM080_230_A099_RH:
1826+
p_common_ctable = dependency_ym080_230_099_rh;
1827+
break;
1828+
16501829
default:
16511830
break;
16521831
}

‎src/actuator.cpp

+98-1
Original file line numberDiff line numberDiff line change
@@ -416,6 +416,88 @@ const ModelControlTableInfo_t pro_ra_pro_plus_control_table[] PROGMEM = {
416416
{ControlTableItem::LAST_DUMMY_ITEM, 0, 0}
417417
};
418418

419+
const ModelControlTableInfo_t y_control_table[] PROGMEM = {
420+
#if (ENABLE_ACTUATOR_Y)
421+
{ControlTableItem::MODEL_NUMBER, 0, 2},
422+
{ControlTableItem::MODEL_INFORMATION, 2, 4},
423+
{ControlTableItem::FIRMWARE_VERSION, 6, 1},
424+
{ControlTableItem::ID, 7, 1},
425+
{ControlTableItem::BUS_WATCHDOG, 8, 2},
426+
{ControlTableItem::SECONDARY_ID, 10, 1},
427+
{ControlTableItem::BAUD_RATE, 12, 1},
428+
{ControlTableItem::RETURN_DELAY_TIME, 13, 1},
429+
{ControlTableItem::STATUS_RETURN_LEVEL, 15, 1},
430+
{ControlTableItem::REGISTERED_INSTRUCTION, 16, 1},
431+
{ControlTableItem::DRIVE_MODE, 32, 1},
432+
{ControlTableItem::OPERATING_MODE, 33, 1},
433+
{ControlTableItem::STARTUP_CONFIGURATION, 34, 1},
434+
{ControlTableItem::POSITION_LIMIT_THRESHOLD, 38, 2},
435+
{ControlTableItem::IN_POSITION_THRESHOLD, 40, 4},
436+
{ControlTableItem::FOLLOWING_ERROR_THRESHOLD, 44, 4},
437+
{ControlTableItem::MOVING_THRESHOLD, 48, 4},
438+
{ControlTableItem::HOMING_OFFSET, 52, 4},
439+
{ControlTableItem::INVERTER_TEMPERATURE_LIMIT, 56, 1},
440+
{ControlTableItem::MOTOR_TEMPERATURE_LIMIT, 57, 1},
441+
{ControlTableItem::MAX_VOLTAGE_LIMIT, 60, 2},
442+
{ControlTableItem::MIN_VOLTAGE_LIMIT, 62, 2},
443+
{ControlTableItem::PWM_LIMIT, 64, 2},
444+
{ControlTableItem::CURRENT_LIMIT, 66, 2},
445+
{ControlTableItem::ACCELERATION_LIMIT, 68, 4},
446+
{ControlTableItem::VELOCITY_LIMIT, 72, 4},
447+
{ControlTableItem::MAX_POSITION_LIMIT, 76, 4},
448+
{ControlTableItem::MIN_POSITION_LIMIT, 84, 4},
449+
{ControlTableItem::ELECTRONIC_GEAR_RATIO_NUMERATOR, 96, 4},
450+
{ControlTableItem::ELECTRONIC_GEAR_RATIO_DENOMINATOR, 100, 4},
451+
{ControlTableItem::Safe_STOP_TIME, 104, 2},
452+
{ControlTableItem::BRAKE_DELAY, 106, 2},
453+
{ControlTableItem::GOAL_UPDATE_DELAY, 108, 2},
454+
{ControlTableItem::OVEREXCITATION_VOLTAGE, 110, 1},
455+
{ControlTableItem::NORMAL_EXCITATION_VOLTAGE, 111, 1},
456+
{ControlTableItem::OVEREXCITATION_TIME, 112, 2},
457+
{ControlTableItem::PRESENT_VELOCITY_LPF_FREQUENCY, 132, 2},
458+
{ControlTableItem::GOAL_CURRENT_LPF_FREQUENCY, 134, 2},
459+
{ControlTableItem::POSITION_FF_LPF_TIME, 136, 2},
460+
{ControlTableItem::VELOCITY_FF_LPF_TIME, 138, 2},
461+
{ControlTableItem::CONTROLLER_STATE, 152, 1},
462+
{ControlTableItem::ERROR_CODE, 153, 1},
463+
{ControlTableItem::ERROR_CODE_HISTORY1, 154, 1},
464+
465+
{ControlTableItem::HYBRID_SAVE, 170, 1},
466+
{ControlTableItem::VELOCITY_I_GAIN, 212, 4},
467+
{ControlTableItem::VELOCITY_P_GAIN, 216, 4},
468+
{ControlTableItem::FEEDFORWARD_2ND_GAIN, 220, 4}, // Velocity FF Gain
469+
{ControlTableItem::POSITION_D_GAIN, 224, 4},
470+
{ControlTableItem::POSITION_I_GAIN, 228, 4},
471+
{ControlTableItem::POSITION_P_GAIN, 232, 4},
472+
{ControlTableItem::FEEDFORWARD_1ST_GAIN, 236, 4}, // Position FF Gain
473+
{ControlTableItem::PROFILE_ACCELERATION, 240, 4},
474+
{ControlTableItem::PROFILE_VELOCITY, 244, 4},
475+
{ControlTableItem::PROFILE_ACCELERATION_TIME, 248, 4},
476+
{ControlTableItem::PROFIIE_TIME, 252, 4},
477+
{ControlTableItem::TORQUE_ENABLE, 512, 1},
478+
{ControlTableItem::LED, 513, 1},
479+
{ControlTableItem::PWM_OFFSET, 516, 2},
480+
{ControlTableItem::CURRENT_OFFSET, 518, 2},
481+
{ControlTableItem::VELOCITY_OFFSET, 520, 4},
482+
{ControlTableItem::GOAL_PWM, 524, 2},
483+
{ControlTableItem::GOAL_CURRENT, 526, 2},
484+
{ControlTableItem::GOAL_VELOCITY, 528, 4},
485+
{ControlTableItem::GOAL_POSITION, 532, 4},
486+
{ControlTableItem::MOVING_STATUS, 541, 1},
487+
{ControlTableItem::REALTIME_TICK, 542, 2},
488+
{ControlTableItem::PRESENT_PWM, 544, 2},
489+
{ControlTableItem::PRESENT_CURRENT, 546, 2},
490+
{ControlTableItem::PRESENT_VELOCITY, 548, 4},
491+
{ControlTableItem::PRESENT_POSITION, 552, 4},
492+
{ControlTableItem::POSITION_TRAJECTORY, 560, 4},
493+
{ControlTableItem::VELOCITY_TRAJECTORY, 564, 4},
494+
{ControlTableItem::PRESENT_INPUT_VOLTAGE, 568, 2},
495+
{ControlTableItem::PRESENT_TEMPERATURE, 570, 1}, // Present Inverter Temperature
496+
{ControlTableItem::PRESENT_MOTOR_TEMPERATURE, 571, 4},
497+
#endif
498+
{ControlTableItem::LAST_DUMMY_ITEM, 0, 0}
499+
};
500+
419501
ControlTableItemInfo_t DYNAMIXEL::getControlTableItemInfo(uint16_t model_num, uint8_t control_item)
420502
{
421503
uint8_t item_idx, i = 0;
@@ -549,7 +631,22 @@ ControlTableItemInfo_t DYNAMIXEL::getControlTableItemInfo(uint16_t model_num, ui
549631
case PRO_M54P_060_S250_R:
550632
p_common_ctable = pro_ra_pro_plus_control_table;
551633
break;
552-
634+
635+
case YM070_210_M001_RH:
636+
case YM070_210_B001_RH:
637+
case YM070_210_R051_RH:
638+
case YM070_210_R099_RH:
639+
case YM070_210_A051_RH:
640+
case YM070_210_A099_RH:
641+
case YM080_230_M001_RH:
642+
case YM080_230_B001_RH:
643+
case YM080_230_R051_RH:
644+
case YM080_230_R099_RH:
645+
case YM080_230_A051_RH:
646+
case YM080_230_A099_RH:
647+
p_common_ctable = y_control_table;
648+
break;
649+
553650
default:
554651
break;
555652
}

0 commit comments

Comments
 (0)