From 614214db9b942831c13e1e96dd357320146c75d0 Mon Sep 17 00:00:00 2001 From: Kei Date: Fri, 7 Jun 2019 20:45:54 +0900 Subject: [PATCH] Modified some examples. --- .../bulk_read_write_raw.ino | 11 ++++--- .../advanced/sync_bulk_raw/sync_bulk_raw.ino | 19 ++++++----- .../sync_read_write_raw.ino | 33 ++++++++++--------- 3 files changed, 33 insertions(+), 30 deletions(-) diff --git a/examples/advanced/bulk_read_write_raw/bulk_read_write_raw.ino b/examples/advanced/bulk_read_write_raw/bulk_read_write_raw.ino index 5932dfb..4565980 100644 --- a/examples/advanced/bulk_read_write_raw/bulk_read_write_raw.ino +++ b/examples/advanced/bulk_read_write_raw/bulk_read_write_raw.ino @@ -36,7 +36,7 @@ const uint8_t RS485_DIR_PIN = 2; //DYNAMIXEL Shield #endif -/* +/* XelInfoForBulkReadParam_t A structure that contains the information needed for the parameters of the 'bulkRead packet'. typedef struct ParamForBulkReadInst{ @@ -50,9 +50,8 @@ uint16_t length; } XelInfoForBulkReadParam_t; */ -ParamForBulkReadInst_t bulk_read_param; -/* +/* XelInfoForBulkWriteParam_t A structure that contains the information needed for the parameters of the 'bulkWrite packet'. typedef struct ParamForBulkWriteInst{ @@ -67,9 +66,8 @@ ParamForBulkReadInst_t bulk_read_param; uint8_t data[DXL_MAX_NODE_BUFFER_SIZE]; } XelInfoForBulkWriteParam_t; */ -ParamForBulkWriteInst_t bulk_write_param; -/* +/* RecvInfoFromStatusInst_t A structure used to receive data from multiple XELs. typedef struct RecvInfoFromStatusInst{ @@ -84,6 +82,9 @@ ParamForBulkWriteInst_t bulk_write_param; uint8_t data[DXL_MAX_NODE_BUFFER_SIZE]; } XelInfoForStatusInst_t; */ + +ParamForBulkReadInst_t bulk_read_param; +ParamForBulkWriteInst_t bulk_write_param; RecvInfoFromStatusInst_t read_result; Dynamixel2Arduino dxl(DXL_SERIAL, RS485_DIR_PIN); diff --git a/examples/advanced/sync_bulk_raw/sync_bulk_raw.ino b/examples/advanced/sync_bulk_raw/sync_bulk_raw.ino index 5b944e3..9b6ff3e 100644 --- a/examples/advanced/sync_bulk_raw/sync_bulk_raw.ino +++ b/examples/advanced/sync_bulk_raw/sync_bulk_raw.ino @@ -36,7 +36,7 @@ const uint8_t RS485_DIR_PIN = 2; //DYNAMIXEL Shield #endif -/* +/* ParamForSyncReadInst_t A structure that contains the information needed for the parameters of the 'syncRead packet'. typedef struct ParamForSyncReadInst{ @@ -50,9 +50,8 @@ uint8_t id; } InfoForSyncReadParam_t; */ -ParamForSyncReadInst_t sync_read_param; -/* +/* ParamForSyncWriteInst_t A structure that contains the information needed for the parameters of the 'syncWrite packet'. typedef struct ParamForSyncWriteInst{ @@ -67,9 +66,8 @@ ParamForSyncReadInst_t sync_read_param; uint8_t data[DXL_MAX_NODE_BUFFER_SIZE]; } XelInfoForSyncWriteParam_t; */ -ParamForSyncWriteInst_t sync_write_param; -/* +/* ParamForBulkReadInst_t A structure that contains the information needed for the parameters of the 'bulkRead packet'. typedef struct ParamForBulkReadInst{ @@ -83,9 +81,8 @@ ParamForSyncWriteInst_t sync_write_param; uint16_t length; } XelInfoForBulkReadParam_t; */ -ParamForBulkReadInst_t bulk_read_param; -/* +/* ParamForBulkWriteInst_t A structure that contains the information needed for the parameters of the 'bulkWrite packet'. typedef struct ParamForBulkWriteInst{ @@ -100,9 +97,8 @@ ParamForBulkReadInst_t bulk_read_param; uint8_t data[DXL_MAX_NODE_BUFFER_SIZE]; } XelInfoForBulkWriteParam_t; */ -ParamForBulkWriteInst_t bulk_write_param; -/* +/* RecvInfoFromStatusInst_t A structure used to receive data from multiple XELs. typedef struct RecvInfoFromStatusInst{ @@ -117,6 +113,11 @@ ParamForBulkWriteInst_t bulk_write_param; uint8_t data[DXL_MAX_NODE_BUFFER_SIZE]; } XelInfoForStatusInst_t; */ + +ParamForSyncReadInst_t sync_read_param; +ParamForSyncWriteInst_t sync_write_param; +ParamForBulkReadInst_t bulk_read_param; +ParamForBulkWriteInst_t bulk_write_param; RecvInfoFromStatusInst_t read_result; Dynamixel2Arduino dxl(DXL_SERIAL, RS485_DIR_PIN); diff --git a/examples/advanced/sync_read_write_raw/sync_read_write_raw.ino b/examples/advanced/sync_read_write_raw/sync_read_write_raw.ino index 08c6192..81e12ae 100644 --- a/examples/advanced/sync_read_write_raw/sync_read_write_raw.ino +++ b/examples/advanced/sync_read_write_raw/sync_read_write_raw.ino @@ -36,7 +36,7 @@ const uint8_t RS485_DIR_PIN = 2; //DYNAMIXEL Shield #endif -/* +/* ParamForSyncReadInst_t A structure that contains the information needed for the parameters of the 'syncRead packet'. typedef struct ParamForSyncReadInst{ @@ -50,9 +50,8 @@ uint8_t id; } InfoForSyncReadParam_t; */ -ParamForSyncReadInst_t sync_read_param; -/* +/* ParamForSyncWriteInst_t A structure that contains the information needed for the parameters of the 'syncWrite packet'. typedef struct ParamForSyncWriteInst{ @@ -67,9 +66,8 @@ ParamForSyncReadInst_t sync_read_param; uint8_t data[DXL_MAX_NODE_BUFFER_SIZE]; } XelInfoForSyncWriteParam_t; */ -ParamForSyncWriteInst_t sync_write_param; -/* +/* RecvInfoFromStatusInst_t A structure used to receive data from multiple XELs. typedef struct RecvInfoFromStatusInst{ @@ -84,6 +82,9 @@ ParamForSyncWriteInst_t sync_write_param; uint8_t data[DXL_MAX_NODE_BUFFER_SIZE]; } XelInfoForStatusInst_t; */ + +ParamForSyncReadInst_t sync_read_param; +ParamForSyncWriteInst_t sync_write_param; RecvInfoFromStatusInst_t read_result; Dynamixel2Arduino dxl(DXL_SERIAL, RS485_DIR_PIN); @@ -109,23 +110,23 @@ void setup() { sync_read_param.id_count = 2; dxl.torqueOff(1); - dxl.setOperatingMode(1, OP_POSITION); + dxl.setOperatingMode(1, OP_VELOCITY); dxl.torqueOn(1); dxl.torqueOff(3); - dxl.setOperatingMode(3, OP_POSITION); + dxl.setOperatingMode(3, OP_VELOCITY); dxl.torqueOn(3); } void loop() { // put your main code here, to run repeatedly: - static int32_t position = 0; - int32_t recv_position[2]; + static int32_t velocity = 0; + int32_t recv_velocity[2]; // set value to data buffer for syncWrite - position = position >= 4095 ? 0 : position+409; - memcpy(sync_write_param.xel[0].data, &position, sizeof(position)); - memcpy(sync_write_param.xel[1].data, &position, sizeof(position)); + velocity = velocity >= 200 ? -200 : velocity+10; + memcpy(sync_write_param.xel[0].data, &velocity, sizeof(velocity)); + memcpy(sync_write_param.xel[1].data, &velocity, sizeof(velocity)); // send command using syncWrite dxl.syncWrite(sync_write_param); @@ -134,15 +135,15 @@ void loop() { // Print the read data using SyncRead dxl.syncRead(sync_read_param, read_result); DEBUG_SERIAL.println("======= Sync Read ======="); - memcpy(&recv_position[0], read_result.xel[0].data, read_result.xel[0].length); - memcpy(&recv_position[1], read_result.xel[1].data, read_result.xel[1].length); + memcpy(&recv_velocity[0], read_result.xel[0].data, read_result.xel[0].length); + memcpy(&recv_velocity[1], read_result.xel[1].data, read_result.xel[1].length); DEBUG_SERIAL.print("ID: ");DEBUG_SERIAL.print(read_result.xel[0].id);DEBUG_SERIAL.print(" "); - DEBUG_SERIAL.print(", Present Velocity: ");DEBUG_SERIAL.print(recv_position[0]);DEBUG_SERIAL.print(" "); + DEBUG_SERIAL.print(", Present Velocity: ");DEBUG_SERIAL.print(recv_velocity[0]);DEBUG_SERIAL.print(" "); DEBUG_SERIAL.print(", Packet Error: ");DEBUG_SERIAL.print(read_result.xel[0].error);DEBUG_SERIAL.print(" "); DEBUG_SERIAL.print(", Param Length: ");DEBUG_SERIAL.print(read_result.xel[0].length);DEBUG_SERIAL.print(" "); DEBUG_SERIAL.println(); DEBUG_SERIAL.print("ID: ");DEBUG_SERIAL.print(read_result.xel[1].id);DEBUG_SERIAL.print(" "); - DEBUG_SERIAL.print(", Present Velocity: ");DEBUG_SERIAL.print(recv_position[1]);DEBUG_SERIAL.print(" "); + DEBUG_SERIAL.print(", Present Velocity: ");DEBUG_SERIAL.print(recv_velocity[1]);DEBUG_SERIAL.print(" "); DEBUG_SERIAL.print(", Packet Error: ");DEBUG_SERIAL.print(read_result.xel[1].error);DEBUG_SERIAL.print(" "); DEBUG_SERIAL.print(", Param Length: ");DEBUG_SERIAL.print(read_result.xel[1].length);DEBUG_SERIAL.print(" "); DEBUG_SERIAL.println();