diff --git a/examples/advanced/add_custom_SerialPortHandler/add_custom_SerialPortHandler.ino b/examples/advanced/add_custom_SerialPortHandler/add_custom_SerialPortHandler.ino index 772b441..01fb083 100644 --- a/examples/advanced/add_custom_SerialPortHandler/add_custom_SerialPortHandler.ino +++ b/examples/advanced/add_custom_SerialPortHandler/add_custom_SerialPortHandler.ino @@ -96,6 +96,8 @@ class NewSerialPortHandler : public DYNAMIXEL::SerialPortHandler }; const uint8_t DXL_ID = 1; +// MX and AX servos use DYNAMIXEL Protocol 1.0 by default. +// to use MX and AX servos with this example, change the following line to: const float DXL_PROTOCOL_VERSION = 1.0; const float DXL_PROTOCOL_VERSION = 2.0; Dynamixel2Arduino dxl; diff --git a/examples/advanced/fast_sync_read/fast_sync_read.ino b/examples/advanced/fast_sync_read/fast_sync_read.ino index 03eb626..5ca5967 100644 --- a/examples/advanced/fast_sync_read/fast_sync_read.ino +++ b/examples/advanced/fast_sync_read/fast_sync_read.ino @@ -94,6 +94,8 @@ } __attribute__((packed)); */ +// MX and AX servos use DYNAMIXEL Protocol 1.0 by default. +// to use MX and AX servos with this example, change the following line to: const float DYNAMIXEL_PROTOCOL_VERSION = 1.0; const float DYNAMIXEL_PROTOCOL_VERSION = 2.0; const uint8_t BROADCAST_ID = 254; const uint8_t DXL_ID_CNT = 2; diff --git a/examples/advanced/indirect_address/indirect_address.ino b/examples/advanced/indirect_address/indirect_address.ino index c680551..4cf54f2 100644 --- a/examples/advanced/indirect_address/indirect_address.ino +++ b/examples/advanced/indirect_address/indirect_address.ino @@ -62,6 +62,8 @@ const uint8_t BROADCAST_ID = 254; +// MX and AX servos use DYNAMIXEL Protocol 1.0 by default. +// to use MX and AX servos with this example, change the following line to: const float DYNAMIXEL_PROTOCOL_VERSION = 1.0; const float DYNAMIXEL_PROTOCOL_VERSION = 2.0; const uint8_t DXL_ID_CNT = 2; diff --git a/examples/advanced/pid_tuning/pid_tuning.ino b/examples/advanced/pid_tuning/pid_tuning.ino index a6419e4..1f5f78a 100644 --- a/examples/advanced/pid_tuning/pid_tuning.ino +++ b/examples/advanced/pid_tuning/pid_tuning.ino @@ -60,6 +60,8 @@ const uint8_t DXL_ID = 1; const uint32_t DXL_BAUDRATE = 57600; +// MX and AX servos use DYNAMIXEL Protocol 1.0 by default. +// to use MX and AX servos with this example, change the following line to: const float DYNAMIXEL_PROTOCOL_VERSION = 1.0; const float DXL_PROTOCOL_VERSION = 2.0; int32_t goal_position[2] = {1200, 1600}; diff --git a/examples/advanced/read_write_ControlTableItem/read_write_ControlTableItem.ino b/examples/advanced/read_write_ControlTableItem/read_write_ControlTableItem.ino index 238ac95..b812ace 100644 --- a/examples/advanced/read_write_ControlTableItem/read_write_ControlTableItem.ino +++ b/examples/advanced/read_write_ControlTableItem/read_write_ControlTableItem.ino @@ -162,6 +162,8 @@ using namespace ControlTableItem; void setup() { // put your setup code here, to run once: DEBUG_SERIAL.begin(115200); +// MX and AX servos use DYNAMIXEL Protocol 1.0 by default. +// to use MX and AX servos with this example, change the following line to: dxl.setPortProtocolVersion(2.0); dxl.setPortProtocolVersion(2.0); dxl.begin(57600); dxl.scan(); @@ -211,4 +213,4 @@ void loop() { } delay(2000); -} \ No newline at end of file +} diff --git a/examples/advanced/slave/slave.ino b/examples/advanced/slave/slave.ino index 51e5938..32b464b 100644 --- a/examples/advanced/slave/slave.ino +++ b/examples/advanced/slave/slave.ino @@ -85,6 +85,8 @@ void setup() { // Speed setting for communication (not necessary for USB) dxl_port.begin(1000000); +// MX and AX servos use DYNAMIXEL Protocol 1.0 by default. +// to use MX and AX servos with this example, change the following line to: dxl.setPortProtocolVersion(DXL_PROTOCOL_VER_1_0); dxl.setPortProtocolVersion(DXL_PROTOCOL_VER_2_0); dxl.setFirmwareVersion(1); dxl.setID(1); @@ -111,4 +113,4 @@ void loop() { // Update or use variables for each control item. digitalWrite(LED_BUILTIN, control_item_led); control_item_analog = analogRead(A0); -} \ No newline at end of file +} diff --git a/examples/advanced/slave_callback/slave_callback.ino b/examples/advanced/slave_callback/slave_callback.ino index f87dfb3..fd6146c 100644 --- a/examples/advanced/slave_callback/slave_callback.ino +++ b/examples/advanced/slave_callback/slave_callback.ino @@ -85,6 +85,8 @@ void setup() { // Speed setting for communication (not necessary for USB) dxl_port.begin(1000000); +// MX and AX servos use DYNAMIXEL Protocol 1.0 by default. +// to use MX and AX servos with this example, change the following line to: dxl.setPortProtocolVersion(DXL_PROTOCOL_VER_1_0); dxl.setPortProtocolVersion(DXL_PROTOCOL_VER_2_0); dxl.setFirmwareVersion(1); dxl.setID(1); diff --git a/examples/advanced/sync_read_write_position/sync_read_write_position.ino b/examples/advanced/sync_read_write_position/sync_read_write_position.ino index 4861a3f..2785329 100644 --- a/examples/advanced/sync_read_write_position/sync_read_write_position.ino +++ b/examples/advanced/sync_read_write_position/sync_read_write_position.ino @@ -99,6 +99,8 @@ */ const uint8_t BROADCAST_ID = 254; +// MX and AX servos use DYNAMIXEL Protocol 1.0 by default. +// to use MX and AX servos with this example, change the following line to: const float DYNAMIXEL_PROTOCOL_VERSION = 2.0; const float DYNAMIXEL_PROTOCOL_VERSION = 2.0; const uint8_t DXL_ID_CNT = 2; const uint8_t DXL_ID_LIST[DXL_ID_CNT] = {1, 2}; diff --git a/examples/advanced/sync_read_write_velocity/sync_read_write_velocity.ino b/examples/advanced/sync_read_write_velocity/sync_read_write_velocity.ino index ad8b34f..e0f2a0c 100644 --- a/examples/advanced/sync_read_write_velocity/sync_read_write_velocity.ino +++ b/examples/advanced/sync_read_write_velocity/sync_read_write_velocity.ino @@ -93,6 +93,8 @@ */ const uint8_t BROADCAST_ID = 254; +// MX and AX servos use DYNAMIXEL Protocol 1.0 by default. +// to use MX and AX servos with this example, change the following line to: const float DYNAMIXEL_PROTOCOL_VERSION = 2.0; const float DYNAMIXEL_PROTOCOL_VERSION = 2.0; const uint8_t DXL_ID_CNT = 2; const uint8_t DXL_ID_LIST[DXL_ID_CNT] = {1, 2}; diff --git a/examples/basic/baudrate/baudrate.ino b/examples/basic/baudrate/baudrate.ino index ae30b0e..4a8db14 100644 --- a/examples/basic/baudrate/baudrate.ino +++ b/examples/basic/baudrate/baudrate.ino @@ -54,6 +54,8 @@ const uint8_t DXL_ID = 1; +// MX and AX servos use DYNAMIXEL Protocol 1.0 by default. +// to use MX and AX servos with this example, change the following line to: const float DXL_PROTOCOL_VERSION = 1.0; const float DXL_PROTOCOL_VERSION = 2.0; uint32_t BAUDRATE = 57600; uint32_t NEW_BAUDRATE = 1000000; //1Mbsp @@ -105,4 +107,4 @@ void setup() { void loop() { // put your main code here, to run repeatedly: -} \ No newline at end of file +} diff --git a/examples/basic/current_mode/current_mode.ino b/examples/basic/current_mode/current_mode.ino index be22626..74b9d20 100644 --- a/examples/basic/current_mode/current_mode.ino +++ b/examples/basic/current_mode/current_mode.ino @@ -54,6 +54,8 @@ const uint8_t DXL_ID = 1; +// MX and AX servos use DYNAMIXEL Protocol 1.0 by default. +// to use MX and AX servos with this example, change the following line to: const float DXL_PROTOCOL_VERSION = 1.0; const float DXL_PROTOCOL_VERSION = 2.0; Dynamixel2Arduino dxl(DXL_SERIAL, DXL_DIR_PIN); diff --git a/examples/basic/current_position_mode/current_position_mode.ino b/examples/basic/current_position_mode/current_position_mode.ino index bae88c1..e350d50 100644 --- a/examples/basic/current_position_mode/current_position_mode.ino +++ b/examples/basic/current_position_mode/current_position_mode.ino @@ -58,6 +58,8 @@ const uint8_t DXL_ID = 1; +// MX and AX servos use DYNAMIXEL Protocol 1.0 by default. +// to use MX and AX servos with this example, change the following line to: const float DXL_PROTOCOL_VERSION = 1.0; const float DXL_PROTOCOL_VERSION = 2.0; Dynamixel2Arduino dxl(DXL_SERIAL, DXL_DIR_PIN); diff --git a/examples/basic/id/id.ino b/examples/basic/id/id.ino index 035812d..e754dd3 100644 --- a/examples/basic/id/id.ino +++ b/examples/basic/id/id.ino @@ -54,6 +54,8 @@ const uint8_t DEFAULT_DXL_ID = 1; +// MX and AX servos use DYNAMIXEL Protocol 1.0 by default. +// to use MX and AX servos with this example, change the following line to: const float DXL_PROTOCOL_VERSION = 1.0; const float DXL_PROTOCOL_VERSION = 2.0; Dynamixel2Arduino dxl(DXL_SERIAL, DXL_DIR_PIN); @@ -116,4 +118,4 @@ void setup() { void loop() { // put your main code here, to run repeatedly: -} \ No newline at end of file +} diff --git a/examples/basic/led/led.ino b/examples/basic/led/led.ino index 3605c99..ad7ed1a 100644 --- a/examples/basic/led/led.ino +++ b/examples/basic/led/led.ino @@ -54,6 +54,8 @@ const uint8_t DXL_ID = 1; +// MX and AX servos use DYNAMIXEL Protocol 1.0 by default. +// to use MX and AX servos with this example, change the following line to: const float DXL_PROTOCOL_VERSION = 1.0; const float DXL_PROTOCOL_VERSION = 2.0; Dynamixel2Arduino dxl(DXL_SERIAL, DXL_DIR_PIN); diff --git a/examples/basic/operation_mode/operation_mode.ino b/examples/basic/operation_mode/operation_mode.ino index 02f4d2d..795262d 100644 --- a/examples/basic/operation_mode/operation_mode.ino +++ b/examples/basic/operation_mode/operation_mode.ino @@ -64,6 +64,8 @@ const uint8_t DXL_ID = 1; +// MX and AX servos use DYNAMIXEL Protocol 1.0 by default. +// to use MX and AX servos with this example, change the following line to: const float DXL_PROTOCOL_VERSION = 1.0; const float DXL_PROTOCOL_VERSION = 2.0; Dynamixel2Arduino dxl(DXL_SERIAL, DXL_DIR_PIN); diff --git a/examples/basic/ping/ping.ino b/examples/basic/ping/ping.ino index 6c3292c..fd11b1e 100644 --- a/examples/basic/ping/ping.ino +++ b/examples/basic/ping/ping.ino @@ -54,6 +54,8 @@ const uint8_t DXL_ID = 1; +// MX and AX servos use DYNAMIXEL Protocol 1.0 by default. +// to use MX and AX servos with this example, change the following line to: const float DXL_PROTOCOL_VERSION = 1.0; const float DXL_PROTOCOL_VERSION = 2.0; Dynamixel2Arduino dxl(DXL_SERIAL, DXL_DIR_PIN); @@ -117,4 +119,4 @@ void FindServos(void) { Serial.print("Broadcast returned no items : "); Serial.println(dxl.getLastLibErrCode()); } -} \ No newline at end of file +} diff --git a/examples/basic/position_mode/position_mode.ino b/examples/basic/position_mode/position_mode.ino index 1498a29..cf97536 100644 --- a/examples/basic/position_mode/position_mode.ino +++ b/examples/basic/position_mode/position_mode.ino @@ -54,6 +54,8 @@ const uint8_t DXL_ID = 1; +// MX and AX servos use DYNAMIXEL Protocol 1.0 by default. +// to use MX and AX servos with this example, change the following line to: const float DXL_PROTOCOL_VERSION = 1.0; const float DXL_PROTOCOL_VERSION = 2.0; Dynamixel2Arduino dxl(DXL_SERIAL, DXL_DIR_PIN); diff --git a/examples/basic/profile_velocity_acceleration/profile_velocity_acceleration.ino b/examples/basic/profile_velocity_acceleration/profile_velocity_acceleration.ino index e3c3a4f..61f0547 100644 --- a/examples/basic/profile_velocity_acceleration/profile_velocity_acceleration.ino +++ b/examples/basic/profile_velocity_acceleration/profile_velocity_acceleration.ino @@ -59,6 +59,8 @@ const uint8_t DXL_ID = 1; +// MX and AX servos use DYNAMIXEL Protocol 1.0 by default. +// to use MX and AX servos with this example, change the following line to: const float DXL_PROTOCOL_VERSION = 1.0; const float DXL_PROTOCOL_VERSION = 2.0; Dynamixel2Arduino dxl(DXL_SERIAL, DXL_DIR_PIN); diff --git a/examples/basic/pwm_mode/pwm_mode.ino b/examples/basic/pwm_mode/pwm_mode.ino index 49630fb..5fe2922 100644 --- a/examples/basic/pwm_mode/pwm_mode.ino +++ b/examples/basic/pwm_mode/pwm_mode.ino @@ -54,6 +54,8 @@ const uint8_t DXL_ID = 1; +// MX and AX servos use DYNAMIXEL Protocol 1.0 by default. +// to use MX and AX servos with this example, change the following line to: const float DXL_PROTOCOL_VERSION = 1.0; const float DXL_PROTOCOL_VERSION = 2.0; Dynamixel2Arduino dxl(DXL_SERIAL, DXL_DIR_PIN); diff --git a/examples/basic/torque/torque.ino b/examples/basic/torque/torque.ino index 67cabc8..2f444ff 100644 --- a/examples/basic/torque/torque.ino +++ b/examples/basic/torque/torque.ino @@ -58,6 +58,8 @@ const uint8_t DXL_ID = 1; +// MX and AX servos use DYNAMIXEL Protocol 1.0 by default. +// to use MX and AX servos with this example, change the following line to: const float DXL_PROTOCOL_VERSION = 1.0; const float DXL_PROTOCOL_VERSION = 2.0; Dynamixel2Arduino dxl(DXL_SERIAL, DXL_DIR_PIN); diff --git a/examples/basic/velocity_mode/velocity_mode.ino b/examples/basic/velocity_mode/velocity_mode.ino index e6a174c..486d65d 100644 --- a/examples/basic/velocity_mode/velocity_mode.ino +++ b/examples/basic/velocity_mode/velocity_mode.ino @@ -54,6 +54,8 @@ const uint8_t DXL_ID = 1; +// MX and AX servos use DYNAMIXEL Protocol 1.0 by default. +// to use MX and AX servos with this example, change the following line to: const float DXL_PROTOCOL_VERSION = 1.0; const float DXL_PROTOCOL_VERSION = 2.0; Dynamixel2Arduino dxl(DXL_SERIAL, DXL_DIR_PIN); diff --git a/examples/dynamixel_protocol/factory_reset/factory_reset.ino b/examples/dynamixel_protocol/factory_reset/factory_reset.ino index a357ae8..aa39095 100644 --- a/examples/dynamixel_protocol/factory_reset/factory_reset.ino +++ b/examples/dynamixel_protocol/factory_reset/factory_reset.ino @@ -54,6 +54,8 @@ #define TIMEOUT 10 //default communication timeout 10ms const uint8_t DXL_ID = 1; +// MX and AX servos use DYNAMIXEL Protocol 1.0 by default. +// to use MX and AX servos with this example, change the following line to: const float DXL_PROTOCOL_VERSION = 1.0; const float DXL_PROTOCOL_VERSION = 2.0; bool ret = false; @@ -131,4 +133,4 @@ void loop() { } delay(1000); -} \ No newline at end of file +} diff --git a/examples/dynamixel_protocol/ping/ping.ino b/examples/dynamixel_protocol/ping/ping.ino index e291130..651e54c 100644 --- a/examples/dynamixel_protocol/ping/ping.ino +++ b/examples/dynamixel_protocol/ping/ping.ino @@ -64,6 +64,8 @@ uint8_t ret = 0; uint8_t recv_count = 0; const uint8_t DXL_ID = BROADCAST_ID; +// MX and AX servos use DYNAMIXEL Protocol 1.0 by default. +// to use MX and AX servos with this example, change the following line to: const float DXL_PROTOCOL_VERSION = 1.0; const float DXL_PROTOCOL_VERSION = 2.0; Dynamixel2Arduino dxl(DXL_SERIAL, DXL_DIR_PIN); @@ -104,4 +106,4 @@ void setup() { void loop() { // put your main code here, to run repeatedly: -} \ No newline at end of file +} diff --git a/examples/dynamixel_protocol/reboot/reboot.ino b/examples/dynamixel_protocol/reboot/reboot.ino index 2d8b1d0..4f19895 100644 --- a/examples/dynamixel_protocol/reboot/reboot.ino +++ b/examples/dynamixel_protocol/reboot/reboot.ino @@ -54,6 +54,8 @@ #define TIMEOUT 10 //default communication timeout 10ms const uint8_t DXL_ID = 1; +// MX and AX servos use DYNAMIXEL Protocol 1.0 by default. +// to use MX and AX servos with this example, change the following line to: const float DXL_PROTOCOL_VERSION = 1.0; const float DXL_PROTOCOL_VERSION = 2.0; uint8_t option = 0; @@ -98,4 +100,4 @@ void loop() { } delay(1000); -} \ No newline at end of file +} diff --git a/keywords.txt b/keywords.txt index 5e4dbed..bad13d1 100644 --- a/keywords.txt +++ b/keywords.txt @@ -193,9 +193,9 @@ SHUTDOWN LITERAL1 TORQUE_ENABLE LITERAL1 LED LITERAL1 -LED_RED LITERAL1 -LED_GREEN LITERAL1 -LED_BLUE LITERAL1 +RGB_LED_RED LITERAL1 +RGB_LED_GREEN LITERAL1 +RGB_LED_BLUE LITERAL1 REGISTERED_INSTRUCTION LITERAL1 HARDWARE_ERROR_STATUS LITERAL1 VELOCITY_P_GAIN LITERAL1 diff --git a/src/Dynamixel2Arduino.cpp b/src/Dynamixel2Arduino.cpp index dc53fdf..2df4c05 100644 --- a/src/Dynamixel2Arduino.cpp +++ b/src/Dynamixel2Arduino.cpp @@ -558,10 +558,10 @@ 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::RGB_LED_GREEN, id, state); + writeControlTableItem(ControlTableItem::RGB_LED_BLUE, id, state); } - ret = writeControlTableItem(ControlTableItem::LED_RED, id, state); + ret = writeControlTableItem(ControlTableItem::RGB_LED_RED, id, state); break; default: diff --git a/src/actuator.cpp b/src/actuator.cpp index f30348e..deac025 100644 --- a/src/actuator.cpp +++ b/src/actuator.cpp @@ -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::RGB_LED_RED, 563, 1}, + {ControlTableItem::RGB_LED_GREEN, 564, 1}, + {ControlTableItem::RGB_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::RGB_LED_RED, 513, 1}, + {ControlTableItem::RGB_LED_GREEN, 514, 1}, + {ControlTableItem::RGB_LED_BLUE, 515, 1}, {ControlTableItem::STATUS_RETURN_LEVEL, 516, 1}, {ControlTableItem::REGISTERED_INSTRUCTION, 517, 1}, {ControlTableItem::HARDWARE_ERROR_STATUS, 518, 1}, @@ -416,6 +416,103 @@ const ModelControlTableInfo_t pro_ra_pro_plus_control_table[] PROGMEM = { {ControlTableItem::LAST_DUMMY_ITEM, 0, 0} }; +const ModelControlTableInfo_t dy_control_table[] PROGMEM = { +#if (ENABLE_ACTUATOR_DY) + {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::PROTOCOL_TYPE, 11, 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_TEMPURATURE_LIMIT,56, 1}, + {ControlTableItem::MOTOR_TEMPURATURE_LIMIT, 56, 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_HISTORY_1, 154, 1}, + {ControlTableItem::ERROR_CODE_HISTORY_2, 155, 1}, + {ControlTableItem::ERROR_CODE_HISTORY_3, 156, 1}, + {ControlTableItem::ERROR_CODE_HISTORY_4, 157, 1}, + {ControlTableItem::ERROR_CODE_HISTORY_5, 158, 1}, + {ControlTableItem::ERROR_CODE_HISTORY_6, 159, 1}, + {ControlTableItem::ERROR_CODE_HISTORY_7, 160, 1}, + {ControlTableItem::ERROR_CODE_HISTORY_8, 161, 1}, + {ControlTableItem::ERROR_CODE_HISTORY_9, 162, 1}, + {ControlTableItem::ERROR_CODE_HISTORY_10, 163, 1}, + {ControlTableItem::ERROR_CODE_HISTORY_11, 164, 1}, + {ControlTableItem::ERROR_CODE_HISTORY_12, 165, 1}, + {ControlTableItem::ERROR_CODE_HISTORY_13, 166, 1}, + {ControlTableItem::ERROR_CODE_HISTORY_14, 167, 1}, + {ControlTableItem::ERROR_CODE_HISTORY_15, 168, 1}, + {ControlTableItem::ERROR_CODE_HISTORY_16, 169, 1}, + {ControlTableItem::HYBRID_SAVE, 170, 1}, + {ControlTableItem::VELOCITY_I_GAIN, 212, 4}, + {ControlTableItem::VELOCITY_P_GAIN, 216, 4}, + {ControlTableItem::VELOCITY_FF_GAIN, 220, 4}, + {ControlTableItem::POSITION_D_GAIN, 224, 4}, + {ControlTableItem::POSITION_I_GAIN, 228, 4}, + {ControlTableItem::POSITION_P_GAIN, 232, 4}, + {ControlTableItem::POSITION_FF_GAIN, 236, 4}, + {ControlTableItem::PROFILE_ACCELERATION, 240, 4}, + {ControlTableItem::PROFILE_VELOCITY, 244, 4}, + {ControlTableItem::PROFILE_ACCELERATION_TIME,248, 4}, + {ControlTableItem::PROFILE_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, 2}, + {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_INVERTER_TEMPERATURE,570, 1}, + {ControlTableItem::PRESENT_MOTOR_TEMPURATURE,571, 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; @@ -521,11 +618,11 @@ ControlTableItemInfo_t DYNAMIXEL::getControlTableItemInfo(uint16_t model_num, ui p_dep_ctable = xw430_540_control_table; break; - // case PRO_L42_10_S300_R: - // case PRO_L54_30_S400_R: - // case PRO_L54_30_S500_R: - // case PRO_L54_50_S290_R: - // case PRO_L54_50_S500_R: + case PRO_L42_10_S300_R: + case PRO_L54_30_S400_R: + case PRO_L54_30_S500_R: + case PRO_L54_50_S290_R: + case PRO_L54_50_S500_R: case PRO_M42_10_S260_R: case PRO_M54_40_S250_R: case PRO_M54_60_S250_R: @@ -550,6 +647,15 @@ ControlTableItemInfo_t DYNAMIXEL::getControlTableItemInfo(uint16_t model_num, ui p_common_ctable = pro_ra_pro_plus_control_table; break; + case YM070_210_M001_RH: + case YM070_210_R051_RH: + case YM070_210_R099_RH: + case YM080_230_M001_RH: + case YM080_230_R051_RH: + case YM080_230_R099_RH: + p_common_ctable = dy_control_table; + break; + default: break; } diff --git a/src/actuator.h b/src/actuator.h index dcef381..2380db3 100644 --- a/src/actuator.h +++ b/src/actuator.h @@ -253,6 +253,26 @@ #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_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 YM080_230_M001_RH +#define YM080_230_M001_RH (uint16_t)4120 +#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 + namespace ControlTableItem{ enum ControlTableItemIndex{ MODEL_NUMBER = 0, @@ -261,6 +281,7 @@ namespace ControlTableItem{ PROTOCOL_VERSION, ID, SECONDARY_ID, + PROTOCOL_TYPE, BAUD_RATE, DRIVE_MODE, CONTROL_MODE, @@ -292,9 +313,9 @@ namespace ControlTableItem{ TORQUE_ENABLE, LED, - LED_RED, - LED_GREEN, - LED_BLUE, + RGB_LED_RED, + RGB_LED_GREEN, + RGB_LED_BLUE, REGISTERED_INSTRUCTION, HARDWARE_ERROR_STATUS, VELOCITY_P_GAIN, @@ -347,6 +368,53 @@ namespace ControlTableItem{ EXTERNAL_PORT_DATA_3, EXTERNAL_PORT_DATA_4, + STARTUP_CONFIGURATION, + POSITION_LIMIT_THRESHOLD, + IN_POSITION_THRESHOLD, + FOLLOWING_ERROR_THRESHOLD, + INVERTER_TEMPURATURE_LIMIT, + MOTOR_TEMPURATURE_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_HISTORY_1, + ERROR_CODE_HISTORY_2, + ERROR_CODE_HISTORY_3, + ERROR_CODE_HISTORY_4, + ERROR_CODE_HISTORY_5, + ERROR_CODE_HISTORY_6, + ERROR_CODE_HISTORY_7, + ERROR_CODE_HISTORY_8, + ERROR_CODE_HISTORY_9, + ERROR_CODE_HISTORY_10, + ERROR_CODE_HISTORY_11, + ERROR_CODE_HISTORY_12, + ERROR_CODE_HISTORY_13, + ERROR_CODE_HISTORY_14, + ERROR_CODE_HISTORY_15, + ERROR_CODE_HISTORY_16, + HYBRID_SAVE, + VELOCITY_FF_GAIN, + POSITION_FF_GAIN, + PROFILE_ACCELERATION_TIME, + PROFILE_TIME, + PWM_OFFSET, + CURRENT_OFFSET, + VELOCITY_OFFSET, + PRESENT_INVERTER_TEMPERATURE, + PRESENT_MOTOR_TEMPURATURE, + LAST_DUMMY_ITEM = 0xFF }; } @@ -362,4 +430,4 @@ ControlTableItemInfo_t getControlTableItemInfo(uint16_t model_num, uint8_t contr } // namespace DYNAMIXEL -#endif /* DYNAMIXEL_ACTUATOR_HPP_ */ \ No newline at end of file +#endif /* DYNAMIXEL_ACTUATOR_HPP_ */ diff --git a/src/utility/config.h b/src/utility/config.h index a12d246..b092846 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_DY 1 + #define DXL_BYTE_STUFF_SAFE_CNT 8 @@ -102,4 +104,4 @@ #define pgm_read_word_near(x) (*(uint16_t*)(x)) #endif -#endif /* DYNAMIXEL_CONFIG_H_ */ \ No newline at end of file +#endif /* DYNAMIXEL_CONFIG_H_ */ diff --git a/tests/control_table/control_table.ino b/tests/control_table/control_table.ino new file mode 100644 index 0000000..094855d --- /dev/null +++ b/tests/control_table/control_table.ino @@ -0,0 +1,82 @@ +/******************************************************************************* +* Copyright 2016 ROBOTIS CO., LTD. +* +* Licensed under the Apache License, Version 2.0 (the "License"); +* you may not use this file except in compliance with the License. +* You may obtain a copy of the License at +* +* http://www.apache.org/licenses/LICENSE-2.0 +* +* Unless required by applicable law or agreed to in writing, software +* distributed under the License is distributed on an "AS IS" BASIS, +* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +* See the License for the specific language governing permissions and +* limitations under the License. +*******************************************************************************/ + +#include + +// Please modify it to suit your hardware. +#if defined(ARDUINO_AVR_UNO) || defined(ARDUINO_AVR_MEGA2560) // When using DynamixelShield + #include + SoftwareSerial soft_serial(7, 8); // DYNAMIXELShield UART RX/TX + #define DXL_SERIAL Serial + #define DEBUG_SERIAL soft_serial + const int DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN +#elif defined(ARDUINO_SAM_DUE) // When using DynamixelShield + #define DXL_SERIAL Serial + #define DEBUG_SERIAL SerialUSB + const int DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN +#elif defined(ARDUINO_SAM_ZERO) // When using DynamixelShield + #define DXL_SERIAL Serial1 + #define DEBUG_SERIAL SerialUSB + const int DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN +#elif defined(ARDUINO_OpenCM904) // When using official ROBOTIS board with DXL circuit. + #define DXL_SERIAL Serial3 //OpenCM9.04 EXP Board's DXL port Serial. (Serial1 for the DXL port on the OpenCM 9.04 board) + #define DEBUG_SERIAL Serial + const int DXL_DIR_PIN = 22; //OpenCM9.04 EXP Board's DIR PIN. (28 for the DXL port on the OpenCM 9.04 board) +#elif defined(ARDUINO_OpenCR) // When using official ROBOTIS board with DXL circuit. + // For OpenCR, there is a DXL Power Enable pin, so you must initialize and control it. + // Reference link : https://github.com/ROBOTIS-GIT/OpenCR/blob/master/arduino/opencr_arduino/opencr/libraries/DynamixelSDK/src/dynamixel_sdk/port_handler_arduino.cpp#L78 + #define DXL_SERIAL Serial3 + #define DEBUG_SERIAL Serial + const int DXL_DIR_PIN = 84; // OpenCR Board's DIR PIN. +#elif defined(ARDUINO_OpenRB) // When using OpenRB-150 + //OpenRB does not require the DIR control pin. + #define DXL_SERIAL Serial1 + #define DEBUG_SERIAL Serial + const int DXL_DIR_PIN = -1; +#else // Other boards when using DynamixelShield + #define DXL_SERIAL Serial1 + #define DEBUG_SERIAL Serial + const int DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN +#endif + + +const uint8_t DXL_ID = 1; +// MX and AX servos use DYNAMIXEL Protocol 1.0 by default. +// to use MX and AX servos with this example, change the following line to: const float DXL_PROTOCOL_VERSION = 1.0; +const float DXL_PROTOCOL_VERSION = 2.0; + +Dynamixel2Arduino dxl(DXL_SERIAL, DXL_DIR_PIN); + +//This namespace is required to use Control table item names +using namespace ControlTableItem; + +void setup() { + // put your setup code here, to run once: + + // Use Serial to debug. + DEBUG_SERIAL.begin(115200); + +DEBUG_SERIAL.println("test"); + + // Set Port baudrate to 57600bps. This has to match with DYNAMIXEL baudrate. + dxl.begin(57600); + // Set Port Protocol Version. This has to match with DYNAMIXEL protocol version. + dxl.setPortProtocolVersion(DXL_PROTOCOL_VERSION); +} + +void loop() { + +}