diff --git a/.travis.yml b/.travis.yml index b26450c..99e7d2a 100644 --- a/.travis.yml +++ b/.travis.yml @@ -25,3 +25,4 @@ notifications: branches: only: - master + - develop diff --git a/README.md b/README.md index 684841f..7f02b7e 100644 --- a/README.md +++ b/README.md @@ -1,9 +1,23 @@ # Dynamixel2Arduino [![Build Status](https://travis-ci.org/ROBOTIS-GIT/Dynamixel2Arduino.svg?branch=master)](https://travis-ci.org/ROBOTIS-GIT/Dynamixel2Arduino/branches) +## Serial and Direction Pin definitions by board + - The examples default to pins based on DynamixelShield. Therefore, when using hardware other than DynamixelShield (eg OpenCM9.04, OpenCR, Custom DXL boards), you need to change the Serial and Direction Pin. + - We provide the information below to make it easier to define Serial and Direction pins for specific hardware. + + |Board Name|Serial|Direction Pin|Note| + |:-:|:-:|:-:|:-:| + |OpenCM9.04|Serial1|28|because of the OpenCM 9.04 driver code, you must call Serial1.setDxlMode(true); before dxl.begin();.| + |OpenCM9.04 EXP|Serial3|22|| + |OpenCR|Serial3|84|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))| + + ## How to add new DYNAMIXEL model. - For the convenience of the user, Dynamixel2Arduino API hardcodes some information in the control table and stores it in flash. - To do this, you need to add code to some files. In this regard, please refer to [PR#3](https://github.com/ROBOTIS-GIT/Dynamixel2Arduino/pull/3) and [PR#7](https://github.com/ROBOTIS-GIT/Dynamixel2Arduino/pull/7) +## How to create custom PortHandler Class + - Please refer to [port_handler.h](https://github.com/ROBOTIS-GIT/Dynamixel2Arduino/blob/master/src/utility/port_handler.h) + - Create a new class by inheriting PortHandler as public. (Like SerialPortHandler and USBSerialPortHandler) ## TODO - Separation of protocol codes (protocol, packet handler) \ No newline at end of file diff --git a/examples/advanced/DEPRECATED_bulk_read_write_raw/DEPRECATED_bulk_read_write_raw.ino b/examples/advanced/DEPRECATED_bulk_read_write_raw/DEPRECATED_bulk_read_write_raw.ino new file mode 100644 index 0000000..59a0e23 --- /dev/null +++ b/examples/advanced/DEPRECATED_bulk_read_write_raw/DEPRECATED_bulk_read_write_raw.ino @@ -0,0 +1,174 @@ +/******************************************************************************* +* 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. +*******************************************************************************/ + +/* + This is an example deprecated. (Not recommended, just an example for legacy) +*/ + +#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 uint8_t 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 uint8_t 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 uint8_t 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 uint8_t 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 uint8_t DXL_DIR_PIN = 84; // OpenCR Board's DIR PIN. +#else // Other boards when using DynamixelShield + #define DXL_SERIAL Serial1 + #define DEBUG_SERIAL Serial + const uint8_t DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN +#endif + +/* XelInfoForBulkReadParam_t + A structure that contains the information needed for the parameters of the 'bulkRead packet'. + + typedef struct ParamForBulkReadInst{ + uint8_t id_count; + XelInfoForBulkReadParam_t xel[DXL_MAX_NODE]; //refer to below. + } ParamForBulkReadInst_t; + + typedef struct XelInfoForBulkReadParam{ + uint8_t id; + uint16_t addr; + uint16_t length; + } XelInfoForBulkReadParam_t; +*/ + +/* XelInfoForBulkWriteParam_t + A structure that contains the information needed for the parameters of the 'bulkWrite packet'. + + typedef struct ParamForBulkWriteInst{ + uint8_t id_count; + XelInfoForBulkWriteParam_t xel[DXL_MAX_NODE]; //refer to below. + } ParamForBulkWriteInst_t; + + typedef struct XelInfoForBulkWriteParam{ + uint8_t id; + uint16_t addr; + uint16_t length; + uint8_t data[DXL_MAX_NODE_BUFFER_SIZE]; + } XelInfoForBulkWriteParam_t; +*/ + +/* RecvInfoFromStatusInst_t + A structure used to receive data from multiple XELs. + + typedef struct RecvInfoFromStatusInst{ + uint8_t id_count; + XelInfoForStatusInst_t xel[DXL_MAX_NODE]; //refer to below. + } RecvInfoFromStatusInst_t; + + typedef struct XelInfoForStatusInst{ + uint8_t id; + uint16_t length; + uint8_t error; + 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, DXL_DIR_PIN); + +void setup() { + // put your setup code here, to run once: + DEBUG_SERIAL.begin(115200); + dxl.begin(57600); + dxl.scan(); + + // fill the members of structure for bulkWrite + bulk_write_param.xel[0].id = 1; + bulk_write_param.xel[1].id = 2; + bulk_write_param.xel[0].addr = 116; //Goal Position on X serise + bulk_write_param.xel[1].addr = 104; //Goal Velocity on X serise + bulk_write_param.xel[0].length = 4; + bulk_write_param.xel[1].length = 4; + bulk_write_param.id_count = 2; + + // fill the members of structure for bulkRead + bulk_read_param.xel[0].id = 1; + bulk_read_param.xel[1].id = 2; + bulk_read_param.xel[0].addr = 132; //Present Position on X serise + bulk_read_param.xel[1].addr = 128; //Present Velocity on X serise + bulk_read_param.xel[0].length = 4; + bulk_read_param.xel[1].length = 4; + bulk_read_param.id_count = 2; + + dxl.torqueOff(1); + dxl.setOperatingMode(1, OP_POSITION); + dxl.torqueOn(1); + + dxl.torqueOff(3); + dxl.setOperatingMode(3, OP_VELOCITY); + dxl.torqueOn(3); +} + +void loop() { + // put your main code here, to run repeatedly: + static int32_t position = 0, velocity = 0; + int32_t recv_position = 0, recv_velocity = 0; + + // set value to data buffer for bulkWrite + position = position >= 4095 ? 0 : position+409; + memcpy(bulk_write_param.xel[0].data, &position, sizeof(position)); + velocity = velocity >= 200 ? -200 : velocity+10; + memcpy(bulk_write_param.xel[1].data, &velocity, sizeof(velocity)); + + // send command using bulkWrite + dxl.bulkWrite(bulk_write_param); + delay(100); + + // Print the read data using bulkRead + dxl.bulkRead(bulk_read_param, read_result); + DEBUG_SERIAL.println(F("======= Bulk Read ========")); + memcpy(&recv_position, read_result.xel[0].data, read_result.xel[0].length); + memcpy(&recv_velocity, read_result.xel[1].data, read_result.xel[1].length); + DEBUG_SERIAL.print(F("ID: "));DEBUG_SERIAL.print(read_result.xel[0].id);DEBUG_SERIAL.print(" "); + DEBUG_SERIAL.print(F(", Present Position: "));DEBUG_SERIAL.print(recv_position);DEBUG_SERIAL.print(" "); + DEBUG_SERIAL.print(F(", Packet Error: "));DEBUG_SERIAL.print(read_result.xel[0].error);DEBUG_SERIAL.print(" "); + DEBUG_SERIAL.print(F(", Param Length: "));DEBUG_SERIAL.print(read_result.xel[0].length);DEBUG_SERIAL.print(" "); + DEBUG_SERIAL.println(); + DEBUG_SERIAL.print(F("ID: "));DEBUG_SERIAL.print(read_result.xel[1].id);DEBUG_SERIAL.print(" "); + DEBUG_SERIAL.print(F(", Present Velocity: "));DEBUG_SERIAL.print(recv_velocity);DEBUG_SERIAL.print(" "); + DEBUG_SERIAL.print(F(", Packet Error: "));DEBUG_SERIAL.print(read_result.xel[1].error);DEBUG_SERIAL.print(" "); + DEBUG_SERIAL.print(F(", Param Length: "));DEBUG_SERIAL.print(read_result.xel[1].length);DEBUG_SERIAL.print(" "); + DEBUG_SERIAL.println(); + DEBUG_SERIAL.println(); + delay(100); +} + + diff --git a/examples/advanced/sync_bulk_raw/sync_bulk_raw.ino b/examples/advanced/DEPRECATED_sync_bulk_raw/DEPRECATED_sync_bulk_raw.ino similarity index 82% rename from examples/advanced/sync_bulk_raw/sync_bulk_raw.ino rename to examples/advanced/DEPRECATED_sync_bulk_raw/DEPRECATED_sync_bulk_raw.ino index 394210d..eb8b765 100644 --- a/examples/advanced/sync_bulk_raw/sync_bulk_raw.ino +++ b/examples/advanced/DEPRECATED_sync_bulk_raw/DEPRECATED_sync_bulk_raw.ino @@ -14,15 +14,38 @@ * limitations under the License. *******************************************************************************/ +/* + This is an example deprecated. (Not recommended, just an example for legacy) +*/ + #include -#if defined(ARDUINO_AVR_UNO) || defined(ARDUINO_AVR_MEGA2560) +// 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 uint8_t DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN -#else +#elif defined(ARDUINO_SAM_DUE) // When using DynamixelShield + #define DXL_SERIAL Serial + #define DEBUG_SERIAL SerialUSB + const uint8_t 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 uint8_t 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 uint8_t 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 uint8_t DXL_DIR_PIN = 84; // OpenCR Board's DIR PIN. +#else // Other boards when using DynamixelShield #define DXL_SERIAL Serial1 #define DEBUG_SERIAL Serial const uint8_t DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN @@ -124,7 +147,7 @@ void setup() { sync_write_param.addr = 64; //Torque Enable on X serise sync_write_param.length = 1; sync_write_param.xel[0].id = 1; - sync_write_param.xel[1].id = 3; + sync_write_param.xel[1].id = 2; sync_write_param.xel[0].data[0] = 0; sync_write_param.xel[1].data[0] = 0; sync_write_param.id_count = 2; @@ -133,12 +156,12 @@ void setup() { sync_read_param.addr = 126; //Present Current on X serise sync_read_param.length = 2; sync_read_param.xel[0].id = 1; - sync_read_param.xel[1].id = 3; + sync_read_param.xel[1].id = 2; sync_read_param.id_count = 2; // fill the members of structure for bulkWrite bulk_write_param.xel[0].id = 1; - bulk_write_param.xel[1].id = 3; + bulk_write_param.xel[1].id = 2; bulk_write_param.xel[0].addr = 116; //Goal Position on X serise bulk_write_param.xel[1].addr = 104; //Goal Velocity on X serise bulk_write_param.xel[0].length = 4; @@ -147,7 +170,7 @@ void setup() { // fill the members of structure for bulkRead bulk_read_param.xel[0].id = 1; - bulk_read_param.xel[1].id = 3; + bulk_read_param.xel[1].id = 2; bulk_read_param.xel[0].addr = 132; //Present Position on X serise bulk_read_param.xel[1].addr = 128; //Present Velocity on X serise bulk_read_param.xel[0].length = 4; diff --git a/examples/advanced/DEPRECATED_sync_read_write_raw/DEPRECATED_sync_read_write_raw.ino b/examples/advanced/DEPRECATED_sync_read_write_raw/DEPRECATED_sync_read_write_raw.ino new file mode 100644 index 0000000..2d2b5a7 --- /dev/null +++ b/examples/advanced/DEPRECATED_sync_read_write_raw/DEPRECATED_sync_read_write_raw.ino @@ -0,0 +1,169 @@ +/******************************************************************************* +* 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. +*******************************************************************************/ + +/* + This is an example deprecated. (Not recommended, just an example for legacy) +*/ + +#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 uint8_t 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 uint8_t 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 uint8_t 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 uint8_t 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 uint8_t DXL_DIR_PIN = 84; // OpenCR Board's DIR PIN. +#else // Other boards when using DynamixelShield + #define DXL_SERIAL Serial1 + #define DEBUG_SERIAL Serial + const uint8_t DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN +#endif + +/* ParamForSyncReadInst_t + A structure that contains the information needed for the parameters of the 'syncRead packet'. + + typedef struct ParamForSyncReadInst{ + uint16_t addr; + uint16_t length; + uint8_t id_count; + InfoForSyncReadParam_t xel[DXL_MAX_NODE]; //refer to below. + } ParamForSyncReadInst_t; + + typedef struct InfoForSyncReadParam{ + uint8_t id; + } InfoForSyncReadParam_t; +*/ + +/* ParamForSyncWriteInst_t + A structure that contains the information needed for the parameters of the 'syncWrite packet'. + + typedef struct ParamForSyncWriteInst{ + uint16_t addr; + uint16_t length; + uint8_t id_count; + XelInfoForSyncWriteParam_t xel[DXL_MAX_NODE]; //refer to below. + } ParamForSyncWriteInst_t; + + typedef struct XelInfoForSyncWriteParam{ + uint8_t id; + uint8_t data[DXL_MAX_NODE_BUFFER_SIZE]; + } XelInfoForSyncWriteParam_t; +*/ + +/* RecvInfoFromStatusInst_t + A structure used to receive data from multiple XELs. + + typedef struct RecvInfoFromStatusInst{ + uint8_t id_count; + XelInfoForStatusInst_t xel[DXL_MAX_NODE]; //refer to below. + } RecvInfoFromStatusInst_t; + + typedef struct XelInfoForStatusInst{ + uint8_t id; + uint16_t length; + uint8_t error; + 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, DXL_DIR_PIN); + +void setup() { + // put your setup code here, to run once: + DEBUG_SERIAL.begin(115200); + dxl.begin(57600); + dxl.scan(); + + // fill the members of structure for syncWrite + sync_write_param.addr = 104; //Goal Velocity on X serise + sync_write_param.length = 4; + sync_write_param.xel[0].id = 1; + sync_write_param.xel[1].id = 2; + sync_write_param.id_count = 2; + + // fill the members of structure for syncRead + sync_read_param.addr = 128; //Present Velocity on X serise + sync_read_param.length = 4; + sync_read_param.xel[0].id = 1; + sync_read_param.xel[1].id = 2; + sync_read_param.id_count = 2; + + dxl.torqueOff(1); + dxl.setOperatingMode(1, OP_VELOCITY); + dxl.torqueOn(1); + + dxl.torqueOff(3); + dxl.setOperatingMode(3, OP_VELOCITY); + dxl.torqueOn(3); +} + +void loop() { + // put your main code here, to run repeatedly: + static int32_t velocity = 0; + int32_t recv_velocity[2]; + + // set value to data buffer for syncWrite + 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); + delay(100); + + // Print the read data using SyncRead + dxl.syncRead(sync_read_param, read_result); + DEBUG_SERIAL.println(F("======= Sync Read =======")); + 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(F("ID: "));DEBUG_SERIAL.print(read_result.xel[0].id);DEBUG_SERIAL.print(" "); + DEBUG_SERIAL.print(F(", Present Velocity: "));DEBUG_SERIAL.print(recv_velocity[0]);DEBUG_SERIAL.print(" "); + DEBUG_SERIAL.print(F(", Packet Error: "));DEBUG_SERIAL.print(read_result.xel[0].error);DEBUG_SERIAL.print(" "); + DEBUG_SERIAL.print(F(", Param Length: "));DEBUG_SERIAL.print(read_result.xel[0].length);DEBUG_SERIAL.print(" "); + DEBUG_SERIAL.println(); + DEBUG_SERIAL.print(F("ID: "));DEBUG_SERIAL.print(read_result.xel[1].id);DEBUG_SERIAL.print(" "); + DEBUG_SERIAL.print(F(", Present Velocity: "));DEBUG_SERIAL.print(recv_velocity[1]);DEBUG_SERIAL.print(" "); + DEBUG_SERIAL.print(F(", Packet Error: "));DEBUG_SERIAL.print(read_result.xel[1].error);DEBUG_SERIAL.print(" "); + DEBUG_SERIAL.print(F(", Param Length: "));DEBUG_SERIAL.print(read_result.xel[1].length);DEBUG_SERIAL.print(" "); + DEBUG_SERIAL.println(); + DEBUG_SERIAL.println(); + delay(100); +} + + diff --git a/examples/advanced/add_custom_SerialPortHandler/add_custom_SerialPortHandler.ino b/examples/advanced/add_custom_SerialPortHandler/add_custom_SerialPortHandler.ino new file mode 100644 index 0000000..77a9f72 --- /dev/null +++ b/examples/advanced/add_custom_SerialPortHandler/add_custom_SerialPortHandler.ino @@ -0,0 +1,133 @@ +/******************************************************************************* +* 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 uint8_t 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 uint8_t 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 uint8_t 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 uint8_t 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 uint8_t DXL_DIR_PIN = 84; // OpenCR Board's DIR PIN. +#else // Other boards when using DynamixelShield + #define DXL_SERIAL Serial1 + #define DEBUG_SERIAL Serial + const uint8_t DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN +#endif + + +class NewSerialPortHandler : public DYNAMIXEL::SerialPortHandler +{ + public: + NewSerialPortHandler(HardwareSerial& port, const int dir_pin = -1) + : SerialPortHandler(port, dir_pin), port_(port), dir_pin_(dir_pin) + {} + + virtual size_t write(uint8_t c) override + { + size_t ret = 0; + digitalWrite(dir_pin_, HIGH); + while(digitalRead(dir_pin_) != HIGH); + + ret = port_.write(c); + + port_.flush(); + digitalWrite(dir_pin_, LOW); + while(digitalRead(dir_pin_) != LOW); + + return ret; + } + + virtual size_t write(uint8_t *buf, size_t len) override + { + size_t ret; + digitalWrite(dir_pin_, HIGH); + while(digitalRead(dir_pin_) != HIGH); + + ret = port_.write(buf, len); + + port_.flush(); + digitalWrite(dir_pin_, LOW); + while(digitalRead(dir_pin_) != LOW); + + return ret; + } + + private: + HardwareSerial& port_; + const int dir_pin_; +}; + +const uint8_t DXL_ID = 1; +const float DXL_PROTOCOL_VERSION = 2.0; + +Dynamixel2Arduino dxl; +NewSerialPortHandler dxl_port(DXL_SERIAL, DXL_DIR_PIN); + + +void setup() { + // put your setup code here, to run once: + + // Use Serial to debug. + DEBUG_SERIAL.begin(115200); + + // Set Port instance + dxl.setPort(dxl_port); + // 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() { + // put your main code here, to run repeatedly: + + DEBUG_SERIAL.print("PROTOCOL "); + DEBUG_SERIAL.print(DXL_PROTOCOL_VERSION, 1); + DEBUG_SERIAL.print(", ID "); + DEBUG_SERIAL.print(DXL_ID); + DEBUG_SERIAL.print(": "); + if(dxl.ping(DXL_ID) == true){ + DEBUG_SERIAL.print("ping succeeded!"); + DEBUG_SERIAL.print(", Model Number: "); + DEBUG_SERIAL.println(dxl.getModelNumber(DXL_ID)); + }else{ + DEBUG_SERIAL.println("ping failed!"); + } + delay(500); +} + + + 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 ad5e7fb..90e9a03 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 @@ -16,136 +16,217 @@ #include -#if defined(ARDUINO_AVR_UNO) || defined(ARDUINO_AVR_MEGA2560) +// 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 uint8_t DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN -#else +#elif defined(ARDUINO_SAM_DUE) // When using DynamixelShield + #define DXL_SERIAL Serial + #define DEBUG_SERIAL SerialUSB + const uint8_t 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 uint8_t 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 uint8_t 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 uint8_t DXL_DIR_PIN = 84; // OpenCR Board's DIR PIN. +#else // Other boards when using DynamixelShield #define DXL_SERIAL Serial1 #define DEBUG_SERIAL Serial const uint8_t DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN #endif + -/* XelInfoForBulkReadParam_t - A structure that contains the information needed for the parameters of the 'bulkRead packet'. - - typedef struct ParamForBulkReadInst{ - uint8_t id_count; - XelInfoForBulkReadParam_t xel[DXL_MAX_NODE]; //refer to below. - } ParamForBulkReadInst_t; +/* bulkRead + Structures containing the necessary information to process the 'bulkRead' packet. - typedef struct XelInfoForBulkReadParam{ - uint8_t id; + typedef struct XELInfoBulkRead{ uint16_t addr; - uint16_t length; - } XelInfoForBulkReadParam_t; -*/ - -/* XelInfoForBulkWriteParam_t - A structure that contains the information needed for the parameters of the 'bulkWrite packet'. - - typedef struct ParamForBulkWriteInst{ - uint8_t id_count; - XelInfoForBulkWriteParam_t xel[DXL_MAX_NODE]; //refer to below. - } ParamForBulkWriteInst_t; - - typedef struct XelInfoForBulkWriteParam{ + uint16_t addr_length; + uint8_t *p_recv_buf; uint8_t id; - uint16_t addr; - uint16_t length; - uint8_t data[DXL_MAX_NODE_BUFFER_SIZE]; - } XelInfoForBulkWriteParam_t; + uint8_t error; + } __attribute__((packed)) XELInfoBulkRead_t; + + typedef struct InfoBulkReadInst{ + XELInfoBulkRead_t* p_xels; + uint8_t xel_count; + bool is_info_changed; + InfoSyncBulkBuffer_t packet; + } __attribute__((packed)) InfoBulkReadInst_t; */ -/* RecvInfoFromStatusInst_t - A structure used to receive data from multiple XELs. +/* bulkWrite + Structures containing the necessary information to process the 'bulkWrite' packet. - typedef struct RecvInfoFromStatusInst{ - uint8_t id_count; - XelInfoForStatusInst_t xel[DXL_MAX_NODE]; //refer to below. - } RecvInfoFromStatusInst_t; - - typedef struct XelInfoForStatusInst{ + typedef struct XELInfoBulkWrite{ + uint16_t addr; + uint16_t addr_length; + uint8_t* p_data; uint8_t id; - uint16_t length; - uint8_t error; - uint8_t data[DXL_MAX_NODE_BUFFER_SIZE]; - } XelInfoForStatusInst_t; + } __attribute__((packed)) XELInfoBulkWrite_t; + + typedef struct InfoBulkWriteInst{ + XELInfoBulkWrite_t* p_xels; + uint8_t xel_count; + bool is_info_changed; + InfoSyncBulkBuffer_t packet; + } __attribute__((packed)) InfoBulkWriteInst_t; */ -ParamForBulkReadInst_t bulk_read_param; -ParamForBulkWriteInst_t bulk_write_param; -RecvInfoFromStatusInst_t read_result; +const uint8_t DXL_1_ID = 1; +const uint8_t DXL_2_ID = 2; +const uint8_t DXL_ID_CNT = 2; +const uint16_t user_pkt_buf_cap = 128; +uint8_t user_pkt_buf[user_pkt_buf_cap]; + +struct br_data_xel_1{ + int16_t present_current; + int32_t present_velocity; +} __attribute__((packed)); +struct br_data_xel_2{ + int32_t present_position; +} __attribute__((packed)); +struct bw_data_xel_1{ + int32_t goal_velocity; +} __attribute__((packed)); +struct bw_data_xel_2{ + int32_t goal_position; +} __attribute__((packed)); + +struct br_data_xel_1 br_data_xel_1; +struct br_data_xel_2 br_data_xel_2; +DYNAMIXEL::InfoBulkReadInst_t br_infos; +DYNAMIXEL::XELInfoBulkRead_t info_xels_br[DXL_ID_CNT]; + +struct bw_data_xel_1 bw_data_xel_1; +struct bw_data_xel_2 bw_data_xel_2; +DYNAMIXEL::InfoBulkWriteInst_t bw_infos; +DYNAMIXEL::XELInfoBulkWrite_t info_xels_bw[DXL_ID_CNT]; Dynamixel2Arduino dxl(DXL_SERIAL, DXL_DIR_PIN); void setup() { // put your setup code here, to run once: + pinMode(LED_BUILTIN, OUTPUT); DEBUG_SERIAL.begin(115200); dxl.begin(57600); - dxl.scan(); - - // fill the members of structure for bulkWrite - bulk_write_param.xel[0].id = 1; - bulk_write_param.xel[1].id = 3; - bulk_write_param.xel[0].addr = 116; //Goal Position on X serise - bulk_write_param.xel[1].addr = 104; //Goal Velocity on X serise - bulk_write_param.xel[0].length = 4; - bulk_write_param.xel[1].length = 4; - bulk_write_param.id_count = 2; - - // fill the members of structure for bulkRead - bulk_read_param.xel[0].id = 1; - bulk_read_param.xel[1].id = 3; - bulk_read_param.xel[0].addr = 132; //Present Position on X serise - bulk_read_param.xel[1].addr = 128; //Present Velocity on X serise - bulk_read_param.xel[0].length = 4; - bulk_read_param.xel[1].length = 4; - bulk_read_param.id_count = 2; - - dxl.torqueOff(1); - dxl.setOperatingMode(1, OP_POSITION); - dxl.torqueOn(1); - - dxl.torqueOff(3); - dxl.setOperatingMode(3, OP_VELOCITY); - dxl.torqueOn(3); + + dxl.torqueOff(DXL_1_ID); + dxl.torqueOff(DXL_2_ID); + dxl.setOperatingMode(DXL_1_ID, OP_VELOCITY); + dxl.setOperatingMode(DXL_2_ID, OP_POSITION); + dxl.torqueOn(DXL_1_ID); + dxl.torqueOn(DXL_2_ID); + + // fill the members of structure for bulkRead using external user packet buffer + br_infos.packet.p_buf = user_pkt_buf; + br_infos.packet.buf_capacity = user_pkt_buf_cap; + br_infos.packet.is_completed = false; + br_infos.p_xels = info_xels_br; + br_infos.xel_count = 0; + + info_xels_br[0].id = DXL_1_ID; + info_xels_br[0].addr = 126; // Present Current of X serise. + info_xels_br[0].addr_length = 2+4; // Present Current + Present Velocity + info_xels_br[0].p_recv_buf = (uint8_t*)&br_data_xel_1; + br_infos.xel_count++; + + info_xels_br[1].id = DXL_2_ID; + info_xels_br[1].addr = 132; // Present Position of X serise. + info_xels_br[1].addr_length = 4; // Present Position + Present Velocity + info_xels_br[1].p_recv_buf = (uint8_t*)&br_data_xel_2; + br_infos.xel_count++; + + br_infos.is_info_changed = true; + + + // Fill the members of structure for bulkWrite using internal packet buffer + bw_infos.packet.p_buf = nullptr; + bw_infos.packet.is_completed = false; + bw_infos.p_xels = info_xels_bw; + bw_infos.xel_count = 0; + + bw_data_xel_1.goal_velocity = 0; + info_xels_bw[0].id = DXL_1_ID; + info_xels_bw[0].addr = 104; // Goal Velocity of X serise. + info_xels_bw[0].addr_length = 4; // Goal Velocity + info_xels_bw[0].p_data = (uint8_t*)&bw_data_xel_1; + bw_infos.xel_count++; + + bw_data_xel_2.goal_position = 0; + info_xels_bw[1].id = DXL_2_ID; + info_xels_bw[1].addr = 116; // Goal Position of X serise. + info_xels_bw[1].addr_length = 4; // Goal Position + info_xels_bw[1].p_data = (uint8_t*)&bw_data_xel_2; + bw_infos.xel_count++; + + bw_infos.is_info_changed = true; } void loop() { // put your main code here, to run repeatedly: - static int32_t position = 0, velocity = 0; - int32_t recv_position = 0, recv_velocity = 0; - - // set value to data buffer for bulkWrite - position = position >= 4095 ? 0 : position+409; - memcpy(bulk_write_param.xel[0].data, &position, sizeof(position)); - velocity = velocity >= 200 ? -200 : velocity+10; - memcpy(bulk_write_param.xel[1].data, &velocity, sizeof(velocity)); - - // send command using bulkWrite - dxl.bulkWrite(bulk_write_param); - delay(100); - - // Print the read data using bulkRead - dxl.bulkRead(bulk_read_param, read_result); - DEBUG_SERIAL.println(F("======= Bulk Read ========")); - memcpy(&recv_position, read_result.xel[0].data, read_result.xel[0].length); - memcpy(&recv_velocity, read_result.xel[1].data, read_result.xel[1].length); - DEBUG_SERIAL.print(F("ID: "));DEBUG_SERIAL.print(read_result.xel[0].id);DEBUG_SERIAL.print(" "); - DEBUG_SERIAL.print(F(", Present Position: "));DEBUG_SERIAL.print(recv_position);DEBUG_SERIAL.print(" "); - DEBUG_SERIAL.print(F(", Packet Error: "));DEBUG_SERIAL.print(read_result.xel[0].error);DEBUG_SERIAL.print(" "); - DEBUG_SERIAL.print(F(", Param Length: "));DEBUG_SERIAL.print(read_result.xel[0].length);DEBUG_SERIAL.print(" "); - DEBUG_SERIAL.println(); - DEBUG_SERIAL.print(F("ID: "));DEBUG_SERIAL.print(read_result.xel[1].id);DEBUG_SERIAL.print(" "); - DEBUG_SERIAL.print(F(", Present Velocity: "));DEBUG_SERIAL.print(recv_velocity);DEBUG_SERIAL.print(" "); - DEBUG_SERIAL.print(F(", Packet Error: "));DEBUG_SERIAL.print(read_result.xel[1].error);DEBUG_SERIAL.print(" "); - DEBUG_SERIAL.print(F(", Param Length: "));DEBUG_SERIAL.print(read_result.xel[1].length);DEBUG_SERIAL.print(" "); - DEBUG_SERIAL.println(); + static uint32_t try_count = 0; + uint8_t recv_cnt; + + bw_data_xel_1.goal_velocity+=5; + if(bw_data_xel_1.goal_velocity >= 200){ + bw_data_xel_1.goal_velocity = 0; + } + bw_data_xel_2.goal_position+=255; + if(bw_data_xel_2.goal_position >= 1023){ + bw_data_xel_2.goal_position = 0; + } + bw_infos.is_info_changed = true; + + DEBUG_SERIAL.print("\n>>>>>> Bulk Instruction Test : "); + DEBUG_SERIAL.println(try_count++); + if(dxl.bulkWrite(&bw_infos) == true){ + DEBUG_SERIAL.println("[BulkWrite] Success"); + DEBUG_SERIAL.print(" ID: ");DEBUG_SERIAL.println(bw_infos.p_xels[0].id); + DEBUG_SERIAL.print("\t Goal Velocity: ");DEBUG_SERIAL.println(bw_data_xel_1.goal_velocity); + + DEBUG_SERIAL.print(" ID: ");DEBUG_SERIAL.println(bw_infos.p_xels[1].id); + DEBUG_SERIAL.print("\t Goal Position: ");DEBUG_SERIAL.println(bw_data_xel_2.goal_position); + }else{ + DEBUG_SERIAL.print("[BulkWrite] Fail, Lib error code: "); + DEBUG_SERIAL.print(dxl.getLastLibErrCode()); + } DEBUG_SERIAL.println(); - delay(100); -} + delay(250); + + recv_cnt = dxl.bulkRead(&br_infos); + if(recv_cnt > 0){ + DEBUG_SERIAL.print("[BulkRead] Success, Received ID Count: "); + DEBUG_SERIAL.println(recv_cnt); + + DEBUG_SERIAL.print(" ID: ");DEBUG_SERIAL.print(br_infos.p_xels[0].id); + DEBUG_SERIAL.print(", Error: ");DEBUG_SERIAL.println(br_infos.p_xels[0].error); + DEBUG_SERIAL.print("\t Present Current: ");DEBUG_SERIAL.println(br_data_xel_1.present_current); + DEBUG_SERIAL.print("\t Present Velocity: ");DEBUG_SERIAL.println(br_data_xel_1.present_velocity); + + DEBUG_SERIAL.print(" ID: ");DEBUG_SERIAL.print(br_infos.p_xels[1].id); + DEBUG_SERIAL.print(", Error: ");DEBUG_SERIAL.println(br_infos.p_xels[1].error); + DEBUG_SERIAL.print("\t Present Position: ");DEBUG_SERIAL.println(br_data_xel_2.present_position); + }else{ + DEBUG_SERIAL.print("[BulkRead] Fail, Lib error code: "); + DEBUG_SERIAL.println(dxl.getLastLibErrCode()); + } + DEBUG_SERIAL.println("======================================================="); + + digitalWrite(LED_BUILTIN, !digitalRead(LED_BUILTIN)); + delay(750); +} diff --git a/examples/advanced/operating_mode_advanced/operating_mode_advanced.ino b/examples/advanced/operating_mode_advanced/operating_mode_advanced.ino index b464c47..5308143 100644 --- a/examples/advanced/operating_mode_advanced/operating_mode_advanced.ino +++ b/examples/advanced/operating_mode_advanced/operating_mode_advanced.ino @@ -16,17 +16,37 @@ #include -#if defined(ARDUINO_AVR_UNO) || defined(ARDUINO_AVR_MEGA2560) +// 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 uint8_t DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN -#else +#elif defined(ARDUINO_SAM_DUE) // When using DynamixelShield + #define DXL_SERIAL Serial + #define DEBUG_SERIAL SerialUSB + const uint8_t 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 uint8_t 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 uint8_t 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 uint8_t DXL_DIR_PIN = 84; // OpenCR Board's DIR PIN. +#else // Other boards when using DynamixelShield #define DXL_SERIAL Serial1 #define DEBUG_SERIAL Serial const uint8_t DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN #endif + const uint8_t DXL_ID = 1; diff --git a/examples/advanced/read_write_ControlTableItem/read_write_ControlTableItem.ino b/examples/advanced/read_write_ControlTableItem/read_write_ControlTableItem.ino index 17059ff..b75997f 100644 --- a/examples/advanced/read_write_ControlTableItem/read_write_ControlTableItem.ino +++ b/examples/advanced/read_write_ControlTableItem/read_write_ControlTableItem.ino @@ -115,27 +115,38 @@ EXTERNAL_PORT_DATA_4, */ - -#ifdef ARDUINO_AVR_UNO +// 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); //RX,TX + SoftwareSerial soft_serial(7, 8); // DYNAMIXELShield UART RX/TX #define DXL_SERIAL Serial #define DEBUG_SERIAL soft_serial - const uint8_t DXL_DIR_PIN = 2; //DYNAMIXEL Shield -#elif ARDUINO_AVR_MEGA2560 + const uint8_t DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN +#elif defined(ARDUINO_SAM_DUE) // When using DynamixelShield #define DXL_SERIAL Serial - #define DEBUG_SERIAL Serial1 - const uint8_t DXL_DIR_PIN = 2; //DYNAMIXEL Shield -#elif BOARD_OpenCM904 - #define DXL_SERIAL Serial3 //OpenCM9.04 EXP Board's DXL port Serial. (To use the DXL port on the OpenCM 9.04 board, you must use Serial1 for Serial. And because of the OpenCM 9.04 driver code, you must call Serial1.setDxlMode(true); before dxl.begin();.) + #define DEBUG_SERIAL SerialUSB + const uint8_t 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 uint8_t 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 uint8_t DXL_DIR_PIN = 22; //OpenCM9.04 EXP Board's DIR PIN. (To use the DXL port on the OpenCM 9.04 board, you must use 28 for DIR PIN.) -#else + const uint8_t 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 uint8_t DXL_DIR_PIN = 84; // OpenCR Board's DIR PIN. +#else // Other boards when using DynamixelShield #define DXL_SERIAL Serial1 #define DEBUG_SERIAL Serial - const uint8_t DXL_DIR_PIN = 2; //DYNAMIXEL Shield + const uint8_t DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN #endif + const uint8_t DXL_ID = 1; Dynamixel2Arduino dxl(DXL_SERIAL, DXL_DIR_PIN); diff --git a/examples/advanced/slave/slave.ino b/examples/advanced/slave/slave.ino index 2a5c235..9b36055 100644 --- a/examples/advanced/slave/slave.ino +++ b/examples/advanced/slave/slave.ino @@ -16,19 +16,37 @@ #include -// The following definitions are examples when using the DYNAMIXEL Shield. // Please modify it to suit your hardware. -#if defined(ARDUINO_AVR_UNO) || defined(ARDUINO_AVR_MEGA2560) +#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 uint8_t DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN -#else +#elif defined(ARDUINO_SAM_DUE) // When using DynamixelShield + #define DXL_SERIAL Serial + #define DEBUG_SERIAL SerialUSB + const uint8_t 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 uint8_t 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 uint8_t 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 uint8_t DXL_DIR_PIN = 84; // OpenCR Board's DIR PIN. +#else // Other boards when using DynamixelShield #define DXL_SERIAL Serial1 #define DEBUG_SERIAL Serial const uint8_t DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN #endif + // Create a port object for DYNAMIXEL communication. // The list of available classes is as follows. diff --git a/examples/advanced/slave_callback/slave_callback.ino b/examples/advanced/slave_callback/slave_callback.ino index 17a744e..b2a6410 100644 --- a/examples/advanced/slave_callback/slave_callback.ino +++ b/examples/advanced/slave_callback/slave_callback.ino @@ -16,19 +16,37 @@ #include -// The following definitions are examples when using the DYNAMIXEL Shield. // Please modify it to suit your hardware. -#if defined(ARDUINO_AVR_UNO) || defined(ARDUINO_AVR_MEGA2560) +#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 uint8_t DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN -#else +#elif defined(ARDUINO_SAM_DUE) // When using DynamixelShield + #define DXL_SERIAL Serial + #define DEBUG_SERIAL SerialUSB + const uint8_t 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 uint8_t 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 uint8_t 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 uint8_t DXL_DIR_PIN = 84; // OpenCR Board's DIR PIN. +#else // Other boards when using DynamixelShield #define DXL_SERIAL Serial1 #define DEBUG_SERIAL Serial const uint8_t DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN #endif + // Create a port object for DYNAMIXEL communication. // The list of available classes is as follows. 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 05b1758..0182587 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 @@ -16,131 +16,202 @@ #include -#if defined(ARDUINO_AVR_UNO) || defined(ARDUINO_AVR_MEGA2560) +// 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 uint8_t DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN -#else +#elif defined(ARDUINO_SAM_DUE) // When using DynamixelShield + #define DXL_SERIAL Serial + #define DEBUG_SERIAL SerialUSB + const uint8_t 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 uint8_t 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 uint8_t 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 uint8_t DXL_DIR_PIN = 84; // OpenCR Board's DIR PIN. +#else // Other boards when using DynamixelShield #define DXL_SERIAL Serial1 #define DEBUG_SERIAL Serial const uint8_t DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN #endif -/* ParamForSyncReadInst_t - A structure that contains the information needed for the parameters of the 'syncRead packet'. - - typedef struct ParamForSyncReadInst{ - uint16_t addr; - uint16_t length; - uint8_t id_count; - InfoForSyncReadParam_t xel[DXL_MAX_NODE]; //refer to below. - } ParamForSyncReadInst_t; - - typedef struct InfoForSyncReadParam{ - uint8_t id; - } InfoForSyncReadParam_t; -*/ -/* ParamForSyncWriteInst_t - A structure that contains the information needed for the parameters of the 'syncWrite packet'. +/* syncRead + Structures containing the necessary information to process the 'syncRead' packet. - typedef struct ParamForSyncWriteInst{ + typedef struct XELInfoBulkRead{ uint16_t addr; - uint16_t length; - uint8_t id_count; - XelInfoForSyncWriteParam_t xel[DXL_MAX_NODE]; //refer to below. - } ParamForSyncWriteInst_t; - - typedef struct XelInfoForSyncWriteParam{ + uint16_t addr_length; + uint8_t *p_recv_buf; uint8_t id; - uint8_t data[DXL_MAX_NODE_BUFFER_SIZE]; - } XelInfoForSyncWriteParam_t; + uint8_t error; + } __attribute__((packed)) XELInfoBulkRead_t; + + typedef struct InfoBulkReadInst{ + XELInfoBulkRead_t* p_xels; + uint8_t xel_count; + bool is_info_changed; + InfoSyncBulkBuffer_t packet; + } __attribute__((packed)) InfoBulkReadInst_t; */ -/* RecvInfoFromStatusInst_t - A structure used to receive data from multiple XELs. - - typedef struct RecvInfoFromStatusInst{ - uint8_t id_count; - XelInfoForStatusInst_t xel[DXL_MAX_NODE]; //refer to below. - } RecvInfoFromStatusInst_t; +/* syncWrite + Structures containing the necessary information to process the 'syncWrite' packet. - typedef struct XelInfoForStatusInst{ + typedef struct XELInfoBulkWrite{ + uint16_t addr; + uint16_t addr_length; + uint8_t* p_data; uint8_t id; - uint16_t length; - uint8_t error; - uint8_t data[DXL_MAX_NODE_BUFFER_SIZE]; - } XelInfoForStatusInst_t; + } __attribute__((packed)) XELInfoBulkWrite_t; + + typedef struct InfoBulkWriteInst{ + XELInfoBulkWrite_t* p_xels; + uint8_t xel_count; + bool is_info_changed; + InfoSyncBulkBuffer_t packet; + } __attribute__((packed)) InfoBulkWriteInst_t; */ -ParamForSyncReadInst_t sync_read_param; -ParamForSyncWriteInst_t sync_write_param; -RecvInfoFromStatusInst_t read_result; +const uint8_t DXL_ID_CNT = 2; +const uint8_t DXL_ID_LIST[DXL_ID_CNT] = {1, 2}; +const uint16_t user_pkt_buf_cap = 128; +uint8_t user_pkt_buf[user_pkt_buf_cap]; + +const uint16_t SR_START_ADDR = 126; +const uint16_t SR_ADDR_LEN = 10; //2+4+4 +const uint16_t SW_START_ADDR = 104; //Goal velocity +const uint16_t SW_ADDR_LEN = 4; +typedef struct sr_data{ + int16_t present_current; + int32_t present_velocity; + int32_t present_position; +} __attribute__((packed)) sr_data_t; +typedef struct sw_data{ + int32_t goal_velocity; +} __attribute__((packed)) sw_data_t; + + +sr_data_t sr_data[DXL_ID_CNT]; +DYNAMIXEL::InfoSyncReadInst_t sr_infos; +DYNAMIXEL::XELInfoSyncRead_t info_xels_sr[DXL_ID_CNT]; + +sw_data_t sw_data[DXL_ID_CNT]; +DYNAMIXEL::InfoSyncWriteInst_t sw_infos; +DYNAMIXEL::XELInfoSyncWrite_t info_xels_sw[DXL_ID_CNT]; Dynamixel2Arduino dxl(DXL_SERIAL, DXL_DIR_PIN); void setup() { // put your setup code here, to run once: + uint8_t i; + pinMode(LED_BUILTIN, OUTPUT); DEBUG_SERIAL.begin(115200); dxl.begin(57600); - dxl.scan(); - - // fill the members of structure for syncWrite - sync_write_param.addr = 104; //Goal Velocity on X serise - sync_write_param.length = 4; - sync_write_param.xel[0].id = 1; - sync_write_param.xel[1].id = 3; - sync_write_param.id_count = 2; - - // fill the members of structure for syncRead - sync_read_param.addr = 128; //Present Velocity on X serise - sync_read_param.length = 4; - sync_read_param.xel[0].id = 1; - sync_read_param.xel[1].id = 3; - sync_read_param.id_count = 2; - dxl.torqueOff(1); - dxl.setOperatingMode(1, OP_VELOCITY); - dxl.torqueOn(1); - - dxl.torqueOff(3); - dxl.setOperatingMode(3, OP_VELOCITY); - dxl.torqueOn(3); + for(i=0; i= 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); - delay(100); - - // Print the read data using SyncRead - dxl.syncRead(sync_read_param, read_result); - DEBUG_SERIAL.println(F("======= Sync Read =======")); - 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(F("ID: "));DEBUG_SERIAL.print(read_result.xel[0].id);DEBUG_SERIAL.print(" "); - DEBUG_SERIAL.print(F(", Present Velocity: "));DEBUG_SERIAL.print(recv_velocity[0]);DEBUG_SERIAL.print(" "); - DEBUG_SERIAL.print(F(", Packet Error: "));DEBUG_SERIAL.print(read_result.xel[0].error);DEBUG_SERIAL.print(" "); - DEBUG_SERIAL.print(F(", Param Length: "));DEBUG_SERIAL.print(read_result.xel[0].length);DEBUG_SERIAL.print(" "); - DEBUG_SERIAL.println(); - DEBUG_SERIAL.print(F("ID: "));DEBUG_SERIAL.print(read_result.xel[1].id);DEBUG_SERIAL.print(" "); - DEBUG_SERIAL.print(F(", Present Velocity: "));DEBUG_SERIAL.print(recv_velocity[1]);DEBUG_SERIAL.print(" "); - DEBUG_SERIAL.print(F(", Packet Error: "));DEBUG_SERIAL.print(read_result.xel[1].error);DEBUG_SERIAL.print(" "); - DEBUG_SERIAL.print(F(", Param Length: "));DEBUG_SERIAL.print(read_result.xel[1].length);DEBUG_SERIAL.print(" "); + static uint32_t try_count = 0; + uint8_t i, recv_cnt; + + for(i=0; i= 200){ + sw_data[i].goal_velocity = 0; + } + } + sw_infos.is_info_changed = true; + + DEBUG_SERIAL.print("\n>>>>>> Sync Instruction Test : "); + DEBUG_SERIAL.println(try_count++); + if(dxl.syncWrite(&sw_infos) == true){ + DEBUG_SERIAL.println("[SyncWrite] Success"); + for(i=0; i 0){ + DEBUG_SERIAL.print("[SyncRead] Success, Received ID Count: "); + DEBUG_SERIAL.println(recv_cnt); + for(i=0; i -#if defined(ARDUINO_AVR_UNO) || defined(ARDUINO_AVR_MEGA2560) +// 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 uint8_t DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN -#else +#elif defined(ARDUINO_SAM_DUE) // When using DynamixelShield + #define DXL_SERIAL Serial + #define DEBUG_SERIAL SerialUSB + const uint8_t 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 uint8_t 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 uint8_t 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 uint8_t DXL_DIR_PIN = 84; // OpenCR Board's DIR PIN. +#else // Other boards when using DynamixelShield #define DXL_SERIAL Serial1 #define DEBUG_SERIAL Serial const uint8_t DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN #endif + const uint8_t DXL_ID = 1; const float DXL_PROTOCOL_VERSION = 2.0; diff --git a/examples/basic/copy_eeprom_x/copy_eeprom_x.ino b/examples/basic/copy_eeprom_x/copy_eeprom_x.ino new file mode 100644 index 0000000..39f0b85 --- /dev/null +++ b/examples/basic/copy_eeprom_x/copy_eeprom_x.ino @@ -0,0 +1,439 @@ +/******************************************************************************* +* 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 uint8_t 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 uint8_t 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 uint8_t 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 uint8_t 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 uint8_t DXL_DIR_PIN = 84; // OpenCR Board's DIR PIN. +#else // Other boards when using DynamixelShield + #define DXL_SERIAL Serial1 + #define DEBUG_SERIAL Serial + const uint8_t DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN +#endif + +#define MAX_BAUD 5 +const uint32_t baud[MAX_BAUD] = {57600, 115200, 1000000, 2000000, 3000000}; +#define INVALID_ID 253 + +struct DxlList +{ + uint8_t dxl_id; + uint32_t dxl_baudrate; + uint8_t dxl_protocol; +}; + +uint8_t itemList[] = {ID, BAUD_RATE, PROTOCOL_VERSION, MODEL_NUMBER, FIRMWARE_VERSION, RETURN_DELAY_TIME, DRIVE_MODE, OPERATING_MODE, SECONDARY_ID, HOMING_OFFSET, MOVING_THRESHOLD, TEMPERATURE_LIMIT, MAX_VOLTAGE_LIMIT, MIN_VOLTAGE_LIMIT, PWM_LIMIT, CURRENT_LIMIT, VELOCITY_LIMIT, MAX_POSITION_LIMIT, MIN_POSITION_LIMIT, SHUTDOWN}; +String itemListStr[] = { + "ID ", + "BAUD RATE ", + "PROTOCOL VERSION ", + "MODEL NUMBER ", + "FIRMWARE VERSION ", + "RETURN DELAY TIME ", + "DRIVE MODE ", + "OPERATING MODE ", + "SECONDARY ID ", + "HOMING OFFSET ", + "MOVING THRESHOLD ", + "TEMPERATURE LIMIT ", + "MAX VOLTAGE LIMIT ", + "MIN VOLTAGE LIMIT ", + "PWM LIMIT ", + "CURRENT LIMIT ", + "VELOCITY LIMIT ", + "MAX POSITION LIMIT", + "MIN POSITION LIMIT", + "SHUTDOWN "}; + +struct DxlList DXLArray[10]; +int8_t found_dynamixel = 0; +int MasterString = 0; +uint8_t saved_index_a = 0; +uint8_t saved_index_b = 0; +uint8_t dxl_a = 0; +uint8_t dxl_b = 0; + +Dynamixel2Arduino dxl(DXL_SERIAL, DXL_DIR_PIN); + +void setup() +{ + // Use UART port of DYNAMIXEL Shield to debug. + DEBUG_SERIAL.begin(115200); //set debugging port baudrate to 115200bps + while(!DEBUG_SERIAL); //Wait until the serial port is opened + + DEBUG_SERIAL.println(F("//*********** DYNAMIXEL X Series EEPROM Copy ***********//")); + DEBUG_SERIAL.println(F("//*** This example is written for XM/XH Series Only ****//")); + DEBUG_SERIAL.println(F("//********* Up to 10 DYNAMIXEL can be detected *********//\n")); + scanDynamixel(); +} + +bool scanDynamixel() +{ + memset(DXLArray, 0, sizeof(DXLArray)); + found_dynamixel = 0; + bool result = false; + int8_t index = 0; + + for(uint8_t protocol = 1; protocol < 3; protocol++) + { + // Set Port Protocol Version. This has to match with DYNAMIXEL protocol version. + dxl.setPortProtocolVersion((float)protocol); + DEBUG_SERIAL.print(F("SCAN PROTOCOL ")); + DEBUG_SERIAL.println(protocol); + + for(index = 0; index < MAX_BAUD; index++) + { + // Set Port baudrate. + DEBUG_SERIAL.print(F("BAUDRATE ")); + DEBUG_SERIAL.println(baud[index]); + dxl.begin(baud[index]); + for(uint8_t id = 0; id < DXL_BROADCAST_ID; id++) + { + //iterate until all ID in each baudrate is scanned. + if(dxl.ping(id)) + { + DEBUG_SERIAL.print(F("\tID : ")); + DEBUG_SERIAL.print(id); + DEBUG_SERIAL.print(F(", Model Number: ")); + DEBUG_SERIAL.println(dxl.getModelNumber(id)); + // Save detected DYNAMIXEL info (ID & baudrate) to global variable to access from copyEEPROM() + DXLArray[found_dynamixel] = {id, baud[index], protocol}; + found_dynamixel++; + result = true; + } + } + } + } + DEBUG_SERIAL.print(F("\nTotal ")); + DEBUG_SERIAL.print(found_dynamixel); + DEBUG_SERIAL.println(F(" DYNAMIXEL(s) found!")); + + return result; +} + +uint8_t validIdCheck(uint8_t id) +{ + for(int i = 0; i <= found_dynamixel; i++) + { + if(DXLArray[i].dxl_id == id) + { + return i; + } + } + return INVALID_ID; +} + +void safetyCheck(uint8_t id_a, uint8_t id_b) +{ + uint16_t modelNumberA, modelNumberB; + uint8_t fwVersionA, fwVersionB; + saved_index_a = validIdCheck(id_a); + saved_index_b = validIdCheck(id_b); + + //Torque off all DYNAMIXEL + for(int i=0 ; i < found_dynamixel; i++) + { + dxl.begin(DXLArray[i].dxl_baudrate); + dxl.torqueOff(DXLArray[i].dxl_id); + } + + // Check Model Number, F/W version match + dxl.setPortProtocolVersion((float)DXLArray[validIdCheck(id_a)].dxl_protocol); + dxl.begin(DXLArray[validIdCheck(id_a)].dxl_baudrate); + modelNumberA = dxl.getModelNumber(id_a); + fwVersionA = dxl.readControlTableItem(FIRMWARE_VERSION, id_a); + dxl.setPortProtocolVersion((float)DXLArray[validIdCheck(id_b)].dxl_protocol); + dxl.begin(DXLArray[validIdCheck(id_b)].dxl_baudrate); + modelNumberB = dxl.getModelNumber(id_b); + fwVersionB = dxl.readControlTableItem(FIRMWARE_VERSION, id_b); + + if(modelNumberA != modelNumberB) + { + DEBUG_SERIAL.println(F("[ERROR] Please select identical DYNAMIXEL series!")); + DEBUG_SERIAL.print(F("ID ")); + DEBUG_SERIAL.print(id_a); + DEBUG_SERIAL.print(F(" model number : ")); + DEBUG_SERIAL.println(modelNumberA); + DEBUG_SERIAL.print(F("ID ")); + DEBUG_SERIAL.print(id_b); + DEBUG_SERIAL.print(F(" model number : ")); + DEBUG_SERIAL.println(modelNumberB); + } + else + { + if(fwVersionA != fwVersionB) + { + DEBUG_SERIAL.println(F("[ERROR] DYNAMIXEL Firmware version do not match!")); + DEBUG_SERIAL.print(F("ID ")); + DEBUG_SERIAL.print(id_a); + DEBUG_SERIAL.print(F(" firmware version : ")); + DEBUG_SERIAL.println(modelNumberA); + DEBUG_SERIAL.print(F("ID ")); + DEBUG_SERIAL.print(id_b); + DEBUG_SERIAL.print(F(" firmware version : ")); + DEBUG_SERIAL.println(modelNumberB); + } + // Compare EEPROM Data + else + { + if((saved_index_a != INVALID_ID) && (saved_index_b != INVALID_ID)) + { + for (uint8_t i = 0; i < sizeof(itemList); i++) + { + dxl.setPortProtocolVersion((float)DXLArray[saved_index_a].dxl_protocol); + dxl.begin(DXLArray[saved_index_a].dxl_baudrate); + DEBUG_SERIAL.print(itemListStr[i]); + DEBUG_SERIAL.print(": "); + if(i == 9) {DEBUG_SERIAL.print(dxl.readControlTableItem(itemList[i], id_a));} + else if(i == 0 || i == 5 || i == 8) {DEBUG_SERIAL.print((uint8_t)dxl.readControlTableItem(itemList[i], id_a));} + else {DEBUG_SERIAL.print((uint32_t)dxl.readControlTableItem(itemList[i], id_a));} + dxl.setPortProtocolVersion((float)DXLArray[saved_index_b].dxl_protocol); + dxl.begin(DXLArray[saved_index_b].dxl_baudrate); + DEBUG_SERIAL.print(" \t"); + if(i == 9) {DEBUG_SERIAL.println(dxl.readControlTableItem(itemList[i], id_b));} + else if(i == 0 || i == 5 || i == 8) {DEBUG_SERIAL.println((uint8_t)dxl.readControlTableItem(itemList[i], id_b));} + else {DEBUG_SERIAL.println((uint32_t)dxl.readControlTableItem(itemList[i], id_b));} + } + } + } + } +} + +void copyEEPROM(uint8_t start_index) +{ + for (uint8_t i = start_index; i < sizeof(itemList); i++) + { + int32_t data = 0; + dxl.setPortProtocolVersion((float)DXLArray[saved_index_a].dxl_protocol); + dxl.begin(DXLArray[saved_index_a].dxl_baudrate); + + DEBUG_SERIAL.print(F("Copying ")); + DEBUG_SERIAL.print(itemListStr[i]); + + data = dxl.readControlTableItem(itemList[i], DXLArray[saved_index_a].dxl_id); + + dxl.setPortProtocolVersion((float)DXLArray[saved_index_b].dxl_protocol); + dxl.begin(DXLArray[saved_index_b].dxl_baudrate); + + if(dxl.writeControlTableItem(itemList[i], DXLArray[saved_index_b].dxl_id, data)) + { + DEBUG_SERIAL.println(F(" Succeeded.")); + } + else + { + DEBUG_SERIAL.println(F(" Failed.")); + } + } +} + +bool copyBaudProtocol() +{ + int32_t baudrate, protocol = 0; + bool ret = false; + + dxl.setPortProtocolVersion((float)DXLArray[saved_index_a].dxl_protocol); + dxl.begin(DXLArray[saved_index_a].dxl_baudrate); + + baudrate = dxl.readControlTableItem(BAUD_RATE, DXLArray[saved_index_a].dxl_id); + protocol = dxl.readControlTableItem(PROTOCOL_VERSION, DXLArray[saved_index_a].dxl_id); + + dxl.setPortProtocolVersion((float)DXLArray[saved_index_b].dxl_protocol); + dxl.begin(DXLArray[saved_index_b].dxl_baudrate); + DEBUG_SERIAL.print(F("Copying Protocol Version")); + if(dxl.writeControlTableItem(PROTOCOL_VERSION, DXLArray[saved_index_b].dxl_id, protocol)) + { + DEBUG_SERIAL.println(F("\tSucceeded.")); + ret = true; + } + else + { + DEBUG_SERIAL.println(F("\tFailed.")); + } + + dxl.setPortProtocolVersion((float)DXLArray[saved_index_a].dxl_protocol); + DEBUG_SERIAL.print(F("Copying Baud Rate")); + if(dxl.writeControlTableItem(BAUD_RATE, DXLArray[saved_index_b].dxl_id, baudrate)) + { + DEBUG_SERIAL.println(F("\tSucceeded.")); + ret = true; + } + else + { + DEBUG_SERIAL.println(F("\tFailed.")); + } + + if(ret==true) + { + DEBUG_SERIAL.println(F("Also copy ID? [Y/N]")); + DEBUG_SERIAL.println(F("**WARNING** Copying ID will cause communication collision!")); + while (!DEBUG_SERIAL.available()); + MasterString = DEBUG_SERIAL.read(); + DEBUG_SERIAL.read(); // This is called just to reset the Serial.available(); + + if(MasterString == 'y' || MasterString == 'Y') + { + DEBUG_SERIAL.print(F("Copying ID")); + if(!copyId()) + { + DEBUG_SERIAL.println(F("\tFailed.")); + } + else + { + DEBUG_SERIAL.println(F("\tSucceeded.")); + } + } + else + { + DEBUG_SERIAL.println(F("Exit without copying ID.")); + } + } + return ret; +} + +bool copyId() +{ + int32_t id = 0; + + dxl.setPortProtocolVersion((float)DXLArray[saved_index_a].dxl_protocol); + dxl.begin(DXLArray[saved_index_a].dxl_baudrate); + id = dxl.readControlTableItem(ID, DXLArray[saved_index_a].dxl_id); + if(dxl.writeControlTableItem(ID, DXLArray[saved_index_b].dxl_id, id)) + { + return true; + } + else + { + return false; + } +} + +void loop() +{ + // Rescan if none of DYNAMIXEL detected + if(found_dynamixel == 0) + { + scanDynamixel(); + } + DEBUG_SERIAL.print(F("\n[STEP 1] Please enter the DYNAMIXEL ID to read: ")); + + while (!DEBUG_SERIAL.available()); + MasterString = DEBUG_SERIAL.parseInt(); + DEBUG_SERIAL.read(); // This is called just to reset the Serial.available(); + DEBUG_SERIAL.println(MasterString); + + if((MasterString < 0) || (MasterString > INVALID_ID)) + { + DEBUG_SERIAL.println(F("\tERROR : ID out of range!!!\n")); + } + else + { + dxl_a = (uint8_t)MasterString; + + if(validIdCheck(dxl_a) == INVALID_ID) + { + DEBUG_SERIAL.println(F("\tERROR : Invalid ID!!!\n")); + } + else + { + // compareEEPROM(dxl_a); + DEBUG_SERIAL.print(F("[STEP 2] Please enter the DYNAMIXEL ID to write: ")); + + while (!DEBUG_SERIAL.available()); + MasterString = DEBUG_SERIAL.parseInt(); + DEBUG_SERIAL.read(); // This is called just to reset the Serial.available(); + DEBUG_SERIAL.println(MasterString); + if((MasterString < 0) || (MasterString > INVALID_ID)) + { + DEBUG_SERIAL.println(F("\tERROR : ID out of range!!!\n")); + } + else + { + dxl_b = (uint8_t)MasterString; + if((validIdCheck(dxl_b) == INVALID_ID) || (dxl_b == dxl_a)) + { + DEBUG_SERIAL.println(F("\tERROR : Invalid ID!!!\n")); + } + else + { + safetyCheck(dxl_a, dxl_b); + + // Start copying EEPROM data + if((saved_index_a != INVALID_ID) && (saved_index_b != INVALID_ID)) + { + DEBUG_SERIAL.println(F("Start EEPROM copy? [Y/N/A]")); + DEBUG_SERIAL.println(F("WARNING!!! [A] option will also copy ID, Baudrate, Protocol")); + DEBUG_SERIAL.println(F("that will cause communication collision of connected DYNAMIXEL.")); + while (!DEBUG_SERIAL.available()); + MasterString = DEBUG_SERIAL.read(); + DEBUG_SERIAL.read(); // This is called just to reset the Serial.available(); + + switch(MasterString) + { + case 'Y': + case 'y': + copyEEPROM(5); + break; + case 'N': + case 'n': + break; + case 'A': + case 'a': + copyEEPROM(5); + copyBaudProtocol(); + break; + default: + break; + } + } + } + } + } + } + + DEBUG_SERIAL.println(F("\nScan DYNAMIXEL again? [Y/N]")); + + while (!DEBUG_SERIAL.available()); + MasterString = DEBUG_SERIAL.read(); + DEBUG_SERIAL.read(); // This is called just to reset the Serial.available(); + + if(MasterString == 'y' || MasterString == 'Y') + { + scanDynamixel(); + } +} diff --git a/examples/basic/current_mode/current_mode.ino b/examples/basic/current_mode/current_mode.ino index db5cc6f..f5b05de 100644 --- a/examples/basic/current_mode/current_mode.ino +++ b/examples/basic/current_mode/current_mode.ino @@ -16,17 +16,37 @@ #include -#if defined(ARDUINO_AVR_UNO) || defined(ARDUINO_AVR_MEGA2560) +// 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 uint8_t DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN -#else +#elif defined(ARDUINO_SAM_DUE) // When using DynamixelShield + #define DXL_SERIAL Serial + #define DEBUG_SERIAL SerialUSB + const uint8_t 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 uint8_t 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 uint8_t 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 uint8_t DXL_DIR_PIN = 84; // OpenCR Board's DIR PIN. +#else // Other boards when using DynamixelShield #define DXL_SERIAL Serial1 #define DEBUG_SERIAL Serial const uint8_t DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN #endif + const uint8_t DXL_ID = 1; const float DXL_PROTOCOL_VERSION = 2.0; diff --git a/examples/basic/current_position_mode/current_position_mode.ino b/examples/basic/current_position_mode/current_position_mode.ino index 50718c2..f8449fa 100644 --- a/examples/basic/current_position_mode/current_position_mode.ino +++ b/examples/basic/current_position_mode/current_position_mode.ino @@ -20,17 +20,37 @@ #include -#if defined(ARDUINO_AVR_UNO) || defined(ARDUINO_AVR_MEGA2560) +// 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 uint8_t DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN -#else +#elif defined(ARDUINO_SAM_DUE) // When using DynamixelShield + #define DXL_SERIAL Serial + #define DEBUG_SERIAL SerialUSB + const uint8_t 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 uint8_t 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 uint8_t 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 uint8_t DXL_DIR_PIN = 84; // OpenCR Board's DIR PIN. +#else // Other boards when using DynamixelShield #define DXL_SERIAL Serial1 #define DEBUG_SERIAL Serial const uint8_t DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN #endif + const uint8_t DXL_ID = 1; const float DXL_PROTOCOL_VERSION = 2.0; diff --git a/examples/basic/id/id.ino b/examples/basic/id/id.ino index 403ddb9..6a9d29f 100644 --- a/examples/basic/id/id.ino +++ b/examples/basic/id/id.ino @@ -16,28 +16,51 @@ #include -#if defined(ARDUINO_AVR_UNO) || defined(ARDUINO_AVR_MEGA2560) +// 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 uint8_t DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN -#else +#elif defined(ARDUINO_SAM_DUE) // When using DynamixelShield + #define DXL_SERIAL Serial + #define DEBUG_SERIAL SerialUSB + const uint8_t 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 uint8_t 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 uint8_t 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 uint8_t DXL_DIR_PIN = 84; // OpenCR Board's DIR PIN. +#else // Other boards when using DynamixelShield #define DXL_SERIAL Serial1 #define DEBUG_SERIAL Serial const uint8_t DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN #endif + -const uint8_t DXL_ID = 1; +const uint8_t DEFAULT_DXL_ID = 1; const float DXL_PROTOCOL_VERSION = 2.0; Dynamixel2Arduino dxl(DXL_SERIAL, DXL_DIR_PIN); void setup() { // put your setup code here, to run once: + uint8_t present_id = DEFAULT_DXL_ID; + uint8_t new_id = 0; // Use UART port of DYNAMIXEL Shield to debug. DEBUG_SERIAL.begin(115200); + while(!DEBUG_SERIAL); // Set Port baudrate to 57600bps. This has to match with DYNAMIXEL baudrate. dxl.begin(57600); @@ -47,22 +70,36 @@ void setup() { DEBUG_SERIAL.print("PROTOCOL "); DEBUG_SERIAL.print(DXL_PROTOCOL_VERSION, 1); DEBUG_SERIAL.print(", ID "); - DEBUG_SERIAL.print(DXL_ID); + DEBUG_SERIAL.print(present_id); DEBUG_SERIAL.print(": "); - if(dxl.ping(DXL_ID) == true) { + if(dxl.ping(present_id) == true) { DEBUG_SERIAL.print("ping succeeded!"); DEBUG_SERIAL.print(", Model Number: "); - DEBUG_SERIAL.println(dxl.getModelNumber(DXL_ID)); + DEBUG_SERIAL.println(dxl.getModelNumber(present_id)); // Turn off torque when configuring items in EEPROM area - dxl.torqueOff(DXL_ID); + dxl.torqueOff(present_id); // set a new ID for DYNAMIXEL. Do not use ID 200 - dxl.setID(DXL_ID, 100); - DEBUG_SERIAL.println("ID has been successfully changed to 100"); + new_id = 100; + if(dxl.setID(present_id, new_id) == true){ + present_id = new_id; + DEBUG_SERIAL.print("ID has been successfully changed to "); + DEBUG_SERIAL.println(new_id); - dxl.setID(100, DXL_ID); - DEBUG_SERIAL.println("ID has been successfully changed back to Original ID"); + new_id = DEFAULT_DXL_ID; + if(dxl.setID(present_id, new_id) == true){ + present_id = new_id; + DEBUG_SERIAL.print("ID has been successfully changed back to Original ID "); + DEBUG_SERIAL.println(new_id); + }else{ + DEBUG_SERIAL.print("Failed to change ID to "); + DEBUG_SERIAL.println(new_id); + } + }else{ + DEBUG_SERIAL.print("Failed to change ID to "); + DEBUG_SERIAL.println(new_id); + } } else{ DEBUG_SERIAL.println("ping failed!"); diff --git a/examples/basic/led/led.ino b/examples/basic/led/led.ino index 0674329..9068122 100644 --- a/examples/basic/led/led.ino +++ b/examples/basic/led/led.ino @@ -16,13 +16,37 @@ #include -#if defined(ARDUINO_AVR_UNO) || defined(ARDUINO_AVR_MEGA2560) +// 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 uint8_t DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN -#else +#elif defined(ARDUINO_SAM_DUE) // When using DynamixelShield + #define DXL_SERIAL Serial + #define DEBUG_SERIAL SerialUSB + const uint8_t 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 uint8_t 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 uint8_t 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 uint8_t DXL_DIR_PIN = 84; // OpenCR Board's DIR PIN. +#else // Other boards when using DynamixelShield #define DXL_SERIAL Serial1 + #define DEBUG_SERIAL Serial const uint8_t DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN #endif + const uint8_t DXL_ID = 1; const float DXL_PROTOCOL_VERSION = 2.0; diff --git a/examples/basic/operation_mode/operation_mode.ino b/examples/basic/operation_mode/operation_mode.ino index c341928..5059f33 100644 --- a/examples/basic/operation_mode/operation_mode.ino +++ b/examples/basic/operation_mode/operation_mode.ino @@ -26,17 +26,37 @@ #include -#if defined(ARDUINO_AVR_UNO) || defined(ARDUINO_AVR_MEGA2560) +// 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 uint8_t DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN -#else +#elif defined(ARDUINO_SAM_DUE) // When using DynamixelShield + #define DXL_SERIAL Serial + #define DEBUG_SERIAL SerialUSB + const uint8_t 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 uint8_t 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 uint8_t 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 uint8_t DXL_DIR_PIN = 84; // OpenCR Board's DIR PIN. +#else // Other boards when using DynamixelShield #define DXL_SERIAL Serial1 #define DEBUG_SERIAL Serial const uint8_t DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN #endif + const uint8_t DXL_ID = 1; const float DXL_PROTOCOL_VERSION = 2.0; diff --git a/examples/basic/ping/ping.ino b/examples/basic/ping/ping.ino index 3558026..f2f7a9c 100644 --- a/examples/basic/ping/ping.ino +++ b/examples/basic/ping/ping.ino @@ -16,17 +16,37 @@ #include -#if defined(ARDUINO_AVR_UNO) || defined(ARDUINO_AVR_MEGA2560) +// 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 uint8_t DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN -#else +#elif defined(ARDUINO_SAM_DUE) // When using DynamixelShield + #define DXL_SERIAL Serial + #define DEBUG_SERIAL SerialUSB + const uint8_t 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 uint8_t 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 uint8_t 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 uint8_t DXL_DIR_PIN = 84; // OpenCR Board's DIR PIN. +#else // Other boards when using DynamixelShield #define DXL_SERIAL Serial1 #define DEBUG_SERIAL Serial const uint8_t DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN #endif + const uint8_t DXL_ID = 1; const float DXL_PROTOCOL_VERSION = 2.0; @@ -58,7 +78,35 @@ void loop() { DEBUG_SERIAL.print(", Model Number: "); DEBUG_SERIAL.println(dxl.getModelNumber(DXL_ID)); }else{ - DEBUG_SERIAL.println("ping failed!"); + DEBUG_SERIAL.print("ping failed!, err code: "); + DEBUG_SERIAL.println(dxl.getLastLibErrCode()); } delay(500); + + FindServos(); +} + + +DYNAMIXEL::InfoFromPing_t ping_info[32]; +void FindServos(void) { + Serial.println(" Try Protocol 2 - broadcast ping: "); + Serial.flush(); // flush it as ping may take awhile... + + if (uint8_t count_pinged = dxl.ping(DXL_BROADCAST_ID, ping_info, + sizeof(ping_info)/sizeof(ping_info[0]))) { + Serial.print("Detected Dynamixel : \n"); + for (int i = 0; i < count_pinged; i++) + { + Serial.print(" "); + Serial.print(ping_info[i].id, DEC); + Serial.print(", Model:"); + Serial.print(ping_info[i].model_number); + Serial.print(", Ver:"); + Serial.println(ping_info[i].firmware_version, HEX); + //g_servo_protocol[i] = 2; + } + }else{ + 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 f03588a..9107950 100644 --- a/examples/basic/position_mode/position_mode.ino +++ b/examples/basic/position_mode/position_mode.ino @@ -16,17 +16,37 @@ #include -#if defined(ARDUINO_AVR_UNO) || defined(ARDUINO_AVR_MEGA2560) +// 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 uint8_t DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN -#else +#elif defined(ARDUINO_SAM_DUE) // When using DynamixelShield + #define DXL_SERIAL Serial + #define DEBUG_SERIAL SerialUSB + const uint8_t 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 uint8_t 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 uint8_t 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 uint8_t DXL_DIR_PIN = 84; // OpenCR Board's DIR PIN. +#else // Other boards when using DynamixelShield #define DXL_SERIAL Serial1 #define DEBUG_SERIAL Serial const uint8_t DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN #endif + const uint8_t DXL_ID = 1; const float DXL_PROTOCOL_VERSION = 2.0; diff --git a/examples/basic/profile_velocity_acceleration/profile_velocity_acceleration.ino b/examples/basic/profile_velocity_acceleration/profile_velocity_acceleration.ino index 1381efe..f87af2e 100644 --- a/examples/basic/profile_velocity_acceleration/profile_velocity_acceleration.ino +++ b/examples/basic/profile_velocity_acceleration/profile_velocity_acceleration.ino @@ -21,17 +21,37 @@ #include -#if defined(ARDUINO_AVR_UNO) || defined(ARDUINO_AVR_MEGA2560) +// 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 uint8_t DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN -#else +#elif defined(ARDUINO_SAM_DUE) // When using DynamixelShield + #define DXL_SERIAL Serial + #define DEBUG_SERIAL SerialUSB + const uint8_t 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 uint8_t 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 uint8_t 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 uint8_t DXL_DIR_PIN = 84; // OpenCR Board's DIR PIN. +#else // Other boards when using DynamixelShield #define DXL_SERIAL Serial1 #define DEBUG_SERIAL Serial const uint8_t DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN #endif + const uint8_t DXL_ID = 1; const float DXL_PROTOCOL_VERSION = 2.0; diff --git a/examples/basic/pwm_mode/pwm_mode.ino b/examples/basic/pwm_mode/pwm_mode.ino index 1a08be8..057b609 100644 --- a/examples/basic/pwm_mode/pwm_mode.ino +++ b/examples/basic/pwm_mode/pwm_mode.ino @@ -16,17 +16,37 @@ #include -#if defined(ARDUINO_AVR_UNO) || defined(ARDUINO_AVR_MEGA2560) +// 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 uint8_t DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN -#else +#elif defined(ARDUINO_SAM_DUE) // When using DynamixelShield + #define DXL_SERIAL Serial + #define DEBUG_SERIAL SerialUSB + const uint8_t 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 uint8_t 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 uint8_t 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 uint8_t DXL_DIR_PIN = 84; // OpenCR Board's DIR PIN. +#else // Other boards when using DynamixelShield #define DXL_SERIAL Serial1 #define DEBUG_SERIAL Serial const uint8_t DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN #endif + const uint8_t DXL_ID = 1; const float DXL_PROTOCOL_VERSION = 2.0; diff --git a/examples/basic/scan_dynamixel/scan_dynamixel.ino b/examples/basic/scan_dynamixel/scan_dynamixel.ino index 4845d8d..d62bbad 100644 --- a/examples/basic/scan_dynamixel/scan_dynamixel.ino +++ b/examples/basic/scan_dynamixel/scan_dynamixel.ino @@ -16,17 +16,37 @@ #include -#if defined(ARDUINO_AVR_UNO) || defined(ARDUINO_AVR_MEGA2560) +// 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 uint8_t DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN -#else +#elif defined(ARDUINO_SAM_DUE) // When using DynamixelShield + #define DXL_SERIAL Serial + #define DEBUG_SERIAL SerialUSB + const uint8_t 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 uint8_t 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 uint8_t 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 uint8_t DXL_DIR_PIN = 84; // OpenCR Board's DIR PIN. +#else // Other boards when using DynamixelShield #define DXL_SERIAL Serial1 #define DEBUG_SERIAL Serial const uint8_t DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN #endif + #define MAX_BAUD 5 const int32_t buad[MAX_BAUD] = {57600, 115200, 1000000, 2000000, 3000000}; diff --git a/examples/basic/torque/torque.ino b/examples/basic/torque/torque.ino index 8582617..2cc4978 100644 --- a/examples/basic/torque/torque.ino +++ b/examples/basic/torque/torque.ino @@ -20,13 +20,37 @@ #include -#if defined(ARDUINO_AVR_UNO) || defined(ARDUINO_AVR_MEGA2560) +// 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 uint8_t DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN -#else +#elif defined(ARDUINO_SAM_DUE) // When using DynamixelShield + #define DXL_SERIAL Serial + #define DEBUG_SERIAL SerialUSB + const uint8_t 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 uint8_t 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 uint8_t 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 uint8_t DXL_DIR_PIN = 84; // OpenCR Board's DIR PIN. +#else // Other boards when using DynamixelShield #define DXL_SERIAL Serial1 + #define DEBUG_SERIAL Serial const uint8_t DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN #endif + const uint8_t DXL_ID = 1; const float DXL_PROTOCOL_VERSION = 2.0; diff --git a/examples/basic/velocity_mode/velocity_mode.ino b/examples/basic/velocity_mode/velocity_mode.ino index deb517e..efc2498 100644 --- a/examples/basic/velocity_mode/velocity_mode.ino +++ b/examples/basic/velocity_mode/velocity_mode.ino @@ -16,17 +16,37 @@ #include -#if defined(ARDUINO_AVR_UNO) || defined(ARDUINO_AVR_MEGA2560) +// 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 uint8_t DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN -#else +#elif defined(ARDUINO_SAM_DUE) // When using DynamixelShield + #define DXL_SERIAL Serial + #define DEBUG_SERIAL SerialUSB + const uint8_t 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 uint8_t 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 uint8_t 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 uint8_t DXL_DIR_PIN = 84; // OpenCR Board's DIR PIN. +#else // Other boards when using DynamixelShield #define DXL_SERIAL Serial1 #define DEBUG_SERIAL Serial const uint8_t DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN #endif + const uint8_t DXL_ID = 1; const float DXL_PROTOCOL_VERSION = 2.0; diff --git a/keywords.txt b/keywords.txt index 6204592..6b8bd92 100644 --- a/keywords.txt +++ b/keywords.txt @@ -21,6 +21,7 @@ begin KEYWORD2 getPortBaud KEYWORD2 ping KEYWORD2 scan KEYWORD2 +setModelNumber KEYWORD2 getModelNumber KEYWORD2 setID KEYWORD2 setProtocol KEYWORD2 @@ -42,15 +43,23 @@ readControlTableItem KEYWORD2 writeControlTableItem KEYWORD2 # Master Class +setPacketBuffer KEYWORD2 +getPacketBuffer KEYWORD2 +getPacketBufferCapacity KEYWORD2 setPortProtocolVersion KEYWORD2 +setPortProtocolVersionUsingIndex KEYWORD2 getPortProtocolVersion KEYWORD2 setPort KEYWORD2 +getPort KEYWORD2 ping KEYWORD2 read KEYWORD2 write KEYWORD2 writeNoResp KEYWORD2 +regWrite KEYWORD2 +action KEYWORD2 factoryReset KEYWORD2 reboot KEYWORD2 +clear KEYWORD2 syncWrite KEYWORD2 syncRead KEYWORD2 bulkWrite KEYWORD2 @@ -58,8 +67,13 @@ bulkRead KEYWORD2 getLastStatusPacketError KEYWORD2 getLastLibErrCode KEYWORD2 setLastLibErrCode KEYWORD2 +txInstPacket KEYWORD2 +rxStatusPacket KEYWORD2 # Slave Class +setPacketBuffer KEYWORD2 +getPacketBuffer KEYWORD2 +getPacketBufferCapacity KEYWORD2 getModelNumber KEYWORD2 setID KEYWORD2 getID KEYWORD2 @@ -67,31 +81,44 @@ setFirmwareVersion KEYWORD2 getFirmwareVersion KEYWORD2 setPort KEYWORD2 setPortProtocolVersion KEYWORD2 +setPortProtocolVersionUsingIndex KEYWORD2 getPortProtocolVersion KEYWORD2 +getPortProtocolVersionIndex KEYWORD2 setWriteCallbackFunc KEYWORD2 setReadCallbackFunc KEYWORD2 +getNumCanBeRegistered KEYWORD2 +isEnoughSpaceInControlTable KEYWORD2 processPacket KEYWORD2 getLastStatusPacketError KEYWORD2 getLastLibErrCode KEYWORD2 addControlItem KEYWORD2 +txInstPacket KEYWORD2 +rxStatusPacket KEYWORD2 + +# protocol.h +begin_make_dxl_packet KEYWORD2 +add_param_to_dxl_packet KEYWORD2 +end_make_dxl_packet KEYWORD2 +begin_parse_dxl_packet KEYWORD2 +parse_dxl_packet KEYWORD2 ########################################### # Structures (KEYWORD3) ########################################### # Master Class -ParamForSyncReadInst_t KEYWORD3 -ParamForSyncWriteInst_t KEYWORD3 -ParamForBulkReadInst_t KEYWORD3 -ParamForBulkWriteInst_t KEYWORD3 -RecvInfoFromPing_t KEYWORD3 -RecvInfoFromStatusInst_t KEYWORD3 +InfoFromPing_t KEYWORD3 + +# protocol.h +InfoToParseDXLPacket_t KEYWORD3 +InfoToMakeDXLPacket_t KEYWORD3 +DXLLibErrorCode_t KEYWORD3 ########################################### # Constants (LITERAL1) ########################################### -# Dynamixel2Arduino Class +# Dynamixel2Arduino Class (Operting mode) OP_POSITION LITERAL1 OP_EXTENDED_POSITION LITERAL1 OP_CURRENT_BASED_POSITION LITERAL1 @@ -99,13 +126,18 @@ OP_VELOCITY LITERAL1 OP_PWM LITERAL1 OP_CURRENT LITERAL1 -# Dynamixel2Arduino Class +# Dynamixel2Arduino Class (Unit) UNIT_RAW LITERAL1 UNIT_PERCENT LITERAL1 UNIT_RPM LITERAL1 UNIT_DEGREE LITERAL1 UNIT_MILLI_AMPERE LITERAL1 +# Dynamixel2Arduino Class (Library error code) +D2A_LIB_ERROR_NULLPTR_PORT_HANDLER LITERAL1 +D2A_LIB_ERROR_NOT_SUPPORT_FUNCTION LITERAL1 +D2A_LIB_ERROR_UNKNOWN_MODEL_NUMBER LITERAL1 + # Actuator.h MODEL_NUMBER LITERAL1 MODEL_INFORMATION LITERAL1 @@ -197,4 +229,7 @@ PRESENT_INPUT_VOLTAGE LITERAL1 EXTERNAL_PORT_DATA_1 LITERAL1 EXTERNAL_PORT_DATA_2 LITERAL1 EXTERNAL_PORT_DATA_3 LITERAL1 -EXTERNAL_PORT_DATA_4 LITERAL1 \ No newline at end of file +EXTERNAL_PORT_DATA_4 LITERAL1 + +# protocol.h +DXL_BROADCAST_ID LITERAL1 \ No newline at end of file diff --git a/library.properties b/library.properties index f491327..f7ff7ed 100644 --- a/library.properties +++ b/library.properties @@ -1,5 +1,5 @@ name=Dynamixel2Arduino -version=0.3.0 +version=0.4.0 author=ROBOTIS license=Apache-2.0 maintainer=Kei(kkw@robotis.com) diff --git a/src/Dynamixel2Arduino.cpp b/src/Dynamixel2Arduino.cpp index 08bc7ba..5b08f60 100644 --- a/src/Dynamixel2Arduino.cpp +++ b/src/Dynamixel2Arduino.cpp @@ -15,12 +15,45 @@ *******************************************************************************/ #include "Dynamixel2Arduino.h" -#include "actuator.h" - -using namespace DYNAMIXEL; namespace DYNAMIXEL{ +const uint16_t model_number_table[] PROGMEM = { + AX12A, AX12W, AX18A, + + RX10, RX24F, RX28, RX64, + + DX113, DX116, DX117, + + EX106, + + MX12W, MX28, MX64, MX106, + MX28_2, MX64_2, MX106_2, + + XL320, + XL430_W250, + XXL430_W250, + XC430_W150, XC430_W240, + XM430_W210, XM430_W350, + XM540_W150, XM540_W270, + XH430_V210, XH430_V350, XH430_W210, XH430_W350, + XH540_V150, XH540_V270, XH540_W150, XH540_W270, + + PRO_L42_10_S300_R, + PRO_L54_30_S400_R, PRO_L54_30_S500_R, PRO_L54_50_S290_R, PRO_L54_50_S500_R, + PRO_M42_10_S260_R, PRO_M42_10_S260_RA, + PRO_M54_40_S250_R, PRO_M54_40_S250_RA, PRO_M54_60_S250_R, PRO_M54_60_S250_RA, + PRO_H42_20_S300_R, PRO_H42_20_S300_RA, + PRO_H54_100_S500_R, PRO_H54_100_S500_RA, PRO_H54_200_S500_R, PRO_H54_200_S500_RA, + + 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 +}; + +const uint8_t model_number_table_count = sizeof(model_number_table)/sizeof(model_number_table[0]); + enum Functions{ SET_ID, SET_BAUD_RATE, @@ -42,7 +75,9 @@ enum Functions{ LAST_DUMMY_FUNC = 0xFF }; -} +} //namespace DYNAMIXEL + +using namespace DYNAMIXEL; typedef struct ModelDependencyFuncItemAndRangeInfo{ uint8_t func_idx; //enum Functions @@ -67,21 +102,43 @@ static bool checkAndconvertWriteData(float in_data, int32_t &out_data, uint8_t u static bool checkAndconvertReadData(int32_t in_data, float &out_data, uint8_t unit, ItemAndRangeInfo_t &item_info); -Dynamixel2Arduino::Dynamixel2Arduino(HardwareSerial& port, int dir_pin) - : Master(), dxl_port_(port, dir_pin) +Dynamixel2Arduino::Dynamixel2Arduino(uint16_t packet_buf_size) +: Master(2.0, packet_buf_size), model_number_idx_last_index_(0) +{ + memset(&model_number_idx_, 0xff, sizeof(model_number_idx_)); +} + +Dynamixel2Arduino::Dynamixel2Arduino(HardwareSerial& port, int dir_pin, uint16_t packet_buf_size) +: Master(2.0, packet_buf_size), model_number_idx_last_index_(0) { - setPort(dxl_port_); + p_dxl_port_ = new SerialPortHandler(port, dir_pin); + setPort(p_dxl_port_); + memset(&model_number_idx_, 0xff, sizeof(model_number_idx_)); } /* For Master configuration */ void Dynamixel2Arduino::begin(unsigned long baud) { - dxl_port_.begin(baud); + p_dxl_port_ = (SerialPortHandler*)getPort(); + + if(p_dxl_port_ == nullptr){ + setLastLibErrCode(D2A_LIB_ERROR_NULLPTR_PORT_HANDLER); + return; + } + + p_dxl_port_->begin(baud); } -unsigned long Dynamixel2Arduino::getPortBaud() const +unsigned long Dynamixel2Arduino::getPortBaud() { - return dxl_port_.getBaud(); + p_dxl_port_ = (SerialPortHandler*)getPort(); + + if(p_dxl_port_ == nullptr){ + setLastLibErrCode(D2A_LIB_ERROR_NULLPTR_PORT_HANDLER); + return 0; + } + + return p_dxl_port_->getBaud(); } bool Dynamixel2Arduino::scan() @@ -96,54 +153,52 @@ bool Dynamixel2Arduino::scan() bool Dynamixel2Arduino::ping(uint8_t id) { bool ret = false; - uint8_t i; - - if (id == DXL_BROADCAST_ID){ - RecvInfoFromPing_t recv_info; - registered_dxl_cnt_ = 0; - - if(Master::ping(DXL_BROADCAST_ID, recv_info, 3*253) == true){ - for (i=0; i 0){ + if(recv_info.id == id){ + if(getPortProtocolVersion() == 1.0){ + recv_info.model_number = getModelNumber(id); } - registered_dxl_cnt_++; + ret = setModelNumber(id, recv_info.model_number); } - ret = true; - } + } }else{ - XelInfoFromPing_t recv_info; - if(Master::ping(id, &recv_info, 1, 20) > 0){ - if(recv_info.id != id) - return false; - for (i=0; i 0){ + for (uint8_t i=0; i 0) + { + recv_len = read(id, item_info.addr, item_info.addr_length, (uint8_t*)&ret, sizeof(ret), timeout); + + if(recv_len == 1){ + int8_t t_data = (int8_t)ret; + ret = (int32_t)t_data; + }else if(recv_len == 2){ + int16_t t_data = (int16_t)ret; + ret = (int32_t)t_data; + } } return ret; @@ -749,41 +839,58 @@ int32_t Dynamixel2Arduino::readControlTableItem(uint16_t model_num, uint8_t item bool Dynamixel2Arduino::writeControlTableItem(uint16_t model_num, uint8_t item_idx, uint8_t id, int32_t data, uint32_t timeout) { + bool ret = false; ControlTableItemInfo_t item_info; + + p_dxl_port_ = (SerialPortHandler*)getPort(); + if(p_dxl_port_ == nullptr){ + setLastLibErrCode(D2A_LIB_ERROR_NULLPTR_PORT_HANDLER); + return false; + } item_info = getControlTableItemInfo(model_num, item_idx); + if(item_info.addr_length > 0){ + ret = write(id, item_info.addr, (uint8_t*)&data, item_info.addr_length, timeout); + } - if(item_info.addr_length == 0) - return false; + return ret; +} + +uint8_t +Dynamixel2Arduino::getModelNumberIndex(uint16_t model_num) +{ + uint8_t i, ret = 0xFF; + + // quick shortcut + if(model_num == model_number_idx_[model_number_idx_last_index_]){ + ret = model_number_idx_last_index_; + }else{ + for(i=0; i 254){ + setLastLibErrCode(DXL_LIB_ERROR_INVAILD_ID); + return UNREGISTERED_MODEL; } - if(i == DXL_MAX_NODE && registered_dxl_cnt_ < DXL_MAX_NODE){ - if(ping(id) == true){ - for(i = 0; i < DXL_MAX_NODE; i++) - { - if(registered_dxl_[i].id == id){ - model_num = registered_dxl_[i].model_num; - break; - } - } - } - } + idx = model_number_idx_[id]; + model_num = (idx < model_number_table_count) ? pgm_read_word(&model_number_table[idx]) : UNREGISTERED_MODEL; return model_num; } @@ -791,16 +898,29 @@ uint16_t Dynamixel2Arduino::getModelNumberFromTable(uint8_t id) float Dynamixel2Arduino::readForRangeDependencyFunc(uint8_t func_idx, uint8_t id, uint8_t unit) { float ret = 0; - int32_t ret_data = 0; + int32_t recv_data = 0; uint16_t model_num = getModelNumberFromTable(id); - ItemAndRangeInfo_t item_info = getModelDependencyFuncInfo(model_num, func_idx); + ItemAndRangeInfo_t item_info; - if(item_info.item_idx == LAST_DUMMY_ITEM) - return false; + // To use the command function without ping() or model addition. + if(model_num == UNREGISTERED_MODEL){ + if(setModelNumber(id, getModelNumber(id)) == true){ + model_num = getModelNumberFromTable(id); + } + } - ret_data = readControlTableItem(model_num, item_info.item_idx, id); - if(checkAndconvertReadData(ret_data, ret, unit, item_info) == false) - return 0; + if(model_num != UNREGISTERED_MODEL){ + item_info = getModelDependencyFuncInfo(model_num, func_idx); + + if(item_info.item_idx != LAST_DUMMY_ITEM){ + recv_data = readControlTableItem(model_num, item_info.item_idx, id); + checkAndconvertReadData(recv_data, ret, unit, item_info); + }else{ + setLastLibErrCode(D2A_LIB_ERROR_NOT_SUPPORT_FUNCTION); + } + }else{ + setLastLibErrCode(D2A_LIB_ERROR_UNKNOWN_MODEL_NUMBER); + } return ret; } @@ -810,15 +930,28 @@ bool Dynamixel2Arduino::writeForRangeDependencyFunc(uint8_t func_idx, uint8_t id bool ret = false; int32_t data = 0; uint16_t model_num = getModelNumberFromTable(id); - ItemAndRangeInfo_t item_info = getModelDependencyFuncInfo(model_num, func_idx); + ItemAndRangeInfo_t item_info; - if(item_info.item_idx == LAST_DUMMY_ITEM) - return false; + // To use the command function without ping() or model addition. + if(model_num == UNREGISTERED_MODEL){ + if(setModelNumber(id, getModelNumber(id)) == true){ + model_num = getModelNumberFromTable(id); + } + } - if(checkAndconvertWriteData(value, data, unit, item_info) == false) - return false; + if(model_num != UNREGISTERED_MODEL){ + item_info = getModelDependencyFuncInfo(model_num, func_idx); + + if(item_info.item_idx == LAST_DUMMY_ITEM) + return false; - ret = writeControlTableItem(model_num, item_info.item_idx, id, data); + if(checkAndconvertWriteData(value, data, unit, item_info) == false) + return false; + + ret = writeControlTableItem(model_num, item_info.item_idx, id, data); + }else{ + setLastLibErrCode(D2A_LIB_ERROR_UNKNOWN_MODEL_NUMBER); + } return ret; } @@ -1154,10 +1287,11 @@ const ModelDependencyFuncItemAndRangeInfo_t dependency_pro_ra_plus_h54_200[] PRO static ItemAndRangeInfo_t getModelDependencyFuncInfo(uint16_t model_num, uint8_t func_num) { - uint8_t func_idx, i = 0; const ModelDependencyFuncItemAndRangeInfo_t *p_common_ctable = nullptr; const ModelDependencyFuncItemAndRangeInfo_t *p_dep_ctable = nullptr; + uint8_t func_idx, i = 0; ItemAndRangeInfo_t item_info; + memset(&item_info, 0, sizeof(item_info)); item_info.item_idx = LAST_DUMMY_ITEM; diff --git a/src/Dynamixel2Arduino.h b/src/Dynamixel2Arduino.h index e2540e0..2f083ad 100644 --- a/src/Dynamixel2Arduino.h +++ b/src/Dynamixel2Arduino.h @@ -41,10 +41,24 @@ enum ParamUnit{ UNIT_MILLI_AMPERE }; +enum D2ALibErrorCode +{ + D2A_LIB_ERROR_NULLPTR_PORT_HANDLER = 0x0040, + D2A_LIB_ERROR_NOT_SUPPORT_FUNCTION, + D2A_LIB_ERROR_UNKNOWN_MODEL_NUMBER +}; class Dynamixel2Arduino : public DYNAMIXEL::Master { public: + /** + * @brief The constructor. + * @code + * Dynamixel2Arduino dxl; + * @endcode + */ + Dynamixel2Arduino(uint16_t packet_buf_size = DEFAULT_DXL_BUF_LENGTH); + /** * @brief The constructor. * @code @@ -55,7 +69,7 @@ class Dynamixel2Arduino : public DYNAMIXEL::Master * It is automatically initialized baudrate to 57600 by calling the begin () function. * @param dir_pin Directional pins for using half-duplex communication. -1 uses full duplex. (default : -1) */ - Dynamixel2Arduino(HardwareSerial& port, int dir_pin = -1); + Dynamixel2Arduino(HardwareSerial& port, int dir_pin = -1, uint16_t packet_buf_size = DEFAULT_DXL_BUF_LENGTH); /** * @brief Initialization function to start communication with DYNAMIXEL. @@ -79,7 +93,7 @@ class Dynamixel2Arduino : public DYNAMIXEL::Master * @endcode * @return It returns serial baudrate of board port. */ - unsigned long getPortBaud() const; + unsigned long getPortBaud(); /** * @brief It is API for To checking the communication connection status of DYNAMIXEL. @@ -90,7 +104,8 @@ class Dynamixel2Arduino : public DYNAMIXEL::Master * @endcode * @param id DYNAMIXEL Actuator's ID or BROADCAST ID (0xFE). (default : 0xFE) * @return It returns true(1) on success, false(0) on failure. - */ + */ + using DYNAMIXEL::Master::ping; bool ping(uint8_t id = DXL_BROADCAST_ID); /** @@ -104,6 +119,20 @@ class Dynamixel2Arduino : public DYNAMIXEL::Master */ bool scan(); + /** + * @brief It is API for getting model number of DYNAMIXEL. + * @code + * const int DXL_DIR_PIN = 2; + * Dynamixel2Arduino dxl(Serial1, DXL_DIR_PIN); + * dxl.setModelNumber(1, 1020); + * Serial.print(dxl.getModelNumber(1)); + * @endcode + * @param id DYNAMIXEL Actuator's ID. + * @param model_number DYNAMIXEL Actuator's model number. + * @return It returns true(1) on success, false(0) on failure. + */ + bool setModelNumber(uint8_t id, uint16_t model_number); + /** * @brief It is API for getting model number of DYNAMIXEL. * @code @@ -245,7 +274,7 @@ class Dynamixel2Arduino : public DYNAMIXEL::Master * @param unit The unit you want to return (the function converts the raw value to the unit you specified and returns it) (default : UNIT_RAW) * Only support UNIT_RAW, UNIT_DEGREE. * @return It returns the data read from DXL control table item.(Returns the value appropriate for @unit.) - * If the read fails, 0 is returned. Whether or not this is an actual value can be confirmed with @getLastErrorCode(). + * If the read fails, 0 is returned. Whether or not this is an actual value can be confirmed with @getLastLibErrCode(). */ float getPresentPosition(uint8_t id, uint8_t unit = UNIT_RAW); @@ -277,7 +306,7 @@ class Dynamixel2Arduino : public DYNAMIXEL::Master * @param unit The unit you want to return (the function converts the raw value to the unit you specified and returns it) (default : UNIT_RAW) * Only support UNIT_RAW, UNIT_PERCENT, UNIT_RPM. * @return It returns the data read from DXL control table item.(Returns the value appropriate for @unit.) - * If the read fails, 0 is returned. Whether or not this is an actual value can be confirmed with @getLastErrorCode(). + * If the read fails, 0 is returned. Whether or not this is an actual value can be confirmed with @getLastLibErrCode(). */ float getPresentVelocity(uint8_t id, uint8_t unit = UNIT_RAW); @@ -309,7 +338,7 @@ class Dynamixel2Arduino : public DYNAMIXEL::Master * @param unit The unit you want to return (the function converts the raw value to the unit you specified and returns it) (default : UNIT_RAW) * Only support UNIT_RAW, UNIT_PERCENT. * @return It returns the data read from DXL control table item.(Returns the value appropriate for @unit.) - * If the read fails, 0 is returned. Whether or not this is an actual value can be confirmed with @getLastErrorCode(). + * If the read fails, 0 is returned. Whether or not this is an actual value can be confirmed with @getLastLibErrCode(). */ float getPresentPWM(uint8_t id, uint8_t unit = UNIT_RAW); @@ -341,7 +370,7 @@ class Dynamixel2Arduino : public DYNAMIXEL::Master * @param unit The unit you want to return (the function converts the raw value to the unit you specified and returns it) (default : UNIT_RAW) * Only support UNIT_RAW, UNIT_PERCENT, UNIT_MILLI_AMPERE. * @return It returns the data read from DXL control table item.(Returns the value appropriate for @unit.) - * If the read fails, 0 is returned. Whether or not this is an actual value can be confirmed with @getLastErrorCode(). + * If the read fails, 0 is returned. Whether or not this is an actual value can be confirmed with @getLastLibErrCode(). */ float getPresentCurrent(uint8_t id, uint8_t unit = UNIT_RAW); @@ -359,7 +388,7 @@ class Dynamixel2Arduino : public DYNAMIXEL::Master * @param id DYNAMIXEL Actuator's ID. * @param timeout A timeout waiting for a response to a data transfer. * @return It returns the data read from DXL control table item.(Returns the value appropriate for @unit.) - * If the read fails, 0 is returned. Whether or not this is an actual value can be confirmed with @getLastErrorCode(). + * If the read fails, 0 is returned. Whether or not this is an actual value can be confirmed with @getLastLibErrCode(). */ int32_t readControlTableItem(uint8_t item_idx, uint8_t id, uint32_t timeout = 100); @@ -404,17 +433,12 @@ class Dynamixel2Arduino : public DYNAMIXEL::Master #endif private: - typedef struct IdAndModelNum{ - uint16_t model_num; - uint8_t id; - } IdAndModelNum_t; - - DYNAMIXEL::SerialPortHandler dxl_port_; + DYNAMIXEL::SerialPortHandler *p_dxl_port_; - IdAndModelNum_t registered_dxl_[DXL_MAX_NODE]; - uint8_t registered_dxl_cnt_; - uint32_t err_code_; + uint8_t model_number_idx_[254]; + uint8_t model_number_idx_last_index_; + uint8_t getModelNumberIndex(uint16_t model_num); uint16_t getModelNumberFromTable(uint8_t id); bool setTorqueEnable(uint8_t id, bool enable); diff --git a/src/actuator.cpp b/src/actuator.cpp index bb4c103..faed26f 100644 --- a/src/actuator.cpp +++ b/src/actuator.cpp @@ -3,14 +3,9 @@ #include -#if defined(__arm__) && !defined(PROGMEM) -#define PROGMEM -#define PSTR(STR) STR -#endif using namespace DYNAMIXEL; - typedef struct ModelControlTableInfo{ uint8_t index; uint16_t addr; diff --git a/src/actuator.h b/src/actuator.h index 0ac53f9..b95fdef 100644 --- a/src/actuator.h +++ b/src/actuator.h @@ -1,88 +1,204 @@ -#ifndef DYNAMIXEL_ACTUATOR_H_ -#define DYNAMIXEL_ACTUATOR_H_ +#ifndef DYNAMIXEL_ACTUATOR_HPP_ +#define DYNAMIXEL_ACTUATOR_HPP_ #include "stdint.h" #include "utility/config.h" +// The reason for checking #ifndef here is to avoid conflict with Dynamixel SDK. +#ifndef AX12A #define AX12A (uint16_t)12 +#endif +#ifndef AX12W #define AX12W (uint16_t)300 +#endif +#ifndef AX18A #define AX18A (uint16_t)18 +#endif +#ifndef RX10 #define RX10 (uint16_t)10 +#endif +#ifndef RX24F #define RX24F (uint16_t)24 +#endif +#ifndef RX28 #define RX28 (uint16_t)28 +#endif +#ifndef RX64 #define RX64 (uint16_t)64 +#endif +#ifndef DX113 #define DX113 (uint16_t)113 +#endif +#ifndef DX116 #define DX116 (uint16_t)116 +#endif +#ifndef DX117 #define DX117 (uint16_t)117 +#endif +#ifndef EX106 #define EX106 (uint16_t)107 +#endif +#ifndef MX12W #define MX12W (uint16_t)360 +#endif +#ifndef MX28 #define MX28 (uint16_t)29 +#endif +#ifndef MX64 #define MX64 (uint16_t)310 +#endif +#ifndef MX106 #define MX106 (uint16_t)320 +#endif +#ifndef MX28_2 #define MX28_2 (uint16_t)30 +#endif +#ifndef MX64_2 #define MX64_2 (uint16_t)311 +#endif +#ifndef MX106_2 #define MX106_2 (uint16_t)321 +#endif +#ifndef XL320 #define XL320 (uint16_t)350 +#endif +#ifndef XC430_W150 #define XC430_W150 (uint16_t)1070 +#endif +#ifndef XC430_W240 #define XC430_W240 (uint16_t)1080 +#endif +#ifndef XL430_W250 #define XL430_W250 (uint16_t)1060 - +#endif +#ifndef XXL430_W250 #define XXL430_W250 (uint16_t)1090 +#endif +#ifndef XM430_W210 #define XM430_W210 (uint16_t)1030 +#endif +#ifndef XM430_W350 #define XM430_W350 (uint16_t)1020 +#endif +#ifndef XM540_W150 #define XM540_W150 (uint16_t)1130 +#endif +#ifndef XM540_W270 #define XM540_W270 (uint16_t)1120 +#endif +#ifndef XH430_V210 #define XH430_V210 (uint16_t)1050 +#endif +#ifndef XH430_V350 #define XH430_V350 (uint16_t)1040 +#endif +#ifndef XH430_W210 #define XH430_W210 (uint16_t)1010 +#endif +#ifndef XH430_W350 #define XH430_W350 (uint16_t)1000 +#endif +#ifndef XH540_W150 #define XH540_W150 (uint16_t)1110 +#endif +#ifndef XH540_W270 #define XH540_W270 (uint16_t)1100 +#endif +#ifndef XH540_V150 #define XH540_V150 (uint16_t)1150 +#endif +#ifndef XH540_V270 #define XH540_V270 (uint16_t)1140 +#endif +#ifndef PRO_L42_10_S300_R #define PRO_L42_10_S300_R (uint16_t)35072 +#endif +#ifndef PRO_L54_30_S400_R #define PRO_L54_30_S400_R (uint16_t)37928 +#endif +#ifndef PRO_L54_30_S500_R #define PRO_L54_30_S500_R (uint16_t)37896 +#endif +#ifndef PRO_L54_50_S290_R #define PRO_L54_50_S290_R (uint16_t)38176 +#endif +#ifndef PRO_L54_50_S500_R #define PRO_L54_50_S500_R (uint16_t)38152 +#endif +#ifndef PRO_M42_10_S260_R #define PRO_M42_10_S260_R (uint16_t)43288 +#endif +#ifndef PRO_M54_40_S250_R #define PRO_M54_40_S250_R (uint16_t)46096 +#endif +#ifndef PRO_M54_60_S250_R #define PRO_M54_60_S250_R (uint16_t)46352 +#endif +#ifndef PRO_H42_20_S300_R #define PRO_H42_20_S300_R (uint16_t)51200 +#endif +#ifndef PRO_H54_100_S500_R #define PRO_H54_100_S500_R (uint16_t)53768 +#endif +#ifndef PRO_H54_200_S500_R #define PRO_H54_200_S500_R (uint16_t)54024 +#endif +#ifndef PRO_M42_10_S260_RA #define PRO_M42_10_S260_RA (uint16_t)43289 +#endif +#ifndef PRO_M54_40_S250_RA #define PRO_M54_40_S250_RA (uint16_t)46097 +#endif +#ifndef PRO_M54_60_S250_RA #define PRO_M54_60_S250_RA (uint16_t)46353 +#endif +#ifndef PRO_H42_20_S300_RA #define PRO_H42_20_S300_RA (uint16_t)51201 +#endif +#ifndef PRO_H54_100_S500_RA #define PRO_H54_100_S500_RA (uint16_t)53761 +#endif +#ifndef PRO_H54_200_S500_RA #define PRO_H54_200_S500_RA (uint16_t)54025 +#endif +#ifndef PRO_H42P_020_S300_R #define PRO_H42P_020_S300_R (uint16_t)2000 +#endif +#ifndef PRO_H54P_100_S500_R #define PRO_H54P_100_S500_R (uint16_t)2010 +#endif +#ifndef PRO_H54P_200_S500_R #define PRO_H54P_200_S500_R (uint16_t)2020 +#endif +#ifndef PRO_M42P_010_S260_R #define PRO_M42P_010_S260_R (uint16_t)2100 +#endif +#ifndef PRO_M54P_040_S250_R #define PRO_M54P_040_S250_R (uint16_t)2110 +#endif +#ifndef PRO_M54P_060_S250_R #define PRO_M54P_060_S250_R (uint16_t)2120 +#endif enum ControlTableItem{ @@ -192,4 +308,4 @@ ControlTableItemInfo_t getControlTableItemInfo(uint16_t model_num, uint8_t contr } // namespace DYNAMIXEL -#endif /* DYNAMIXEL_ACTUATOR_H_ */ \ No newline at end of file +#endif /* DYNAMIXEL_ACTUATOR_HPP_ */ \ No newline at end of file diff --git a/src/dxl_c/protocol.c b/src/dxl_c/protocol.c new file mode 100644 index 0000000..110b29c --- /dev/null +++ b/src/dxl_c/protocol.c @@ -0,0 +1,711 @@ +#include "protocol.h" + +#if defined(ARDUINO) + #include + #if !defined(ESP_PLATFORM) + #include + #endif +#endif + +#if !defined(PROGMEM) + #define PROGMEM +#endif + +#if !defined(pgm_read_word_near) + #define pgm_read_word_near(x) (*(uint16_t*)(x)) +#endif + + +// 2.0 Protocol +#define DXL2_0_PACKET_IDX_HEADER_1 0 +#define DXL2_0_PACKET_IDX_HEADER_2 1 +#define DXL2_0_PACKET_IDX_HEADER_3 2 +#define DXL2_0_PACKET_IDX_RESERVED 3 +#define DXL2_0_PACKET_IDX_ID 4 +#define DXL2_0_PACKET_IDX_LENGTH_L 5 +#define DXL2_0_PACKET_IDX_LENGTH_H 6 +#define DXL2_0_PACKET_IDX_INST 7 +#define DXL2_0_PACKET_IDX_ERROR 8 +#define DXL2_0_PACKET_IDX_INST_PARAM 8 +#define DXL2_0_PACKET_IDX_STATUS_PARAM 9 + +// 1.0 Protocol +#define DXL1_0_PACKET_IDX_HEADER_1 0 +#define DXL1_0_PACKET_IDX_HEADER_2 1 +#define DXL1_0_PACKET_IDX_ID 2 +#define DXL1_0_PACKET_IDX_LENGTH 3 +#define DXL1_0_PACKET_IDX_INST 4 +#define DXL1_0_PACKET_IDX_ERROR 4 +#define DXL1_0_PACKET_IDX_PARAM 5 + + +//-- Internal Variables +// + + +//-- External Variables +// + + +//-- Internal Functions +// +static DXLLibErrorCode_t parse_dxl1_0_packet(InfoToParseDXLPacket_t* p_parse_packet, uint8_t recv_data); +static DXLLibErrorCode_t parse_dxl2_0_packet(InfoToParseDXLPacket_t* p_parse_packet, uint8_t recv_data); + +static DXLLibErrorCode_t add_param_to_dxl1_0_packet(InfoToMakeDXLPacket_t* p_make_packet, uint8_t *p_param, uint16_t param_len); +static DXLLibErrorCode_t add_param_to_dxl2_0_packet(InfoToMakeDXLPacket_t* p_make_packet, uint8_t *p_param, uint16_t param_len); + +static DXLLibErrorCode_t end_make_dxl1_0_packet(InfoToMakeDXLPacket_t* p_make_packet); +static DXLLibErrorCode_t end_make_dxl2_0_packet(InfoToMakeDXLPacket_t* p_make_packet); + +static void update_dxl_crc(uint16_t *p_crc_cur, uint8_t recv_data); + +//-- External Functions +// +DXLLibErrorCode_t begin_make_dxl_packet(InfoToMakeDXLPacket_t* p_make_packet, + uint8_t id, uint8_t protocol_ver, uint8_t inst_idx, uint8_t err_idx, + uint8_t* p_packet_buf, uint16_t packet_buf_capacity) +{ + if(p_packet_buf == NULL){ + return DXL_LIB_ERROR_NULLPTR; + } + + if(protocol_ver == 2){ + if(id == 0xFD && id == 0xFF){ //http://emanual.robotis.com/docs/en/dxl/protocol2/#packet-id + return DXL_LIB_ERROR_INVAILD_ID; + } + if(packet_buf_capacity < 11){ //http://emanual.robotis.com/docs/en/dxl/protocol2/#status-packet + return DXL_LIB_ERROR_NOT_ENOUGH_BUFFER_SIZE; + } + }else if(protocol_ver == 1){ + if(id == 0xFF){ //http://emanual.robotis.com/docs/en/dxl/protocol1/#packet-id + return DXL_LIB_ERROR_INVAILD_ID; + } + if(packet_buf_capacity < 6){ //http://emanual.robotis.com/docs/en/dxl/protocol1/#instruction-packet + return DXL_LIB_ERROR_NOT_ENOUGH_BUFFER_SIZE; + } + }else{ + return DXL_LIB_ERROR_INVAILD_PROTOCOL_VERSION; + } + + p_make_packet->id = id; + p_make_packet->protocol_ver = protocol_ver; + p_make_packet->inst_idx = inst_idx; + p_make_packet->err_idx = err_idx; + p_make_packet->p_packet_buf = p_packet_buf; + p_make_packet->packet_buf_capacity = packet_buf_capacity; + p_make_packet->param_length = 0; + p_make_packet->generated_packet_length = 0; + p_make_packet->is_init = true; + + return DXL_LIB_OK; +} + + +DXLLibErrorCode_t add_param_to_dxl_packet(InfoToMakeDXLPacket_t* p_make_packet, uint8_t *p_param, uint16_t param_len) +{ + DXLLibErrorCode_t ret; + + if(p_make_packet == NULL){ + return DXL_LIB_ERROR_NULLPTR; + }else if(p_make_packet->is_init == false){ + return DXL_LIB_ERROR_NOT_INITIALIZED; + } + + if(p_make_packet->protocol_ver == 2){ + ret = add_param_to_dxl2_0_packet(p_make_packet, p_param, param_len); + }else if(p_make_packet->protocol_ver == 1){ + ret = add_param_to_dxl1_0_packet(p_make_packet, p_param, param_len); + }else{ + ret = DXL_LIB_ERROR_INVAILD_PROTOCOL_VERSION; + } + + return ret; +} + +static DXLLibErrorCode_t add_param_to_dxl1_0_packet(InfoToMakeDXLPacket_t* p_make_packet, uint8_t *p_param, uint16_t param_len) +{ + uint8_t *p_param_buf; + uint16_t i, generated_param_len; + + generated_param_len = p_make_packet->param_length; + if(p_make_packet->packet_buf_capacity < generated_param_len+param_len+6 //6 = Header(2)+ID(1)+Length(1)+Instruction(1)+CheckSum(1) : http://emanual.robotis.com/docs/en/dxl/protocol1/#instruction-packet + || generated_param_len+param_len > 255-2){ //Length Frame Range(1byte) = Instruction(1)+Param(N)+CheckSum(1) : http://emanual.robotis.com/docs/en/dxl/protocol1/#length + return DXL_LIB_ERROR_BUFFER_OVERFLOW; + } + + p_param_buf = &p_make_packet->p_packet_buf[DXL1_0_PACKET_IDX_PARAM]; + + for (i=0; iparam_length = generated_param_len; + + return DXL_LIB_OK; +} + +static DXLLibErrorCode_t add_param_to_dxl2_0_packet(InfoToMakeDXLPacket_t* p_make_packet, uint8_t *p_param, uint16_t param_len) +{ + uint8_t *p_packet, *p_param_buf; + uint16_t i, generated_param_len; + + generated_param_len = p_make_packet->param_length; + p_packet = p_make_packet->p_packet_buf; + if(p_make_packet->inst_idx == DXL_INST_STATUS){ + if(p_make_packet->packet_buf_capacity < generated_param_len+param_len+11){ + return DXL_LIB_ERROR_BUFFER_OVERFLOW; + } + p_param_buf = &p_packet[DXL2_0_PACKET_IDX_STATUS_PARAM]; + + for (i=0; i= 3){ + if(p_param_buf[generated_param_len-3] == 0xFF + && p_param_buf[generated_param_len-2] == 0xFF + && p_param_buf[generated_param_len-1] == 0xFD){ + p_param_buf[generated_param_len++] = 0xFD; + } + }else if(generated_param_len == 2){ + if(p_packet[DXL2_0_PACKET_IDX_ERROR] == 0xFF + && p_param_buf[generated_param_len-2] == 0xFF + && p_param_buf[generated_param_len-1] == 0xFD){ + p_param_buf[generated_param_len++] = 0xFD; + } + }else if(generated_param_len == 1){ + if(p_packet[DXL2_0_PACKET_IDX_INST] == 0xFF + && p_packet[DXL2_0_PACKET_IDX_ERROR] == 0xFF + && p_param_buf[generated_param_len-1] == 0xFD){ + p_param_buf[generated_param_len++] = 0xFD; + } + } + + if(p_make_packet->packet_buf_capacity < generated_param_len+11){ + return DXL_LIB_ERROR_BUFFER_OVERFLOW; + } + } + }else{ //Instruction Packet + if(p_make_packet->packet_buf_capacity < generated_param_len+param_len+10){ + return DXL_LIB_ERROR_BUFFER_OVERFLOW; + } + p_param_buf = &p_packet[DXL2_0_PACKET_IDX_INST_PARAM]; + + for (i=0; i= 3){ + if(p_param_buf[generated_param_len-3] == 0xFF + && p_param_buf[generated_param_len-2] == 0xFF + && p_param_buf[generated_param_len-1] == 0xFD){ + p_param_buf[generated_param_len++] = 0xFD; + } + }else if(generated_param_len == 2){ + if(p_packet[DXL2_0_PACKET_IDX_INST] == 0xFF + && p_param_buf[generated_param_len-2] == 0xFF + && p_param_buf[generated_param_len-1] == 0xFD){ + p_param_buf[generated_param_len++] = 0xFD; + } + } + + if(p_make_packet->packet_buf_capacity < generated_param_len+10){ + return DXL_LIB_ERROR_BUFFER_OVERFLOW; + } + } + } + + p_make_packet->param_length = generated_param_len; + + return DXL_LIB_OK; +} + + + +DXLLibErrorCode_t end_make_dxl_packet(InfoToMakeDXLPacket_t* p_make_packet) +{ + DXLLibErrorCode_t ret; + + if(p_make_packet == NULL){ + return DXL_LIB_ERROR_NULLPTR; + } + if(p_make_packet->is_init == false){ + return DXL_LIB_ERROR_NOT_INITIALIZED; + } + if(p_make_packet->p_packet_buf == NULL){ + return DXL_LIB_ERROR_NULLPTR; + } + + if(p_make_packet->protocol_ver == 2){ + ret = end_make_dxl2_0_packet(p_make_packet); + }else if(p_make_packet->protocol_ver == 1){ + ret = end_make_dxl1_0_packet(p_make_packet); + }else{ + ret = DXL_LIB_ERROR_INVAILD_PROTOCOL_VERSION; + } + + return ret; +} + +static DXLLibErrorCode_t end_make_dxl1_0_packet(InfoToMakeDXLPacket_t* p_make_packet) +{ + uint8_t *p_packet; + uint8_t check_sum = 0; + uint16_t i, generated_packet_len = 0; + + if(p_make_packet->packet_buf_capacity < p_make_packet->param_length+6 //6 = Header(2)+ID(1)+Length(1)+Instruction(1)+CheckSum(1) : http://emanual.robotis.com/docs/en/dxl/protocol1/#instruction-packet + || p_make_packet->param_length > 255-2){ //Length Frame Range(1byte) = Instruction(1)+Param(N)+CheckSum(1) : http://emanual.robotis.com/docs/en/dxl/protocol1/#length + return DXL_LIB_ERROR_BUFFER_OVERFLOW; + } + + p_packet = p_make_packet->p_packet_buf; + + p_packet[generated_packet_len++] = 0xFF; + p_packet[generated_packet_len++] = 0xFF; + p_packet[generated_packet_len++] = p_make_packet->id; + p_packet[generated_packet_len++] = p_make_packet->param_length+2; //2 = Instruction(1)+CheckSum(1) + if(p_make_packet->inst_idx == DXL_INST_STATUS){ + p_packet[generated_packet_len++] = p_make_packet->err_idx; + }else{ + p_packet[generated_packet_len++] = p_make_packet->inst_idx; + } + generated_packet_len += p_make_packet->param_length; + + for (i=DXL1_0_PACKET_IDX_ID; igenerated_packet_length = generated_packet_len; + + return DXL_LIB_OK; +} + +static DXLLibErrorCode_t end_make_dxl2_0_packet(InfoToMakeDXLPacket_t* p_make_packet) +{ + uint8_t *p_packet; + uint16_t i, generated_packet_len = 0, calculated_crc = 0; + + if(p_make_packet->inst_idx == DXL_INST_STATUS && p_make_packet->packet_buf_capacity < p_make_packet->param_length+11){ + return DXL_LIB_ERROR_BUFFER_OVERFLOW; + }else if(p_make_packet->packet_buf_capacity < p_make_packet->param_length+10){ + return DXL_LIB_ERROR_BUFFER_OVERFLOW; + } + + p_packet = p_make_packet->p_packet_buf; + + p_packet[generated_packet_len++] = 0xFF; + p_packet[generated_packet_len++] = 0xFF; + p_packet[generated_packet_len++] = 0xFD; + p_packet[generated_packet_len++] = 0x00; + p_packet[generated_packet_len++] = p_make_packet->id; + if(p_make_packet->inst_idx == DXL_INST_STATUS){ + p_packet[generated_packet_len++] = (p_make_packet->param_length+4) >> 0; //4 = Instruction(1)+Error(1)+CRC(2) + p_packet[generated_packet_len++] = (p_make_packet->param_length+4) >> 8; //4 = Instruction(1)+Error(1)+CRC(2) + p_packet[generated_packet_len++] = p_make_packet->inst_idx; + p_packet[generated_packet_len++] = p_make_packet->err_idx; + }else{ + p_packet[generated_packet_len++] = (p_make_packet->param_length+3) >> 0; //3 = Instruction(1)+CRC(2) + p_packet[generated_packet_len++] = (p_make_packet->param_length+3) >> 8; //3 = Instruction(1)+CRC(2) + p_packet[generated_packet_len++] = p_make_packet->inst_idx; + } + generated_packet_len += p_make_packet->param_length; + + for (i=0; i> 0; + p_packet[generated_packet_len++] = calculated_crc >> 8; + + p_make_packet->generated_packet_length = generated_packet_len; + + return DXL_LIB_OK; +} + + + +DXLLibErrorCode_t begin_parse_dxl_packet(InfoToParseDXLPacket_t* p_parse_packet, + uint8_t protocol_ver, uint8_t* p_param_buf, uint16_t param_buf_capacity) +{ + if(param_buf_capacity > 0 && p_param_buf == NULL){ + return DXL_LIB_ERROR_NULLPTR; + } + + if(protocol_ver != 1 && protocol_ver != 2){ + return DXL_LIB_ERROR_INVAILD_PROTOCOL_VERSION; + } + + p_parse_packet->protocol_ver = protocol_ver; + p_parse_packet->p_param_buf = p_param_buf; + p_parse_packet->param_buf_capacity = param_buf_capacity; + // p_parse_packet->recv_param_len = 0; + // p_parse_packet->parse_state = 0; + p_parse_packet->is_init = true; + + return DXL_LIB_OK; +} + +DXLLibErrorCode_t parse_dxl_packet(InfoToParseDXLPacket_t* p_parse_packet, uint8_t recv_data) +{ + DXLLibErrorCode_t ret; + + if(p_parse_packet == NULL){ + return DXL_LIB_ERROR_NULLPTR; + }else if(p_parse_packet->is_init == false){ + return DXL_LIB_ERROR_NOT_INITIALIZED; + } + + if(p_parse_packet->protocol_ver == 2){ + ret = parse_dxl2_0_packet(p_parse_packet, recv_data); + }else if(p_parse_packet->protocol_ver == 1){ + ret = parse_dxl1_0_packet(p_parse_packet, recv_data); + }else{ + ret = DXL_LIB_ERROR_INVAILD_PROTOCOL_VERSION; + } + + return ret; +} + +static DXLLibErrorCode_t parse_dxl1_0_packet(InfoToParseDXLPacket_t* p_parse_packet, uint8_t recv_data) +{ + DXLLibErrorCode_t ret = DXL_LIB_PROCEEDING; + + switch(p_parse_packet->parse_state) + { + case DXL1_0_PACKET_PARSING_STATE_IDLE: + if(p_parse_packet->header_cnt >= 2){ + p_parse_packet->header_cnt = 0; + } + p_parse_packet->header[p_parse_packet->header_cnt++] = recv_data; + if(p_parse_packet->header_cnt == 2){ + if(p_parse_packet->header[0] == 0xFF + && p_parse_packet->header[1] == 0xFF){ + p_parse_packet->recv_param_len = 0; + p_parse_packet->calculated_check_sum = 0; + p_parse_packet->parse_state = DXL1_0_PACKET_PARSING_STATE_ID; + }else{ + p_parse_packet->header[0] = p_parse_packet->header[1]; + p_parse_packet->header[1] = 0; + p_parse_packet->header_cnt--; + } + } + break; + + case DXL1_0_PACKET_PARSING_STATE_ID: + if(recv_data != 0xFF){ //http://emanual.robotis.com/docs/en/dxl/protocol1/#packet-id + p_parse_packet->id = recv_data; + p_parse_packet->calculated_check_sum += recv_data; + p_parse_packet->parse_state = DXL1_0_PACKET_PARSING_STATE_LENGTH; + }else{ + p_parse_packet->parse_state = DXL1_0_PACKET_PARSING_STATE_IDLE; + } + break; + + case DXL1_0_PACKET_PARSING_STATE_LENGTH: + if((uint16_t)recv_data > p_parse_packet->param_buf_capacity+2){ // 2 = Instruction(1) + CheckSum(1) + ret = DXL_LIB_ERROR_BUFFER_OVERFLOW; + p_parse_packet->parse_state = DXL1_0_PACKET_PARSING_STATE_IDLE; + }else if(recv_data < 2){ // 2 = Instruction(1) + CheckSum(1) + ret = DXL_LIB_ERROR_LENGTH; + p_parse_packet->parse_state = DXL1_0_PACKET_PARSING_STATE_IDLE; + }else{ + p_parse_packet->packet_len = recv_data; + p_parse_packet->calculated_check_sum += recv_data; + p_parse_packet->parse_state = DXL1_0_PACKET_PARSING_STATE_INST_ERR; + } + break; + + case DXL1_0_PACKET_PARSING_STATE_INST_ERR: + p_parse_packet->inst_idx = recv_data; + p_parse_packet->err_idx = recv_data; + p_parse_packet->calculated_check_sum += recv_data; + p_parse_packet->recv_param_len = 0; + if(p_parse_packet->packet_len == 2){ // 2 = Instruction(1) + CheckSum(1) + p_parse_packet->parse_state = DXL1_0_PACKET_PARSING_STATE_CHECK_SUM; + }else{ + p_parse_packet->parse_state = DXL1_0_PACKET_PARSING_STATE_PARAM; + if(p_parse_packet->p_param_buf == NULL){ + ret = DXL_LIB_ERROR_NULLPTR; + p_parse_packet->parse_state = DXL1_0_PACKET_PARSING_STATE_IDLE; + } + } + break; + + case DXL1_0_PACKET_PARSING_STATE_PARAM: + p_parse_packet->p_param_buf[p_parse_packet->recv_param_len++] = recv_data; + p_parse_packet->calculated_check_sum += recv_data; + if(p_parse_packet->recv_param_len == p_parse_packet->packet_len-2){ // 2 = Instruction(1) + CheckSum(1) + p_parse_packet->parse_state = DXL1_0_PACKET_PARSING_STATE_CHECK_SUM; + } + break; + + case DXL1_0_PACKET_PARSING_STATE_CHECK_SUM: + p_parse_packet->calculated_check_sum = ~(p_parse_packet->calculated_check_sum); + p_parse_packet->recv_check_sum = recv_data; + if(p_parse_packet->calculated_check_sum == p_parse_packet->recv_check_sum){ + ret = DXL_LIB_OK; + }else{ + ret = DXL_LIB_ERROR_CHECK_SUM; + } + p_parse_packet->parse_state = DXL1_0_PACKET_PARSING_STATE_IDLE; + break; + + default: + p_parse_packet->parse_state = DXL1_0_PACKET_PARSING_STATE_IDLE; + break; + } + + return ret; +} + +static DXLLibErrorCode_t parse_dxl2_0_packet(InfoToParseDXLPacket_t* p_parse_packet, uint8_t recv_data) +{ + DXLLibErrorCode_t ret = DXL_LIB_PROCEEDING; + uint16_t byte_stuffing_cnt = 0; + + switch(p_parse_packet->parse_state) + { + case DXL2_0_PACKET_PARSING_STATE_IDLE: + if(p_parse_packet->header_cnt >= 3){ + p_parse_packet->header_cnt = 0; + } + p_parse_packet->header[p_parse_packet->header_cnt++] = recv_data; + if(p_parse_packet->header_cnt == 3){ + if(p_parse_packet->header[0] == 0xFF + && p_parse_packet->header[1] == 0xFF + && p_parse_packet->header[2] == 0xFD){ + p_parse_packet->recv_param_len = 0; + p_parse_packet->calculated_crc = 0; + update_dxl_crc(&p_parse_packet->calculated_crc, 0xFF); + update_dxl_crc(&p_parse_packet->calculated_crc, 0xFF); + update_dxl_crc(&p_parse_packet->calculated_crc, 0xFD); + p_parse_packet->parse_state = DXL2_0_PACKET_PARSING_STATE_RESERVED; + }else{ + p_parse_packet->header[0] = p_parse_packet->header[1]; + p_parse_packet->header[1] = p_parse_packet->header[2]; + p_parse_packet->header_cnt--; + } + } + break; + + case DXL2_0_PACKET_PARSING_STATE_RESERVED: + if(recv_data != 0xFD){ // http://emanual.robotis.com/docs/en/dxl/protocol2/#reserved + p_parse_packet->reserved = recv_data; + update_dxl_crc(&p_parse_packet->calculated_crc, recv_data); + p_parse_packet->parse_state = DXL2_0_PACKET_PARSING_STATE_ID; + }else{ + ret = DXL_LIB_ERROR_WRONG_PACKET; + p_parse_packet->parse_state = DXL2_0_PACKET_PARSING_STATE_IDLE; + } + break; + + case DXL2_0_PACKET_PARSING_STATE_ID: + if(recv_data < 0xFD || recv_data == DXL_BROADCAST_ID){ //http://emanual.robotis.com/docs/en/dxl/protocol2/#packet-id + p_parse_packet->id = recv_data; + update_dxl_crc(&p_parse_packet->calculated_crc, recv_data); + p_parse_packet->parse_state = DXL2_0_PACKET_PARSING_STATE_LENGTH_L; + }else{ + ret = DXL_LIB_ERROR_INVAILD_ID; + p_parse_packet->parse_state = DXL2_0_PACKET_PARSING_STATE_IDLE; + } + break; + + case DXL2_0_PACKET_PARSING_STATE_LENGTH_L: + p_parse_packet->packet_len = recv_data; + update_dxl_crc(&p_parse_packet->calculated_crc, recv_data); + p_parse_packet->parse_state = DXL2_0_PACKET_PARSING_STATE_LENGTH_H; + break; + + case DXL2_0_PACKET_PARSING_STATE_LENGTH_H: + p_parse_packet->packet_len |= recv_data<<8; + update_dxl_crc(&p_parse_packet->calculated_crc, recv_data); + if(p_parse_packet->packet_len < 3){ // 3 = Instruction(1)+CRC(2) + ret = DXL_LIB_ERROR_LENGTH; + p_parse_packet->parse_state = DXL2_0_PACKET_PARSING_STATE_IDLE; + }else{ + p_parse_packet->parse_state = DXL2_0_PACKET_PARSING_STATE_INST; + } + break; + + case DXL2_0_PACKET_PARSING_STATE_INST: + p_parse_packet->inst_idx = recv_data; + update_dxl_crc(&p_parse_packet->calculated_crc, recv_data); + p_parse_packet->recv_param_len = 0; + p_parse_packet->err_idx = 0; + if(recv_data == DXL_INST_STATUS){ + if(p_parse_packet->packet_len < 4){ // 4 = Instruction(1)+Error(1)+CRC(2) + ret = DXL_LIB_ERROR_LENGTH; + p_parse_packet->parse_state = DXL2_0_PACKET_PARSING_STATE_IDLE; + }else if(p_parse_packet->packet_len > p_parse_packet->param_buf_capacity+4){ // 4 = Instruction(1)+Error(1)+CRC(2) + ret = DXL_LIB_ERROR_BUFFER_OVERFLOW; + p_parse_packet->parse_state = DXL2_0_PACKET_PARSING_STATE_IDLE; + }else{ + p_parse_packet->parse_state = DXL2_0_PACKET_PARSING_STATE_ERROR; + } + }else{ + if(p_parse_packet->packet_len > p_parse_packet->param_buf_capacity+3){ // 3 = Instruction(1)+CRC(2) + ret = DXL_LIB_ERROR_BUFFER_OVERFLOW; + p_parse_packet->parse_state = DXL2_0_PACKET_PARSING_STATE_IDLE; + }else if(p_parse_packet->packet_len == 3){ // 3 = Instruction(1)+CRC(2) + p_parse_packet->parse_state = DXL2_0_PACKET_PARSING_STATE_CRC_L; + }else{ + p_parse_packet->parse_state = DXL2_0_PACKET_PARSING_STATE_PARAM; + } + } + break; + + case DXL2_0_PACKET_PARSING_STATE_ERROR: + p_parse_packet->err_idx = recv_data; + update_dxl_crc(&p_parse_packet->calculated_crc, recv_data); + if(p_parse_packet->packet_len == 4){ // 4 = Instruction(1)+Error(1)+CRC(2) + p_parse_packet->parse_state = DXL2_0_PACKET_PARSING_STATE_CRC_L; + break; + } + p_parse_packet->parse_state = DXL2_0_PACKET_PARSING_STATE_PARAM; + break; + + case DXL2_0_PACKET_PARSING_STATE_PARAM: + if(p_parse_packet->p_param_buf == NULL){ + ret = DXL_LIB_ERROR_NULLPTR; + p_parse_packet->parse_state = DXL2_0_PACKET_PARSING_STATE_IDLE; + } + + p_parse_packet->p_param_buf[p_parse_packet->recv_param_len++] = recv_data; + update_dxl_crc(&p_parse_packet->calculated_crc, recv_data); + + //Remove byte stuffing (http://emanual.robotis.com/docs/en/dxl/protocol2/#processing-order-of-reception) + if(p_parse_packet->inst_idx == DXL_INST_STATUS){ + if(p_parse_packet->recv_param_len >= 4){ + if(p_parse_packet->p_param_buf[p_parse_packet->recv_param_len-4] == 0xFF + && p_parse_packet->p_param_buf[p_parse_packet->recv_param_len-3] == 0xFF + && p_parse_packet->p_param_buf[p_parse_packet->recv_param_len-2] == 0xFD + && p_parse_packet->p_param_buf[p_parse_packet->recv_param_len-1] == 0xFD){ + p_parse_packet->recv_param_len--; + byte_stuffing_cnt++; + } + }else if(p_parse_packet->recv_param_len == 3){ + if(p_parse_packet->err_idx == 0xFF + && p_parse_packet->p_param_buf[0] == 0xFF + && p_parse_packet->p_param_buf[1] == 0xFD + && p_parse_packet->p_param_buf[2] == 0xFD){ + p_parse_packet->recv_param_len--; + byte_stuffing_cnt++; + } + }else if(p_parse_packet->recv_param_len == 2){ + if(p_parse_packet->inst_idx == 0xFF + && p_parse_packet->err_idx == 0xFF + && p_parse_packet->p_param_buf[0] == 0xFD + && p_parse_packet->p_param_buf[1] == 0xFD){ + p_parse_packet->recv_param_len--; + byte_stuffing_cnt++; + } + } + if(p_parse_packet->recv_param_len+byte_stuffing_cnt+4 == p_parse_packet->packet_len){ // 4 = Instruction(1)+Error(1)+CRC(2) + p_parse_packet->parse_state = DXL2_0_PACKET_PARSING_STATE_CRC_L; + } + }else{ + if(p_parse_packet->recv_param_len >= 4){ + if(p_parse_packet->p_param_buf[p_parse_packet->recv_param_len-4] == 0xFF + && p_parse_packet->p_param_buf[p_parse_packet->recv_param_len-3] == 0xFF + && p_parse_packet->p_param_buf[p_parse_packet->recv_param_len-2] == 0xFD + && p_parse_packet->p_param_buf[p_parse_packet->recv_param_len-1] == 0xFD){ + p_parse_packet->recv_param_len--; + byte_stuffing_cnt++; + } + }else if(p_parse_packet->recv_param_len == 3){ + if(p_parse_packet->inst_idx == 0xFF + && p_parse_packet->p_param_buf[0] == 0xFF + && p_parse_packet->p_param_buf[1] == 0xFD + && p_parse_packet->p_param_buf[2] == 0xFD){ + p_parse_packet->recv_param_len--; + byte_stuffing_cnt++; + } + } + if(p_parse_packet->recv_param_len+byte_stuffing_cnt+3 == p_parse_packet->packet_len){ // 3 = Instruction(1)+CRC(2) + p_parse_packet->parse_state = DXL2_0_PACKET_PARSING_STATE_CRC_L; + } + } + break; + + case DXL2_0_PACKET_PARSING_STATE_CRC_L: + p_parse_packet->recv_crc = recv_data; + p_parse_packet->parse_state = DXL2_0_PACKET_PARSING_STATE_CRC_H; + break; + + case DXL2_0_PACKET_PARSING_STATE_CRC_H: + p_parse_packet->recv_crc |= recv_data<<8; + if (p_parse_packet->calculated_crc == p_parse_packet->recv_crc){ + ret = DXL_LIB_OK; + }else{ + ret = DXL_LIB_ERROR_CRC; + } + p_parse_packet->parse_state = DXL2_0_PACKET_PARSING_STATE_IDLE; + break; + + default: + p_parse_packet->parse_state = DXL2_0_PACKET_PARSING_STATE_IDLE; + break; + } + + return ret; +} + + + +const unsigned short crc_table[256] PROGMEM = { 0x0000, + 0x8005, 0x800F, 0x000A, 0x801B, 0x001E, 0x0014, 0x8011, + 0x8033, 0x0036, 0x003C, 0x8039, 0x0028, 0x802D, 0x8027, + 0x0022, 0x8063, 0x0066, 0x006C, 0x8069, 0x0078, 0x807D, + 0x8077, 0x0072, 0x0050, 0x8055, 0x805F, 0x005A, 0x804B, + 0x004E, 0x0044, 0x8041, 0x80C3, 0x00C6, 0x00CC, 0x80C9, + 0x00D8, 0x80DD, 0x80D7, 0x00D2, 0x00F0, 0x80F5, 0x80FF, + 0x00FA, 0x80EB, 0x00EE, 0x00E4, 0x80E1, 0x00A0, 0x80A5, + 0x80AF, 0x00AA, 0x80BB, 0x00BE, 0x00B4, 0x80B1, 0x8093, + 0x0096, 0x009C, 0x8099, 0x0088, 0x808D, 0x8087, 0x0082, + 0x8183, 0x0186, 0x018C, 0x8189, 0x0198, 0x819D, 0x8197, + 0x0192, 0x01B0, 0x81B5, 0x81BF, 0x01BA, 0x81AB, 0x01AE, + 0x01A4, 0x81A1, 0x01E0, 0x81E5, 0x81EF, 0x01EA, 0x81FB, + 0x01FE, 0x01F4, 0x81F1, 0x81D3, 0x01D6, 0x01DC, 0x81D9, + 0x01C8, 0x81CD, 0x81C7, 0x01C2, 0x0140, 0x8145, 0x814F, + 0x014A, 0x815B, 0x015E, 0x0154, 0x8151, 0x8173, 0x0176, + 0x017C, 0x8179, 0x0168, 0x816D, 0x8167, 0x0162, 0x8123, + 0x0126, 0x012C, 0x8129, 0x0138, 0x813D, 0x8137, 0x0132, + 0x0110, 0x8115, 0x811F, 0x011A, 0x810B, 0x010E, 0x0104, + 0x8101, 0x8303, 0x0306, 0x030C, 0x8309, 0x0318, 0x831D, + 0x8317, 0x0312, 0x0330, 0x8335, 0x833F, 0x033A, 0x832B, + 0x032E, 0x0324, 0x8321, 0x0360, 0x8365, 0x836F, 0x036A, + 0x837B, 0x037E, 0x0374, 0x8371, 0x8353, 0x0356, 0x035C, + 0x8359, 0x0348, 0x834D, 0x8347, 0x0342, 0x03C0, 0x83C5, + 0x83CF, 0x03CA, 0x83DB, 0x03DE, 0x03D4, 0x83D1, 0x83F3, + 0x03F6, 0x03FC, 0x83F9, 0x03E8, 0x83ED, 0x83E7, 0x03E2, + 0x83A3, 0x03A6, 0x03AC, 0x83A9, 0x03B8, 0x83BD, 0x83B7, + 0x03B2, 0x0390, 0x8395, 0x839F, 0x039A, 0x838B, 0x038E, + 0x0384, 0x8381, 0x0280, 0x8285, 0x828F, 0x028A, 0x829B, + 0x029E, 0x0294, 0x8291, 0x82B3, 0x02B6, 0x02BC, 0x82B9, + 0x02A8, 0x82AD, 0x82A7, 0x02A2, 0x82E3, 0x02E6, 0x02EC, + 0x82E9, 0x02F8, 0x82FD, 0x82F7, 0x02F2, 0x02D0, 0x82D5, + 0x82DF, 0x02DA, 0x82CB, 0x02CE, 0x02C4, 0x82C1, 0x8243, + 0x0246, 0x024C, 0x8249, 0x0258, 0x825D, 0x8257, 0x0252, + 0x0270, 0x8275, 0x827F, 0x027A, 0x826B, 0x026E, 0x0264, + 0x8261, 0x0220, 0x8225, 0x822F, 0x022A, 0x823B, 0x023E, + 0x0234, 0x8231, 0x8213, 0x0216, 0x021C, 0x8219, 0x0208, + 0x820D, 0x8207, 0x0202 }; + +static void update_dxl_crc(uint16_t *p_crc_cur, uint8_t recv_data) +{ + uint16_t crc; + uint16_t i; + + crc = *p_crc_cur; + + i = ((unsigned short)(crc >> 8) ^ recv_data) & 0xFF; + *p_crc_cur = (crc << 8) ^ pgm_read_word_near(crc_table + i); +} \ No newline at end of file diff --git a/src/dxl_c/protocol.h b/src/dxl_c/protocol.h new file mode 100644 index 0000000..8388e48 --- /dev/null +++ b/src/dxl_c/protocol.h @@ -0,0 +1,167 @@ +/******************************************************************************* +* 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. +*******************************************************************************/ +#ifndef DYNAMIXEL_PROTOCOL_H_ +#define DYNAMIXEL_PROTOCOL_H_ + +#include +#include + +#define DXL_BROADCAST_ID 0xFE + + +#ifdef __cplusplus +extern "C" { +#endif + +// http://emanual.robotis.com/docs/en/dxl/protocol1/#instruction +// http://emanual.robotis.com/docs/en/dxl/protocol2/#instruction +enum DXLInstruction{ + DXL_INST_PING = 0x01, + DXL_INST_READ = 0x02, + DXL_INST_WRITE = 0x03, + DXL_INST_REG_WRITE = 0x04, + DXL_INST_ACTION = 0x05, + DXL_INST_FACTORY_RESET = 0x06, + DXL_INST_REBOOT = 0x08, + DXL_INST_CLEAR = 0x10, //ONLY Protocol2.0 + DXL_INST_STATUS = 0x55, //ONLY Protocol2.0 + DXL_INST_SYNC_READ = 0x82, //ONLY Protocol2.0 + DXL_INST_SYNC_WRITE = 0x83, + DXL_INST_BULK_READ = 0x92, + DXL_INST_BULK_WRITE = 0x93 //ONLY Protocol2.0 +}; + +enum DXL1_0PacketError{ + DXL1_0_ERR_INPUT_VOLTAGE_BIT = 0, + DXL1_0_ERR_ANGLE_LIMIT_BIT, + DXL1_0_ERR_OVERHEATING_BIT, + DXL1_0_ERR_RANGE_BIT, + DXL1_0_ERR_CHECKSUM_BIT, + DXL1_0_ERR_OVERLOAD_BIT, + DXL1_0_ERR_INSTRUCTION_BIT +}; + +enum DXL2_0PacketError{ + DXL2_0_ERR_NONE = 0x00, + DXL2_0_ERR_RESULT_FAIL, + DXL2_0_ERR_INST_ERROR, + DXL2_0_ERR_CRC_ERROR, + DXL2_0_ERR_DATA_RANGE, + DXL2_0_ERR_DATA_LENGTH, + DXL2_0_ERR_DATA_LIMIT, + DXL2_0_ERR_ACCESS +}; + +enum DXL1_0PacketState{ + DXL1_0_PACKET_PARSING_STATE_IDLE = 0, + DXL1_0_PACKET_PARSING_STATE_ID, + DXL1_0_PACKET_PARSING_STATE_LENGTH, + DXL1_0_PACKET_PARSING_STATE_INST_ERR, + DXL1_0_PACKET_PARSING_STATE_PARAM, + DXL1_0_PACKET_PARSING_STATE_CHECK_SUM +}; + +enum DXL2_0PacketState{ + DXL2_0_PACKET_PARSING_STATE_IDLE = 0, + DXL2_0_PACKET_PARSING_STATE_RESERVED, + DXL2_0_PACKET_PARSING_STATE_ID, + DXL2_0_PACKET_PARSING_STATE_LENGTH_L, + DXL2_0_PACKET_PARSING_STATE_LENGTH_H, + DXL2_0_PACKET_PARSING_STATE_INST, + DXL2_0_PACKET_PARSING_STATE_ERROR, + DXL2_0_PACKET_PARSING_STATE_PARAM, + DXL2_0_PACKET_PARSING_STATE_CRC_L, + DXL2_0_PACKET_PARSING_STATE_CRC_H +}; + +enum DXLLibErrorCode +{ + DXL_LIB_OK = 0, + DXL_LIB_PROCEEDING, + + DXL_LIB_ERROR_NOT_SUPPORTED, + DXL_LIB_ERROR_TIMEOUT, + DXL_LIB_ERROR_INVAILD_ID, + DXL_LIB_ERROR_NOT_SUPPORT_BROADCAST, + DXL_LIB_ERROR_NULLPTR, + DXL_LIB_ERROR_LENGTH, + DXL_LIB_ERROR_INVAILD_ADDR, + DXL_LIB_ERROR_ADDR_LENGTH, + DXL_LIB_ERROR_BUFFER_OVERFLOW, + DXL_LIB_ERROR_PORT_NOT_OPEN, + DXL_LIB_ERROR_WRONG_PACKET, + DXL_LIB_ERROR_CHECK_SUM, + DXL_LIB_ERROR_CRC, + DXL_LIB_ERROR_INVAILD_DATA_LENGTH, + DXL_LIB_ERROR_MEMORY_ALLOCATION_FAIL, + DXL_LIB_ERROR_INVAILD_PROTOCOL_VERSION, + DXL_LIB_ERROR_NOT_INITIALIZED, + DXL_LIB_ERROR_NOT_ENOUGH_BUFFER_SIZE +}; + +typedef struct InfoToParseDXLPacket{ + uint8_t header[3]; + uint8_t header_cnt; + uint8_t id; + uint8_t protocol_ver; + uint8_t inst_idx; + uint8_t err_idx; + uint8_t *p_param_buf; + uint16_t param_buf_capacity; + uint16_t recv_param_len; + uint16_t packet_len; + uint16_t calculated_crc; + uint16_t recv_crc; + uint8_t calculated_check_sum; + uint8_t recv_check_sum; + uint8_t reserved; + uint8_t parse_state; + bool is_init; +}InfoToParseDXLPacket_t; + +typedef struct InfoToMakeDXLPacket{ + uint8_t id; + uint8_t protocol_ver; + uint8_t inst_idx; + uint8_t err_idx; + uint8_t *p_packet_buf; + uint16_t packet_buf_capacity; + uint16_t param_length; + uint16_t generated_packet_length; + bool is_init; +}InfoToMakeDXLPacket_t; + + +typedef int32_t DXLLibErrorCode_t; + +DXLLibErrorCode_t begin_make_dxl_packet(InfoToMakeDXLPacket_t* p_make_packet, + uint8_t id, uint8_t protocol_ver, uint8_t inst_idx, uint8_t err_idx, + uint8_t* p_packet_buf, uint16_t packet_buf_capacity); +DXLLibErrorCode_t add_param_to_dxl_packet(InfoToMakeDXLPacket_t* p_make_packet, + uint8_t *p_param, uint16_t param_len); +DXLLibErrorCode_t end_make_dxl_packet(InfoToMakeDXLPacket_t* p_make_packet); + + +DXLLibErrorCode_t begin_parse_dxl_packet(InfoToParseDXLPacket_t* p_parse_packet, + uint8_t protocol_ver, uint8_t* p_param_buf, uint16_t param_buf_cap); +DXLLibErrorCode_t parse_dxl_packet(InfoToParseDXLPacket_t* p_parse_packet, uint8_t recv_data); + + +#ifdef __cplusplus +} +#endif + +#endif /* DYNAMIXEL_PROTOCOL_H_ */ \ No newline at end of file diff --git a/src/utility/config.h b/src/utility/config.h index afb4ec7..f84542e 100644 --- a/src/utility/config.h +++ b/src/utility/config.h @@ -28,6 +28,25 @@ #define ENABLE_ACTUATOR_PRO_PLUS 1 +#if defined (ARDUINO_AVR_UNO) || defined (ARDUINO_AVR_YUN) \ + || defined (ARDUINO_AVR_INDUSTRIAL101) +#define DEFAULT_DXL_BUF_LENGTH 192 +#elif defined (ARDUINO_AVR_LEONARDO) +#define DEFAULT_DXL_BUF_LENGTH 256 +#elif defined (OpenCR) +#define DEFAULT_DXL_BUF_LENGTH 2048 +#else +#define DEFAULT_DXL_BUF_LENGTH 1024 +#endif + + +#if (DEFAULT_DXL_BUF_LENGTH > 0xFFFF) +#error "\r\nError : DEFAULT_DXL_BUF_LENGTH is OVERFLOW! This must be a 16 bit range." +#endif + + + +// >> Legacy (Deprecated since v0.4.0) #if defined (ARDUINO_AVR_UNO) || defined (ARDUINO_AVR_YUN) \ || defined (ARDUINO_AVR_INDUSTRIAL101) #define DXL_MAX_NODE 16 @@ -44,8 +63,10 @@ #endif #define DXL_BUF_LENGTH (DXL_MAX_NODE*DXL_MAX_NODE_BUFFER_SIZE + 11) // 11 = Header(3)+Reserved(1)+ID(1)+Length(2)+Instruction(1)+Error(1)+crc(2) - - +#if (DXL_BUF_LENGTH > DEFAULT_DXL_BUF_LENGTH) +#undef DEFAULT_DXL_BUF_LENGTH +#define DEFAULT_DXL_BUF_LENGTH DXL_BUF_LENGTH +#endif #if (DXL_MAX_NODE > 253) #error "\r\nError : DXL_MAX_NODE is OVERFLOW! This should be less or equal than 253 by the protocol." @@ -56,7 +77,7 @@ #if (DXL_BUF_LENGTH > 0xFFFF) #error "\r\nError : DXL_BUF_LENGTH is OVERFLOW! This must be a 16 bit range." #endif - +// << Legacy (Deprecated since v0.4.0) diff --git a/src/utility/master.cpp b/src/utility/master.cpp index 5386859..62b449b 100644 --- a/src/utility/master.cpp +++ b/src/utility/master.cpp @@ -3,663 +3,1338 @@ using namespace DYNAMIXEL; -Master::Master(PortHandler &port, float protocol_ver) - : last_status_packet_error_(0), last_lib_err_code_(DXL_LIB_OK) + +Master::Master(DXLPortHandler &port, float protocol_ver, uint16_t malloc_buf_size) +: protocol_ver_idx_(2), + is_buf_malloced_(false), packet_buf_capacity_(0), + last_lib_err_(DXL_LIB_OK) { setPort(port); - dxlInit(&packet_, protocol_ver); + setPortProtocolVersion(protocol_ver); + + if(malloc_buf_size > 0){ + p_packet_buf_ = new uint8_t[malloc_buf_size]; + if(p_packet_buf_ != nullptr){ + packet_buf_capacity_ = malloc_buf_size; + is_buf_malloced_ = true; + } + } + info_tx_packet_.is_init = false; + info_rx_packet_.is_init = false; } -Master::Master(float protocol_ver) - : last_status_packet_error_(0), last_lib_err_code_(DXL_LIB_OK) + +Master::Master(float protocol_ver, uint16_t malloc_buf_size) +: protocol_ver_idx_(2), + is_buf_malloced_(false), packet_buf_capacity_(0), + last_lib_err_(DXL_LIB_OK) { - dxlInit(&packet_, protocol_ver); + setPortProtocolVersion(protocol_ver); + + if(malloc_buf_size > 0){ + p_packet_buf_ = new uint8_t[malloc_buf_size]; + if(p_packet_buf_ != nullptr){ + packet_buf_capacity_ = malloc_buf_size; + is_buf_malloced_ = true; + } + } + info_tx_packet_.is_init = false; + info_rx_packet_.is_init = false; } -bool Master::setPortProtocolVersion(float version) + +bool +Master::setPacketBuffer(uint8_t* p_buf, uint16_t buf_capacity) { - return dxlSetProtocolVersion(&packet_, version); + if(p_buf == nullptr){ + last_lib_err_ = DXL_LIB_ERROR_NULLPTR; + return false; + } + if(packet_buf_capacity_ == 0){ + last_lib_err_ = DXL_LIB_ERROR_NOT_ENOUGH_BUFFER_SIZE; + return false; + } + + if(is_buf_malloced_ == true){ + delete p_packet_buf_; + } + p_packet_buf_ = p_buf; + packet_buf_capacity_ = buf_capacity; + + return true; } -bool Master::setPortProtocolVersionUsingIndex(uint8_t version_idx) + +uint8_t* +Master::getPacketBuffer() const { - float version_float; + return p_packet_buf_; +} + + +uint16_t +Master::getPacketBufferCapacity() const +{ + return packet_buf_capacity_; +} + - if(version_idx == 1){ - version_float = 1.0; - }else if(version_idx == 2){ - version_float = 2.0; +// Refer to http://emanual.robotis.com/#protocol +bool +Master::setPortProtocolVersion(float version) +{ + uint8_t version_idx; + + if(version == 2.0){ + version_idx = 2; + }else if(version == 1.0){ + version_idx = 1; }else{ + last_lib_err_ = DXL_LIB_ERROR_INVAILD_PROTOCOL_VERSION; return false; } - return setPortProtocolVersion(version_float); + return setPortProtocolVersionUsingIndex(version_idx); } -float Master::getPortProtocolVersion() + +bool +Master::setPortProtocolVersionUsingIndex(uint8_t version_idx) { - return dxlGetProtocolVersion(&packet_); + if(version_idx != 2 && version_idx != 1){ + return false; + } + protocol_ver_idx_ = version_idx; + + return true; } -bool Master::setPort(PortHandler &port) + +float +Master::getPortProtocolVersion() const +{ + return (float)protocol_ver_idx_; +} + + +bool +Master::setPort(DXLPortHandler *p_port) { - bool ret = setDxlPort(&packet_, &port); + if(p_port == nullptr){ + last_lib_err_ = DXL_LIB_ERROR_NULLPTR; + return false; + } + + p_port_ = p_port; + + return true; +} + +bool +Master::setPort(DXLPortHandler &port) +{ p_port_ = &port; - return ret; + return true; } -PortHandler* Master::getPort() const + +DXLPortHandler* +Master::getPort() const { return p_port_; } -uint8_t Master::ping(uint8_t id, XelInfoFromPing_t *recv_info_array, uint8_t recv_array_cnt, uint32_t timeout) + +// (Protocol 1.0) Refer to http://emanual.robotis.com/docs/en/dxl/protocol1/#ping +// (Protocol 2.0) Refer to http://emanual.robotis.com/docs/en/dxl/protocol2/#ping +uint8_t +Master::ping(uint8_t id, uint8_t *p_recv_id_array, uint8_t recv_array_capacity, uint32_t timeout_ms) { - uint8_t recv_id_cnt = 0; - uint32_t pre_time_ms, pre_time_us; + uint8_t ret_id_cnt = 0; + uint32_t pre_time_ms; + uint8_t rx_param[3]; - if(recv_info_array == nullptr){ - last_lib_err_code_ = DXL_LIB_ERROR_NULLPTR; + // Parameter exception handling + if(p_recv_id_array == nullptr){ + last_lib_err_ = DXL_LIB_ERROR_NULLPTR; + return 0; + }else if(recv_array_capacity == 0){ + last_lib_err_ = DXL_LIB_ERROR_NOT_ENOUGH_BUFFER_SIZE; return 0; } - if(recv_array_cnt == 0){ - last_lib_err_code_ = DXL_LIB_ERROR_BUFFER_OVERFLOW; - return 0; + // Send Ping Instruction + if(txInstPacket(id, DXL_INST_PING, nullptr, 0) == true){ + // Receive Status Packet + if(id != DXL_BROADCAST_ID){ + if(rxStatusPacket(rx_param, 3, timeout_ms) != nullptr){ + if(info_rx_packet_.id == id){ + p_recv_id_array[ret_id_cnt++] = info_rx_packet_.id; + } + } + }else{ + pre_time_ms = millis(); + while(ret_id_cnt < recv_array_capacity) + { + if(rxStatusPacket(rx_param, 3, 3) != nullptr){ + p_recv_id_array[ret_id_cnt++] = info_rx_packet_.id; + } + + if (millis()-pre_time_ms >= timeout_ms) { + last_lib_err_ = DXL_LIB_ERROR_TIMEOUT; + break; + } + } + } } - if (p_port_->getOpenState() != true) { - last_lib_err_code_ = DXL_LIB_ERROR_PORT_NOT_OPEN; + return ret_id_cnt; +} + +uint8_t +Master::ping(uint8_t id, InfoFromPing_t *recv_ping_info_array, uint8_t recv_array_cnt, uint32_t timeout_ms) +{ + uint8_t ret_id_cnt = 0; + InfoFromPing_t *p_info = recv_ping_info_array; + uint32_t pre_time_ms; + + // Parameter exception handling + if(p_info == nullptr){ + last_lib_err_ = DXL_LIB_ERROR_NULLPTR; + return 0; + }else if(recv_array_cnt == 0){ + last_lib_err_ = DXL_LIB_ERROR_NOT_ENOUGH_BUFFER_SIZE; return 0; } - pre_time_us = micros(); - last_lib_err_code_ = dxlTxPacketInst(&packet_, id, INST_PING, nullptr, 0); - if(last_lib_err_code_ != DXL_LIB_OK) - return 0; - packet_.tx_time = micros() - pre_time_us; - - pre_time_ms = millis(); - pre_time_us = micros(); - while(recv_id_cnt < recv_array_cnt) - { - last_lib_err_code_ = dxlRxPacket(&packet_); - if (last_lib_err_code_ == DXL_LIB_OK - && packet_.rx.type == RX_PACKET_TYPE_STATUS) { - packet_.rx_time = micros() - pre_time_us; - pre_time_ms = millis(); - - recv_info_array[recv_id_cnt].id = packet_.rx.id; - if(getPortProtocolVersion() == DXL_PACKET_VER_2_0) { - recv_info_array[recv_id_cnt].model_number = packet_.rx.p_param[0]<<0; - recv_info_array[recv_id_cnt].model_number |= packet_.rx.p_param[1]<<8; - recv_info_array[recv_id_cnt].firmware_version = packet_.rx.p_param[2]; - } - recv_id_cnt++; - - if (id != DXL_BROADCAST_ID) { - last_lib_err_code_ = DXL_LIB_OK; - break; + // Send Ping Instruction + if(txInstPacket(id, DXL_INST_PING, nullptr, 0) == true){ + // Receive Status Packet + if(id != DXL_BROADCAST_ID){ + if(rxStatusPacket((uint8_t*)&p_info[ret_id_cnt].model_number, 3, timeout_ms) != nullptr){ + if(info_rx_packet_.id == id){ + p_info[ret_id_cnt++].id = info_rx_packet_.id; + } + } + }else{ + pre_time_ms = millis(); + while(ret_id_cnt < recv_array_cnt) + { + if(rxStatusPacket((uint8_t*)&p_info[ret_id_cnt].model_number, 3, 3) != nullptr){ + p_info[ret_id_cnt++].id = info_rx_packet_.id; + } + + if (millis()-pre_time_ms >= timeout_ms) { + last_lib_err_ = DXL_LIB_ERROR_TIMEOUT; + break; + } } } + } + + return ret_id_cnt; +} - if (millis()-pre_time_ms >= timeout) { - last_lib_err_code_ = DXL_LIB_ERROR_TIMEOUT; - break; + +// (Protocol 1.0) Refer to http://emanual.robotis.com/docs/en/dxl/protocol1/#read +// (Protocol 2.0) Refer to http://emanual.robotis.com/docs/en/dxl/protocol2/#read +int32_t +Master::read(uint8_t id, uint16_t addr, uint16_t addr_length, + uint8_t *p_recv_buf, uint16_t recv_buf_capacity, uint32_t timeout_ms) +{ + int32_t ret_param_len = -1; + DXLLibErrorCode_t err = DXL_LIB_OK; + uint8_t param_len = 0; + uint8_t tx_param[4]; + + // Parameter exception handling + if(p_recv_buf == nullptr){ + err = DXL_LIB_ERROR_NULLPTR; + }else if(addr_length == 0) { + err = DXL_LIB_ERROR_ADDR_LENGTH; + }else if(recv_buf_capacity < addr_length){ + err = DXL_LIB_ERROR_NOT_ENOUGH_BUFFER_SIZE; + }else if (id == DXL_BROADCAST_ID) { + err = DXL_LIB_ERROR_NOT_SUPPORT_BROADCAST; + } + if(err != DXL_LIB_OK){ + last_lib_err_ = err; + return -1; + } + + // Send Read Instruction + if(protocol_ver_idx_ == 2){ + tx_param[param_len++] = addr >> 0; + tx_param[param_len++] = addr >> 8; + tx_param[param_len++] = addr_length >> 0; + tx_param[param_len++] = addr_length >> 8; + }else if(protocol_ver_idx_ == 1){ + tx_param[param_len++] = (uint8_t)addr&0xFF; + tx_param[param_len++] = (uint8_t)addr_length&0xFF; + } + if(txInstPacket(id, DXL_INST_READ, (uint8_t*)&tx_param, param_len) == true){ + if(rxStatusPacket(p_recv_buf, recv_buf_capacity, timeout_ms) != nullptr){ + ret_param_len = (int32_t)info_rx_packet_.recv_param_len; } } - return recv_id_cnt; + + return ret_param_len; } -bool Master::ping(uint8_t id, RecvInfoFromPing_t &recv_info, uint32_t timeout) + +// (Protocol 1.0) Refer to http://emanual.robotis.com/docs/en/dxl/protocol1/#write +// (Protocol 2.0) Refer to http://emanual.robotis.com/docs/en/dxl/protocol2/#write +bool +Master::write(uint8_t id, uint16_t addr, + const uint8_t *p_data, uint16_t data_length, uint32_t timeout_ms) { bool ret = false; - - recv_info.id_count = ping(id, recv_info.xel, sizeof(recv_info.xel)/sizeof(XelInfoFromPing_t), timeout); - if(recv_info.id_count > 0) - ret = true; + + // Send Write Instruction + if(writeNoResp(id, addr, p_data, data_length) == true){ + // Receive Status Packet + if(id != DXL_BROADCAST_ID){ + if(rxStatusPacket(nullptr, 0, timeout_ms) != nullptr){ + if(protocol_ver_idx_ == 2){ + if(info_rx_packet_.err_idx == 0 || info_rx_packet_.err_idx == 0x80){ + ret = true; + } + }else if(protocol_ver_idx_ == 1){ + if(info_rx_packet_.err_idx == 0 + || (info_rx_packet_.err_idx & (1<getOpenState() != true){ + err = DXL_LIB_ERROR_PORT_NOT_OPEN; + }else if(data_length == 0) { + err = DXL_LIB_ERROR_INVAILD_DATA_LENGTH; } - - if(addr_length == 0) { - last_lib_err_code_ = DXL_LIB_ERROR_ADDR_LENGTH; - return -1; + if(err != DXL_LIB_OK){ + last_lib_err_ = err; + return false; } - - if (p_port_->getOpenState() != true) { - last_lib_err_code_ = DXL_LIB_ERROR_PORT_NOT_OPEN; - return -1; + + // Send Write Instruction + if(protocol_ver_idx_ == 2){ + param_len = 2; + }else if(protocol_ver_idx_ == 1){ + param_len = 1; } - - // Send Read Instruction - if (packet_.packet_ver == DXL_PACKET_VER_1_0 ) { - tx_param[0] = addr; - tx_param[1] = addr_length; - }else{ - tx_param[0] = addr >> 0; - tx_param[1] = addr >> 8; - tx_param[2] = addr_length >> 0; - tx_param[3] = addr_length >> 8; + begin_make_dxl_packet(&info_tx_packet_, id, protocol_ver_idx_, + DXL_INST_WRITE, 0, p_packet_buf_, packet_buf_capacity_); + add_param_to_dxl_packet(&info_tx_packet_, (uint8_t*)&addr, param_len); + param_len = data_length; + add_param_to_dxl_packet(&info_tx_packet_, (uint8_t*)p_data, param_len); + err = end_make_dxl_packet(&info_tx_packet_); + if(err == DXL_LIB_OK){ + p_port_->write(info_tx_packet_.p_packet_buf, info_tx_packet_.generated_packet_length); + ret = true; } - pre_time_us = micros(); - last_lib_err_code_ = dxlTxPacketInst(&packet_, id, INST_READ, tx_param, 4); - packet_.tx_time = micros() - pre_time_us; + last_lib_err_ = err; - pre_time_ms = millis(); - pre_time_us = micros(); + return ret; +} - // Receive Status Packet - while(1) { - last_lib_err_code_ = dxlRxPacket(&packet_); - if (last_lib_err_code_ == DXL_LIB_OK && packet_.rx.type == RX_PACKET_TYPE_STATUS) { - pre_time_ms = millis(); - packet_.rx_time = micros() - pre_time_us; - recv_param_len = packet_.rx.param_length; - if(recv_param_len > recv_buf_length) { - recv_param_len = recv_buf_length; - } +// (Protocol 1.0) Refer to http://emanual.robotis.com/docs/en/dxl/protocol1/#reg-write +// (Protocol 2.0) Refer to http://emanual.robotis.com/docs/en/dxl/protocol2/#reg-write +bool +Master::regWrite(uint8_t id, uint16_t addr, + const uint8_t *p_data, uint16_t data_length, uint32_t timeout_ms) +{ + bool ret = false; + DXLLibErrorCode_t err = DXL_LIB_OK; + uint8_t param_len = 0; + + // Parameter exception handling + if(p_data == nullptr || p_port_ == nullptr){ + err = DXL_LIB_ERROR_NULLPTR; + }else if(p_port_->getOpenState() != true){ + err = DXL_LIB_ERROR_PORT_NOT_OPEN; + }else if(data_length == 0) { + err = DXL_LIB_ERROR_INVAILD_DATA_LENGTH; + } + if(err != DXL_LIB_OK){ + last_lib_err_ = err; + return false; + } - for (i=0; iwrite(info_tx_packet_.p_packet_buf, info_tx_packet_.generated_packet_length); + + // Receive Status Packet + if(id != DXL_BROADCAST_ID){ + if(rxStatusPacket(nullptr, 0, timeout_ms) != nullptr){ + if(protocol_ver_idx_ == 2){ + if(info_rx_packet_.err_idx == 0 || info_rx_packet_.err_idx == 0x80){ + ret = true; + } + }else if(protocol_ver_idx_ == 1){ + if(info_rx_packet_.err_idx == 0 + || (info_rx_packet_.err_idx & (1<= timeout) { - last_lib_err_code_ = DXL_LIB_ERROR_TIMEOUT; - break; + last_lib_err_ = err; + + return ret; +} + + +// (Protocol 1.0) Refer to http://emanual.robotis.com/docs/en/dxl/protocol1/#action +// (Protocol 2.0) Refer to http://emanual.robotis.com/docs/en/dxl/protocol2/#action +bool +Master::action(uint8_t id, uint32_t timeout_ms) +{ + bool ret = false; + + // Send Reboot Instruction + if(txInstPacket(id, DXL_INST_ACTION, nullptr, 0) == true){ + if(id != DXL_BROADCAST_ID){ + if(rxStatusPacket(nullptr, 0, timeout_ms) != nullptr){ + if(protocol_ver_idx_ == 2){ + if(info_rx_packet_.err_idx == 0 || info_rx_packet_.err_idx == 0x80){ + ret = true; + } + }else if(protocol_ver_idx_ == 1){ + if(info_rx_packet_.err_idx == 0 + || (info_rx_packet_.err_idx & (1<= timeout){ - last_lib_err_code_ = DXL_LIB_ERROR_TIMEOUT; - break; + // Send Reboot Instruction + if(txInstPacket(id, DXL_INST_REBOOT, nullptr, 0) == true){ + if(id != DXL_BROADCAST_ID){ + if(rxStatusPacket(nullptr, 0, timeout_ms) != nullptr){ + if(protocol_ver_idx_ == 2){ + if(info_rx_packet_.err_idx == 0 || info_rx_packet_.err_idx == 0x80){ + ret = true; + } + }else if(protocol_ver_idx_ == 1){ + if(info_rx_packet_.err_idx == 0 + || (info_rx_packet_.err_idx & (1<getOpenState() != true) - { - last_lib_err_code_ = DXL_LIB_ERROR_PORT_NOT_OPEN; - return false; - } + return ret; +} - if (packet_.packet_ver == DXL_PACKET_VER_1_0 ) - { - if ((size_t)(PKT_1_0_INST_PARAM_IDX + 1 + data_length + 1) > sizeof(packet_.tx.data)){ - last_lib_err_code_ = DXL_LIB_ERROR_BUFFER_OVERFLOW; - return false; - } - p_tx_data = &packet_.tx.data[PKT_1_0_INST_PARAM_IDX]; - p_tx_data[tx_length++] = addr; - for (i=0; igetOpenState() != true){ + err = DXL_LIB_ERROR_PORT_NOT_OPEN; + }else if(protocol_ver_idx_ != 2){ + err = DXL_LIB_ERROR_NOT_SUPPORTED; } - else - { - if ((size_t)(PKT_INST_PARAM_IDX + 2 + data_length + 2) > sizeof(packet_.tx.data)){ - last_lib_err_code_ = DXL_LIB_ERROR_BUFFER_OVERFLOW; - return false; + if(err != DXL_LIB_OK){ + last_lib_err_ = err; + return 0; + } + + if(p_info->packet.p_buf != nullptr){ + p_packet_buf = p_info->packet.p_buf; + packet_buf_cap = p_info->packet.buf_capacity; + }else{ + p_packet_buf = p_packet_buf_; + packet_buf_cap = packet_buf_capacity_; + } + + if(p_info->packet.p_buf == nullptr && info_tx_packet_.inst_idx != DXL_INST_SYNC_READ){ + p_info->packet.is_completed = false; + } + + if(p_info->packet.is_completed == false || p_info->is_info_changed == true){ + err = begin_make_dxl_packet(&info_tx_packet_, DXL_BROADCAST_ID, protocol_ver_idx_, + DXL_INST_SYNC_READ, 0, p_packet_buf, packet_buf_cap); + if(err == DXL_LIB_OK){ + tx_param[param_len++] = p_info->addr >> 0; + tx_param[param_len++] = p_info->addr >> 8; + tx_param[param_len++] = p_info->addr_length >> 0; + tx_param[param_len++] = p_info->addr_length >> 8; + err = add_param_to_dxl_packet(&info_tx_packet_, tx_param, param_len); + if(err == DXL_LIB_OK){ + for(i=0; ixel_count; i++){ + p_xel = &p_info->p_xels[i]; + err = add_param_to_dxl_packet(&info_tx_packet_, &p_xel->id, 1); + if(err != DXL_LIB_OK){ + break; + } + } + if(err == DXL_LIB_OK){ + err = end_make_dxl_packet(&info_tx_packet_); + if(err == DXL_LIB_OK){ + p_info->packet.gen_length = info_tx_packet_.generated_packet_length; + p_info->packet.is_completed = true; + p_info->is_info_changed = false; + } + } + } } - p_tx_data = &packet_.tx.data[PKT_INST_PARAM_IDX]; - p_tx_data[tx_length++] = addr >> 0; - p_tx_data[tx_length++] = addr >> 8; - for (i=0; iwrite(p_packet_buf, p_info->packet.gen_length); + + for(i=0; ixel_count; i++) { - p_tx_data[tx_length++] = p_data[i]; + p_xel = &p_info->p_xels[i]; + if(rxStatusPacket(p_xel->p_recv_buf, p_info->addr_length, timeout_ms) != nullptr){ + if(info_rx_packet_.id == p_xel->id){ + p_xel->error = info_rx_packet_.err_idx; + recv_cnt++; + }else{ + break; + } + }else{ + break; + } } + }else{ + p_info->packet.is_completed = false; } + last_lib_err_ = err; - pre_time_us = micros(); - last_lib_err_code_ = dxlTxPacketInst(&packet_, id, INST_WRITE, p_tx_data, tx_length); - if(last_lib_err_code_ != DXL_LIB_OK) - return false; - packet_.tx_time = micros() - pre_time_us; - ret = true; - - return ret; + return recv_cnt; } -bool Master::factoryReset(uint8_t id, uint8_t option, uint32_t timeout) + +// (Protocol 1.0) Refer to http://emanual.robotis.com/docs/en/dxl/protocol1/#sync-write +// (Protocol 2.0) Refer to http://emanual.robotis.com/docs/en/dxl/protocol2/#sync-write +bool +Master::syncWrite(InfoSyncWriteInst_t* p_info) { bool ret = false; - uint32_t pre_time_us; - uint32_t pre_time_ms; - uint8_t tx_param[1]; - - if (id == DXL_BROADCAST_ID){ - last_lib_err_code_ = DXL_LIB_ERROR_NOT_SUPPORT_BROADCAST; + DXLLibErrorCode_t err = DXL_LIB_OK; + uint8_t i, tx_param[4], param_len = 0; + uint8_t* p_packet_buf = nullptr; + uint16_t packet_buf_cap; + XELInfoSyncWrite_t* p_xel; + + // Parameter exception handling + if(p_port_ == nullptr){ + err = DXL_LIB_ERROR_NULLPTR; + }else if(p_port_->getOpenState() != true){ + err = DXL_LIB_ERROR_PORT_NOT_OPEN; + }else if(protocol_ver_idx_ != 2 && protocol_ver_idx_ != 1){ + err = DXL_LIB_ERROR_NOT_SUPPORTED; + } + if(err != DXL_LIB_OK){ + last_lib_err_ = err; return false; } - if (p_port_->getOpenState() != true){ - last_lib_err_code_ = DXL_LIB_ERROR_PORT_NOT_OPEN; - return false; + if(p_info->packet.p_buf != nullptr){ + p_packet_buf = p_info->packet.p_buf; + packet_buf_cap = p_info->packet.buf_capacity; + }else{ + p_packet_buf = p_packet_buf_; + packet_buf_cap = packet_buf_capacity_; } - - tx_param[0] = option; - pre_time_us = micros(); - last_lib_err_code_ = dxlTxPacketInst(&packet_, id, INST_RESET, tx_param, 1); - packet_.tx_time = micros() - pre_time_us; + if(p_info->packet.p_buf == nullptr && info_tx_packet_.inst_idx != DXL_INST_SYNC_WRITE){ + p_info->packet.is_completed = false; + } - pre_time_ms = millis(); - pre_time_us = micros(); - while(1) - { - last_lib_err_code_ = dxlRxPacket(&packet_); - if (last_lib_err_code_ == DXL_LIB_OK && packet_.rx.type == RX_PACKET_TYPE_STATUS) { - pre_time_ms = millis(); - packet_.rx_time = micros() - pre_time_us; - last_status_packet_error_ = packet_.rx.error; - ret = true; - break; - }else if (last_lib_err_code_ != DXL_LIB_PROCEEDING){ - break; + if(p_info->packet.is_completed == false || p_info->is_info_changed == true){ + err = begin_make_dxl_packet(&info_tx_packet_, DXL_BROADCAST_ID, protocol_ver_idx_, + DXL_INST_SYNC_WRITE, 0, p_packet_buf, packet_buf_cap); + if(err == DXL_LIB_OK){ + if(protocol_ver_idx_ == 2){ + tx_param[param_len++] = p_info->addr >> 0; + tx_param[param_len++] = p_info->addr >> 8; + tx_param[param_len++] = p_info->addr_length >> 0; + tx_param[param_len++] = p_info->addr_length >> 8; + }else if(protocol_ver_idx_ == 1){ + tx_param[param_len++] = p_info->addr & 0xFF; + tx_param[param_len++] = p_info->addr_length & 0xFF; + } + err = add_param_to_dxl_packet(&info_tx_packet_, tx_param, param_len); + if(err == DXL_LIB_OK){ + for(i=0; ixel_count; i++){ + p_xel = &p_info->p_xels[i]; + err = add_param_to_dxl_packet(&info_tx_packet_, &p_xel->id, 1); + if(err == DXL_LIB_OK){ + err = add_param_to_dxl_packet(&info_tx_packet_, p_xel->p_data, p_info->addr_length); + if(err != DXL_LIB_OK){ + break; + } + }else{ + break; + } + } + if(err == DXL_LIB_OK){ + err = end_make_dxl_packet(&info_tx_packet_); + if(err == DXL_LIB_OK){ + p_info->packet.gen_length = info_tx_packet_.generated_packet_length; + p_info->packet.is_completed = true; + p_info->is_info_changed = false; + } + } + } } + } - if (millis()-pre_time_ms >= timeout){ - last_lib_err_code_ = DXL_LIB_ERROR_TIMEOUT; - break; - } + if(err == DXL_LIB_OK){ + p_port_->write(p_packet_buf, p_info->packet.gen_length); + ret = true; + }else{ + p_info->packet.is_completed = false; } + last_lib_err_ = err; + return ret; } -bool Master::reboot(uint8_t id, uint32_t timeout) -{ - bool ret = false; - uint32_t pre_time_us; - uint32_t pre_time_ms; - uint8_t tx_param[1]; - if (id == DXL_BROADCAST_ID){ - last_lib_err_code_ = DXL_LIB_ERROR_NOT_SUPPORT_BROADCAST; - return false; +// (Protocol 1.0) Refer to http://emanual.robotis.com/docs/en/dxl/protocol1/#bulk-read +// (Protocol 2.0) Refer to http://emanual.robotis.com/docs/en/dxl/protocol2/#bulk-read +uint8_t +Master::bulkRead(InfoBulkReadInst_t* p_info, uint32_t timeout_ms) +{ + uint8_t i, recv_cnt = 0; + DXLLibErrorCode_t err = DXL_LIB_OK; + uint8_t tx_param[5], param_len = 0; + uint8_t* p_packet_buf = nullptr; + uint16_t packet_buf_cap; + XELInfoBulkRead_t* p_xel; + + // Parameter exception handling + if(p_port_ == nullptr){ + err = DXL_LIB_ERROR_NULLPTR; + }else if(p_port_->getOpenState() != true){ + err = DXL_LIB_ERROR_PORT_NOT_OPEN; + }else if(protocol_ver_idx_ != 2 && protocol_ver_idx_ != 1){ + err = DXL_LIB_ERROR_NOT_SUPPORTED; + } + if(err != DXL_LIB_OK){ + last_lib_err_ = err; + return 0; } - if (p_port_->getOpenState() != true){ - last_lib_err_code_ = DXL_LIB_ERROR_PORT_NOT_OPEN; - return false; + if(p_info->packet.p_buf != nullptr){ + p_packet_buf = p_info->packet.p_buf; + packet_buf_cap = p_info->packet.buf_capacity; + }else{ + p_packet_buf = p_packet_buf_; + packet_buf_cap = packet_buf_capacity_; } - pre_time_us = micros(); - last_lib_err_code_ = dxlTxPacketInst(&packet_, id, INST_REBOOT, tx_param, 1); - packet_.tx_time = micros() - pre_time_us; + if(p_info->packet.p_buf == nullptr && info_tx_packet_.inst_idx != DXL_INST_BULK_READ){ + p_info->packet.is_completed = false; + } - pre_time_ms = millis(); - pre_time_us = micros(); - while(1) - { - last_lib_err_code_ = dxlRxPacket(&packet_); - if (last_lib_err_code_ == DXL_LIB_OK && packet_.rx.type == RX_PACKET_TYPE_STATUS) { - pre_time_ms = millis(); - packet_.rx_time = micros() - pre_time_us; - last_status_packet_error_ = packet_.rx.error; - ret = true; - break; - }else if (last_lib_err_code_ != DXL_LIB_PROCEEDING){ - break; + if(p_info->packet.is_completed == false || p_info->is_info_changed == true){ + err = begin_make_dxl_packet(&info_tx_packet_, DXL_BROADCAST_ID, protocol_ver_idx_, + DXL_INST_BULK_READ, 0, p_packet_buf, packet_buf_cap); + if(protocol_ver_idx_ == 1 && err == DXL_LIB_OK){ + tx_param[0] = 0x00; + err = add_param_to_dxl_packet(&info_tx_packet_, (uint8_t*)&tx_param, 1); + } + if(err == DXL_LIB_OK){ + for(i=0; ixel_count; i++){ + p_xel = &p_info->p_xels[i]; + param_len = 0; + if(protocol_ver_idx_ == 2){ + tx_param[param_len++] = p_xel->id; + tx_param[param_len++] = p_xel->addr >> 0; + tx_param[param_len++] = p_xel->addr >> 8; + tx_param[param_len++] = p_xel->addr_length >> 0; + tx_param[param_len++] = p_xel->addr_length >> 8; + }else if(protocol_ver_idx_ == 1){ + tx_param[param_len++] = p_xel->id; + tx_param[param_len++] = p_xel->addr & 0xFF; + tx_param[param_len++] = p_xel->addr_length & 0xFF; + } + err = add_param_to_dxl_packet(&info_tx_packet_, tx_param, param_len); + if(err != DXL_LIB_OK){ + break; + } + } + if(err == DXL_LIB_OK){ + err = end_make_dxl_packet(&info_tx_packet_); + if(err == DXL_LIB_OK){ + p_info->packet.gen_length = info_tx_packet_.generated_packet_length; + p_info->packet.is_completed = true; + p_info->is_info_changed = false; + } + } } + } + + if(err == DXL_LIB_OK){ + p_port_->write(p_packet_buf, p_info->packet.gen_length); - if (millis()-pre_time_ms >= timeout){ - last_lib_err_code_ = DXL_LIB_ERROR_TIMEOUT; - break; + for(i=0; ixel_count; i++) + { + p_xel = &p_info->p_xels[i]; + if(rxStatusPacket(p_xel->p_recv_buf, p_xel->addr_length, timeout_ms) != nullptr){ + if(info_rx_packet_.id == p_xel->id){ + p_xel->error = info_rx_packet_.err_idx; + recv_cnt++; + }else{ + break; + } + }else{ + break; + } } + }else{ + p_info->packet.is_completed = false; } - return ret; + last_lib_err_ = err; + + return recv_cnt; } -bool Master::syncRead(const ParamForSyncReadInst_t ¶m_info, RecvInfoFromStatusInst_t &recv_info, uint32_t timeout) + +// (Protocol 1.0) Not Supported +// (Protocol 2.0) Refer to http://emanual.robotis.com/docs/en/dxl/protocol2/#bulk-write +bool +Master::bulkWrite(InfoBulkWriteInst_t* p_info) { bool ret = false; - uint32_t i, pre_time_us, pre_time_ms; - uint16_t tx_length = 0; - uint8_t *p_tx_data; + DXLLibErrorCode_t err = DXL_LIB_OK; + uint8_t i, tx_param[5], param_len = 0; + uint8_t* p_packet_buf = nullptr; + uint16_t packet_buf_cap; + XELInfoBulkWrite_t* p_xel; + + // Parameter exception handling + if(p_port_ == nullptr){ + err = DXL_LIB_ERROR_NULLPTR; + }else if(p_port_->getOpenState() != true){ + err = DXL_LIB_ERROR_PORT_NOT_OPEN; + }else if(protocol_ver_idx_ != 2){ + err = DXL_LIB_ERROR_NOT_SUPPORTED; + } + if(err != DXL_LIB_OK){ + last_lib_err_ = err; + return 0; + } - if (packet_.packet_ver == DXL_PACKET_VER_1_0 ){ - last_lib_err_code_ = DXL_LIB_ERROR_NOT_SUPPORTED; - return false; + if(p_info->packet.p_buf != nullptr){ + p_packet_buf = p_info->packet.p_buf; + packet_buf_cap = p_info->packet.buf_capacity; + }else{ + p_packet_buf = p_packet_buf_; + packet_buf_cap = packet_buf_capacity_; } - if(param_info.id_count > DXL_MAX_NODE - || (size_t)(PKT_INST_PARAM_IDX + param_info.id_count * 5 + 2) > sizeof(packet_.tx.data)){ - last_lib_err_code_ = DXL_LIB_ERROR_BUFFER_OVERFLOW; - return false; + if(p_info->packet.p_buf == nullptr && info_tx_packet_.inst_idx != DXL_INST_BULK_WRITE){ + p_info->packet.is_completed = false; } - if (p_port_->getOpenState() != true){ - last_lib_err_code_ = DXL_LIB_ERROR_PORT_NOT_OPEN; - return false; + if(p_info->packet.is_completed == false || p_info->is_info_changed == true){ + err = begin_make_dxl_packet(&info_tx_packet_, DXL_BROADCAST_ID, protocol_ver_idx_, + DXL_INST_BULK_WRITE, 0, p_packet_buf, packet_buf_cap); + if(err == DXL_LIB_OK){ + for(i=0; ixel_count; i++){ + p_xel = &p_info->p_xels[i]; + param_len = 0; + tx_param[param_len++] = p_xel->id; + tx_param[param_len++] = p_xel->addr >> 0; + tx_param[param_len++] = p_xel->addr >> 8; + tx_param[param_len++] = p_xel->addr_length >> 0; + tx_param[param_len++] = p_xel->addr_length >> 8; + err = add_param_to_dxl_packet(&info_tx_packet_, tx_param, param_len); + if(err == DXL_LIB_OK){ + err = add_param_to_dxl_packet(&info_tx_packet_, p_xel->p_data, p_xel->addr_length); + } + if(err != DXL_LIB_OK){ + break; + } + } + if(err == DXL_LIB_OK){ + err = end_make_dxl_packet(&info_tx_packet_); + if(err == DXL_LIB_OK){ + p_info->packet.gen_length = info_tx_packet_.generated_packet_length; + p_info->packet.is_completed = true; + p_info->is_info_changed = false; + } + } + } } - p_tx_data = &packet_.tx.data[PKT_INST_PARAM_IDX]; + if(err == DXL_LIB_OK){ + p_port_->write(p_packet_buf, p_info->packet.gen_length); + ret = true; + }else{ + p_info->packet.is_completed = false; + } + last_lib_err_ = err; - p_tx_data[tx_length++] = param_info.addr >> 0; - p_tx_data[tx_length++] = param_info.addr >> 8; - p_tx_data[tx_length++] = param_info.length >> 0; - p_tx_data[tx_length++] = param_info.length >> 8; + return ret; +} - for( i=0; i= param_info.id_count){ - ret = true; - break; - } - } +DXLLibErrorCode_t +Master::getLastLibErrCode() const +{ + return last_lib_err_; +} - if (millis()-pre_time_ms >= timeout){ - last_lib_err_code_ = DXL_LIB_ERROR_TIMEOUT; - return false; - } - } - - return ret; +void +Master::setLastLibErrCode(DXLLibErrorCode_t err_code) +{ + last_lib_err_ = err_code; } -bool Master::syncWrite(const ParamForSyncWriteInst_t ¶m_info) +bool +Master::txInstPacket(uint8_t id, uint8_t inst_idx, uint8_t *p_param, uint16_t param_len) { bool ret = false; - uint32_t i, j, pre_time_us; - uint16_t tx_length = 0; - uint8_t *p_tx_data; - - if (p_port_->getOpenState() != true){ - last_lib_err_code_ = DXL_LIB_ERROR_PORT_NOT_OPEN; + DXLLibErrorCode_t err = DXL_LIB_OK; + + // Parameter exception handling + if(p_port_ == nullptr + || (param_len > 0 && p_param == nullptr)){ + err = DXL_LIB_ERROR_NULLPTR; + }else if(p_port_->getOpenState() != true){ + err = DXL_LIB_ERROR_PORT_NOT_OPEN; + }else if(inst_idx == DXL_INST_STATUS){ + err = DXL_LIB_ERROR_NOT_SUPPORTED; + } + if(err != DXL_LIB_OK){ + last_lib_err_ = err; return false; } - if (packet_.packet_ver == DXL_PACKET_VER_1_0 ){ - if(param_info.id_count > DXL_MAX_NODE - || (size_t)(PKT_1_0_INST_PARAM_IDX + 2 + param_info.length + 1) > sizeof(packet_.tx.data)){ - last_lib_err_code_ = DXL_LIB_ERROR_BUFFER_OVERFLOW; - return false; - } - p_tx_data = &packet_.tx.data[PKT_1_0_INST_PARAM_IDX]; - p_tx_data[tx_length++] = param_info.addr; - p_tx_data[tx_length++] = param_info.length; - }else{ - if(param_info.id_count > DXL_MAX_NODE - || (size_t)(PKT_INST_PARAM_IDX + 4 + param_info.length + 2) > sizeof(packet_.tx.data)){ - last_lib_err_code_ = DXL_LIB_ERROR_BUFFER_OVERFLOW; - return false; - } - p_tx_data = &packet_.tx.data[PKT_INST_PARAM_IDX]; - p_tx_data[tx_length++] = param_info.addr >> 0; - p_tx_data[tx_length++] = param_info.addr >> 8; - p_tx_data[tx_length++] = param_info.length >> 0; - p_tx_data[tx_length++] = param_info.length >> 8; - } - - for( i=0; iwrite(info_tx_packet_.p_packet_buf, info_tx_packet_.generated_packet_length); + ret = true; + } - pre_time_us = micros(); - last_lib_err_code_ = dxlTxPacketInst(&packet_, DXL_BROADCAST_ID, INST_SYNC_WRITE, p_tx_data, tx_length); - if(last_lib_err_code_ != DXL_LIB_OK) - return false; - packet_.tx_time = micros() - pre_time_us; - ret = true; + last_lib_err_ = err; return ret; } -bool Master::bulkRead(const ParamForBulkReadInst_t ¶m_info, RecvInfoFromStatusInst_t &recv_info, uint32_t timeout) + +const InfoToParseDXLPacket_t* +Master::rxStatusPacket(uint8_t* p_param_buf, uint16_t param_buf_cap, uint32_t timeout_ms) { - bool ret = false; - uint32_t i, pre_time_us, pre_time_ms; - uint16_t tx_length = 0; - uint8_t *p_tx_data; + InfoToParseDXLPacket_t *p_ret = nullptr; + DXLLibErrorCode_t err = DXL_LIB_OK; + uint32_t pre_time_ms; - if (p_port_->getOpenState() != true){ - last_lib_err_code_ = DXL_LIB_ERROR_PORT_NOT_OPEN; - return false; + // Parameter exception handling + if(p_port_ == nullptr + || (param_buf_cap > 0 && p_param_buf == nullptr)){ + err = DXL_LIB_ERROR_NULLPTR; + }else if(p_port_->getOpenState() != true){ + err = DXL_LIB_ERROR_PORT_NOT_OPEN; + } + if(err != DXL_LIB_OK){ + last_lib_err_ = err; + return nullptr; } - if(getPortProtocolVersion() == DXL_PACKET_VER_1_0){ - if(param_info.id_count > DXL_MAX_NODE - || (size_t)(PKT_1_0_INST_PARAM_IDX + 1 + param_info.id_count * 3 + 1) > sizeof(packet_.tx.data)){ - last_lib_err_code_ = DXL_LIB_ERROR_BUFFER_OVERFLOW; - return false; - } - p_tx_data = &packet_.tx.data[PKT_1_0_INST_PARAM_IDX]; - p_tx_data[tx_length++] = 0x00; - for( i=0; i DXL_MAX_NODE - || (size_t)(PKT_INST_PARAM_IDX + param_info.id_count * 5 + 2) > sizeof(packet_.tx.data)){ - last_lib_err_code_ = DXL_LIB_ERROR_BUFFER_OVERFLOW; - return false; - } - p_tx_data = &packet_.tx.data[PKT_INST_PARAM_IDX]; - for( i=0; i> 0; - p_tx_data[tx_length++] = param_info.xel[i].addr >> 8; - p_tx_data[tx_length++] = param_info.xel[i].length >> 0; - p_tx_data[tx_length++] = param_info.xel[i].length >> 8; + if(p_port_->available() > 0){ + err = parse_dxl_packet(&info_rx_packet_, p_port_->read()); + if(err == DXL_LIB_OK){ + if((protocol_ver_idx_ == 2 && info_rx_packet_.inst_idx == DXL_INST_STATUS) + || protocol_ver_idx_ == 1){ + p_ret = &info_rx_packet_; + break; + } + }else if(err != DXL_LIB_PROCEEDING){ + break; + } + } + + if (millis()-pre_time_ms >= timeout_ms) { + err = DXL_LIB_ERROR_TIMEOUT; + break; + } } } - pre_time_us = micros(); - last_lib_err_code_ = dxlTxPacketInst(&packet_, DXL_BROADCAST_ID, INST_BULK_READ, p_tx_data, tx_length); - if(last_lib_err_code_ != DXL_LIB_OK) - return false; - packet_.tx_time = micros() - pre_time_us; - - recv_info.id_count = 0; - pre_time_ms = millis(); - pre_time_us = micros(); - while(1) - { - last_lib_err_code_ = dxlRxPacket(&packet_); - if (last_lib_err_code_ == DXL_LIB_OK && packet_.rx.type == RX_PACKET_TYPE_STATUS) { - pre_time_ms = millis(); - packet_.rx_time = micros() - pre_time_us; + last_lib_err_ = err; - recv_info.xel[recv_info.id_count].id = packet_.rx.id; - recv_info.xel[recv_info.id_count].error = packet_.rx.error; - recv_info.xel[recv_info.id_count].length = packet_.rx.param_length; + return p_ret; +} - for (i=0; i> Legacy (Deprecated since v0.4.0) +bool Master::syncRead(const ParamForSyncReadInst_t ¶m_info, RecvInfoFromStatusInst_t &recv_info, uint32_t timeout_ms) +{ + bool ret = false; + DXLLibErrorCode_t err = DXL_LIB_OK; + uint8_t i, tx_param[4], param_len = 0; + + // Parameter exception handling + if(p_port_ == nullptr){ + err = DXL_LIB_ERROR_NULLPTR; + }else if(p_port_->getOpenState() != true){ + err = DXL_LIB_ERROR_PORT_NOT_OPEN; + }else if(protocol_ver_idx_ != 2){ + err = DXL_LIB_ERROR_NOT_SUPPORTED; + } + if(err != DXL_LIB_OK){ + last_lib_err_ = err; + return false; + } + + err = begin_make_dxl_packet(&info_tx_packet_, DXL_BROADCAST_ID, protocol_ver_idx_, + DXL_INST_SYNC_READ, 0, p_packet_buf_, packet_buf_capacity_); + if(err == DXL_LIB_OK){ + tx_param[param_len++] = param_info.addr >> 0; + tx_param[param_len++] = param_info.addr >> 8; + tx_param[param_len++] = param_info.length >> 0; + tx_param[param_len++] = param_info.length >> 8; + err = add_param_to_dxl_packet(&info_tx_packet_, tx_param, param_len); + if(err == DXL_LIB_OK){ + for(i=0; iwrite(p_packet_buf_, info_tx_packet_.generated_packet_length); - if (recv_info.id_count >= param_info.id_count){ - ret = true; + for(i=0; i= timeout){ - last_lib_err_code_ = DXL_LIB_ERROR_TIMEOUT; - return false; + recv_info.id_count = i; + if(recv_info.id_count > 0){ + ret = true; } - } + } + last_lib_err_ = err; + return ret; } -bool Master::bulkWrite(const ParamForBulkWriteInst_t ¶m_info) + +bool Master::syncWrite(const ParamForSyncWriteInst_t ¶m_info) { bool ret = false; - uint32_t i, j, pre_time_us; - uint16_t tx_length = 0, total_data_length = 0; - uint8_t *p_tx_data; - - if (packet_.packet_ver == DXL_PACKET_VER_1_0 ){ - last_lib_err_code_ = DXL_LIB_ERROR_NOT_SUPPORTED; + DXLLibErrorCode_t err = DXL_LIB_OK; + uint8_t i, tx_param[4], param_len = 0; + + // Parameter exception handling + if(p_port_ == nullptr){ + err = DXL_LIB_ERROR_NULLPTR; + }else if(p_port_->getOpenState() != true){ + err = DXL_LIB_ERROR_PORT_NOT_OPEN; + }else if(protocol_ver_idx_ != 2 && protocol_ver_idx_ != 1){ + err = DXL_LIB_ERROR_NOT_SUPPORTED; + } + if(err != DXL_LIB_OK){ + last_lib_err_ = err; return false; } - for( i=0; i> 0; + tx_param[param_len++] = param_info.addr >> 8; + tx_param[param_len++] = param_info.length >> 0; + tx_param[param_len++] = param_info.length >> 8; + }else if(protocol_ver_idx_ == 1){ + tx_param[param_len++] = param_info.addr & 0xFF; + tx_param[param_len++] = param_info.length & 0xFF; + } + err = add_param_to_dxl_packet(&info_tx_packet_, tx_param, param_len); + if(err == DXL_LIB_OK){ + for(i=0; i DXL_MAX_NODE - || (size_t)(PKT_INST_PARAM_IDX + param_info.id_count * 5 + total_data_length +2) > sizeof(packet_.tx.data)){ - last_lib_err_code_ = DXL_LIB_ERROR_BUFFER_OVERFLOW; - return false; + if(err == DXL_LIB_OK){ + p_port_->write(p_packet_buf_, info_tx_packet_.generated_packet_length); + ret = true; } + last_lib_err_ = err; - if (p_port_->getOpenState() != true){ - last_lib_err_code_ = DXL_LIB_ERROR_PORT_NOT_OPEN; - return false; + return ret; +} + + +bool Master::bulkRead(const ParamForBulkReadInst_t ¶m_info, RecvInfoFromStatusInst_t &recv_info, uint32_t timeout_ms) +{ + bool ret = false; + DXLLibErrorCode_t err = DXL_LIB_OK; + uint8_t i, tx_param[5], param_len = 0; + const XelInfoForBulkReadParam_t* p_xel; + + // Parameter exception handling + if(p_port_ == nullptr){ + err = DXL_LIB_ERROR_NULLPTR; + }else if(p_port_->getOpenState() != true){ + err = DXL_LIB_ERROR_PORT_NOT_OPEN; + }else if(protocol_ver_idx_ != 2 && protocol_ver_idx_ != 1){ + err = DXL_LIB_ERROR_NOT_SUPPORTED; + } + if(err != DXL_LIB_OK){ + last_lib_err_ = err; + return 0; } - p_tx_data = &packet_.tx.data[PKT_INST_PARAM_IDX]; - for( i=0; i> 0; - p_tx_data[tx_length++] = param_info.xel[i].addr >> 8; - p_tx_data[tx_length++] = param_info.xel[i].length >> 0; - p_tx_data[tx_length++] = param_info.xel[i].length >> 8; - for (j=0; jid; + tx_param[param_len++] = p_xel->addr >> 0; + tx_param[param_len++] = p_xel->addr >> 8; + tx_param[param_len++] = p_xel->length >> 0; + tx_param[param_len++] = p_xel->length >> 8; + }else if(protocol_ver_idx_ == 1){ + tx_param[param_len++] = p_xel->id; + tx_param[param_len++] = p_xel->addr & 0xFF; + tx_param[param_len++] = p_xel->length & 0xFF; + } + err = add_param_to_dxl_packet(&info_tx_packet_, tx_param, param_len); + if(err != DXL_LIB_OK){ + break; + } + } + if(err == DXL_LIB_OK){ + err = end_make_dxl_packet(&info_tx_packet_); + } + } + + if(err == DXL_LIB_OK){ + p_port_->write(p_packet_buf_, info_tx_packet_.generated_packet_length); + + for(i=0; ilength, timeout_ms) != nullptr){ + if(info_rx_packet_.id == param_info.xel[i].id){ + recv_info.xel[recv_info.id_count].id = info_rx_packet_.id; + recv_info.xel[recv_info.id_count].error = info_rx_packet_.err_idx; + recv_info.xel[recv_info.id_count].length = info_rx_packet_.recv_param_len; + recv_info.id_count++; + }else{ + break; + } + }else{ + break; + } + } + if(i == param_info.id_count){ + ret = true; } } + last_lib_err_ = err; - pre_time_us = micros(); - last_lib_err_code_ = dxlTxPacketInst(&packet_, DXL_BROADCAST_ID, INST_BULK_WRITE, p_tx_data, tx_length); - if(last_lib_err_code_ != DXL_LIB_OK) - return false; - packet_.tx_time = micros() - pre_time_us; - ret = true; + + if(err == DXL_LIB_OK){ + p_port_->write(p_packet_buf_, info_tx_packet_.generated_packet_length); + + for(i=0; i 0){ + ret = true; + } + } return ret; } -uint8_t Master::getLastStatusPacketError() const -{ - return last_status_packet_error_; -} -lib_err_code_t Master::getLastLibErrCode() const +bool Master::bulkWrite(const ParamForBulkWriteInst_t ¶m_info) { - return last_lib_err_code_; -} + bool ret = false; + DXLLibErrorCode_t err = DXL_LIB_OK; + uint8_t i, tx_param[5], param_len = 0; + const XelInfoForBulkWriteParam_t* p_xel; + + // Parameter exception handling + if(p_port_ == nullptr){ + err = DXL_LIB_ERROR_NULLPTR; + }else if(p_port_->getOpenState() != true){ + err = DXL_LIB_ERROR_PORT_NOT_OPEN; + }else if(protocol_ver_idx_ != 2){ + err = DXL_LIB_ERROR_NOT_SUPPORTED; + } + if(err != DXL_LIB_OK){ + last_lib_err_ = err; + return 0; + } + + err = begin_make_dxl_packet(&info_tx_packet_, DXL_BROADCAST_ID, protocol_ver_idx_, + DXL_INST_BULK_WRITE, 0, p_packet_buf_, packet_buf_capacity_); + if(err == DXL_LIB_OK){ + for(i=0; iid; + tx_param[param_len++] = p_xel->addr >> 0; + tx_param[param_len++] = p_xel->addr >> 8; + tx_param[param_len++] = p_xel->length >> 0; + tx_param[param_len++] = p_xel->length >> 8; + err = add_param_to_dxl_packet(&info_tx_packet_, tx_param, param_len); + if(err == DXL_LIB_OK){ + err = add_param_to_dxl_packet(&info_tx_packet_, (uint8_t*)p_xel->data, p_xel->length); + } + if(err != DXL_LIB_OK){ + break; + } + } + if(err == DXL_LIB_OK){ + err = end_make_dxl_packet(&info_tx_packet_); + } + } + if(err == DXL_LIB_OK){ + p_port_->write(p_packet_buf_, info_tx_packet_.generated_packet_length); + ret = true; + } + last_lib_err_ = err; + + return ret; +} +// << Legacy (Deprecated since v0.4.0) \ No newline at end of file diff --git a/src/utility/master.h b/src/utility/master.h index 3b0d9a5..b44c22e 100644 --- a/src/utility/master.h +++ b/src/utility/master.h @@ -14,36 +14,20 @@ * limitations under the License. *******************************************************************************/ -#ifndef DYNAMIXEL_MASTER_H_ -#define DYNAMIXEL_MASTER_H_ +#ifndef DYNAMIXEL_MASTER_HPP_ +#define DYNAMIXEL_MASTER_HPP_ -#include "packet_handler.h" -#include "protocol.h" +#include "dxl_c/protocol.h" +#include "port_handler.h" +#include "config.h" -typedef struct InfoFromPing { - uint8_t id; - uint8_t firmware_version; - uint16_t model_number; -} XelInfoFromPing_t; - -typedef struct XelsInfoFromPing { - uint8_t id_count; - XelInfoFromPing_t xel[DXL_MAX_NODE]; -} RecvInfoFromPing_t; - -typedef struct XelInfoForStatusInst{ - uint8_t id; - uint16_t length; - uint8_t error; - uint8_t data[DXL_MAX_NODE_BUFFER_SIZE]; -} XelInfoForStatusInst_t; -typedef struct RecvInfoFromStatusInst{ - uint8_t id_count; - XelInfoForStatusInst_t xel[DXL_MAX_NODE]; -} RecvInfoFromStatusInst_t; +#define UNREGISTERED_MODEL (uint16_t)0xFFFF +#define COMMON_MODEL_NUMBER_ADDR 0 +#define COMMON_MODEL_NUMBER_ADDR_LENGTH 2 +// >> Legacy (Deprecated since v0.4.0) typedef struct InfoForSyncReadParam{ uint8_t id; } InfoForSyncReadParam_t; @@ -52,7 +36,7 @@ typedef struct ParamForSyncReadInst{ uint16_t addr; uint16_t length; uint8_t id_count; - InfoForSyncReadParam_t xel[DXL_MAX_NODE]; + InfoForSyncReadParam_t xel[DXL_MAX_NODE]; //refer to below. } ParamForSyncReadInst_t; typedef struct XelInfoForSyncWriteParam{ @@ -64,7 +48,7 @@ typedef struct ParamForSyncWriteInst{ uint16_t addr; uint16_t length; uint8_t id_count; - XelInfoForSyncWriteParam_t xel[DXL_MAX_NODE]; + XelInfoForSyncWriteParam_t xel[DXL_MAX_NODE]; //refer to below. } ParamForSyncWriteInst_t; typedef struct XelInfoForBulkReadParam{ @@ -75,7 +59,7 @@ typedef struct XelInfoForBulkReadParam{ typedef struct ParamForBulkReadInst{ uint8_t id_count; - XelInfoForBulkReadParam_t xel[DXL_MAX_NODE]; + XelInfoForBulkReadParam_t xel[DXL_MAX_NODE]; //refer to below. } ParamForBulkReadInst_t; typedef struct XelInfoForBulkWriteParam{ @@ -87,28 +71,101 @@ typedef struct XelInfoForBulkWriteParam{ typedef struct ParamForBulkWriteInst{ uint8_t id_count; - XelInfoForBulkWriteParam_t xel[DXL_MAX_NODE]; + XelInfoForBulkWriteParam_t xel[DXL_MAX_NODE]; //refer to below. } ParamForBulkWriteInst_t; -typedef union -{ - ParamForSyncReadInst_t sync_read; - ParamForSyncWriteInst_t sync_write; - ParamForBulkReadInst_t bulk_read; - ParamForBulkWriteInst_t bulk_write; -} send_param_t; +typedef struct XelInfoForStatusInst{ + uint8_t id; + uint16_t length; + uint8_t error; + uint8_t data[DXL_MAX_NODE_BUFFER_SIZE]; +} XelInfoForStatusInst_t; + +typedef struct RecvInfoFromStatusInst{ + uint8_t id_count; + XelInfoForStatusInst_t xel[DXL_MAX_NODE]; //refer to below. +} RecvInfoFromStatusInst_t; +// << Legacy (Deprecated since v0.4.0) -typedef union -{ - RecvInfoFromPing_t ping; - RecvInfoFromStatusInst_t read; - RecvInfoFromStatusInst_t sync_read; - RecvInfoFromStatusInst_t bulk_read; -} recv_info_t; namespace DYNAMIXEL { +typedef struct InfoFromPing{ + uint8_t id; + uint16_t model_number; + uint8_t firmware_version; +} InfoFromPing_t; + +typedef struct InfoSyncBulkBuffer{ + uint8_t* p_buf; + uint16_t buf_capacity; + uint16_t gen_length; + bool is_completed; +} __attribute__((packed)) InfoSyncBulkBuffer_t; + +/* Sync Instructions */ +typedef struct XELInfoSyncRead{ + uint8_t *p_recv_buf; + uint8_t id; + uint8_t error; +} __attribute__((packed)) XELInfoSyncRead_t; + +typedef struct InfoSyncReadInst{ + uint16_t addr; + uint16_t addr_length; + XELInfoSyncRead_t* p_xels; + uint8_t xel_count; + bool is_info_changed; + InfoSyncBulkBuffer_t packet; +} __attribute__((packed)) InfoSyncReadInst_t; + +typedef struct XELInfoSyncWrite{ + uint8_t* p_data; + uint8_t id; +} __attribute__((packed)) XELInfoSyncWrite_t; + +typedef struct InfoSyncWriteInst{ + uint16_t addr; + uint16_t addr_length; + XELInfoSyncWrite_t* p_xels; + uint8_t xel_count; + bool is_info_changed; + InfoSyncBulkBuffer_t packet; +} __attribute__((packed)) InfoSyncWriteInst_t; + +/* Bulk Instructions */ +typedef struct XELInfoBulkRead{ + uint16_t addr; + uint16_t addr_length; + uint8_t *p_recv_buf; + uint8_t id; + uint8_t error; +} __attribute__((packed)) XELInfoBulkRead_t; + +typedef struct InfoBulkReadInst{ + XELInfoBulkRead_t* p_xels; + uint8_t xel_count; + bool is_info_changed; + InfoSyncBulkBuffer_t packet; +} __attribute__((packed)) InfoBulkReadInst_t; + +typedef struct XELInfoBulkWrite{ + uint16_t addr; + uint16_t addr_length; + uint8_t* p_data; + uint8_t id; +} __attribute__((packed)) XELInfoBulkWrite_t; + +typedef struct InfoBulkWriteInst{ + XELInfoBulkWrite_t* p_xels; + uint8_t xel_count; + bool is_info_changed; + InfoSyncBulkBuffer_t packet; +} __attribute__((packed)) InfoBulkWriteInst_t; + + + class Master { public: @@ -120,11 +177,11 @@ class Master * DYNAMIXEL::SerialPortHandler dxl_port(Serial1, DXL_DIR_PIN); * DYNAMIXEL::Master dxl_master(dxl_port, PROTOCOL_VER); * @endcode - * @param port The PortHandler instance you want to use on the board to communicate with DYNAMIXELs. + * @param port The DXLPortHandler instance you want to use on the board to communicate with DYNAMIXELs. * It can be used not only for Serial but also for other communication port handlers like SerialPortHandler class. * @param protocol_ver DYNAMIXEL protocol version used for communications. (default : 2.0) */ - Master(PortHandler &port, float protocol_ver = DXL_PACKET_VER_2_0); + Master(DXLPortHandler &port, float protocol_ver = 2.0, uint16_t malloc_buf_size = 256); /** * @brief The constructor. @@ -135,54 +192,81 @@ class Master * @endcode * @param protocol_ver DYNAMIXEL protocol version used for communications. (default : 2.0) */ - Master(float protocol_ver = DXL_PACKET_VER_2_0); + Master(float protocol_ver = 2.0, uint16_t malloc_buf_size = 256); + + bool setPacketBuffer(uint8_t* p_buf, uint16_t buf_capacity); + uint8_t* getPacketBuffer() const; + uint16_t getPacketBufferCapacity() const; bool setPortProtocolVersion(float version); bool setPortProtocolVersionUsingIndex(uint8_t version_idx); - float getPortProtocolVersion(); + float getPortProtocolVersion() const; - bool setPort(PortHandler &port); - PortHandler* getPort() const; + bool setPort(DXLPortHandler &port); + bool setPort(DXLPortHandler *p_port); + DXLPortHandler* getPort() const; - uint8_t ping(uint8_t id, - XelInfoFromPing_t *recv_info_array, uint8_t recv_array_cnt, uint32_t timeout = 100); - bool ping(uint8_t id, - RecvInfoFromPing_t &recv_info, uint32_t timeout = 100); - + /* Instructions */ + uint8_t ping(uint8_t id, uint8_t *p_recv_id_array, uint8_t recv_array_capacity, + uint32_t timeout_ms = 10); + uint8_t ping(uint8_t id, InfoFromPing_t *recv_ping_info_array, uint8_t recv_array_cnt, + uint32_t timeout_ms = 10); int32_t read(uint8_t id, uint16_t addr, uint16_t addr_length, - uint8_t *p_recv_buf, uint16_t recv_buf_length, uint32_t timeout = 100); - + uint8_t *p_recv_buf, uint16_t recv_buf_capacity, uint32_t timeout_ms = 10); bool write(uint8_t id, uint16_t addr, - const uint8_t *p_data, uint16_t data_length, uint32_t timeout = 100); - + const uint8_t *p_data, uint16_t data_length, uint32_t timeout_ms = 10); bool writeNoResp(uint8_t id, uint16_t addr, const uint8_t *p_data, uint16_t data_length); + bool regWrite(uint8_t id, uint16_t addr, + const uint8_t *p_data, uint16_t data_length, uint32_t timeout_ms = 10); + bool action(uint8_t id, uint32_t timeout_ms = 10); + bool factoryReset(uint8_t id, uint8_t option, uint32_t timeout_ms = 10); + bool reboot(uint8_t id, uint32_t timeout_ms = 10); + bool clear(uint8_t id, uint8_t option, uint32_t ex_option, uint32_t timeout_ms = 10); + uint8_t syncRead(InfoSyncReadInst_t* p_info, uint32_t timeout_ms = 10); + bool syncWrite(InfoSyncWriteInst_t* p_info); + uint8_t bulkRead(InfoBulkReadInst_t* p_info, uint32_t timeout_ms = 10); + bool bulkWrite(InfoBulkWriteInst_t* p_info); - //TODO: bool regWrite(); - //TODO: bool action(); - bool factoryReset(uint8_t id, uint8_t option, uint32_t timeout = 100); - bool reboot(uint8_t id, uint32_t timeout); + uint8_t getLastStatusPacketError() const; + + void setLastLibErrCode(DXLLibErrorCode_t err_code); + DXLLibErrorCode_t getLastLibErrCode() const; - //TODO: bool clear(); + // raw APIs + bool txInstPacket(uint8_t id, uint8_t inst_idx, uint8_t *p_param, uint16_t param_len); + const InfoToParseDXLPacket_t* rxStatusPacket(uint8_t* p_param_buf, uint16_t param_buf_cap, uint32_t timeout_ms = 10); - bool syncRead(const ParamForSyncReadInst_t ¶m_info, RecvInfoFromStatusInst_t &recv_info, uint32_t timeout = 100); + // >> Legacy (Deprecated since v0.4.0) + bool syncRead(const ParamForSyncReadInst_t ¶m_info, RecvInfoFromStatusInst_t &recv_info, uint32_t timeout_ms = 100); bool syncWrite(const ParamForSyncWriteInst_t ¶m_info); - - bool bulkRead(const ParamForBulkReadInst_t ¶m_info, RecvInfoFromStatusInst_t &recv_info, uint32_t timeout = 100); + bool bulkRead(const ParamForBulkReadInst_t ¶m_info, RecvInfoFromStatusInst_t &recv_info, uint32_t timeout_ms = 100); bool bulkWrite(const ParamForBulkWriteInst_t ¶m_info); + // << Legacy (Deprecated since v0.4.0) - uint8_t getLastStatusPacketError() const; - lib_err_code_t getLastLibErrCode() const; + private: + DXLPortHandler *p_port_; - void setLastLibErrCode(lib_err_code_t err_code); + uint8_t protocol_ver_idx_; - private: - PortHandler *p_port_; - dxl_t packet_; - uint8_t last_status_packet_error_; - lib_err_code_t last_lib_err_code_; - }; -} + bool is_buf_malloced_; + uint8_t *p_packet_buf_; + uint16_t packet_buf_capacity_; + InfoToMakeDXLPacket_t info_tx_packet_; + InfoToParseDXLPacket_t info_rx_packet_; + + DXLLibErrorCode_t last_lib_err_; +}; + + +} //namespace DYNAMIXEL + + + + + +//legacy +typedef DYNAMIXEL::InfoFromPing_t XelInfoFromPing_t; -#endif /* DYNAMIXEL_MASTER_H_ */ \ No newline at end of file +#endif /* DYNAMIXEL_MASTER_HPP_ */ \ No newline at end of file diff --git a/src/utility/packet_handler.cpp b/src/utility/packet_handler.cpp deleted file mode 100644 index 8016892..0000000 --- a/src/utility/packet_handler.cpp +++ /dev/null @@ -1,5 +0,0 @@ - -#include "packet_handler.h" - - -using namespace DYNAMIXEL; diff --git a/src/utility/packet_handler.h b/src/utility/packet_handler.h deleted file mode 100644 index c84e95d..0000000 --- a/src/utility/packet_handler.h +++ /dev/null @@ -1,35 +0,0 @@ -/******************************************************************************* -* 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. -*******************************************************************************/ - -#ifndef DYNAMIXEL_PACKET_HANDLER_H_ -#define DYNAMIXEL_PACKET_HANDLER_H_ - -#include "port_handler.h" -#include "protocol.h" - -namespace DYNAMIXEL{ - -class PacketHandler -{ - public: - - private: - -}; - -} //namespace DYNAMIXEL - -#endif /* DYNAMIXEL_PACKET_HANDLER_H_ */ \ No newline at end of file diff --git a/src/utility/port_handler.cpp b/src/utility/port_handler.cpp index 81dd5ef..b976dab 100644 --- a/src/utility/port_handler.cpp +++ b/src/utility/port_handler.cpp @@ -1,29 +1,27 @@ - - - #include "port_handler.h" -using namespace DYNAMIXEL; -PortHandler::PortHandler() +DXLPortHandler::DXLPortHandler() : open_state_(false) {} -/* PortHandler */ -bool PortHandler::getOpenState() +/* DXLPortHandler */ +bool DXLPortHandler::getOpenState() { return open_state_; } -void PortHandler::setOpenState(bool state) +void DXLPortHandler::setOpenState(bool state) { open_state_ = state; } +using namespace DYNAMIXEL; + /* SerialPortHandler */ SerialPortHandler::SerialPortHandler(HardwareSerial& port, const int dir_pin) - : PortHandler(), port_(port), dir_pin_(dir_pin), baud_(57600) + : DXLPortHandler(), port_(port), dir_pin_(dir_pin), baud_(57600) {} void SerialPortHandler::begin() @@ -33,6 +31,18 @@ void SerialPortHandler::begin() void SerialPortHandler::begin(unsigned long baud) { +#if defined(ARDUINO_OpenCM904) + if(port_ == Serial1 && getOpenState() == false){ + Serial1.setDxlMode(true); + } +#elif defined(ARDUINO_OpenCR) + if(port_ == Serial3 && getOpenState() == false){ + pinMode(BDPIN_DXL_PWR_EN, OUTPUT); + digitalWrite(BDPIN_DXL_PWR_EN, HIGH); + } + delay(100); // Wait for the DYNAMIXEL to power up normally. +#endif + baud_ = baud; port_.begin(baud_); @@ -47,6 +57,11 @@ void SerialPortHandler::begin(unsigned long baud) void SerialPortHandler::end(void) { +#if defined(ARDUINO_OpenCR) + if(port_ == Serial3 && getOpenState() == true){ + digitalWrite(BDPIN_DXL_PWR_EN, LOW); + } +#endif port_.end(); setOpenState(false); } @@ -107,7 +122,7 @@ unsigned long SerialPortHandler::getBaud() const /* USBSerialPortHandler */ USBSerialPortHandler::USBSerialPortHandler(USB_SERIAL_CLASS& port) - : PortHandler(), port_(port) + : DXLPortHandler(), port_(port) {} void USBSerialPortHandler::begin() @@ -149,3 +164,4 @@ size_t USBSerialPortHandler::write(uint8_t *buf, size_t len) return ret; } + diff --git a/src/utility/port_handler.h b/src/utility/port_handler.h index c26472f..8e6b758 100644 --- a/src/utility/port_handler.h +++ b/src/utility/port_handler.h @@ -14,18 +14,17 @@ * limitations under the License. *******************************************************************************/ -#ifndef DYNAMIXEL_PORT_HANDLER_H_ -#define DYNAMIXEL_PORT_HANDLER_H_ +#ifndef DYNAMIXEL_PORT_HANDLER_HPP_ +#define DYNAMIXEL_PORT_HANDLER_HPP_ #include -namespace DYNAMIXEL{ -class PortHandler +class DXLPortHandler { public: - PortHandler(); + DXLPortHandler(); virtual void begin() = 0; virtual void end() = 0; @@ -40,7 +39,10 @@ class PortHandler bool open_state_; }; -class SerialPortHandler : public PortHandler + +namespace DYNAMIXEL{ + +class SerialPortHandler : public DXLPortHandler { public: SerialPortHandler(HardwareSerial& port, const int dir_pin = -1); @@ -52,8 +54,8 @@ class SerialPortHandler : public PortHandler virtual size_t write(uint8_t) override; virtual size_t write(uint8_t *buf, size_t len) override; - void begin(unsigned long baud); - unsigned long getBaud() const; + virtual void begin(unsigned long baud); + virtual unsigned long getBaud() const; private: HardwareSerial& port_; @@ -77,7 +79,7 @@ class SerialPortHandler : public PortHandler #define USB_SERIAL_CLASS HardwareSerial #endif -class USBSerialPortHandler : public PortHandler +class USBSerialPortHandler : public DXLPortHandler { public: USBSerialPortHandler(USB_SERIAL_CLASS& port); @@ -93,6 +95,6 @@ class USBSerialPortHandler : public PortHandler USB_SERIAL_CLASS& port_; }; -} //namespace DYNAMIXEL +}//namespace DYNAMIXEL -#endif /* DYNAMIXEL_PORT_HANDLER_H_ */ \ No newline at end of file +#endif /* DYNAMIXEL_PORT_HANDLER_HPP_ */ \ No newline at end of file diff --git a/src/utility/protocol.cpp b/src/utility/protocol.cpp deleted file mode 100644 index 1962607..0000000 --- a/src/utility/protocol.cpp +++ /dev/null @@ -1,645 +0,0 @@ - - - -#include "protocol.h" - -using namespace DYNAMIXEL; - -#define PACKET_2_0_STATE_IDLE 0 -#define PACKET_2_0_STATE_RESERVED 1 -#define PACKET_2_0_STATE_ID 2 -#define PACKET_2_0_STATE_LENGTH_L 3 -#define PACKET_2_0_STATE_LENGTH_H 4 -#define PACKET_2_0_STATE_DATA 5 -#define PACKET_2_0_STATE_CRC_L 6 -#define PACKET_2_0_STATE_CRC_H 7 - - -#define PACKET_1_0_STATE_IDLE 0 -#define PACKET_1_0_STATE_ID 1 -#define PACKET_1_0_STATE_LENGTH 2 -#define PACKET_1_0_STATE_DATA 3 -#define PACKET_1_0_STATE_CHECK_SUM 4 - - -#define DXL_DIR_PIN_RX 0 -#define DXL_DIR_PIN_TX 1 - -//-- Internal Variables -// - -//-- External Variables -// - - -//-- Internal Functions -// -static lib_err_code_t dxlRxPacket1_0(dxl_t *p_packet, uint8_t data_in); -static lib_err_code_t dxlTxPacketInst1_0(dxl_t *p_packet, uint8_t id, uint8_t inst_cmd, uint8_t *p_data, uint16_t length ); - -static lib_err_code_t dxlRxPacket2_0(dxl_t *p_packet, uint8_t data_in); -static lib_err_code_t dxlTxPacketInst2_0(dxl_t *p_packet, uint8_t id, uint8_t inst_cmd, uint8_t *p_data, uint16_t length ); - - -//-- External Functions -// - - -bool DYNAMIXEL::setDxlPort(dxl_t *p_packet, PortHandler *port) -{ - if(port == nullptr) - return false; - - p_packet->p_port = port; - return true; -} - -bool DYNAMIXEL::dxlInit(dxl_t *p_packet, float protocol_ver, uint8_t mode) -{ - p_packet->header_cnt = 0; - p_packet->packet_ver = protocol_ver; - p_packet->dxl_mode = mode; - - p_packet->rx_state = PACKET_2_0_STATE_IDLE; - - p_packet->id = 200; - - p_packet->rx_timeout = (uint32_t)500*(uint32_t)1000; - p_packet->tx_timeout = (uint32_t)500*(uint32_t)1000; - - memset(&p_packet->rx, 0, sizeof(p_packet->rx)); - memset(&p_packet->rx, 0, sizeof(p_packet->tx)); - - return true; -} - -bool DYNAMIXEL::dxlSetId(dxl_t *p_packet, uint8_t id) -{ - p_packet->id = id; - - return true; -} - -uint8_t DYNAMIXEL::dxlGetId(dxl_t *p_packet) -{ - return p_packet->id; -} - -bool DYNAMIXEL::dxlSetProtocolVersion(dxl_t *p_packet, float protocol_version) -{ - bool ret = true; - - if(protocol_version == DXL_PACKET_VER_1_0){ - p_packet->packet_ver = protocol_version; - }else if(protocol_version == DXL_PACKET_VER_2_0){ - p_packet->packet_ver = protocol_version; - }else{ - ret = false; - } - - return ret; -} - -float DYNAMIXEL::dxlGetProtocolVersion(dxl_t *p_packet) -{ - return p_packet->packet_ver; -} - -uint32_t DYNAMIXEL::dxlRxAvailable(dxl_t *p_packet) -{ - return (uint32_t)p_packet->p_port->available(); -} - -uint8_t DYNAMIXEL::dxlRxRead(dxl_t *p_packet) -{ - return (uint8_t)p_packet->p_port->read(); -} - -void DYNAMIXEL::dxlTxWrite(dxl_t *p_packet, uint8_t *p_data, uint32_t length) -{ - p_packet->p_port->write(p_data, length); -} - -lib_err_code_t DYNAMIXEL::dxlRxPacket(dxl_t *p_packet) -{ - uint8_t data; - lib_err_code_t ret = DXL_LIB_PROCEEDING; - - while (dxlRxAvailable(p_packet) > 0) - { - data = dxlRxRead(p_packet); - ret = dxlRxPacketDataIn(p_packet, data); - - if (ret != DXL_LIB_PROCEEDING){ - break; - } - } - - return ret; -} - -lib_err_code_t DYNAMIXEL::dxlRxPacketDataIn(dxl_t *p_packet, uint8_t data_in) -{ - lib_err_code_t ret = DXL_LIB_PROCEEDING; - - if (p_packet->packet_ver == DXL_PACKET_VER_1_0){ - ret = dxlRxPacket1_0(p_packet, data_in); - }else{ - ret = dxlRxPacket2_0(p_packet, data_in); - } - - return ret; -} - -lib_err_code_t dxlRxPacket1_0(dxl_t *p_packet, uint8_t data_in) -{ - lib_err_code_t ret = DXL_LIB_PROCEEDING; - - // time out - if((micros() - p_packet->prev_time) > p_packet->rx_timeout){ - p_packet->rx_state = PACKET_1_0_STATE_IDLE; - p_packet->prev_time = micros(); - p_packet->header_cnt = 0; - } - p_packet->prev_time = micros(); - - switch(p_packet->rx_state) - { - case PACKET_1_0_STATE_IDLE: - - if( p_packet->header_cnt >= 1 ) - { - p_packet->rx.header[1] = data_in; - - if( p_packet->rx.header[0] == 0xFF - && p_packet->rx.header[1] == 0xFF ) - { - p_packet->header_cnt = 0; - p_packet->rx.check_sum = 0; - p_packet->rx_state = PACKET_1_0_STATE_ID; - } - else - { - p_packet->rx.header[0] = p_packet->rx.header[1]; - p_packet->rx.header[1] = 0; - } - } - else - { - p_packet->rx.header[p_packet->header_cnt] = data_in; - p_packet->header_cnt++; - } - break; - - case PACKET_1_0_STATE_ID: - p_packet->rx.id = data_in; - p_packet->rx_state = PACKET_1_0_STATE_LENGTH; - p_packet->rx.check_sum += data_in; - break; - - case PACKET_1_0_STATE_LENGTH: - p_packet->rx.packet_length = data_in; - p_packet->rx_state = PACKET_1_0_STATE_DATA; - p_packet->rx.index = 0; - p_packet->rx.check_sum += data_in; - - if (p_packet->rx.packet_length > DXL_BUF_LENGTH-4) - { - p_packet->rx_state = PACKET_1_0_STATE_IDLE; - ret = DXL_LIB_ERROR_BUFFER_OVERFLOW; - } - if (p_packet->rx.packet_length < 2) - { - p_packet->rx_state = PACKET_1_0_STATE_IDLE; - ret = DXL_LIB_ERROR_LENGTH; - } - break; - - case PACKET_1_0_STATE_DATA: - p_packet->rx.data[p_packet->rx.index] = data_in; - p_packet->rx.check_sum += data_in; - - p_packet->rx.index++; - - if (p_packet->rx.index >= p_packet->rx.packet_length-1) - { - p_packet->rx_state = PACKET_1_0_STATE_CHECK_SUM; - } - break; - - case PACKET_1_0_STATE_CHECK_SUM: - p_packet->rx.check_sum = ~(p_packet->rx.check_sum); - p_packet->rx.check_sum_received = data_in; - - if (p_packet->rx.check_sum_received == p_packet->rx.check_sum) - { - p_packet->rx.cmd = p_packet->rx.data[0]; - p_packet->rx.error = p_packet->rx.data[0]; - - if (p_packet->dxl_mode == DXL_MODE_MASTER) - { - p_packet->rx.p_param = &p_packet->rx.data[1]; - p_packet->rx.param_length = p_packet->rx.packet_length - 2; - p_packet->rx.type = RX_PACKET_TYPE_STATUS; - } - else - { - p_packet->rx.p_param = &p_packet->rx.data[1]; - p_packet->rx.param_length = p_packet->rx.packet_length - 2; - p_packet->rx.type = RX_PACKET_TYPE_INST; - } - - ret = DXL_LIB_OK; - } - else - { - ret = DXL_LIB_ERROR_CHECK_SUM; - } - - p_packet->rx_state = PACKET_1_0_STATE_IDLE; - break; - - default: - p_packet->rx_state = PACKET_1_0_STATE_IDLE; - break; - } - - return ret; -} - -lib_err_code_t dxlRxPacket2_0(dxl_t *p_packet, uint8_t data_in) -{ - lib_err_code_t ret = DXL_LIB_PROCEEDING; - uint16_t stuff_length; - - // time out - if( (micros() - p_packet->prev_time) > p_packet->rx_timeout ) - { - p_packet->rx_state = PACKET_2_0_STATE_IDLE; - p_packet->header_cnt = 0; - } - p_packet->prev_time = micros(); - - - switch(p_packet->rx_state) - { - case PACKET_2_0_STATE_IDLE: - if( p_packet->header_cnt >= 2 ) - { - p_packet->rx.header[2] = data_in; - - if( p_packet->rx.header[0] == 0xFF - && p_packet->rx.header[1] == 0xFF - && p_packet->rx.header[2] == 0xFD ) - { - p_packet->header_cnt = 0; - p_packet->rx.crc = 0; - dxlUpdateCrc(&p_packet->rx.crc, 0xFF); - dxlUpdateCrc(&p_packet->rx.crc, 0xFF); - dxlUpdateCrc(&p_packet->rx.crc, 0xFD); - p_packet->rx_state = PACKET_2_0_STATE_RESERVED; - } - else - { - p_packet->rx.header[0] = p_packet->rx.header[1]; - p_packet->rx.header[1] = p_packet->rx.header[2]; - p_packet->rx.header[2] = 0; - } - } - else - { - p_packet->rx.header[p_packet->header_cnt] = data_in; - p_packet->header_cnt++; - } - break; - - case PACKET_2_0_STATE_RESERVED: - if( data_in == 0xFD ) - { - p_packet->rx_state = PACKET_2_0_STATE_IDLE; - ret = DXL_LIB_ERROR_WRONG_PACKET; - } - else - { - p_packet->rx.reserved = data_in; - p_packet->rx_state = PACKET_2_0_STATE_ID; - } - dxlUpdateCrc(&p_packet->rx.crc, data_in); - break; - - case PACKET_2_0_STATE_ID: - p_packet->rx.id = data_in; - p_packet->rx_state = PACKET_2_0_STATE_LENGTH_L; - dxlUpdateCrc(&p_packet->rx.crc, data_in); - break; - - case PACKET_2_0_STATE_LENGTH_L: - p_packet->rx.packet_length = data_in; - p_packet->rx_state = PACKET_2_0_STATE_LENGTH_H; - dxlUpdateCrc(&p_packet->rx.crc, data_in); - break; - - case PACKET_2_0_STATE_LENGTH_H: - p_packet->rx.packet_length |= data_in<<8; - p_packet->rx_state = PACKET_2_0_STATE_DATA; - p_packet->rx.index = 0; - dxlUpdateCrc(&p_packet->rx.crc, data_in); - - if (p_packet->rx.packet_length > DXL_BUF_LENGTH) - { - p_packet->rx_state = PACKET_2_0_STATE_IDLE; - ret = DXL_LIB_ERROR_BUFFER_OVERFLOW; - } - if (p_packet->rx.packet_length < 3) - { - p_packet->rx_state = PACKET_2_0_STATE_IDLE; - ret = DXL_LIB_ERROR_LENGTH; - } - break; - - case PACKET_2_0_STATE_DATA: - p_packet->rx.data[p_packet->rx.index] = data_in; - dxlUpdateCrc(&p_packet->rx.crc, data_in); - - p_packet->rx.index++; - - if (p_packet->rx.index >= p_packet->rx.packet_length-2) - { - p_packet->rx_state = PACKET_2_0_STATE_CRC_L; - } - break; - - case PACKET_2_0_STATE_CRC_L: - p_packet->rx.crc_received = data_in; - p_packet->rx_state = PACKET_2_0_STATE_CRC_H; - break; - - case PACKET_2_0_STATE_CRC_H: - p_packet->rx.crc_received |= data_in<<8; - - stuff_length = dxlRemoveStuffing(p_packet->rx.data, p_packet->rx.packet_length); - p_packet->rx.packet_length -= stuff_length; - - if (p_packet->rx.crc_received == p_packet->rx.crc) - { - p_packet->rx.cmd = p_packet->rx.data[0]; - p_packet->rx.error = p_packet->rx.data[1]; - - if (p_packet->rx.data[0] == INST_STATUS) - { - p_packet->rx.p_param = &p_packet->rx.data[2]; - p_packet->rx.param_length = p_packet->rx.packet_length - 4; - p_packet->rx.type = RX_PACKET_TYPE_STATUS; - } - else - { - p_packet->rx.p_param = &p_packet->rx.data[1]; - p_packet->rx.param_length = p_packet->rx.packet_length - 3; - p_packet->rx.type = RX_PACKET_TYPE_INST; - } - ret = DXL_LIB_OK; - } - else - { - ret = DXL_LIB_ERROR_CRC; - } - - p_packet->rx_state = PACKET_2_0_STATE_IDLE; - break; - - default: - p_packet->rx_state = PACKET_2_0_STATE_IDLE; - break; - } - - return ret; -} - -uint16_t DYNAMIXEL::dxlRemoveStuffing(uint8_t *p_data, uint16_t length) -{ - uint16_t i; - uint16_t stuff_length; - uint16_t index; - - index = 0; - stuff_length = 0; - for( i=0; i= 2 ) - { - if( p_data[i-2] == 0xFF && p_data[i-1] == 0xFF && p_data[i] == 0xFD ) - { - i++; - stuff_length++; - } - } - p_data[index++] = p_data[i]; - } - - return stuff_length; -} - -uint16_t DYNAMIXEL::dxlAddStuffing(dxl_t *p_packet, uint8_t *p_data, uint16_t length) -{ - uint8_t *stuff_buf; - uint16_t i; - uint16_t index; - uint16_t stuff_length = 0; - - stuff_buf = p_packet->rx.data; - - if (length <= 2) - { - return 0; - } - - index = 0; - stuff_length = 0; - for( i=0; i= 2 ) - { - if( p_data[i-2] == 0xFF && p_data[i-1] == 0xFF && p_data[i] == 0xFD ) - { - stuff_buf[index++] = 0xFD; - stuff_length++; - } - } - } - - if( stuff_length > 0 ) - { - for( i=0; itx.data, p_packet->tx.packet_length); - - return ret; -} - -lib_err_code_t DYNAMIXEL::dxlTxPacketInst(dxl_t *p_packet, uint8_t id, uint8_t inst_cmd, uint8_t *p_data, uint16_t length ) -{ - lib_err_code_t ret = DXL_LIB_PROCEEDING; - - if (p_packet->packet_ver == DXL_PACKET_VER_1_0) - { - ret = dxlTxPacketInst1_0(p_packet, id, inst_cmd, p_data, length); - } - else - { - ret = dxlTxPacketInst2_0(p_packet, id, inst_cmd, p_data, length); - } - - return ret; -} - -lib_err_code_t dxlTxPacketInst1_0(dxl_t *p_packet, uint8_t id, uint8_t inst_cmd, uint8_t *p_data, uint16_t length ) -{ - lib_err_code_t ret = DXL_LIB_OK; - uint16_t i = 0; - uint16_t packet_length; - uint8_t check_sum; - - - if(length > DXL_BUF_LENGTH){ - return DXL_LIB_ERROR_BUFFER_OVERFLOW; - } - if(length > 0xFF){ - return DXL_LIB_ERROR_LENGTH; - } - - check_sum = 0; - packet_length = length + 2; // param_length + Instruction + CheckSum - - p_packet->tx.data[PKT_1_0_HDR_1_IDX] = 0xFF; - p_packet->tx.data[PKT_1_0_HDR_2_IDX] = 0xFF; - p_packet->tx.data[PKT_1_0_ID_IDX] = id; - p_packet->tx.data[PKT_1_0_INST_IDX] = inst_cmd; - - check_sum += id; - check_sum += packet_length; - check_sum += inst_cmd; - - for (i=0; itx.data[PKT_1_0_INST_PARAM_IDX + i] = p_data[i]; - check_sum += p_data[i]; - } - - p_packet->tx.data[PKT_1_0_LEN_IDX] = packet_length; - p_packet->tx.data[PKT_1_0_INST_IDX + packet_length - 1] = ~(check_sum); - - dxlTxWrite(p_packet, p_packet->tx.data, packet_length + 4); - - return ret; -} - -lib_err_code_t dxlTxPacketInst2_0(dxl_t *p_packet, uint8_t id, uint8_t inst_cmd, uint8_t *p_data, uint16_t length ) -{ - lib_err_code_t ret = DXL_LIB_OK; - uint16_t i = 0; - uint16_t packet_length; - uint16_t stuff_length; - uint16_t crc; - - if(length > DXL_BUF_LENGTH){ - return DXL_LIB_ERROR_BUFFER_OVERFLOW; - } - - packet_length = length + 3; // param_length + Instruction + CRC_L + CRC_H - - p_packet->tx.data[PKT_HDR_1_IDX] = 0xFF; - p_packet->tx.data[PKT_HDR_2_IDX] = 0xFF; - p_packet->tx.data[PKT_HDR_3_IDX] = 0xFD; - p_packet->tx.data[PKT_RSV_IDX] = 0x00; - p_packet->tx.data[PKT_ID_IDX] = id; - p_packet->tx.data[PKT_INST_IDX] = inst_cmd; - - for (i=0; itx.data[PKT_INST_PARAM_IDX + i] = p_data[i]; - } - - stuff_length = dxlAddStuffing(p_packet, &p_packet->tx.data[PKT_INST_IDX], length + 1); // + instruction - packet_length += stuff_length; - - p_packet->tx.data[PKT_LEN_L_IDX] = packet_length >> 0; - p_packet->tx.data[PKT_LEN_H_IDX] = packet_length >> 8; - - crc = 0; - for (i=0; itx.data[i]); - } - - p_packet->tx.data[PKT_INST_IDX + packet_length - 2] = crc >> 0; - p_packet->tx.data[PKT_INST_IDX + packet_length - 1] = crc >> 8; - - dxlTxWrite(p_packet, p_packet->tx.data, packet_length + 7); - - return ret; -} - - - -const unsigned short crc_table[256] PROGMEM = {0x0000, - 0x8005, 0x800F, 0x000A, 0x801B, 0x001E, 0x0014, 0x8011, - 0x8033, 0x0036, 0x003C, 0x8039, 0x0028, 0x802D, 0x8027, - 0x0022, 0x8063, 0x0066, 0x006C, 0x8069, 0x0078, 0x807D, - 0x8077, 0x0072, 0x0050, 0x8055, 0x805F, 0x005A, 0x804B, - 0x004E, 0x0044, 0x8041, 0x80C3, 0x00C6, 0x00CC, 0x80C9, - 0x00D8, 0x80DD, 0x80D7, 0x00D2, 0x00F0, 0x80F5, 0x80FF, - 0x00FA, 0x80EB, 0x00EE, 0x00E4, 0x80E1, 0x00A0, 0x80A5, - 0x80AF, 0x00AA, 0x80BB, 0x00BE, 0x00B4, 0x80B1, 0x8093, - 0x0096, 0x009C, 0x8099, 0x0088, 0x808D, 0x8087, 0x0082, - 0x8183, 0x0186, 0x018C, 0x8189, 0x0198, 0x819D, 0x8197, - 0x0192, 0x01B0, 0x81B5, 0x81BF, 0x01BA, 0x81AB, 0x01AE, - 0x01A4, 0x81A1, 0x01E0, 0x81E5, 0x81EF, 0x01EA, 0x81FB, - 0x01FE, 0x01F4, 0x81F1, 0x81D3, 0x01D6, 0x01DC, 0x81D9, - 0x01C8, 0x81CD, 0x81C7, 0x01C2, 0x0140, 0x8145, 0x814F, - 0x014A, 0x815B, 0x015E, 0x0154, 0x8151, 0x8173, 0x0176, - 0x017C, 0x8179, 0x0168, 0x816D, 0x8167, 0x0162, 0x8123, - 0x0126, 0x012C, 0x8129, 0x0138, 0x813D, 0x8137, 0x0132, - 0x0110, 0x8115, 0x811F, 0x011A, 0x810B, 0x010E, 0x0104, - 0x8101, 0x8303, 0x0306, 0x030C, 0x8309, 0x0318, 0x831D, - 0x8317, 0x0312, 0x0330, 0x8335, 0x833F, 0x033A, 0x832B, - 0x032E, 0x0324, 0x8321, 0x0360, 0x8365, 0x836F, 0x036A, - 0x837B, 0x037E, 0x0374, 0x8371, 0x8353, 0x0356, 0x035C, - 0x8359, 0x0348, 0x834D, 0x8347, 0x0342, 0x03C0, 0x83C5, - 0x83CF, 0x03CA, 0x83DB, 0x03DE, 0x03D4, 0x83D1, 0x83F3, - 0x03F6, 0x03FC, 0x83F9, 0x03E8, 0x83ED, 0x83E7, 0x03E2, - 0x83A3, 0x03A6, 0x03AC, 0x83A9, 0x03B8, 0x83BD, 0x83B7, - 0x03B2, 0x0390, 0x8395, 0x839F, 0x039A, 0x838B, 0x038E, - 0x0384, 0x8381, 0x0280, 0x8285, 0x828F, 0x028A, 0x829B, - 0x029E, 0x0294, 0x8291, 0x82B3, 0x02B6, 0x02BC, 0x82B9, - 0x02A8, 0x82AD, 0x82A7, 0x02A2, 0x82E3, 0x02E6, 0x02EC, - 0x82E9, 0x02F8, 0x82FD, 0x82F7, 0x02F2, 0x02D0, 0x82D5, - 0x82DF, 0x02DA, 0x82CB, 0x02CE, 0x02C4, 0x82C1, 0x8243, - 0x0246, 0x024C, 0x8249, 0x0258, 0x825D, 0x8257, 0x0252, - 0x0270, 0x8275, 0x827F, 0x027A, 0x826B, 0x026E, 0x0264, - 0x8261, 0x0220, 0x8225, 0x822F, 0x022A, 0x823B, 0x023E, - 0x0234, 0x8231, 0x8213, 0x0216, 0x021C, 0x8219, 0x0208, - 0x820D, 0x8207, 0x0202 }; - -void DYNAMIXEL::dxlUpdateCrc(uint16_t *p_crc_cur, uint8_t data_in) -{ - uint16_t crc; - uint16_t i; - - crc = *p_crc_cur; - - i = ((unsigned short)(crc >> 8) ^ data_in) & 0xFF; - *p_crc_cur = (crc << 8) ^ pgm_read_word_near(crc_table + i); -} \ No newline at end of file diff --git a/src/utility/protocol.h b/src/utility/protocol.h deleted file mode 100644 index 60f608a..0000000 --- a/src/utility/protocol.h +++ /dev/null @@ -1,191 +0,0 @@ -/******************************************************************************* -* 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. -*******************************************************************************/ -#ifndef DYNAMIXEL_PROTOCOL_H_ -#define DYNAMIXEL_PROTOCOL_H_ - -#include -#include "port_handler.h" -#include "config.h" - -#define DXL_PACKET_VER_1_0 ((float)(1.0)) -#define DXL_PACKET_VER_2_0 ((float)(2.0)) - -#define DXL_STATE_WAIT_INST 0 -#define DXL_STATE_WAIT_STATUS 1 - -#define DXL_TYPE_INST 0 -#define DXL_TYPE_STATUS 1 - -#define DXL_BROADCAST_ID 0xFE -#define DXL_ALL_ID DXL_BROADCAST_ID -#define DXL_NONE_ID 0xFF - - -#define UNREGISTERED_MODEL (uint16_t)0xFFFF -#define COMMON_MODEL_NUMBER_ADDR 0 -#define COMMON_MODEL_NUMBER_ADDR_LENGTH 2 - - -//-- 2.0 Protocol -// -#define PKT_HDR_1_IDX 0 -#define PKT_HDR_2_IDX 1 -#define PKT_HDR_3_IDX 2 -#define PKT_RSV_IDX 3 -#define PKT_ID_IDX 4 -#define PKT_LEN_L_IDX 5 -#define PKT_LEN_H_IDX 6 -#define PKT_INST_IDX 7 -#define PKT_ERROR_IDX 8 - -#define PKT_INST_PARAM_IDX 8 -#define PKT_STATUS_PARAM_IDX 9 - - -//-- 1.0 Protocol -// -#define PKT_1_0_HDR_1_IDX 0 -#define PKT_1_0_HDR_2_IDX 1 -#define PKT_1_0_ID_IDX 2 -#define PKT_1_0_LEN_IDX 3 -#define PKT_1_0_INST_IDX 4 -#define PKT_1_0_ERROR_IDX 4 - -#define PKT_1_0_INST_PARAM_IDX 5 -#define PKT_1_0_STATUS_PARAM_IDX 5 - -#define DXL_ERR_NONE 0x00 -#define DXL_ERR_RESULT_FAIL 0x01 -#define DXL_ERR_INST_ERROR 0x02 -#define DXL_ERR_CRC_ERROR 0x03 -#define DXL_ERR_DATA_RANGE 0x04 -#define DXL_ERR_DATA_LENGTH 0x05 -#define DXL_ERR_DATA_LIMIT 0x06 -#define DXL_ERR_ACCESS 0x07 - -#define RX_PACKET_TYPE_STATUS 0 -#define RX_PACKET_TYPE_INST 1 - -namespace DYNAMIXEL{ - - enum DXLMode{ - DXL_MODE_MASTER = 0, - DXL_MODE_SLAVE - }; - - enum Instruction{ - INST_PING = 0x01, - INST_READ = 0x02, - INST_WRITE = 0x03, - INST_REG_WRITE = 0x04, - INST_ACTION = 0x05, - INST_RESET = 0x06, - INST_REBOOT = 0x08, - INST_STATUS = 0x55, - INST_SYNC_READ = 0x82, - INST_SYNC_WRITE = 0x83, - INST_BULK_READ = 0x92, - INST_BULK_WRITE = 0x93 - }; - - typedef enum LibErrorCode - { - DXL_LIB_OK = 0, - DXL_LIB_PROCEEDING, - - DXL_LIB_ERROR_NOT_SUPPORTED, - DXL_LIB_ERROR_TIMEOUT, - DXL_LIB_ERROR_INVAILD_ID, - DXL_LIB_ERROR_NOT_SUPPORT_BROADCAST, - DXL_LIB_ERROR_NULLPTR, - DXL_LIB_ERROR_LENGTH, - DXL_LIB_ERROR_INVAILD_ADDR, - DXL_LIB_ERROR_ADDR_LENGTH, - DXL_LIB_ERROR_BUFFER_OVERFLOW, - DXL_LIB_ERROR_PORT_NOT_OPEN, - DXL_LIB_ERROR_WRONG_PACKET, - DXL_LIB_ERROR_CHECK_SUM, - DXL_LIB_ERROR_CRC - } lib_err_code_t; - - typedef struct DxlPacket - { - uint8_t header[3]; - uint8_t reserved; - uint8_t id; - uint8_t cmd; - uint8_t error; - uint8_t type; - uint16_t index; - uint16_t packet_length; - uint16_t param_length; - uint16_t crc; - uint16_t crc_received; - uint8_t check_sum; - uint8_t check_sum_received; - uint8_t *p_param; - uint32_t dummy; - uint8_t data[DXL_BUF_LENGTH]; - } dxl_packet_t; - - typedef struct Dxl - { - float packet_ver; - uint8_t dxl_mode; - - uint8_t rx_pakcet_type; - uint8_t rx_state; - uint8_t id; - - uint32_t rx_timeout; - uint32_t tx_timeout; - uint32_t prev_time; - uint32_t tx_time; - uint32_t rx_time; - uint8_t header_cnt; - - dxl_packet_t rx; - dxl_packet_t tx; - - DYNAMIXEL::PortHandler* p_port; - } dxl_t; - - bool setDxlPort(dxl_t *p_packet, PortHandler *port); - bool dxlInit(dxl_t *p_packet, float protocol_ver, uint8_t mode = DXLMode::DXL_MODE_MASTER); - - bool dxlSetId(dxl_t *p_packet, uint8_t id); - uint8_t dxlGetId(dxl_t *p_packet); - - bool dxlSetProtocolVersion(dxl_t *p_packet, float protocol_version); - float dxlGetProtocolVersion(dxl_t *p_packet); - - uint32_t dxlRxAvailable(dxl_t *p_packet); - uint8_t dxlRxRead(dxl_t *p_packet); - void dxlTxWrite(dxl_t *p_packet, uint8_t *p_data, uint32_t length); - - lib_err_code_t dxlRxPacket(dxl_t *p_packet); - lib_err_code_t dxlRxPacketDataIn(dxl_t *p_packet, uint8_t data_in); - - lib_err_code_t dxlTxPacket(dxl_t *p_packet); - lib_err_code_t dxlTxPacketInst(dxl_t *p_packet, uint8_t id, uint8_t inst_cmd, uint8_t *p_data, uint16_t length); - - void dxlUpdateCrc(uint16_t *p_crc_cur, uint8_t data_in); - - uint16_t dxlAddStuffing(dxl_t *p_packet, uint8_t *p_data, uint16_t length); - uint16_t dxlRemoveStuffing(uint8_t *p_data, uint16_t length); -} - -#endif /* DYNAMIXEL_PROTOCOL_H_ */ \ No newline at end of file diff --git a/src/utility/slave.cpp b/src/utility/slave.cpp index 01007f2..55442cb 100644 --- a/src/utility/slave.cpp +++ b/src/utility/slave.cpp @@ -1,177 +1,181 @@ -#include "config.h" #include "slave.h" using namespace DYNAMIXEL; enum DefaultControlTableItemAddr{ - ADDR_MODEL_NUMBER = 0, - ADDR_FIRMWARE_VER = 6, - ADDR_ID = 7, - ADDR_PROTOCOL_VER = 9 + ADDR_ITEM_MODEL_NUMBER = 0, + ADDR_ITEM_FIRMWARE_VER = 6, + ADDR_ITEM_ID = 7, + ADDR_ITEM_PROTOCOL_VER = 9 }; static bool isAddrInRange(uint16_t addr, uint16_t length, uint16_t range_addr, uint16_t range_length); static bool isAddrInOtherItem(uint16_t start_addr, uint16_t length, uint16_t other_start_addr, uint16_t other_length); -static lib_err_code_t dxlMakePacketStatus1_0(dxl_t &packet, uint8_t error, uint8_t *p_data, uint16_t length ); -static lib_err_code_t dxlMakePacketStatus2_0(dxl_t &packet, uint8_t error, uint8_t *p_data, uint16_t length ); -static lib_err_code_t dxlTxPacketStatus(dxl_t &packet, uint8_t error, uint8_t *p_data, uint16_t length); -static lib_err_code_t dxlMakePacketStatus(dxl_t &packet, uint8_t error, uint8_t *p_data, uint16_t length ); - -Slave::Slave(PortHandler &port, const uint16_t model_num, float protocol_ver) -: model_num_(model_num), - firmware_ver_(1), id_(1), - user_write_callback_(nullptr), user_read_callback_(nullptr), - last_status_packet_error_(0), last_lib_err_code_(DXL_LIB_OK) +Slave::Slave(DXLPortHandler &port, const uint16_t model_num, float protocol_ver) +: model_num_(model_num), protocol_ver_idx_(2), firmware_ver_(1), id_(1), + is_buf_malloced_(false), packet_buf_capacity_(0), + last_lib_err_(DXL_LIB_OK), + registered_item_cnt_(0) { setPort(port); - protocol_ver = protocol_ver == 1.0 ? 1.0:2.0; - protocol_ver_idx_ = protocol_ver == 1.0 ? 1:2; - dxlInit(&packet_, protocol_ver, DXLMode::DXL_MODE_SLAVE); - this->setID(id_); - memset(&control_table_, 0, sizeof(control_table_)); - this->addDefaultControlItem(); + setPortProtocolVersion(protocol_ver); + addDefaultControlItem(); + + p_packet_buf_ = new uint8_t[DEFAULT_DXL_BUF_LENGTH]; + if(p_packet_buf_ != nullptr){ + packet_buf_capacity_ = DEFAULT_DXL_BUF_LENGTH; + is_buf_malloced_ = true; + } + info_tx_packet_.is_init = false; + info_rx_packet_.is_init = false; } Slave::Slave(const uint16_t model_num, float protocol_ver) -: model_num_(model_num), - firmware_ver_(1), id_(1), - user_write_callback_(nullptr), user_read_callback_(nullptr), - last_status_packet_error_(0), last_lib_err_code_(DXL_LIB_OK) -{ - protocol_ver = protocol_ver == 1.0 ? 1.0:2.0; - protocol_ver_idx_ = protocol_ver == 1.0 ? 1:2; - dxlInit(&packet_, protocol_ver, DXLMode::DXL_MODE_SLAVE); - this->setID(id_); - memset(&control_table_, 0, sizeof(control_table_)); - this->addDefaultControlItem(); +: model_num_(model_num), protocol_ver_idx_(2), firmware_ver_(1), id_(1), + is_buf_malloced_(false), packet_buf_capacity_(0), + last_lib_err_(DXL_LIB_OK), + registered_item_cnt_(0) +{ + setPortProtocolVersion(protocol_ver); + addDefaultControlItem(); + + p_packet_buf_ = new uint8_t[DEFAULT_DXL_BUF_LENGTH]; + if(p_packet_buf_ != nullptr){ + packet_buf_capacity_ = DEFAULT_DXL_BUF_LENGTH; + is_buf_malloced_ = true; + } + info_tx_packet_.is_init = false; + info_rx_packet_.is_init = false; } -void Slave::setWriteCallbackFunc(userCallbackFunc callback_func, void* callback_arg) + +bool +Slave::setPacketBuffer(uint8_t* p_buf, uint16_t buf_capacity) { - user_write_callback_ = callback_func; - user_write_callbakc_arg_ = callback_arg; + if(p_packet_buf_ == nullptr){ + last_lib_err_ = DXL_LIB_ERROR_NULLPTR; + return false; + } + if(packet_buf_capacity_ == 0){ + last_lib_err_ = DXL_LIB_ERROR_NOT_ENOUGH_BUFFER_SIZE; + return false; + } + + if(is_buf_malloced_ == true){ + delete p_packet_buf_; + } + p_packet_buf_ = p_buf; + packet_buf_capacity_ = buf_capacity; + + return true; } -void Slave::setReadCallbackFunc(userCallbackFunc callback_func, void* callback_arg) +uint8_t* +Slave::getPacketBuffer() const { - user_read_callback_ = callback_func; - user_read_callbakc_arg_ = callback_arg; + return p_packet_buf_; } -bool Slave::setPort(PortHandler &port) +uint16_t +Slave::getPacketBufferCapacity() const { - bool ret = setDxlPort(&packet_, &port); - - p_port_ = &port; - - return ret; + return packet_buf_capacity_; } -bool Slave::setPortProtocolVersion(float version) +bool +Slave::setPortProtocolVersion(float version) { - bool ret = false; + uint8_t version_idx; - ret = dxlSetProtocolVersion(&packet_, version); - if(ret == true){ - protocol_ver_idx_ = version == 1.0 ? 1:2; + if(version == 2.0){ + version_idx = 2; + }else if(version == 1.0){ + version_idx = 1; + }else{ + last_lib_err_ = DXL_LIB_ERROR_INVAILD_PROTOCOL_VERSION; + return false; } - return ret; + return setPortProtocolVersionUsingIndex(version_idx); } -bool Slave::setPortProtocolVersionUsingIndex(uint8_t version_idx) +bool +Slave::setPortProtocolVersionUsingIndex(uint8_t version_idx) { - bool ret = false; - float version_float; - - if(version_idx == 1){ - version_float = 1.0; - }else if(version_idx == 2){ - version_float = 2.0; - }else{ + if(version_idx != 2 && version_idx != 1){ return false; } + protocol_ver_idx_ = version_idx; - ret = dxlSetProtocolVersion(&packet_, version_float); - if(ret == true){ - protocol_ver_idx_ = version_idx; - } - - return ret; + return true; } -float Slave::getPortProtocolVersion() const +float +Slave::getPortProtocolVersion() const { - float version_float = 0.0; - - if(protocol_ver_idx_ == 1){ - version_float = 1.0; - }else if(protocol_ver_idx_ == 2){ - version_float = 2.0; - } - - return version_float; + return (float)protocol_ver_idx_; } -uint8_t Slave::getPortProtocolVersionIndex() const +uint8_t +Slave::getPortProtocolVersionIndex() const { return protocol_ver_idx_; } -uint16_t Slave::getModelNumber() const +uint16_t +Slave::getModelNumber() const { return model_num_; } -bool Slave::setID(uint8_t id) +bool +Slave::setID(uint8_t id) { - if(getPortProtocolVersion() == 1.0){ - if(id > 253){ - last_lib_err_code_ = DXL_LIB_ERROR_INVAILD_ID; - return false; - } - }else{ - if(id > 252){ - last_lib_err_code_ = DXL_LIB_ERROR_INVAILD_ID; - return false; - } + DXLLibErrorCode_t err = DXL_LIB_OK; + + if(protocol_ver_idx_ == 2 && id >= 0xFD){ //http://emanual.robotis.com/docs/en/dxl/protocol2/#packet-id + err = DXL_LIB_ERROR_INVAILD_ID; + }else if(protocol_ver_idx_ == 1 && id >= DXL_BROADCAST_ID){ //http://emanual.robotis.com/docs/en/dxl/protocol1/#packet-id + err = DXL_LIB_ERROR_INVAILD_ID; + } + last_lib_err_ = err; + if(err != DXL_LIB_OK){ + return false; } id_ = id; - dxlSetId(&packet_, id); return true; } -uint8_t Slave::getID() const +uint8_t +Slave::getID() const { return id_; } -void Slave::setFirmwareVersion(uint8_t version) +void +Slave::setFirmwareVersion(uint8_t version) { firmware_ver_ = version; } -uint8_t Slave::getFirmwareVersion() const +uint8_t +Slave::getFirmwareVersion() const { return firmware_ver_; } -bool Slave::processPacket() +bool +Slave::processPacket() { - bool ret = false; - last_lib_err_code_ = dxlRxPacket(&packet_); + bool ret = true; - if(last_lib_err_code_ == DXL_LIB_OK && packet_.rx.type == RX_PACKET_TYPE_INST){ - if(packet_.rx.id == packet_.id){ - ret = processInst(packet_.rx.cmd); - } - }else if(last_lib_err_code_ == DXL_LIB_PROCEEDING){ - ret = true; + if(rxInstPacket(p_packet_buf_, packet_buf_capacity_) != nullptr){ + ret = processInst(info_rx_packet_.inst_idx); } return ret; @@ -188,12 +192,12 @@ bool Slave::isEnoughSpaceInControlTable(uint16_t start_addr, uint16_t length) uint16_t available_start_addr = control_table_[registered_item_cnt_].start_addr + control_table_[registered_item_cnt_].length; if(start_addr > CONTROL_ITEM_ADDR_LIMIT){ - last_lib_err_code_ = DXL_LIB_ERROR_INVAILD_ADDR; + last_lib_err_ = DXL_LIB_ERROR_INVAILD_ADDR; return false; } if(length == 0 || length > CONTROL_ITEM_ADDR_LIMIT - available_start_addr){ - last_lib_err_code_ = DXL_LIB_ERROR_ADDR_LENGTH; + last_lib_err_ = DXL_LIB_ERROR_ADDR_LENGTH; return false; } @@ -204,23 +208,23 @@ bool Slave::isEnoughSpaceInControlTable(uint16_t start_addr, uint16_t length) uint8_t Slave::addControlItem(uint16_t start_addr, uint8_t* p_data, uint16_t length) { if(registered_item_cnt_ >= CONTROL_ITEM_MAX){ - last_lib_err_code_ = DXL_LIB_ERROR_BUFFER_OVERFLOW; - return last_lib_err_code_; + last_lib_err_ = DXL_LIB_ERROR_BUFFER_OVERFLOW; + return last_lib_err_; } if(p_data == nullptr){ - last_lib_err_code_ = DXL_LIB_ERROR_NULLPTR; - return last_lib_err_code_; + last_lib_err_ = DXL_LIB_ERROR_NULLPTR; + return last_lib_err_; } if(isEnoughSpaceInControlTable(start_addr, length) == false){ - return last_lib_err_code_; + return last_lib_err_; } for(uint16_t i=0; i < registered_item_cnt_; i++){ if(isAddrInOtherItem(start_addr, length, control_table_[i].start_addr, control_table_[i].length)){ - last_lib_err_code_ = DXL_LIB_ERROR_INVAILD_ADDR; - return last_lib_err_code_; + last_lib_err_ = DXL_LIB_ERROR_INVAILD_ADDR; + return last_lib_err_; } } @@ -230,9 +234,9 @@ uint8_t Slave::addControlItem(uint16_t start_addr, uint8_t* p_data, uint16_t len registered_item_cnt_++; - last_lib_err_code_ = DXL_LIB_OK; + last_lib_err_ = DXL_LIB_OK; - return last_lib_err_code_; + return last_lib_err_; } uint8_t Slave::addControlItem(uint16_t start_addr, bool &data) @@ -293,236 +297,332 @@ uint8_t Slave::addControlItem(uint16_t start_addr, double &data) -uint8_t Slave::getLastStatusPacketError() const +void Slave::setWriteCallbackFunc(userCallbackFunc callback_func, void* callback_arg) { - return last_status_packet_error_; + user_write_callback_ = callback_func; + user_write_callbakc_arg_ = callback_arg; } -lib_err_code_t Slave::getLastLibErrCode() const +void Slave::setReadCallbackFunc(userCallbackFunc callback_func, void* callback_arg) { - return last_lib_err_code_; + user_read_callback_ = callback_func; + user_read_callbakc_arg_ = callback_arg; } - - - -bool Slave::processInstPing() +bool +Slave::setPort(DXLPortHandler *p_port) { - bool ret = false; - uint16_t param_length = 0; - uint8_t *p_param_data; - - if(packet_.rx.id == DXL_BROADCAST_ID){ - last_lib_err_code_ = DXL_LIB_ERROR_NOT_SUPPORT_BROADCAST; + if(p_port == nullptr){ + last_lib_err_ = DXL_LIB_ERROR_NULLPTR; return false; } - p_param_data = &packet_.tx.data[PKT_STATUS_PARAM_IDX]; - if(packet_.packet_ver == DXL_PACKET_VER_2_0){ - p_param_data[param_length++] = (uint8_t)(model_num_ >> 0); - p_param_data[param_length++] = (uint8_t)(model_num_ >> 8); - p_param_data[param_length++] = (uint8_t)firmware_ver_; - } + p_port_ = p_port; - last_lib_err_code_ = dxlTxPacketStatus(packet_, 0, p_param_data, param_length); - - if(last_lib_err_code_ == DXL_LIB_OK) - ret = true; - - return ret; + return true; } - -bool Slave::processInstRead() +bool +Slave::setPort(DXLPortHandler &port) { - bool ret = false; - uint16_t addr; - uint16_t length = 0; - uint8_t process_ret = DXL_ERR_NONE; - uint8_t *p_param_data; + p_port_ = &port; - if(packet_.rx.id == DXL_BROADCAST_ID){ - last_lib_err_code_ = DXL_LIB_ERROR_NOT_SUPPORT_BROADCAST; - return false; - } + return true; +} - if(packet_.packet_ver == DXL_PACKET_VER_1_0 ) - { - if( packet_.rx.param_length != 2){ - last_lib_err_code_ = DXL_LIB_ERROR_LENGTH; - return false; - } +DXLPortHandler* +Slave::getPort() const +{ + return p_port_; +} - addr = packet_.rx.p_param[0]; - length = packet_.rx.p_param[1]; +DXLLibErrorCode_t +Slave::getLastLibErrCode() const +{ + return last_lib_err_; +} - if( length > 0xFF - 2){ - dxlTxPacketStatus(packet_, DXL_ERR_DATA_LENGTH, nullptr, 0); - last_lib_err_code_ = DXL_LIB_ERROR_LENGTH; - return false; - } - }else{ - if( packet_.rx.param_length != 4){ - last_lib_err_code_ = DXL_LIB_ERROR_LENGTH; - return false; - } +void +Slave::setLastLibErrCode(DXLLibErrorCode_t err_code) +{ + last_lib_err_ = err_code; +} - addr = ((uint16_t)packet_.rx.p_param[1]<<8) | (uint16_t)packet_.rx.p_param[0]; - length = ((uint16_t)packet_.rx.p_param[3]<<8) | (uint16_t)packet_.rx.p_param[2]; - } +uint8_t +Slave::getLastStatusPacketError() const +{ + return info_tx_packet_.err_idx; +} - if(length > DXL_BUF_LENGTH){ - dxlTxPacketStatus(packet_, DXL_ERR_DATA_LENGTH, nullptr, 0); - last_lib_err_code_ = DXL_LIB_ERROR_BUFFER_OVERFLOW; - return false; - } - memset(packet_.tx.data, 0, sizeof(packet_.tx.data)); - p_param_data = &packet_.tx.data[PKT_STATUS_PARAM_IDX]; - - uint16_t item_start_addr, item_addr_length; - for(uint8_t i=0; i < registered_item_cnt_; i++){ - item_start_addr = control_table_[i].start_addr; - item_addr_length = control_table_[i].length; - if(item_addr_length != 0 - && control_table_[i].p_data != nullptr - && isAddrInRange(item_start_addr, item_addr_length, addr, length)==true){ - - if(item_start_addr == ADDR_ID){ - setID(dxlGetId(&packet_)); - }else if(item_start_addr == ADDR_PROTOCOL_VER){ - setPortProtocolVersion(dxlGetProtocolVersion(&packet_)); - } - - if(user_read_callback_ != nullptr){ - user_read_callback_(item_start_addr, process_ret, user_read_callbakc_arg_); - if(process_ret != DXL_ERR_NONE){ - break; - } +bool +Slave::processInstPing() +{ + bool ret = false; + DXLLibErrorCode_t err = DXL_LIB_OK; + InfoToParseDXLPacket_t *p_rx_info; + uint8_t tx_param[3]; + uint16_t tx_param_len = 0; + + p_rx_info = &info_rx_packet_; + + if(p_rx_info->id != DXL_BROADCAST_ID){ + if(p_rx_info->protocol_ver == 2){ + tx_param_len = 3; + if(tx_param_len+11 <= packet_buf_capacity_){ + tx_param[0] = (uint8_t)(model_num_ >> 0); + tx_param[1] = (uint8_t)(model_num_ >> 8); + tx_param[2] = (uint8_t)firmware_ver_; + }else{ + err = DXL_LIB_ERROR_NOT_ENOUGH_BUFFER_SIZE; } - memcpy(&p_param_data[item_start_addr-addr], control_table_[i].p_data, item_addr_length); + }else if(p_rx_info->protocol_ver == 1){ + // + }else{ + err = DXL_LIB_ERROR_WRONG_PACKET; } + if(err == DXL_LIB_OK){ + ret = txStatusPacket(id_, 0, tx_param, tx_param_len); + } + }else{ + err = DXL_LIB_ERROR_NOT_SUPPORT_BROADCAST; } - last_lib_err_code_ = dxlTxPacketStatus(packet_, process_ret, p_param_data, length); - if(last_lib_err_code_ == DXL_LIB_OK){ - ret = true; - } + last_lib_err_ = err; return ret; } -bool Slave::processInstWrite() +bool +Slave::processInstRead() { bool ret = false; - uint16_t addr; - uint16_t length = 0; - uint8_t *p_data; - uint8_t process_ret = DXL_ERR_NONE; - - if(packet_.rx.id == DXL_BROADCAST_ID){ - last_lib_err_code_ = DXL_LIB_ERROR_NOT_SUPPORT_BROADCAST; + DXLLibErrorCode_t err = DXL_LIB_OK; + InfoToParseDXLPacket_t *p_rx_info; + uint8_t packet_err = 0; + uint8_t *p_rx_param, *p_tx_param; + uint16_t addr, addr_length = 0; + + // Parameter exception handling + if(p_port_ == nullptr){ + err = DXL_LIB_ERROR_NULLPTR; + }else if(p_port_->getOpenState() != true){ + err = DXL_LIB_ERROR_PORT_NOT_OPEN; + } + if(err != DXL_LIB_OK){ + last_lib_err_ = err; return false; } - - if(packet_.packet_ver == DXL_PACKET_VER_1_0 ){ - addr = packet_.rx.p_param[0]; - p_data = &packet_.rx.p_param[1]; - if(packet_.rx.param_length > 1 ){ - length = packet_.rx.param_length - 1; + p_rx_info = &info_rx_packet_; + p_rx_param = p_rx_info->p_param_buf; + + if(p_rx_info->id != DXL_BROADCAST_ID){ + if(p_rx_info->protocol_ver == 2){ + if(p_rx_info->recv_param_len == 4){ //4 = Address(2)+AddressLength(2) + addr = ((uint16_t)p_rx_param[1]<<8) | (uint16_t)p_rx_param[0]; + addr_length = ((uint16_t)p_rx_param[3]<<8) | (uint16_t)p_rx_param[2]; + if(addr_length+11 > packet_buf_capacity_){ + err = DXL_LIB_ERROR_NOT_ENOUGH_BUFFER_SIZE; + } + p_tx_param = &p_packet_buf_[9]; + }else{ + err = DXL_LIB_ERROR_WRONG_PACKET; + } + }else if(p_rx_info->protocol_ver == 1){ + if(p_rx_info->recv_param_len == 2){ //2 = Address(1)+AddressLength(1) + addr = p_rx_param[0]; + addr_length = p_rx_param[1]; + if(addr_length+6 > packet_buf_capacity_){ + err = DXL_LIB_ERROR_NOT_ENOUGH_BUFFER_SIZE; + } + p_tx_param = &p_packet_buf_[5]; + }else{ + err = DXL_LIB_ERROR_WRONG_PACKET; + } }else{ - dxlTxPacketStatus(packet_, DXL_ERR_DATA_LENGTH, nullptr, 0); - last_lib_err_code_ = DXL_LIB_ERROR_LENGTH; - return false; + err = DXL_LIB_ERROR_WRONG_PACKET; } - if( length > 0xFF - 2 ){ - dxlTxPacketStatus(packet_, DXL_ERR_DATA_LENGTH, nullptr, 0); - last_lib_err_code_ = DXL_LIB_ERROR_LENGTH; - return false; + if(err == DXL_LIB_OK){ + uint8_t i, j; + uint16_t item_start_addr, item_addr_length; + ControlItem_t *p_item; + memset(p_packet_buf_, 0, packet_buf_capacity_); + for(i=0; i < registered_item_cnt_; i++){ + p_item = &control_table_[i]; + item_start_addr = p_item->start_addr; + item_addr_length = p_item->length; + if(item_addr_length != 0 + && p_item->p_data != nullptr + && isAddrInRange(item_start_addr, item_addr_length, addr, addr_length) == true){ + if(user_read_callback_ != nullptr){ + user_read_callback_(item_start_addr, packet_err, user_read_callbakc_arg_); + if(packet_err != 0){ + break; + } + } + for(j=0; jp_data[j]; + } + } + } + ret = txStatusPacket(id_, packet_err, p_tx_param, addr_length); } }else{ - addr = ((uint16_t)packet_.rx.p_param[1]<<8) | (uint16_t)packet_.rx.p_param[0]; - p_data = &packet_.rx.p_param[2]; - - if(packet_.rx.param_length > 2 ){ - length = packet_.rx.param_length - 2; - }else{ - dxlTxPacketStatus(packet_, DXL_ERR_DATA_LENGTH, nullptr, 0); - last_lib_err_code_ = DXL_LIB_ERROR_LENGTH; - return false; - } + err = DXL_LIB_ERROR_NOT_SUPPORT_BROADCAST; } - if(length > DXL_BUF_LENGTH){ - dxlTxPacketStatus(packet_, DXL_ERR_DATA_LENGTH, nullptr, 0); - last_lib_err_code_ = DXL_LIB_ERROR_BUFFER_OVERFLOW; - return false; - } + last_lib_err_ = err; + + return ret; +} + + +bool +Slave::processInstWrite() +{ + bool ret = false; + DXLLibErrorCode_t err = DXL_LIB_OK; + InfoToParseDXLPacket_t *p_rx_info; + uint8_t *p_rx_param, *p_data; + uint8_t packet_err = 0; + uint16_t addr, data_length = 0; + + p_rx_info = &info_rx_packet_; + p_rx_param = p_rx_info->p_param_buf; + + if(p_rx_info->id != DXL_BROADCAST_ID){ + if(p_rx_info->protocol_ver == 2){ + if(p_rx_info->recv_param_len > 2){ //2 = Address(2)+Data(n) + addr = ((uint16_t)p_rx_param[1]<<8) | (uint16_t)p_rx_param[0]; + p_data = &p_rx_param[2]; + data_length = p_rx_info->recv_param_len-2; + if(data_length+11 > packet_buf_capacity_){ + err = DXL_LIB_ERROR_NOT_ENOUGH_BUFFER_SIZE; + } + }else{ + err = DXL_LIB_ERROR_WRONG_PACKET; + } + }else if(p_rx_info->protocol_ver == 1){ + if(p_rx_info->recv_param_len > 1){ //1 = Address(1)+Data(n) + addr = p_rx_param[0]; + p_data = &p_rx_param[1]; + data_length = p_rx_info->recv_param_len-1; + if(data_length+6 > packet_buf_capacity_){ + err = DXL_LIB_ERROR_NOT_ENOUGH_BUFFER_SIZE; + } + }else{ + err = DXL_LIB_ERROR_WRONG_PACKET; + } + }else{ + err = DXL_LIB_ERROR_WRONG_PACKET; + } - uint16_t item_start_addr, item_addr_length; - for(uint8_t i=0; i < registered_item_cnt_; i++){ - item_start_addr = control_table_[i].start_addr; - item_addr_length = control_table_[i].length; - if(item_addr_length != 0 - && control_table_[i].p_data != nullptr - && isAddrInRange(item_start_addr, item_addr_length, addr, length)==true){ - if(item_start_addr != ADDR_MODEL_NUMBER - && item_start_addr != ADDR_FIRMWARE_VER){ - - memcpy(control_table_[i].p_data, &p_data[item_start_addr-addr], item_addr_length); - if(user_write_callback_ != nullptr){ - user_write_callback_(item_start_addr, process_ret, user_write_callbakc_arg_); - if(process_ret != DXL_ERR_NONE){ - break; + if(err == DXL_LIB_OK){ + uint8_t i, j, backup_data = 0; + uint16_t item_start_addr, item_addr_length; + ControlItem_t *p_item; + for(i=0; i < registered_item_cnt_; i++){ + p_item = &control_table_[i]; + item_start_addr = p_item->start_addr; + item_addr_length = p_item->length; + if(item_start_addr != ADDR_ITEM_MODEL_NUMBER + && item_start_addr != ADDR_ITEM_FIRMWARE_VER){ + if(item_addr_length != 0 + && p_item->p_data != nullptr + && isAddrInRange(item_start_addr, item_addr_length, addr, data_length) == true){ + // Check data for ID, Protocol Version (Act as a system callback) + if(item_start_addr == ADDR_ITEM_ID){ + backup_data = p_data[item_start_addr-addr]; + if(protocol_ver_idx_ == 2 && backup_data >= 0xFD){ + packet_err = DXL2_0_ERR_DATA_RANGE; + }else if(protocol_ver_idx_ == 1 && backup_data >= 0xFE){ + packet_err |= 1<p_data[j] = p_data[item_start_addr-addr+j]; + } + // Run user callback for Write instruction, + // then check and handle the returned error. + if(user_write_callback_ != nullptr){ + user_write_callback_(item_start_addr, packet_err, user_write_callbakc_arg_); + if(packet_err != 0){ + // If an error occurs for the ID and Protocol Version, + // restore the previous data. (Act as a system callback) + if(item_start_addr == ADDR_ITEM_ID){ + id_ = backup_data; + }else if(item_start_addr == ADDR_ITEM_PROTOCOL_VER){ + protocol_ver_idx_ = backup_data; + } + break; + } + } } } } + ret = txStatusPacket(id_, packet_err, nullptr, 0); } + }else{ + err = DXL_LIB_ERROR_NOT_SUPPORT_BROADCAST; } - last_lib_err_code_ = dxlTxPacketStatus(packet_, process_ret, nullptr, 0); - if(last_lib_err_code_ == DXL_LIB_OK){ - ret = true; - } + last_lib_err_ = err; return ret; } -bool Slave::addDefaultControlItem() +bool +Slave::addDefaultControlItem() { - if(addControlItem(ADDR_MODEL_NUMBER, (uint16_t&)model_num_) != DXL_LIB_OK - || addControlItem(ADDR_FIRMWARE_VER, firmware_ver_) != DXL_LIB_OK - || addControlItem(ADDR_ID, id_) != DXL_LIB_OK - || addControlItem(ADDR_PROTOCOL_VER, protocol_ver_idx_) != DXL_LIB_OK){ + if(addControlItem(ADDR_ITEM_MODEL_NUMBER, (uint16_t&)model_num_) != DXL_LIB_OK + || addControlItem(ADDR_ITEM_FIRMWARE_VER, firmware_ver_) != DXL_LIB_OK + || addControlItem(ADDR_ITEM_ID, id_) != DXL_LIB_OK + || addControlItem(ADDR_ITEM_PROTOCOL_VER, protocol_ver_idx_) != DXL_LIB_OK){ return false; } return true; } -bool Slave::processInst(uint8_t inst_idx) +bool +Slave::processInst(uint8_t inst_idx) { bool ret = false; switch(inst_idx) { - case INST_PING: + case DXL_INST_PING: ret = processInstPing(); break; - case INST_READ: + case DXL_INST_READ: ret = processInstRead(); break; - case INST_WRITE: + case DXL_INST_WRITE: ret = processInstWrite(); break; default: - last_lib_err_code_ = DXL_LIB_ERROR_NOT_SUPPORTED; + last_lib_err_ = DXL_LIB_ERROR_NOT_SUPPORTED; break; } @@ -530,146 +630,102 @@ bool Slave::processInst(uint8_t inst_idx) } - - - - - -static bool isAddrInRange(uint16_t addr, uint16_t length, - uint16_t range_addr, uint16_t range_length) -{ - return (addr >= range_addr && addr+length <= range_addr+range_length) ? true:false; -} - -static bool isAddrInOtherItem(uint16_t start_addr, uint16_t length, - uint16_t other_start_addr, uint16_t other_length) +bool +Slave::txStatusPacket(uint8_t id, uint8_t err_code, uint8_t *p_param, uint16_t param_len) { bool ret = false; - uint16_t addr_end = start_addr + length; - uint16_t other_addr_end = other_start_addr + other_length; - - // The start address of the item is in the range of another item address. - if (start_addr >= other_start_addr && start_addr < other_addr_end){ - ret = true; - // The last address of an item is in the range of another item address. - }else if (addr_end > other_start_addr && addr_end <= other_addr_end){ - ret = true; - } - - return ret; -} - - - + DXLLibErrorCode_t err = DXL_LIB_OK; -static lib_err_code_t dxlTxPacketStatus(dxl_t &packet, uint8_t error, uint8_t *p_data, uint16_t length ) -{ - lib_err_code_t ret; - - ret = dxlMakePacketStatus(packet, error, p_data, length); - if(ret == DXL_LIB_OK) { - dxlTxWrite(&packet, packet.tx.data, packet.tx.packet_length); + // Parameter exception handling + if(p_port_ == nullptr + || (param_len > 0 && p_param == nullptr)){ + err = DXL_LIB_ERROR_NULLPTR; + }else if(p_port_->getOpenState() != true){ + err = DXL_LIB_ERROR_PORT_NOT_OPEN; + } + if(err != DXL_LIB_OK){ + last_lib_err_ = err; + return false; } - return ret; -} - -static lib_err_code_t dxlMakePacketStatus(dxl_t &packet, uint8_t error, uint8_t *p_data, uint16_t length ) -{ - lib_err_code_t ret; - - if(packet.packet_ver == DXL_PACKET_VER_1_0){ - ret = dxlMakePacketStatus1_0(packet, error, p_data, length); - }else{ - ret = dxlMakePacketStatus2_0(packet, error, p_data, length); + // Send Status Packet + begin_make_dxl_packet(&info_tx_packet_, id, protocol_ver_idx_, + DXL_INST_STATUS, err_code, p_packet_buf_, packet_buf_capacity_); + add_param_to_dxl_packet(&info_tx_packet_, p_param, param_len); + err = end_make_dxl_packet(&info_tx_packet_); + if(err == DXL_LIB_OK){ + p_port_->write(info_tx_packet_.p_packet_buf, info_tx_packet_.generated_packet_length); + ret = true; } + last_lib_err_ = err; + return ret; } -static lib_err_code_t dxlMakePacketStatus1_0(dxl_t &packet, uint8_t error, uint8_t *p_data, uint16_t length ) +const InfoToParseDXLPacket_t* +Slave::rxInstPacket(uint8_t* p_param_buf, uint16_t param_buf_cap) { - lib_err_code_t ret = DXL_LIB_OK; - uint16_t i = 0; - uint16_t packet_length; - uint8_t check_sum; - + InfoToParseDXLPacket_t *p_ret = nullptr; + DXLLibErrorCode_t err = DXL_LIB_OK; - if(length > DXL_BUF_LENGTH){ - return DXL_LIB_ERROR_BUFFER_OVERFLOW; + // Parameter exception handling + if(p_port_ == nullptr + || (param_buf_cap > 0 && p_param_buf == nullptr)){ + err = DXL_LIB_ERROR_NULLPTR; + }else if(p_port_->getOpenState() != true){ + err = DXL_LIB_ERROR_PORT_NOT_OPEN; } - - if(length > 0xFF){ - return DXL_LIB_ERROR_LENGTH; + if(err != DXL_LIB_OK){ + last_lib_err_ = err; + return nullptr; } - check_sum = 0; - packet_length = length + 2; // param_length + Instruction + CheckSum - - packet.tx.data[PKT_1_0_HDR_1_IDX] = 0xFF; - packet.tx.data[PKT_1_0_HDR_2_IDX] = 0xFF; - packet.tx.data[PKT_1_0_ID_IDX] = packet.id; - packet.tx.data[PKT_1_0_ERROR_IDX] = error; - - check_sum += packet.id; - check_sum += packet_length; - check_sum += error; - - for (i=0; iavailable() > 0) { - packet.tx.data[PKT_1_0_STATUS_PARAM_IDX + i] = p_data[i]; - check_sum += p_data[i]; + err = parse_dxl_packet(&info_rx_packet_, p_port_->read()); + if(err == DXL_LIB_OK){ + if((protocol_ver_idx_ == 2 && info_rx_packet_.inst_idx != DXL_INST_STATUS) + || protocol_ver_idx_ == 1){ + if(info_rx_packet_.id == id_ || info_rx_packet_.id == DXL_BROADCAST_ID){ + p_ret = &info_rx_packet_; + } + break; + } + }else if(err != DXL_LIB_PROCEEDING){ + break; + } } - packet.tx.data[PKT_1_0_LEN_IDX] = packet_length; - packet.tx.data[PKT_1_0_ERROR_IDX + packet_length - 1] = ~(check_sum); + last_lib_err_ = err; - return ret; + return p_ret; } -static lib_err_code_t dxlMakePacketStatus2_0(dxl_t &packet, uint8_t error, uint8_t *p_data, uint16_t length ) -{ - lib_err_code_t ret = DXL_LIB_OK; - uint16_t i = 0; - uint16_t packet_length; - uint16_t stuff_length; - uint16_t crc; - - if(length > DXL_BUF_LENGTH){ - return DXL_LIB_ERROR_BUFFER_OVERFLOW; - } - - packet_length = length + 4; // param_length + Instruction + Error + CRC_L + CRC_H - packet.tx.data[PKT_HDR_1_IDX] = 0xFF; - packet.tx.data[PKT_HDR_2_IDX] = 0xFF; - packet.tx.data[PKT_HDR_3_IDX] = 0xFD; - packet.tx.data[PKT_RSV_IDX] = 0x00; - packet.tx.data[PKT_ID_IDX] = packet.id; - packet.tx.data[PKT_INST_IDX] = INST_STATUS; - packet.tx.data[PKT_ERROR_IDX] = error; - for (i=0; i> 0; - packet.tx.data[PKT_LEN_H_IDX] = packet_length >> 8; - - crc = 0; - for (i=0; i= range_addr && addr+length <= range_addr+range_length) ? true:false; +} - packet.tx.data[PKT_INST_IDX + packet_length - 2] = crc >> 0; - packet.tx.data[PKT_INST_IDX + packet_length - 1] = crc >> 8; +static bool isAddrInOtherItem(uint16_t start_addr, uint16_t length, + uint16_t other_start_addr, uint16_t other_length) +{ + bool ret = false; + uint16_t addr_end = start_addr + length; + uint16_t other_addr_end = other_start_addr + other_length; - packet.tx.packet_length = packet_length + 7; + // The start address of the item is in the range of another item address. + if (start_addr >= other_start_addr && start_addr < other_addr_end){ + ret = true; + // The last address of an item is in the range of another item address. + }else if (addr_end > other_start_addr && addr_end <= other_addr_end){ + ret = true; + } return ret; } \ No newline at end of file diff --git a/src/utility/slave.h b/src/utility/slave.h index 16bef0e..1cbd207 100644 --- a/src/utility/slave.h +++ b/src/utility/slave.h @@ -14,14 +14,15 @@ * limitations under the License. *******************************************************************************/ -#ifndef DYNAMIXEL_SLAVE_H_ -#define DYNAMIXEL_SLAVE_H_ +#ifndef DYNAMIXEL_SLAVE_HPP_ +#define DYNAMIXEL_SLAVE_HPP_ -#include "protocol.h" +#include "dxl_c/protocol.h" #include "port_handler.h" +#include "config.h" #define CONTROL_ITEM_MAX 64 -#define CONTROL_ITEM_ADDR_LIMIT (DXL_BUF_LENGTH-11) +#define CONTROL_ITEM_ADDR_LIMIT (DEFAULT_DXL_BUF_LENGTH-11) namespace DYNAMIXEL{ @@ -38,8 +39,12 @@ typedef struct ControlItem{ class Slave { public: - Slave(PortHandler &port, const uint16_t model_num, float protocol_ver = DXL_PACKET_VER_2_0); - Slave(const uint16_t model_num, float protocol_ver = DXL_PACKET_VER_2_0); + Slave(DXLPortHandler &port, const uint16_t model_num, float protocol_ver = 2.0); + Slave(const uint16_t model_num, float protocol_ver = 2.0); + + bool setPacketBuffer(uint8_t* p_buf, uint16_t buf_capacity); + uint8_t* getPacketBuffer() const; + uint16_t getPacketBufferCapacity() const; uint16_t getModelNumber() const; @@ -49,8 +54,9 @@ class Slave void setFirmwareVersion(uint8_t version); uint8_t getFirmwareVersion() const; - bool setPort(PortHandler &port); - PortHandler* getPort() const; + bool setPort(DXLPortHandler &port); + bool setPort(DXLPortHandler *p_port); + DXLPortHandler* getPort() const; bool setPortProtocolVersion(float version); bool setPortProtocolVersionUsingIndex(uint8_t version_idx); @@ -59,12 +65,7 @@ class Slave void setWriteCallbackFunc(userCallbackFunc callback_func, void* callback_arg = nullptr); void setReadCallbackFunc(userCallbackFunc callback_func, void* callback_arg = nullptr); - - bool processPacket(); - uint8_t getLastStatusPacketError() const; - lib_err_code_t getLastLibErrCode() const; - uint8_t getNumCanBeRegistered() const; bool isEnoughSpaceInControlTable(uint16_t start_addr, uint16_t length); @@ -81,35 +82,49 @@ class Slave uint8_t addControlItem(uint16_t start_addr, float &data); uint8_t addControlItem(uint16_t start_addr, double &data); + bool processPacket(); + + uint8_t getLastStatusPacketError() const; + + void setLastLibErrCode(DXLLibErrorCode_t err_code); + DXLLibErrorCode_t getLastLibErrCode() const; + + // raw APIs + bool txStatusPacket(uint8_t id, uint8_t err_code, uint8_t *p_param, uint16_t param_len); + const InfoToParseDXLPacket_t* rxInstPacket(uint8_t* p_param_buf, uint16_t param_buf_cap); + private: - PortHandler *p_port_; + DXLPortHandler *p_port_; const uint16_t model_num_; - uint8_t firmware_ver_; uint8_t protocol_ver_idx_; + uint8_t firmware_ver_; uint8_t id_; + bool is_buf_malloced_; + uint8_t *p_packet_buf_; + uint16_t packet_buf_capacity_; + InfoToMakeDXLPacket_t info_tx_packet_; + InfoToParseDXLPacket_t info_rx_packet_; + + DXLLibErrorCode_t last_lib_err_; + uint8_t registered_item_cnt_; ControlItem_t control_table_[CONTROL_ITEM_MAX]; - + userCallbackFunc user_write_callback_; void* user_write_callbakc_arg_; userCallbackFunc user_read_callback_; void* user_read_callbakc_arg_; - dxl_t packet_; - - uint8_t last_status_packet_error_; - lib_err_code_t last_lib_err_code_; - virtual bool processInstPing(); virtual bool processInstRead(); virtual bool processInstWrite(); bool processInst(uint8_t inst_idx); - bool addDefaultControlItem(); + bool addDefaultControlItem(); }; } // namespace DYNAMIXEL -#endif /* DYNAMIXEL_SLAVE_H_ */ \ No newline at end of file +#endif /* DYNAMIXEL_SLAVE_HPP_ */ \ No newline at end of file