diff --git a/.github/workflows/dynamixel2arduino_ci.yml b/.github/workflows/dynamixel2arduino_ci.yml index 0e5a810..554dda2 100644 --- a/.github/workflows/dynamixel2arduino_ci.yml +++ b/.github/workflows/dynamixel2arduino_ci.yml @@ -16,7 +16,7 @@ jobs: - name: Run Arduino CI Script run: | source <(curl -SLs https://raw.githubusercontent.com/ROBOTIS-GIT/Dynamixel2Arduino/master/install.sh) ${{github.ref}} - build_platform opencr + build_platform openrb-150 build_platform uno build_platform mega2560 build_platform mkrzero diff --git a/README.md b/README.md index 4fdd586..e5c8912 100644 --- a/README.md +++ b/README.md @@ -1,15 +1,15 @@ -# Dynamixel2Arduino [![Build Status](https://travis-ci.org/ROBOTIS-GIT/Dynamixel2Arduino.svg?branch=master)](https://travis-ci.org/ROBOTIS-GIT/Dynamixel2Arduino/branches) +# DYNAMIXEL2Arduino [![Build Status](https://github.com/ROBOTIS-GIT/Dynamixel2Arduino/workflows/dynamixel2arduino_ci/badge.svg)](https://github.com/ROBOTIS-GIT/Dynamixel2Arduino) -## Serial and Direction Pin definitions by board - - The examples defines GPIO pins based on the use with DYNAMIXEL Shields. +## Serial and Direction Pin definitions of ROBOTIS controllers - When running DYNAMIXEL without DYNAMIXEL Shields on OpenCM9.04, OpenCR or custom boards, you might need to change the Serial and DYNAMIXEL 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|Serial1|28|Uploading Arduino Sketch will erase the factory firmware. The firmware can be recovered with DYNAMIXEL Wizard 2.0| |OpenCM485EXP|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))| + |OpenCR|Serial3|84|OpenCR has an FET switch to control power supply to DYNAMIXEL. ([Reference link](https://github.com/ROBOTIS-GIT/OpenCR/blob/master/arduino/opencr_arduino/opencr/libraries/DynamixelSDK/src/dynamixel_sdk/port_handler_arduino.cpp#L78))| + |OpenRB-150|Serial1|(-1)Automatic|-| ## How to add new DYNAMIXEL model. @@ -19,6 +19,3 @@ ## 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 index 59a0e23..3842b68 100644 --- 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 @@ -26,29 +26,34 @@ 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 + const int DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN #elif defined(ARDUINO_SAM_DUE) // When using DynamixelShield #define DXL_SERIAL Serial #define DEBUG_SERIAL SerialUSB - const uint8_t DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN + const int DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN #elif defined(ARDUINO_SAM_ZERO) // When using DynamixelShield #define DXL_SERIAL Serial1 #define DEBUG_SERIAL SerialUSB - const uint8_t DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN + const int DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN #elif defined(ARDUINO_OpenCM904) // When using official ROBOTIS board with DXL circuit. #define DXL_SERIAL Serial3 //OpenCM9.04 EXP Board's DXL port Serial. (Serial1 for the DXL port on the OpenCM 9.04 board) #define DEBUG_SERIAL Serial - const uint8_t DXL_DIR_PIN = 22; //OpenCM9.04 EXP Board's DIR PIN. (28 for the DXL port on the OpenCM 9.04 board) + const int DXL_DIR_PIN = 22; //OpenCM9.04 EXP Board's DIR PIN. (28 for the DXL port on the OpenCM 9.04 board) #elif defined(ARDUINO_OpenCR) // When using official ROBOTIS board with DXL circuit. // For OpenCR, there is a DXL Power Enable pin, so you must initialize and control it. // Reference link : https://github.com/ROBOTIS-GIT/OpenCR/blob/master/arduino/opencr_arduino/opencr/libraries/DynamixelSDK/src/dynamixel_sdk/port_handler_arduino.cpp#L78 #define DXL_SERIAL Serial3 #define DEBUG_SERIAL Serial - const uint8_t DXL_DIR_PIN = 84; // OpenCR Board's DIR PIN. + const int DXL_DIR_PIN = 84; // OpenCR Board's DIR PIN. +#elif defined(ARDUINO_OpenRB) // When using OpenRB-150 + //OpenRB does not require the DIR control pin. + #define DXL_SERIAL Serial1 + #define DEBUG_SERIAL Serial + const int DXL_DIR_PIN = -1; #else // Other boards when using DynamixelShield #define DXL_SERIAL Serial1 #define DEBUG_SERIAL Serial - const uint8_t DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN + const int DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN #endif /* XelInfoForBulkReadParam_t diff --git a/examples/advanced/DEPRECATED_sync_bulk_raw/DEPRECATED_sync_bulk_raw.ino b/examples/advanced/DEPRECATED_sync_bulk_raw/DEPRECATED_sync_bulk_raw.ino index eb8b765..4cb058d 100644 --- a/examples/advanced/DEPRECATED_sync_bulk_raw/DEPRECATED_sync_bulk_raw.ino +++ b/examples/advanced/DEPRECATED_sync_bulk_raw/DEPRECATED_sync_bulk_raw.ino @@ -26,29 +26,34 @@ 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 + const int DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN #elif defined(ARDUINO_SAM_DUE) // When using DynamixelShield #define DXL_SERIAL Serial #define DEBUG_SERIAL SerialUSB - const uint8_t DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN + const int DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN #elif defined(ARDUINO_SAM_ZERO) // When using DynamixelShield #define DXL_SERIAL Serial1 #define DEBUG_SERIAL SerialUSB - const uint8_t DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN + const int DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN #elif defined(ARDUINO_OpenCM904) // When using official ROBOTIS board with DXL circuit. #define DXL_SERIAL Serial3 //OpenCM9.04 EXP Board's DXL port Serial. (Serial1 for the DXL port on the OpenCM 9.04 board) #define DEBUG_SERIAL Serial - const uint8_t DXL_DIR_PIN = 22; //OpenCM9.04 EXP Board's DIR PIN. (28 for the DXL port on the OpenCM 9.04 board) + const int DXL_DIR_PIN = 22; //OpenCM9.04 EXP Board's DIR PIN. (28 for the DXL port on the OpenCM 9.04 board) #elif defined(ARDUINO_OpenCR) // When using official ROBOTIS board with DXL circuit. // For OpenCR, there is a DXL Power Enable pin, so you must initialize and control it. // Reference link : https://github.com/ROBOTIS-GIT/OpenCR/blob/master/arduino/opencr_arduino/opencr/libraries/DynamixelSDK/src/dynamixel_sdk/port_handler_arduino.cpp#L78 #define DXL_SERIAL Serial3 #define DEBUG_SERIAL Serial - const uint8_t DXL_DIR_PIN = 84; // OpenCR Board's DIR PIN. + const int DXL_DIR_PIN = 84; // OpenCR Board's DIR PIN. +#elif defined(ARDUINO_OpenRB) // When using OpenRB-150 + //OpenRB does not require the DIR control pin. + #define DXL_SERIAL Serial1 + #define DEBUG_SERIAL Serial + const int DXL_DIR_PIN = -1; #else // Other boards when using DynamixelShield #define DXL_SERIAL Serial1 #define DEBUG_SERIAL Serial - const uint8_t DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN + const int DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN #endif /* ParamForSyncReadInst_t 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 index 2d2b5a7..65ef9a4 100644 --- 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 @@ -26,29 +26,34 @@ 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 + const int DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN #elif defined(ARDUINO_SAM_DUE) // When using DynamixelShield #define DXL_SERIAL Serial #define DEBUG_SERIAL SerialUSB - const uint8_t DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN + const int DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN #elif defined(ARDUINO_SAM_ZERO) // When using DynamixelShield #define DXL_SERIAL Serial1 #define DEBUG_SERIAL SerialUSB - const uint8_t DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN + const int DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN #elif defined(ARDUINO_OpenCM904) // When using official ROBOTIS board with DXL circuit. #define DXL_SERIAL Serial3 //OpenCM9.04 EXP Board's DXL port Serial. (Serial1 for the DXL port on the OpenCM 9.04 board) #define DEBUG_SERIAL Serial - const uint8_t DXL_DIR_PIN = 22; //OpenCM9.04 EXP Board's DIR PIN. (28 for the DXL port on the OpenCM 9.04 board) + const int DXL_DIR_PIN = 22; //OpenCM9.04 EXP Board's DIR PIN. (28 for the DXL port on the OpenCM 9.04 board) #elif defined(ARDUINO_OpenCR) // When using official ROBOTIS board with DXL circuit. // For OpenCR, there is a DXL Power Enable pin, so you must initialize and control it. // Reference link : https://github.com/ROBOTIS-GIT/OpenCR/blob/master/arduino/opencr_arduino/opencr/libraries/DynamixelSDK/src/dynamixel_sdk/port_handler_arduino.cpp#L78 #define DXL_SERIAL Serial3 #define DEBUG_SERIAL Serial - const uint8_t DXL_DIR_PIN = 84; // OpenCR Board's DIR PIN. + const int DXL_DIR_PIN = 84; // OpenCR Board's DIR PIN. +#elif defined(ARDUINO_OpenRB) // When using OpenRB-150 + //OpenRB does not require the DIR control pin. + #define DXL_SERIAL Serial1 + #define DEBUG_SERIAL Serial + const int DXL_DIR_PIN = -1; #else // Other boards when using DynamixelShield #define DXL_SERIAL Serial1 #define DEBUG_SERIAL Serial - const uint8_t DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN + const int DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN #endif /* ParamForSyncReadInst_t diff --git a/examples/advanced/add_custom_SerialPortHandler/add_custom_SerialPortHandler.ino b/examples/advanced/add_custom_SerialPortHandler/add_custom_SerialPortHandler.ino index 2e10d4b..772b441 100644 --- a/examples/advanced/add_custom_SerialPortHandler/add_custom_SerialPortHandler.ino +++ b/examples/advanced/add_custom_SerialPortHandler/add_custom_SerialPortHandler.ino @@ -22,29 +22,34 @@ 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 + const int DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN #elif defined(ARDUINO_SAM_DUE) // When using DynamixelShield #define DXL_SERIAL Serial #define DEBUG_SERIAL SerialUSB - const uint8_t DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN + const int DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN #elif defined(ARDUINO_SAM_ZERO) // When using DynamixelShield #define DXL_SERIAL Serial1 #define DEBUG_SERIAL SerialUSB - const uint8_t DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN + const int DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN #elif defined(ARDUINO_OpenCM904) // When using official ROBOTIS board with DXL circuit. #define DXL_SERIAL Serial3 //OpenCM9.04 EXP Board's DXL port Serial. (Serial1 for the DXL port on the OpenCM 9.04 board) #define DEBUG_SERIAL Serial - const uint8_t DXL_DIR_PIN = 22; //OpenCM9.04 EXP Board's DIR PIN. (28 for the DXL port on the OpenCM 9.04 board) + const int DXL_DIR_PIN = 22; //OpenCM9.04 EXP Board's DIR PIN. (28 for the DXL port on the OpenCM 9.04 board) #elif defined(ARDUINO_OpenCR) // When using official ROBOTIS board with DXL circuit. // For OpenCR, there is a DXL Power Enable pin, so you must initialize and control it. // Reference link : https://github.com/ROBOTIS-GIT/OpenCR/blob/master/arduino/opencr_arduino/opencr/libraries/DynamixelSDK/src/dynamixel_sdk/port_handler_arduino.cpp#L78 #define DXL_SERIAL Serial3 #define DEBUG_SERIAL Serial - const uint8_t DXL_DIR_PIN = 84; // OpenCR Board's DIR PIN. + const int DXL_DIR_PIN = 84; // OpenCR Board's DIR PIN. +#elif defined(ARDUINO_OpenRB) // When using OpenRB-150 + //OpenRB does not require the DIR control pin. + #define DXL_SERIAL Serial1 + #define DEBUG_SERIAL Serial + const int DXL_DIR_PIN = -1; #else // Other boards when using DynamixelShield #define DXL_SERIAL Serial1 #define DEBUG_SERIAL Serial - const uint8_t DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN + const int DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN #endif 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 5309e20..8b40f53 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 @@ -22,29 +22,34 @@ 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 + const int DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN #elif defined(ARDUINO_SAM_DUE) // When using DynamixelShield #define DXL_SERIAL Serial #define DEBUG_SERIAL SerialUSB - const uint8_t DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN + const int DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN #elif defined(ARDUINO_SAM_ZERO) // When using DynamixelShield #define DXL_SERIAL Serial1 #define DEBUG_SERIAL SerialUSB - const uint8_t DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN + const int DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN #elif defined(ARDUINO_OpenCM904) // When using official ROBOTIS board with DXL circuit. #define DXL_SERIAL Serial3 //OpenCM9.04 EXP Board's DXL port Serial. (Serial1 for the DXL port on the OpenCM 9.04 board) #define DEBUG_SERIAL Serial - const uint8_t DXL_DIR_PIN = 22; //OpenCM9.04 EXP Board's DIR PIN. (28 for the DXL port on the OpenCM 9.04 board) + const int DXL_DIR_PIN = 22; //OpenCM9.04 EXP Board's DIR PIN. (28 for the DXL port on the OpenCM 9.04 board) #elif defined(ARDUINO_OpenCR) // When using official ROBOTIS board with DXL circuit. // For OpenCR, there is a DXL Power Enable pin, so you must initialize and control it. // Reference link : https://github.com/ROBOTIS-GIT/OpenCR/blob/master/arduino/opencr_arduino/opencr/libraries/DynamixelSDK/src/dynamixel_sdk/port_handler_arduino.cpp#L78 #define DXL_SERIAL Serial3 #define DEBUG_SERIAL Serial - const uint8_t DXL_DIR_PIN = 84; // OpenCR Board's DIR PIN. + const int DXL_DIR_PIN = 84; // OpenCR Board's DIR PIN. +#elif defined(ARDUINO_OpenRB) // When using OpenRB-150 + //OpenRB does not require the DIR control pin. + #define DXL_SERIAL Serial1 + #define DEBUG_SERIAL Serial + const int DXL_DIR_PIN = -1; #else // Other boards when using DynamixelShield #define DXL_SERIAL Serial1 #define DEBUG_SERIAL Serial - const uint8_t DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN + const int DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN #endif diff --git a/examples/advanced/fast_sync_read/fast_sync_read.ino b/examples/advanced/fast_sync_read/fast_sync_read.ino new file mode 100644 index 0000000..9b1080e --- /dev/null +++ b/examples/advanced/fast_sync_read/fast_sync_read.ino @@ -0,0 +1,247 @@ +/******************************************************************************* +* 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. +*******************************************************************************/ + +// Author: Ashe Kim + +#include + +// Please modify it to suit your hardware. +#if defined(ARDUINO_AVR_UNO) || defined(ARDUINO_AVR_MEGA2560) // When using DynamixelShield + #include + SoftwareSerial soft_serial(7, 8); // DYNAMIXELShield UART RX/TX + #define DXL_SERIAL Serial + #define DEBUG_SERIAL soft_serial + const int DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN +#elif defined(ARDUINO_SAM_DUE) // When using DynamixelShield + #define DXL_SERIAL Serial + #define DEBUG_SERIAL SerialUSB + const int DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN +#elif defined(ARDUINO_SAM_ZERO) // When using DynamixelShield + #define DXL_SERIAL Serial1 + #define DEBUG_SERIAL SerialUSB + const int DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN +#elif defined(ARDUINO_OpenCM904) // When using official ROBOTIS board with DXL circuit. + #define DXL_SERIAL Serial3 //OpenCM9.04 EXP Board's DXL port Serial. (Serial1 for the DXL port on the OpenCM 9.04 board) + #define DEBUG_SERIAL Serial + const int DXL_DIR_PIN = 22; //OpenCM9.04 EXP Board's DIR PIN. (28 for the DXL port on the OpenCM 9.04 board) +#elif defined(ARDUINO_OpenCR) // When using official ROBOTIS board with DXL circuit. + // For OpenCR, there is a DXL Power Enable pin, so you must initialize and control it. + // Reference link : https://github.com/ROBOTIS-GIT/OpenCR/blob/master/arduino/opencr_arduino/opencr/libraries/DynamixelSDK/src/dynamixel_sdk/port_handler_arduino.cpp#L78 + #define DXL_SERIAL Serial3 + #define DEBUG_SERIAL Serial + const int DXL_DIR_PIN = 84; // OpenCR Board's DIR PIN. +#elif defined(ARDUINO_OpenRB) // When using OpenRB-150 + //OpenRB does not require the DIR control pin. + #define DXL_SERIAL Serial1 + #define DEBUG_SERIAL Serial + const int DXL_DIR_PIN = -1; +#else // Other boards when using DynamixelShield + #define DXL_SERIAL Serial1 + #define DEBUG_SERIAL Serial + const int DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN +#endif +#define DXL_MOVING_STATUS_THRESHOLD 10 + +/* FastsyncRead + DYNAMIXEL PROTOCOL 1.0 does NOT support Sync Read feature. + Structures containing the necessary information to process the 'fastSyncRead' packet. + + typedef struct XELInfoFastSyncRead{ + uint8_t *p_recv_buf; + uint8_t id; + uint8_t error; +} __attribute__((packed)) XELInfoFastSyncRead_t; + +typedef struct InfoFastSyncReadInst{ + uint16_t addr; + uint16_t addr_length; + XELInfoFastSyncRead_t* p_xels; + uint8_t xel_count; + bool is_info_changed; + InfoSyncBulkBuffer_t packet; +} __attribute__((packed)) InfoFastSyncReadInst_t; +*/ + +/* syncWrite + DYNAMIXEL PROTOCOL 1.0 supports Control Table address up to 255. + Structures containing the necessary information to process the 'syncWrite' packet. + + 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; +*/ + +const float DYNAMIXEL_PROTOCOL_VERSION = 2.0; +const uint8_t BROADCAST_ID = 254; +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 = 132; //Present position +const uint16_t SR_ADDR_LEN = 4; +const uint16_t SW_START_ADDR = 116; //Goal position +const uint16_t SW_ADDR_LEN = 4; + +typedef struct sr_data{ + int32_t present_position; +} __attribute__((packed)) sr_data_t; +typedef struct sw_data{ + int32_t goal_position; +} __attribute__((packed)) sw_data_t; + + +sr_data_t sr_data[DXL_ID_CNT]; +DYNAMIXEL::InfoFastSyncReadInst_t sr_infos; +DYNAMIXEL::XELInfoFastSyncRead_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); + +//This namespace is required to use Control table item names +using namespace ControlTableItem; + +int32_t dxl_goal_position = 1024; +char keyboard; + +void setup() { + uint8_t i; + pinMode(LED_BUILTIN, OUTPUT); + DEBUG_SERIAL.begin(115200); + dxl.begin(57600); + dxl.setPortProtocolVersion(DYNAMIXEL_PROTOCOL_VERSION); + + for(i=0; i>>>>> Sync Instruction Test : "); + DEBUG_SERIAL.println(try_count++); + + // Build a syncWrite Packet and transmit to DYNAMIXEL + if(dxl.syncWrite(&sw_infos) == true){ + DEBUG_SERIAL.println("[SyncWrite] Success"); + for(i=0; i 0){ + DEBUG_SERIAL.print("[fastSyncRead] Success, Received ID Count: "); + DEBUG_SERIAL.println(recv_cnt); + for(i=0; i + +// Please modify it to suit your hardware. +#if defined(ARDUINO_AVR_UNO) || defined(ARDUINO_AVR_MEGA2560) // When using DynamixelShield + #include + SoftwareSerial soft_serial(7, 8); // DYNAMIXELShield UART RX/TX + #define DXL_SERIAL Serial + #define DEBUG_SERIAL soft_serial + const int DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN +#elif defined(ARDUINO_SAM_DUE) // When using DynamixelShield + #define DXL_SERIAL Serial + #define DEBUG_SERIAL SerialUSB + const int DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN +#elif defined(ARDUINO_SAM_ZERO) // When using DynamixelShield + #define DXL_SERIAL Serial1 + #define DEBUG_SERIAL SerialUSB + const int DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN +#elif defined(ARDUINO_OpenCM904) // When using official ROBOTIS board with DXL circuit. + #define DXL_SERIAL Serial3 //OpenCM9.04 EXP Board's DXL port Serial. (Serial1 for the DXL port on the OpenCM 9.04 board) + #define DEBUG_SERIAL Serial + const int DXL_DIR_PIN = 22; //OpenCM9.04 EXP Board's DIR PIN. (28 for the DXL port on the OpenCM 9.04 board) +#elif defined(ARDUINO_OpenCR) // When using official ROBOTIS board with DXL circuit. + // For OpenCR, there is a DXL Power Enable pin, so you must initialize and control it. + // Reference link : https://github.com/ROBOTIS-GIT/OpenCR/blob/master/arduino/opencr_arduino/opencr/libraries/DynamixelSDK/src/dynamixel_sdk/port_handler_arduino.cpp#L78 + #define DXL_SERIAL Serial3 + #define DEBUG_SERIAL Serial + const int DXL_DIR_PIN = 84; // OpenCR Board's DIR PIN. +#elif defined(ARDUINO_OpenRB) // When using OpenRB-150 + //OpenRB does not require the DIR control pin. + #define DXL_SERIAL Serial1 + #define DEBUG_SERIAL Serial + const int DXL_DIR_PIN = -1; +#else // Other boards when using DynamixelShield + #define DXL_SERIAL Serial1 + #define DEBUG_SERIAL Serial + const int DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN +#endif + + +const uint8_t BROADCAST_ID = 254; +const float DYNAMIXEL_PROTOCOL_VERSION = 2.0; + +const uint8_t DXL_ID_CNT = 2; +const uint8_t DXL_ID_LIST[DXL_ID_CNT] = {1, 2}; + +const uint16_t user_pkt_buf_cap = 128; +uint8_t user_pkt_buf[user_pkt_buf_cap]; + +const uint16_t INDIRECT_ADDR_NUM = 5; +uint16_t INDIRECT_ADDR_ARRY[INDIRECT_ADDR_NUM] = {65, 116, 117, 118, 119}; + +const uint16_t ID_ADDR_LEN = INDIRECT_ADDR_NUM * 2; // Data Length 2*INDIRECT_ADDR_NUM, Can differ depending on how many address to access. +const uint16_t ID_START_ADDR = 168; // Indirect Address1 Address. Starting Data Addr, Can differ Depending on what address to access + +const uint16_t SW_ADDR_LEN = 5; // Data Length (1+4), Can differ depending on how many address to access. +const uint16_t SW_START_ADDR = 224; // Indirect Data1 Address. Starting Data Addr, Can differ Depending on what address to access + +const uint16_t SR_ADDR_LEN = 4; // Data Length 4, Can differ depending on how many address to access. +const uint16_t SR_START_ADDR = 132; // Present Position Address. Starting Data Addr, Can differ Depending on what address to access + +typedef struct id_data{ + uint8_t* indirect_addr; +} __attribute__((packed)) id_data_t; + +typedef struct sw_data{ + int8_t led; + int32_t goal_position; +} __attribute__((packed)) sw_data_t; + +typedef struct sr_data{ + int32_t present_position; +} __attribute__((packed)) sr_data_t; + +id_data_t id_data[DXL_ID_CNT]; +DYNAMIXEL::InfoSyncWriteInst_t id_infos; +DYNAMIXEL::XELInfoSyncWrite_t info_xels_id[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]; + +sr_data_t sr_data[DXL_ID_CNT]; +DYNAMIXEL::InfoSyncReadInst_t sr_infos; +DYNAMIXEL::XELInfoSyncRead_t info_xels_sr[DXL_ID_CNT]; + +Dynamixel2Arduino dxl(DXL_SERIAL, DXL_DIR_PIN); + +//This namespace is required to use Control table item names +using namespace ControlTableItem; + +int8_t led_state[2] = {0, 1}; +int32_t position_state[2] = {1024, 1536}; +uint8_t state_index = 0; + +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.setPortProtocolVersion(DYNAMIXEL_PROTOCOL_VERSION); + + for(i=0; i>>>>>>>>>>>> Indirect Address Test : "); + DEBUG_SERIAL.println(try_count++); + if(dxl.syncWrite(&id_infos) == true){ + DEBUG_SERIAL.println("[Indirect Address] Success!!"); + for(i=0; i 0){ + DEBUG_SERIAL.print("[SyncRead] Success!! Received ID Count: "); + DEBUG_SERIAL.println(recv_cnt); + for(i=0; i - -// 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 - - -const uint8_t DXL_ID = 1; -const float DXL_PROTOCOL_VERSION = 2.0; -uint32_t BAUDRATE = 57600; -uint32_t NEW_BAUDRATE = 1000000; //1Mbsp - -Dynamixel2Arduino dxl(DXL_SERIAL, DXL_DIR_PIN); - -//This namespace is required to use Control table item names -using namespace ControlTableItem; - -void setup() { - // put your setup code here, to run once: - - // Use UART port of DYNAMIXEL Shield to debug. - DEBUG_SERIAL.begin(115200); - - // Set Port baudrate to 57600bps. This has to match with DYNAMIXEL baudrate. - dxl.begin(BAUDRATE); - // Set Port Protocol Version. This has to match with DYNAMIXEL protocol version. - dxl.setPortProtocolVersion(DXL_PROTOCOL_VERSION); - - 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(", Baudrate: "); - DEBUG_SERIAL.println(BAUDRATE); - - // Turn off torque when configuring items in EEPROM area - dxl.torqueOff(DXL_ID); - - // Set a new baudrate(1Mbps) for DYNAMIXEL - dxl.setBaudrate(DXL_ID, NEW_BAUDRATE); - DEBUG_SERIAL.println("Baudrate has been successfully changed to 1Mbps"); - - // Change to the new baudrate for communication. - dxl.begin(NEW_BAUDRATE); - // Change back to the initial baudrate - dxl.setBaudrate(DXL_ID, BAUDRATE); - DEBUG_SERIAL.println("Baudrate has been successfully changed back to initial baudrate"); - } - else{ - DEBUG_SERIAL.println("ping failed!"); - } -} - -void loop() { - // put your main code here, to run repeatedly: +/******************************************************************************* +* Copyright 2016 ROBOTIS CO., LTD. +* +* Licensed under the Apache License, Version 2.0 (the "License"); +* you may not use this file except in compliance with the License. +* You may obtain a copy of the License at +* +* http://www.apache.org/licenses/LICENSE-2.0 +* +* Unless required by applicable law or agreed to in writing, software +* distributed under the License is distributed on an "AS IS" BASIS, +* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +* See the License for the specific language governing permissions and +* limitations under the License. +*******************************************************************************/ + +#include + +// Please modify it to suit your hardware. +#if defined(ARDUINO_AVR_UNO) || defined(ARDUINO_AVR_MEGA2560) // When using DynamixelShield + #include + SoftwareSerial soft_serial(7, 8); // DYNAMIXELShield UART RX/TX + #define DXL_SERIAL Serial + #define DEBUG_SERIAL soft_serial + const int DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN +#elif defined(ARDUINO_SAM_DUE) // When using DynamixelShield + #define DXL_SERIAL Serial + #define DEBUG_SERIAL SerialUSB + const int DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN +#elif defined(ARDUINO_SAM_ZERO) // When using DynamixelShield + #define DXL_SERIAL Serial1 + #define DEBUG_SERIAL SerialUSB + const int DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN +#elif defined(ARDUINO_OpenCM904) // When using official ROBOTIS board with DXL circuit. + #define DXL_SERIAL Serial3 //OpenCM9.04 EXP Board's DXL port Serial. (Serial1 for the DXL port on the OpenCM 9.04 board) + #define DEBUG_SERIAL Serial + const int DXL_DIR_PIN = 22; //OpenCM9.04 EXP Board's DIR PIN. (28 for the DXL port on the OpenCM 9.04 board) +#elif defined(ARDUINO_OpenCR) // When using official ROBOTIS board with DXL circuit. + // For OpenCR, there is a DXL Power Enable pin, so you must initialize and control it. + // Reference link : https://github.com/ROBOTIS-GIT/OpenCR/blob/master/arduino/opencr_arduino/opencr/libraries/DynamixelSDK/src/dynamixel_sdk/port_handler_arduino.cpp#L78 + #define DXL_SERIAL Serial3 + #define DEBUG_SERIAL Serial + const int DXL_DIR_PIN = 84; // OpenCR Board's DIR PIN. +#elif defined(ARDUINO_OpenRB) // When using OpenRB-150 + //OpenRB does not require the DIR control pin. + #define DXL_SERIAL Serial1 + #define DEBUG_SERIAL Serial + const int DXL_DIR_PIN = -1; +#else // Other boards when using DynamixelShield + #define DXL_SERIAL Serial1 + #define DEBUG_SERIAL Serial + const int DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN +#endif + + +const uint8_t DXL_ID = 1; +const float DXL_PROTOCOL_VERSION = 2.0; +uint32_t BAUDRATE = 57600; +uint32_t NEW_BAUDRATE = 1000000; //1Mbsp + +Dynamixel2Arduino dxl(DXL_SERIAL, DXL_DIR_PIN); + +//This namespace is required to use Control table item names +using namespace ControlTableItem; + +void setup() { + // put your setup code here, to run once: + + // Use UART port of DYNAMIXEL Shield to debug. + DEBUG_SERIAL.begin(115200); + + // Set Port baudrate to 57600bps. This has to match with DYNAMIXEL baudrate. + dxl.begin(BAUDRATE); + // Set Port Protocol Version. This has to match with DYNAMIXEL protocol version. + dxl.setPortProtocolVersion(DXL_PROTOCOL_VERSION); + + 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(", Baudrate: "); + DEBUG_SERIAL.println(BAUDRATE); + + // Turn off torque when configuring items in EEPROM area + dxl.torqueOff(DXL_ID); + + // Set a new baudrate(1Mbps) for DYNAMIXEL + dxl.setBaudrate(DXL_ID, NEW_BAUDRATE); + DEBUG_SERIAL.println("Baudrate has been successfully changed to 1Mbps"); + + // Change to the new baudrate for communication. + dxl.begin(NEW_BAUDRATE); + // Change back to the initial baudrate + dxl.setBaudrate(DXL_ID, BAUDRATE); + DEBUG_SERIAL.println("Baudrate has been successfully changed back to initial baudrate"); + } + else{ + DEBUG_SERIAL.println("ping failed!"); + } +} + +void loop() { + // put your main code here, to run repeatedly: } \ No newline at end of file diff --git a/examples/basic/copy_eeprom_x/copy_eeprom_x.ino b/examples/basic/copy_eeprom_x/copy_eeprom_x.ino index 05f5fb4..34f47ec 100644 --- a/examples/basic/copy_eeprom_x/copy_eeprom_x.ino +++ b/examples/basic/copy_eeprom_x/copy_eeprom_x.ino @@ -22,29 +22,34 @@ 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 + const int DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN #elif defined(ARDUINO_SAM_DUE) // When using DynamixelShield #define DXL_SERIAL Serial #define DEBUG_SERIAL SerialUSB - const uint8_t DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN + const int DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN #elif defined(ARDUINO_SAM_ZERO) // When using DynamixelShield #define DXL_SERIAL Serial1 #define DEBUG_SERIAL SerialUSB - const uint8_t DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN + const int DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN #elif defined(ARDUINO_OpenCM904) // When using official ROBOTIS board with DXL circuit. #define DXL_SERIAL Serial3 //OpenCM9.04 EXP Board's DXL port Serial. (Serial1 for the DXL port on the OpenCM 9.04 board) #define DEBUG_SERIAL Serial - const uint8_t DXL_DIR_PIN = 22; //OpenCM9.04 EXP Board's DIR PIN. (28 for the DXL port on the OpenCM 9.04 board) + const int DXL_DIR_PIN = 22; //OpenCM9.04 EXP Board's DIR PIN. (28 for the DXL port on the OpenCM 9.04 board) #elif defined(ARDUINO_OpenCR) // When using official ROBOTIS board with DXL circuit. // For OpenCR, there is a DXL Power Enable pin, so you must initialize and control it. // Reference link : https://github.com/ROBOTIS-GIT/OpenCR/blob/master/arduino/opencr_arduino/opencr/libraries/DynamixelSDK/src/dynamixel_sdk/port_handler_arduino.cpp#L78 #define DXL_SERIAL Serial3 #define DEBUG_SERIAL Serial - const uint8_t DXL_DIR_PIN = 84; // OpenCR Board's DIR PIN. + const int DXL_DIR_PIN = 84; // OpenCR Board's DIR PIN. +#elif defined(ARDUINO_OpenRB) // When using OpenRB-150 + //OpenRB does not require the DIR control pin. + #define DXL_SERIAL Serial1 + #define DEBUG_SERIAL Serial + const int DXL_DIR_PIN = -1; #else // Other boards when using DynamixelShield #define DXL_SERIAL Serial1 #define DEBUG_SERIAL Serial - const uint8_t DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN + const int DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN #endif #define MAX_BAUD 5 diff --git a/examples/basic/current_mode/current_mode.ino b/examples/basic/current_mode/current_mode.ino index 7aac140..be22626 100644 --- a/examples/basic/current_mode/current_mode.ino +++ b/examples/basic/current_mode/current_mode.ino @@ -1,103 +1,108 @@ -/******************************************************************************* -* 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 - - -const uint8_t DXL_ID = 1; -const float DXL_PROTOCOL_VERSION = 2.0; - -Dynamixel2Arduino dxl(DXL_SERIAL, DXL_DIR_PIN); - -//This namespace is required to use Control table item names -using namespace ControlTableItem; - -void setup() { - // put your setup code here, to run once: - - // Use UART port of DYNAMIXEL Shield to debug. - DEBUG_SERIAL.begin(115200); - - // 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); - // Get DYNAMIXEL information - dxl.ping(DXL_ID); - - // Turn off torque when configuring items in EEPROM area - dxl.torqueOff(DXL_ID); - dxl.setOperatingMode(DXL_ID, OP_CURRENT); - dxl.torqueOn(DXL_ID); -} - -void loop() { - // put your main code here, to run repeatedly: - - // Please refer to e-Manual(http://emanual.robotis.com/docs/en/parts/interface/dynamixel_shield/) for available range of value. - // Set Goal Current using RAW unit - dxl.setGoalCurrent(DXL_ID, 200); - delay(1000); - // Print present current - DEBUG_SERIAL.print("Present Current(raw) : "); - DEBUG_SERIAL.println(dxl.getPresentCurrent(DXL_ID)); - delay(1000); - - // Set Goal Current using mA unit - dxl.setGoalCurrent(DXL_ID, 25.8, UNIT_MILLI_AMPERE); - delay(1000); - DEBUG_SERIAL.print("Present Current(mA) : "); - DEBUG_SERIAL.println(dxl.getPresentCurrent(DXL_ID, UNIT_MILLI_AMPERE)); - delay(1000); - - // Set Goal Current using percentage (-100.0 [%] ~ 100.0[%]) - dxl.setGoalCurrent(DXL_ID, -10.2, UNIT_PERCENT); - delay(1000); - DEBUG_SERIAL.print("Present Current(ratio) : "); - DEBUG_SERIAL.println(dxl.getPresentCurrent(DXL_ID, UNIT_PERCENT)); - delay(1000); -} +/******************************************************************************* +* Copyright 2016 ROBOTIS CO., LTD. +* +* Licensed under the Apache License, Version 2.0 (the "License"); +* you may not use this file except in compliance with the License. +* You may obtain a copy of the License at +* +* http://www.apache.org/licenses/LICENSE-2.0 +* +* Unless required by applicable law or agreed to in writing, software +* distributed under the License is distributed on an "AS IS" BASIS, +* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +* See the License for the specific language governing permissions and +* limitations under the License. +*******************************************************************************/ + +#include + +// Please modify it to suit your hardware. +#if defined(ARDUINO_AVR_UNO) || defined(ARDUINO_AVR_MEGA2560) // When using DynamixelShield + #include + SoftwareSerial soft_serial(7, 8); // DYNAMIXELShield UART RX/TX + #define DXL_SERIAL Serial + #define DEBUG_SERIAL soft_serial + const int DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN +#elif defined(ARDUINO_SAM_DUE) // When using DynamixelShield + #define DXL_SERIAL Serial + #define DEBUG_SERIAL SerialUSB + const int DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN +#elif defined(ARDUINO_SAM_ZERO) // When using DynamixelShield + #define DXL_SERIAL Serial1 + #define DEBUG_SERIAL SerialUSB + const int DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN +#elif defined(ARDUINO_OpenCM904) // When using official ROBOTIS board with DXL circuit. + #define DXL_SERIAL Serial3 //OpenCM9.04 EXP Board's DXL port Serial. (Serial1 for the DXL port on the OpenCM 9.04 board) + #define DEBUG_SERIAL Serial + const int DXL_DIR_PIN = 22; //OpenCM9.04 EXP Board's DIR PIN. (28 for the DXL port on the OpenCM 9.04 board) +#elif defined(ARDUINO_OpenCR) // When using official ROBOTIS board with DXL circuit. + // For OpenCR, there is a DXL Power Enable pin, so you must initialize and control it. + // Reference link : https://github.com/ROBOTIS-GIT/OpenCR/blob/master/arduino/opencr_arduino/opencr/libraries/DynamixelSDK/src/dynamixel_sdk/port_handler_arduino.cpp#L78 + #define DXL_SERIAL Serial3 + #define DEBUG_SERIAL Serial + const int DXL_DIR_PIN = 84; // OpenCR Board's DIR PIN. +#elif defined(ARDUINO_OpenRB) // When using OpenRB-150 + //OpenRB does not require the DIR control pin. + #define DXL_SERIAL Serial1 + #define DEBUG_SERIAL Serial + const int DXL_DIR_PIN = -1; +#else // Other boards when using DynamixelShield + #define DXL_SERIAL Serial1 + #define DEBUG_SERIAL Serial + const int DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN +#endif + + +const uint8_t DXL_ID = 1; +const float DXL_PROTOCOL_VERSION = 2.0; + +Dynamixel2Arduino dxl(DXL_SERIAL, DXL_DIR_PIN); + +//This namespace is required to use Control table item names +using namespace ControlTableItem; + +void setup() { + // put your setup code here, to run once: + + // Use UART port of DYNAMIXEL Shield to debug. + DEBUG_SERIAL.begin(115200); + + // 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); + // Get DYNAMIXEL information + dxl.ping(DXL_ID); + + // Turn off torque when configuring items in EEPROM area + dxl.torqueOff(DXL_ID); + dxl.setOperatingMode(DXL_ID, OP_CURRENT); + dxl.torqueOn(DXL_ID); +} + +void loop() { + // put your main code here, to run repeatedly: + + // Please refer to e-Manual(http://emanual.robotis.com/docs/en/parts/interface/dynamixel_shield/) for available range of value. + // Set Goal Current using RAW unit + dxl.setGoalCurrent(DXL_ID, 200); + delay(1000); + // Print present current + DEBUG_SERIAL.print("Present Current(raw) : "); + DEBUG_SERIAL.println(dxl.getPresentCurrent(DXL_ID)); + delay(1000); + + // Set Goal Current using mA unit + dxl.setGoalCurrent(DXL_ID, 25.8, UNIT_MILLI_AMPERE); + delay(1000); + DEBUG_SERIAL.print("Present Current(mA) : "); + DEBUG_SERIAL.println(dxl.getPresentCurrent(DXL_ID, UNIT_MILLI_AMPERE)); + delay(1000); + + // Set Goal Current using percentage (-100.0 [%] ~ 100.0[%]) + dxl.setGoalCurrent(DXL_ID, -10.2, UNIT_PERCENT); + delay(1000); + DEBUG_SERIAL.print("Present Current(ratio) : "); + DEBUG_SERIAL.println(dxl.getPresentCurrent(DXL_ID, UNIT_PERCENT)); + delay(1000); +} diff --git a/examples/basic/current_position_mode/current_position_mode.ino b/examples/basic/current_position_mode/current_position_mode.ino index afc4b7c..bae88c1 100644 --- a/examples/basic/current_position_mode/current_position_mode.ino +++ b/examples/basic/current_position_mode/current_position_mode.ino @@ -1,106 +1,111 @@ -/******************************************************************************* -* 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. -*******************************************************************************/ - -/* NOTE: Please check if your DYNAMIXEL supports Current-based Position Control mode -* Supported Products : MX-64(2.0), MX-106(2.0), All X series(except XL-320, XL430-W250) -*/ - -#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 - - -const uint8_t DXL_ID = 1; -const float DXL_PROTOCOL_VERSION = 2.0; - -Dynamixel2Arduino dxl(DXL_SERIAL, DXL_DIR_PIN); - -//This namespace is required to use Control table item names -using namespace ControlTableItem; - -void setup() { - // put your setup code here, to run once: - - // Use UART port of DYNAMIXEL Shield to debug. - DEBUG_SERIAL.begin(115200); - - // 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); - // Get DYNAMIXEL information - dxl.ping(DXL_ID); - - // Turn off torque when configuring items in EEPROM area - dxl.torqueOff(DXL_ID); - dxl.setOperatingMode(DXL_ID, OP_CURRENT_BASED_POSITION); - dxl.torqueOn(DXL_ID); -} - -void loop() { - // put your main code here, to run repeatedly: - - // Set Goal Current using RAW value - dxl.setGoalCurrent(DXL_ID, 200); - dxl.setGoalPosition(DXL_ID, 350.0, UNIT_DEGREE); - - // Print present current - delay(500); - DEBUG_SERIAL.print("Present Current(raw) : "); - DEBUG_SERIAL.println(dxl.getPresentCurrent(DXL_ID)); - - delay(5000); - - // Set Goal Current 3.0% using percentage (-100.0 [%] ~ 100.0[%]) - dxl.setGoalCurrent(DXL_ID, 3.0, UNIT_PERCENT); - dxl.setGoalPosition(DXL_ID, 0); - - // Print present current in percentage - delay(500); - DEBUG_SERIAL.print("Present Current(ratio) : "); - DEBUG_SERIAL.println(dxl.getPresentCurrent(DXL_ID, UNIT_PERCENT)); - - delay(5000); -} +/******************************************************************************* +* 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. +*******************************************************************************/ + +/* NOTE: Please check if your DYNAMIXEL supports Current-based Position Control mode +* Supported Products : MX-64(2.0), MX-106(2.0), All X series(except XL-320, XL430-W250) +*/ + +#include + +// Please modify it to suit your hardware. +#if defined(ARDUINO_AVR_UNO) || defined(ARDUINO_AVR_MEGA2560) // When using DynamixelShield + #include + SoftwareSerial soft_serial(7, 8); // DYNAMIXELShield UART RX/TX + #define DXL_SERIAL Serial + #define DEBUG_SERIAL soft_serial + const int DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN +#elif defined(ARDUINO_SAM_DUE) // When using DynamixelShield + #define DXL_SERIAL Serial + #define DEBUG_SERIAL SerialUSB + const int DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN +#elif defined(ARDUINO_SAM_ZERO) // When using DynamixelShield + #define DXL_SERIAL Serial1 + #define DEBUG_SERIAL SerialUSB + const int DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN +#elif defined(ARDUINO_OpenCM904) // When using official ROBOTIS board with DXL circuit. + #define DXL_SERIAL Serial3 //OpenCM9.04 EXP Board's DXL port Serial. (Serial1 for the DXL port on the OpenCM 9.04 board) + #define DEBUG_SERIAL Serial + const int DXL_DIR_PIN = 22; //OpenCM9.04 EXP Board's DIR PIN. (28 for the DXL port on the OpenCM 9.04 board) +#elif defined(ARDUINO_OpenCR) // When using official ROBOTIS board with DXL circuit. + // For OpenCR, there is a DXL Power Enable pin, so you must initialize and control it. + // Reference link : https://github.com/ROBOTIS-GIT/OpenCR/blob/master/arduino/opencr_arduino/opencr/libraries/DynamixelSDK/src/dynamixel_sdk/port_handler_arduino.cpp#L78 + #define DXL_SERIAL Serial3 + #define DEBUG_SERIAL Serial + const int DXL_DIR_PIN = 84; // OpenCR Board's DIR PIN. +#elif defined(ARDUINO_OpenRB) // When using OpenRB-150 + //OpenRB does not require the DIR control pin. + #define DXL_SERIAL Serial1 + #define DEBUG_SERIAL Serial + const int DXL_DIR_PIN = -1; +#else // Other boards when using DynamixelShield + #define DXL_SERIAL Serial1 + #define DEBUG_SERIAL Serial + const int DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN +#endif + + +const uint8_t DXL_ID = 1; +const float DXL_PROTOCOL_VERSION = 2.0; + +Dynamixel2Arduino dxl(DXL_SERIAL, DXL_DIR_PIN); + +//This namespace is required to use Control table item names +using namespace ControlTableItem; + +void setup() { + // put your setup code here, to run once: + + // Use UART port of DYNAMIXEL Shield to debug. + DEBUG_SERIAL.begin(115200); + + // 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); + // Get DYNAMIXEL information + dxl.ping(DXL_ID); + + // Turn off torque when configuring items in EEPROM area + dxl.torqueOff(DXL_ID); + dxl.setOperatingMode(DXL_ID, OP_CURRENT_BASED_POSITION); + dxl.torqueOn(DXL_ID); +} + +void loop() { + // put your main code here, to run repeatedly: + + // Set Goal Current using RAW value + dxl.setGoalCurrent(DXL_ID, 200); + dxl.setGoalPosition(DXL_ID, 350.0, UNIT_DEGREE); + + // Print present current + delay(500); + DEBUG_SERIAL.print("Present Current(raw) : "); + DEBUG_SERIAL.println(dxl.getPresentCurrent(DXL_ID)); + + delay(5000); + + // Set Goal Current 3.0% using percentage (-100.0 [%] ~ 100.0[%]) + dxl.setGoalCurrent(DXL_ID, 3.0, UNIT_PERCENT); + dxl.setGoalPosition(DXL_ID, 0); + + // Print present current in percentage + delay(500); + DEBUG_SERIAL.print("Present Current(ratio) : "); + DEBUG_SERIAL.println(dxl.getPresentCurrent(DXL_ID, UNIT_PERCENT)); + + delay(5000); +} diff --git a/examples/basic/id/id.ino b/examples/basic/id/id.ino index 824b4d3..035812d 100644 --- a/examples/basic/id/id.ino +++ b/examples/basic/id/id.ino @@ -1,114 +1,119 @@ -/******************************************************************************* -* 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 - - -const uint8_t DEFAULT_DXL_ID = 1; -const float DXL_PROTOCOL_VERSION = 2.0; - -Dynamixel2Arduino dxl(DXL_SERIAL, DXL_DIR_PIN); - -//This namespace is required to use Control table item names -using namespace ControlTableItem; - -void setup() { - // put your setup code here, to run once: - 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); - // Set Port Protocol Version. This has to match with DYNAMIXEL protocol version. - dxl.setPortProtocolVersion(DXL_PROTOCOL_VERSION); - - DEBUG_SERIAL.print("PROTOCOL "); - DEBUG_SERIAL.print(DXL_PROTOCOL_VERSION, 1); - DEBUG_SERIAL.print(", ID "); - DEBUG_SERIAL.print(present_id); - DEBUG_SERIAL.print(": "); - if(dxl.ping(present_id) == true) { - DEBUG_SERIAL.print("ping succeeded!"); - DEBUG_SERIAL.print(", Model Number: "); - DEBUG_SERIAL.println(dxl.getModelNumber(present_id)); - - // Turn off torque when configuring items in EEPROM area - dxl.torqueOff(present_id); - - // set a new ID for DYNAMIXEL. Do not use ID 200 - 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); - - 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!"); - } -} - -void loop() { - // put your main code here, to run repeatedly: +/******************************************************************************* +* Copyright 2016 ROBOTIS CO., LTD. +* +* Licensed under the Apache License, Version 2.0 (the "License"); +* you may not use this file except in compliance with the License. +* You may obtain a copy of the License at +* +* http://www.apache.org/licenses/LICENSE-2.0 +* +* Unless required by applicable law or agreed to in writing, software +* distributed under the License is distributed on an "AS IS" BASIS, +* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +* See the License for the specific language governing permissions and +* limitations under the License. +*******************************************************************************/ + +#include + +// Please modify it to suit your hardware. +#if defined(ARDUINO_AVR_UNO) || defined(ARDUINO_AVR_MEGA2560) // When using DynamixelShield + #include + SoftwareSerial soft_serial(7, 8); // DYNAMIXELShield UART RX/TX + #define DXL_SERIAL Serial + #define DEBUG_SERIAL soft_serial + const int DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN +#elif defined(ARDUINO_SAM_DUE) // When using DynamixelShield + #define DXL_SERIAL Serial + #define DEBUG_SERIAL SerialUSB + const int DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN +#elif defined(ARDUINO_SAM_ZERO) // When using DynamixelShield + #define DXL_SERIAL Serial1 + #define DEBUG_SERIAL SerialUSB + const int DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN +#elif defined(ARDUINO_OpenCM904) // When using official ROBOTIS board with DXL circuit. + #define DXL_SERIAL Serial3 //OpenCM9.04 EXP Board's DXL port Serial. (Serial1 for the DXL port on the OpenCM 9.04 board) + #define DEBUG_SERIAL Serial + const int DXL_DIR_PIN = 22; //OpenCM9.04 EXP Board's DIR PIN. (28 for the DXL port on the OpenCM 9.04 board) +#elif defined(ARDUINO_OpenCR) // When using official ROBOTIS board with DXL circuit. + // For OpenCR, there is a DXL Power Enable pin, so you must initialize and control it. + // Reference link : https://github.com/ROBOTIS-GIT/OpenCR/blob/master/arduino/opencr_arduino/opencr/libraries/DynamixelSDK/src/dynamixel_sdk/port_handler_arduino.cpp#L78 + #define DXL_SERIAL Serial3 + #define DEBUG_SERIAL Serial + const int DXL_DIR_PIN = 84; // OpenCR Board's DIR PIN. +#elif defined(ARDUINO_OpenRB) // When using OpenRB-150 + //OpenRB does not require the DIR control pin. + #define DXL_SERIAL Serial1 + #define DEBUG_SERIAL Serial + const int DXL_DIR_PIN = -1; +#else // Other boards when using DynamixelShield + #define DXL_SERIAL Serial1 + #define DEBUG_SERIAL Serial + const int DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN +#endif + + +const uint8_t DEFAULT_DXL_ID = 1; +const float DXL_PROTOCOL_VERSION = 2.0; + +Dynamixel2Arduino dxl(DXL_SERIAL, DXL_DIR_PIN); + +//This namespace is required to use Control table item names +using namespace ControlTableItem; + +void setup() { + // put your setup code here, to run once: + 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); + // Set Port Protocol Version. This has to match with DYNAMIXEL protocol version. + dxl.setPortProtocolVersion(DXL_PROTOCOL_VERSION); + + DEBUG_SERIAL.print("PROTOCOL "); + DEBUG_SERIAL.print(DXL_PROTOCOL_VERSION, 1); + DEBUG_SERIAL.print(", ID "); + DEBUG_SERIAL.print(present_id); + DEBUG_SERIAL.print(": "); + if(dxl.ping(present_id) == true) { + DEBUG_SERIAL.print("ping succeeded!"); + DEBUG_SERIAL.print(", Model Number: "); + DEBUG_SERIAL.println(dxl.getModelNumber(present_id)); + + // Turn off torque when configuring items in EEPROM area + dxl.torqueOff(present_id); + + // set a new ID for DYNAMIXEL. Do not use ID 200 + 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); + + 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!"); + } +} + +void loop() { + // put your main code here, to run repeatedly: } \ No newline at end of file diff --git a/examples/basic/led/led.ino b/examples/basic/led/led.ino index 9680be2..3605c99 100644 --- a/examples/basic/led/led.ino +++ b/examples/basic/led/led.ino @@ -1,79 +1,84 @@ -/******************************************************************************* -* 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 - - -const uint8_t DXL_ID = 1; -const float DXL_PROTOCOL_VERSION = 2.0; - -Dynamixel2Arduino dxl(DXL_SERIAL, DXL_DIR_PIN); - -//This namespace is required to use Control table item names -using namespace ControlTableItem; - -void setup() { - // put your setup code here, to run once: - - // 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); - // Get DYNAMIXEL information - dxl.ping(DXL_ID); -} - -void loop() { - // put your main code here, to run repeatedly: - - // Turn on the LED on DYNAMIXEL - dxl.ledOn(DXL_ID); - delay(500); - // Turn off the LED on DYNAMIXEL - dxl.ledOff(DXL_ID); - delay(500); -} +/******************************************************************************* +* Copyright 2016 ROBOTIS CO., LTD. +* +* Licensed under the Apache License, Version 2.0 (the "License"); +* you may not use this file except in compliance with the License. +* You may obtain a copy of the License at +* +* http://www.apache.org/licenses/LICENSE-2.0 +* +* Unless required by applicable law or agreed to in writing, software +* distributed under the License is distributed on an "AS IS" BASIS, +* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +* See the License for the specific language governing permissions and +* limitations under the License. +*******************************************************************************/ + +#include + +// Please modify it to suit your hardware. +#if defined(ARDUINO_AVR_UNO) || defined(ARDUINO_AVR_MEGA2560) // When using DynamixelShield + #include + SoftwareSerial soft_serial(7, 8); // DYNAMIXELShield UART RX/TX + #define DXL_SERIAL Serial + #define DEBUG_SERIAL soft_serial + const int DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN +#elif defined(ARDUINO_SAM_DUE) // When using DynamixelShield + #define DXL_SERIAL Serial + #define DEBUG_SERIAL SerialUSB + const int DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN +#elif defined(ARDUINO_SAM_ZERO) // When using DynamixelShield + #define DXL_SERIAL Serial1 + #define DEBUG_SERIAL SerialUSB + const int DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN +#elif defined(ARDUINO_OpenCM904) // When using official ROBOTIS board with DXL circuit. + #define DXL_SERIAL Serial3 //OpenCM9.04 EXP Board's DXL port Serial. (Serial1 for the DXL port on the OpenCM 9.04 board) + #define DEBUG_SERIAL Serial + const int DXL_DIR_PIN = 22; //OpenCM9.04 EXP Board's DIR PIN. (28 for the DXL port on the OpenCM 9.04 board) +#elif defined(ARDUINO_OpenCR) // When using official ROBOTIS board with DXL circuit. + // For OpenCR, there is a DXL Power Enable pin, so you must initialize and control it. + // Reference link : https://github.com/ROBOTIS-GIT/OpenCR/blob/master/arduino/opencr_arduino/opencr/libraries/DynamixelSDK/src/dynamixel_sdk/port_handler_arduino.cpp#L78 + #define DXL_SERIAL Serial3 + #define DEBUG_SERIAL Serial + const int DXL_DIR_PIN = 84; // OpenCR Board's DIR PIN. +#elif defined(ARDUINO_OpenRB) // When using OpenRB-150 + //OpenRB does not require the DIR control pin. + #define DXL_SERIAL Serial1 + #define DEBUG_SERIAL Serial + const int DXL_DIR_PIN = -1; +#else // Other boards when using DynamixelShield + #define DXL_SERIAL Serial1 + #define DEBUG_SERIAL Serial + const int DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN +#endif + + +const uint8_t DXL_ID = 1; +const float DXL_PROTOCOL_VERSION = 2.0; + +Dynamixel2Arduino dxl(DXL_SERIAL, DXL_DIR_PIN); + +//This namespace is required to use Control table item names +using namespace ControlTableItem; + +void setup() { + // put your setup code here, to run once: + + // 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); + // Get DYNAMIXEL information + dxl.ping(DXL_ID); +} + +void loop() { + // put your main code here, to run repeatedly: + + // Turn on the LED on DYNAMIXEL + dxl.ledOn(DXL_ID); + delay(500); + // Turn off the LED on DYNAMIXEL + dxl.ledOff(DXL_ID); + delay(500); +} diff --git a/examples/basic/operation_mode/operation_mode.ino b/examples/basic/operation_mode/operation_mode.ino index f2e428f..02f4d2d 100644 --- a/examples/basic/operation_mode/operation_mode.ino +++ b/examples/basic/operation_mode/operation_mode.ino @@ -1,146 +1,151 @@ -/******************************************************************************* -* 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. -*******************************************************************************/ - -/** Please refer to each DYNAMIXEL eManual(http://emanual.robotis.com) for supported Operating Mode - * Operating Mode - * 1. OP_POSITION (Position Mode in protocol2.0, Joint Mode in protocol1.0) - * 2. OP_VELOCITY (Velocity Mode in protocol2.0, Speed Mode in protocol1.0) - * 3. OP_PWM (PWM Mode in protocol2.0) - * 4. OP_EXTENDED_POSITION (Extended Position Mode in protocol2.0, Multi-turn Mode(only MX series) in protocol1.0) - * 5. OP_CURRENT (Current Mode in protocol2.0, Torque Mode(only MX64,MX106) in protocol1.0) - * 6. OP_CURRENT_BASED_POSITION (Current Based Postion Mode in protocol2.0 (except MX28, XL430)) - */ - -#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 - - -const uint8_t DXL_ID = 1; -const float DXL_PROTOCOL_VERSION = 2.0; - -Dynamixel2Arduino dxl(DXL_SERIAL, DXL_DIR_PIN); - -//This namespace is required to use Control table item names -using namespace ControlTableItem; - -void setup() { - // put your setup code here, to run once: - - // Use Serial to debug. - DEBUG_SERIAL.begin(115200); - - // 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); - // Get DYNAMIXEL information - dxl.ping(DXL_ID); -} - -void loop() { - // put your main code here, to run repeatedly: - - // Position Control Mode in protocol2.0, Joint Mode in protocol1.0 - // Turn off torquen when configuring items in EEPROM area - dxl.torqueOff(DXL_ID); - dxl.setOperatingMode(DXL_ID, OP_POSITION); - dxl.torqueOn(DXL_ID); - if(dxl.setGoalPosition(DXL_ID, 512)){ - delay(1000); - DEBUG_SERIAL.print("Present Position : "); - DEBUG_SERIAL.println(dxl.getPresentPosition(DXL_ID)); DEBUG_SERIAL.println(); - } - - // Velocity Contorl Mode in protocol2.0, Speed Mode in protocol1.0 - dxl.torqueOff(DXL_ID); - dxl.setOperatingMode(DXL_ID, OP_VELOCITY); - dxl.torqueOn(DXL_ID); - if(dxl.setGoalVelocity(DXL_ID, 128)){ - delay(1000); - DEBUG_SERIAL.print("Present Velocity : "); - DEBUG_SERIAL.println(dxl.getPresentVelocity(DXL_ID)); DEBUG_SERIAL.println(); - } - - // Extended Position Contorl Mode in protocol2.0, Multi-turn Mode(only MX series) in protocol1.0 - dxl.torqueOff(DXL_ID); - dxl.setOperatingMode(DXL_ID, OP_EXTENDED_POSITION); - dxl.torqueOn(DXL_ID); - if(dxl.setGoalPosition(DXL_ID, 4096)){ - delay(1000); - DEBUG_SERIAL.print("Present Extended Position : "); - DEBUG_SERIAL.println(dxl.getPresentPosition(DXL_ID)); DEBUG_SERIAL.println(); - } - - // PWM Contorl Mode in protocol2.0 - dxl.torqueOff(DXL_ID); - dxl.setOperatingMode(DXL_ID, OP_PWM); - dxl.torqueOn(DXL_ID); - if(dxl.setGoalPWM(DXL_ID, 200)){ - delay(1000); - DEBUG_SERIAL.print("Present PWM : "); - DEBUG_SERIAL.println(dxl.getPresentPWM(DXL_ID)); DEBUG_SERIAL.println(); - } - - // Current Contorl Mode in protocol2.0, Torque Mode(only MX64,MX106) in protocol1.0 - dxl.torqueOff(DXL_ID); - dxl.setOperatingMode(DXL_ID, OP_CURRENT); - dxl.torqueOn(DXL_ID); - if(dxl.setGoalCurrent(DXL_ID, 256)){ - delay(1000); - DEBUG_SERIAL.print("Present Current : "); - DEBUG_SERIAL.println(dxl.getPresentCurrent(DXL_ID)); DEBUG_SERIAL.println(); - } - - // Current Based Postion Contorl Mode in protocol2.0 (except MX28, XL430) - dxl.torqueOff(DXL_ID); - dxl.setOperatingMode(DXL_ID, OP_CURRENT_BASED_POSITION); - dxl.torqueOn(DXL_ID); - if(dxl.setGoalPosition(DXL_ID, 8192)){ - delay(1000); - DEBUG_SERIAL.print("Present Current Based Position : "); - DEBUG_SERIAL.println(dxl.getPresentPosition(DXL_ID)); DEBUG_SERIAL.println(); - } -} +/******************************************************************************* +* 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. +*******************************************************************************/ + +/** Please refer to each DYNAMIXEL eManual(http://emanual.robotis.com) for supported Operating Mode + * Operating Mode + * 1. OP_POSITION (Position Mode in protocol2.0, Joint Mode in protocol1.0) + * 2. OP_VELOCITY (Velocity Mode in protocol2.0, Speed Mode in protocol1.0) + * 3. OP_PWM (PWM Mode in protocol2.0) + * 4. OP_EXTENDED_POSITION (Extended Position Mode in protocol2.0, Multi-turn Mode(only MX series) in protocol1.0) + * 5. OP_CURRENT (Current Mode in protocol2.0, Torque Mode(only MX64,MX106) in protocol1.0) + * 6. OP_CURRENT_BASED_POSITION (Current Based Postion Mode in protocol2.0 (except MX28, XL430)) + */ + +#include + +// Please modify it to suit your hardware. +#if defined(ARDUINO_AVR_UNO) || defined(ARDUINO_AVR_MEGA2560) // When using DynamixelShield + #include + SoftwareSerial soft_serial(7, 8); // DYNAMIXELShield UART RX/TX + #define DXL_SERIAL Serial + #define DEBUG_SERIAL soft_serial + const int DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN +#elif defined(ARDUINO_SAM_DUE) // When using DynamixelShield + #define DXL_SERIAL Serial + #define DEBUG_SERIAL SerialUSB + const int DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN +#elif defined(ARDUINO_SAM_ZERO) // When using DynamixelShield + #define DXL_SERIAL Serial1 + #define DEBUG_SERIAL SerialUSB + const int DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN +#elif defined(ARDUINO_OpenCM904) // When using official ROBOTIS board with DXL circuit. + #define DXL_SERIAL Serial3 //OpenCM9.04 EXP Board's DXL port Serial. (Serial1 for the DXL port on the OpenCM 9.04 board) + #define DEBUG_SERIAL Serial + const int DXL_DIR_PIN = 22; //OpenCM9.04 EXP Board's DIR PIN. (28 for the DXL port on the OpenCM 9.04 board) +#elif defined(ARDUINO_OpenCR) // When using official ROBOTIS board with DXL circuit. + // For OpenCR, there is a DXL Power Enable pin, so you must initialize and control it. + // Reference link : https://github.com/ROBOTIS-GIT/OpenCR/blob/master/arduino/opencr_arduino/opencr/libraries/DynamixelSDK/src/dynamixel_sdk/port_handler_arduino.cpp#L78 + #define DXL_SERIAL Serial3 + #define DEBUG_SERIAL Serial + const int DXL_DIR_PIN = 84; // OpenCR Board's DIR PIN. +#elif defined(ARDUINO_OpenRB) // When using OpenRB-150 + //OpenRB does not require the DIR control pin. + #define DXL_SERIAL Serial1 + #define DEBUG_SERIAL Serial + const int DXL_DIR_PIN = -1; +#else // Other boards when using DynamixelShield + #define DXL_SERIAL Serial1 + #define DEBUG_SERIAL Serial + const int DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN +#endif + + +const uint8_t DXL_ID = 1; +const float DXL_PROTOCOL_VERSION = 2.0; + +Dynamixel2Arduino dxl(DXL_SERIAL, DXL_DIR_PIN); + +//This namespace is required to use Control table item names +using namespace ControlTableItem; + +void setup() { + // put your setup code here, to run once: + + // Use Serial to debug. + DEBUG_SERIAL.begin(115200); + + // 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); + // Get DYNAMIXEL information + dxl.ping(DXL_ID); +} + +void loop() { + // put your main code here, to run repeatedly: + + // Position Control Mode in protocol2.0, Joint Mode in protocol1.0 + // Turn off torquen when configuring items in EEPROM area + dxl.torqueOff(DXL_ID); + dxl.setOperatingMode(DXL_ID, OP_POSITION); + dxl.torqueOn(DXL_ID); + if(dxl.setGoalPosition(DXL_ID, 512)){ + delay(1000); + DEBUG_SERIAL.print("Present Position : "); + DEBUG_SERIAL.println(dxl.getPresentPosition(DXL_ID)); DEBUG_SERIAL.println(); + } + + // Velocity Contorl Mode in protocol2.0, Speed Mode in protocol1.0 + dxl.torqueOff(DXL_ID); + dxl.setOperatingMode(DXL_ID, OP_VELOCITY); + dxl.torqueOn(DXL_ID); + if(dxl.setGoalVelocity(DXL_ID, 128)){ + delay(1000); + DEBUG_SERIAL.print("Present Velocity : "); + DEBUG_SERIAL.println(dxl.getPresentVelocity(DXL_ID)); DEBUG_SERIAL.println(); + } + + // Extended Position Contorl Mode in protocol2.0, Multi-turn Mode(only MX series) in protocol1.0 + dxl.torqueOff(DXL_ID); + dxl.setOperatingMode(DXL_ID, OP_EXTENDED_POSITION); + dxl.torqueOn(DXL_ID); + if(dxl.setGoalPosition(DXL_ID, 4096)){ + delay(1000); + DEBUG_SERIAL.print("Present Extended Position : "); + DEBUG_SERIAL.println(dxl.getPresentPosition(DXL_ID)); DEBUG_SERIAL.println(); + } + + // PWM Contorl Mode in protocol2.0 + dxl.torqueOff(DXL_ID); + dxl.setOperatingMode(DXL_ID, OP_PWM); + dxl.torqueOn(DXL_ID); + if(dxl.setGoalPWM(DXL_ID, 200)){ + delay(1000); + DEBUG_SERIAL.print("Present PWM : "); + DEBUG_SERIAL.println(dxl.getPresentPWM(DXL_ID)); DEBUG_SERIAL.println(); + } + + // Current Contorl Mode in protocol2.0, Torque Mode(only MX64,MX106) in protocol1.0 + dxl.torqueOff(DXL_ID); + dxl.setOperatingMode(DXL_ID, OP_CURRENT); + dxl.torqueOn(DXL_ID); + if(dxl.setGoalCurrent(DXL_ID, 256)){ + delay(1000); + DEBUG_SERIAL.print("Present Current : "); + DEBUG_SERIAL.println(dxl.getPresentCurrent(DXL_ID)); DEBUG_SERIAL.println(); + } + + // Current Based Postion Contorl Mode in protocol2.0 (except MX28, XL430) + dxl.torqueOff(DXL_ID); + dxl.setOperatingMode(DXL_ID, OP_CURRENT_BASED_POSITION); + dxl.torqueOn(DXL_ID); + if(dxl.setGoalPosition(DXL_ID, 8192)){ + delay(1000); + DEBUG_SERIAL.print("Present Current Based Position : "); + DEBUG_SERIAL.println(dxl.getPresentPosition(DXL_ID)); DEBUG_SERIAL.println(); + } +} diff --git a/examples/basic/ping/ping.ino b/examples/basic/ping/ping.ino index b481fc6..6c3292c 100644 --- a/examples/basic/ping/ping.ino +++ b/examples/basic/ping/ping.ino @@ -1,115 +1,120 @@ -/******************************************************************************* -* 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 - - -const uint8_t DXL_ID = 1; -const float DXL_PROTOCOL_VERSION = 2.0; - -Dynamixel2Arduino dxl(DXL_SERIAL, DXL_DIR_PIN); - -//This namespace is required to use Control table item names -using namespace ControlTableItem; - -void setup() { - // put your setup code here, to run once: - - // Use Serial to debug. - DEBUG_SERIAL.begin(115200); - - // 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.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, DEC); - //g_servo_protocol[i] = 2; - } - }else{ - Serial.print("Broadcast returned no items : "); - Serial.println(dxl.getLastLibErrCode()); - } +/******************************************************************************* +* Copyright 2016 ROBOTIS CO., LTD. +* +* Licensed under the Apache License, Version 2.0 (the "License"); +* you may not use this file except in compliance with the License. +* You may obtain a copy of the License at +* +* http://www.apache.org/licenses/LICENSE-2.0 +* +* Unless required by applicable law or agreed to in writing, software +* distributed under the License is distributed on an "AS IS" BASIS, +* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +* See the License for the specific language governing permissions and +* limitations under the License. +*******************************************************************************/ + +#include + +// Please modify it to suit your hardware. +#if defined(ARDUINO_AVR_UNO) || defined(ARDUINO_AVR_MEGA2560) // When using DynamixelShield + #include + SoftwareSerial soft_serial(7, 8); // DYNAMIXELShield UART RX/TX + #define DXL_SERIAL Serial + #define DEBUG_SERIAL soft_serial + const int DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN +#elif defined(ARDUINO_SAM_DUE) // When using DynamixelShield + #define DXL_SERIAL Serial + #define DEBUG_SERIAL SerialUSB + const int DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN +#elif defined(ARDUINO_SAM_ZERO) // When using DynamixelShield + #define DXL_SERIAL Serial1 + #define DEBUG_SERIAL SerialUSB + const int DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN +#elif defined(ARDUINO_OpenCM904) // When using official ROBOTIS board with DXL circuit. + #define DXL_SERIAL Serial3 //OpenCM9.04 EXP Board's DXL port Serial. (Serial1 for the DXL port on the OpenCM 9.04 board) + #define DEBUG_SERIAL Serial + const int DXL_DIR_PIN = 22; //OpenCM9.04 EXP Board's DIR PIN. (28 for the DXL port on the OpenCM 9.04 board) +#elif defined(ARDUINO_OpenCR) // When using official ROBOTIS board with DXL circuit. + // For OpenCR, there is a DXL Power Enable pin, so you must initialize and control it. + // Reference link : https://github.com/ROBOTIS-GIT/OpenCR/blob/master/arduino/opencr_arduino/opencr/libraries/DynamixelSDK/src/dynamixel_sdk/port_handler_arduino.cpp#L78 + #define DXL_SERIAL Serial3 + #define DEBUG_SERIAL Serial + const int DXL_DIR_PIN = 84; // OpenCR Board's DIR PIN. +#elif defined(ARDUINO_OpenRB) // When using OpenRB-150 + //OpenRB does not require the DIR control pin. + #define DXL_SERIAL Serial1 + #define DEBUG_SERIAL Serial + const int DXL_DIR_PIN = -1; +#else // Other boards when using DynamixelShield + #define DXL_SERIAL Serial1 + #define DEBUG_SERIAL Serial + const int DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN +#endif + + +const uint8_t DXL_ID = 1; +const float DXL_PROTOCOL_VERSION = 2.0; + +Dynamixel2Arduino dxl(DXL_SERIAL, DXL_DIR_PIN); + +//This namespace is required to use Control table item names +using namespace ControlTableItem; + +void setup() { + // put your setup code here, to run once: + + // Use Serial to debug. + DEBUG_SERIAL.begin(115200); + + // 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.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, DEC); + //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 c274831..1498a29 100644 --- a/examples/basic/position_mode/position_mode.ino +++ b/examples/basic/position_mode/position_mode.ino @@ -1,112 +1,115 @@ -/******************************************************************************* -* 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 - - -const uint8_t DXL_ID = 1; -const float DXL_PROTOCOL_VERSION = 2.0; - -Dynamixel2Arduino dxl(DXL_SERIAL, DXL_DIR_PIN); - -//This namespace is required to use Control table item names -using namespace ControlTableItem; - -void setup() { - // put your setup code here, to run once: - - // Use 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); - // Set Port Protocol Version. This has to match with DYNAMIXEL protocol version. - dxl.setPortProtocolVersion(DXL_PROTOCOL_VERSION); - // Get DYNAMIXEL information - dxl.ping(DXL_ID); - - // Turn off torque when configuring items in EEPROM area - dxl.torqueOff(DXL_ID); - dxl.setOperatingMode(DXL_ID, OP_POSITION); - dxl.torqueOn(DXL_ID); - - // Limit the maximum velocity in Position Control Mode. Use 0 for Max speed - dxl.writeControlTableItem(PROFILE_VELOCITY, DXL_ID, 30); -} - -void loop() { - // put your main code here, to run repeatedly: - - // Please refer to e-Manual(http://emanual.robotis.com/docs/en/parts/interface/dynamixel_shield/) for available range of value. - // Set Goal Position in RAW value - dxl.setGoalPosition(DXL_ID, 512); - - int i_present_position = 0; - float f_present_position = 0.0; - - while (abs(512 - i_present_position) > 10) - { - f_present_position = dxl.getPresentPosition(DXL_ID, UNIT_DEGREE); - i_present_position = dxl.getPresentPosition(DXL_ID); - DEBUG_SERIAL.print("Present_Position(raw) : "); - DEBUG_SERIAL.println(i_present_position); - } - delay(500); - - // Set Goal Position in DEGREE value - dxl.setGoalPosition(DXL_ID, 5.7, UNIT_DEGREE); - - while (abs(5.7 - f_present_position) > 2.0) - { - f_present_position = dxl.getPresentPosition(DXL_ID, UNIT_DEGREE); - i_present_position = dxl.getPresentPosition(DXL_ID); - DEBUG_SERIAL.print("Present_Position(raw) : "); - DEBUG_SERIAL.println(i_present_position); - } - delay(500); -} +/******************************************************************************* +* Copyright 2016 ROBOTIS CO., LTD. +* +* Licensed under the Apache License, Version 2.0 (the "License"); +* you may not use this file except in compliance with the License. +* You may obtain a copy of the License at +* +* http://www.apache.org/licenses/LICENSE-2.0 +* +* Unless required by applicable law or agreed to in writing, software +* distributed under the License is distributed on an "AS IS" BASIS, +* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +* See the License for the specific language governing permissions and +* limitations under the License. +*******************************************************************************/ + +#include + +// Please modify it to suit your hardware. +#if defined(ARDUINO_AVR_UNO) || defined(ARDUINO_AVR_MEGA2560) // When using DynamixelShield + #include + SoftwareSerial soft_serial(7, 8); // DYNAMIXELShield UART RX/TX + #define DXL_SERIAL Serial + #define DEBUG_SERIAL soft_serial + const int DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN +#elif defined(ARDUINO_SAM_DUE) // When using DynamixelShield + #define DXL_SERIAL Serial + #define DEBUG_SERIAL SerialUSB + const int DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN +#elif defined(ARDUINO_SAM_ZERO) // When using DynamixelShield + #define DXL_SERIAL Serial1 + #define DEBUG_SERIAL SerialUSB + const int DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN +#elif defined(ARDUINO_OpenCM904) // When using official ROBOTIS board with DXL circuit. + #define DXL_SERIAL Serial3 //OpenCM9.04 EXP Board's DXL port Serial. (Serial1 for the DXL port on the OpenCM 9.04 board) + #define DEBUG_SERIAL Serial + const int DXL_DIR_PIN = 22; //OpenCM9.04 EXP Board's DIR PIN. (28 for the DXL port on the OpenCM 9.04 board) +#elif defined(ARDUINO_OpenCR) // When using official ROBOTIS board with DXL circuit. + // For OpenCR, there is a DXL Power Enable pin, so you must initialize and control it. + // Reference link : https://github.com/ROBOTIS-GIT/OpenCR/blob/master/arduino/opencr_arduino/opencr/libraries/DynamixelSDK/src/dynamixel_sdk/port_handler_arduino.cpp#L78 + #define DXL_SERIAL Serial3 + #define DEBUG_SERIAL Serial + const int DXL_DIR_PIN = 84; // OpenCR Board's DIR PIN. +#elif defined(ARDUINO_OpenRB) // When using OpenRB-150 + //OpenRB does not require the DIR control pin. + #define DXL_SERIAL Serial1 + #define DEBUG_SERIAL Serial + const int DXL_DIR_PIN = -1; +#else // Other boards when using DynamixelShield + #define DXL_SERIAL Serial1 + #define DEBUG_SERIAL Serial + const int DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN +#endif + + +const uint8_t DXL_ID = 1; +const float DXL_PROTOCOL_VERSION = 2.0; + +Dynamixel2Arduino dxl(DXL_SERIAL, DXL_DIR_PIN); + +//This namespace is required to use Control table item names +using namespace ControlTableItem; + +void setup() { + // put your setup code here, to run once: + + // Use 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); + // Set Port Protocol Version. This has to match with DYNAMIXEL protocol version. + dxl.setPortProtocolVersion(DXL_PROTOCOL_VERSION); + // Get DYNAMIXEL information + dxl.ping(DXL_ID); + + // Turn off torque when configuring items in EEPROM area + dxl.torqueOff(DXL_ID); + dxl.setOperatingMode(DXL_ID, OP_POSITION); + dxl.torqueOn(DXL_ID); + + // Limit the maximum velocity in Position Control Mode. Use 0 for Max speed + dxl.writeControlTableItem(PROFILE_VELOCITY, DXL_ID, 30); +} + +void loop() { + // put your main code here, to run repeatedly: + + // Please refer to e-Manual(http://emanual.robotis.com/docs/en/parts/interface/dynamixel_shield/) for available range of value. + // Set Goal Position in RAW value + dxl.setGoalPosition(DXL_ID, 1000); + + int i_present_position = 0; + float f_present_position = 0.0; + + while (abs(1000 - i_present_position) > 10) + { + i_present_position = dxl.getPresentPosition(DXL_ID); + DEBUG_SERIAL.print("Present_Position(raw) : "); + DEBUG_SERIAL.println(i_present_position); + } + delay(1000); + + // Set Goal Position in DEGREE value + dxl.setGoalPosition(DXL_ID, 5.7, UNIT_DEGREE); + + while (abs(5.7 - f_present_position) > 2.0) + { + f_present_position = dxl.getPresentPosition(DXL_ID, UNIT_DEGREE); + DEBUG_SERIAL.print("Present_Position(degree) : "); + DEBUG_SERIAL.println(f_present_position); + } + delay(1000); +} diff --git a/examples/basic/profile_velocity_acceleration/profile_velocity_acceleration.ino b/examples/basic/profile_velocity_acceleration/profile_velocity_acceleration.ino index 968e3f9..e3c3a4f 100644 --- a/examples/basic/profile_velocity_acceleration/profile_velocity_acceleration.ino +++ b/examples/basic/profile_velocity_acceleration/profile_velocity_acceleration.ino @@ -1,135 +1,140 @@ -/******************************************************************************* -* 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. -*******************************************************************************/ - -/* NOTE: Please check if your DYNAMIXEL supports Profile Control -* Supported Products : MX-28(2.0), MX-64(2.0), MX-106(2.0), All X series(except XL-320) -* PRO(A) series / PRO+ series -*/ - -#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 - - -const uint8_t DXL_ID = 1; -const float DXL_PROTOCOL_VERSION = 2.0; - -Dynamixel2Arduino dxl(DXL_SERIAL, DXL_DIR_PIN); - -//This namespace is required to use Control table item names -using namespace ControlTableItem; - -void setup() { - // put your setup code here, to run once: - - // Use UART port of DYNAMIXEL Shield to debug. - DEBUG_SERIAL.begin(115200); - - // 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); - // Get DYNAMIXEL information - dxl.ping(DXL_ID); - - // Turn on the LED on DYNAMIXEL - dxl.ledOn(DXL_ID); - delay(500); - // Turn off the LED on DYNAMIXEL - dxl.ledOff(DXL_ID); - delay(500); - - // Turn off torque when configuring items in EEPROM area - dxl.torqueOff(DXL_ID); - dxl.setOperatingMode(DXL_ID, OP_POSITION); - dxl.torqueOn(DXL_ID); -} - -void loop() { - // put your main code here, to run repeatedly: - uint8_t key_input; - DEBUG_SERIAL.println("Select Profile Type :"); - DEBUG_SERIAL.println("[1] Low Accel / High Vel"); - DEBUG_SERIAL.println("[2] Max Accel / Low Vel"); - DEBUG_SERIAL.println("[3] Max Accel / Max Vel"); - - while(DEBUG_SERIAL.available()==0); - key_input = DEBUG_SERIAL.read(); - - switch(key_input) { - case '1': - // Low Profile Acceleration, High Profile Velocity - // Refer to API documentation for available parameters - // http://emanual.robotis.com/docs/en/parts/interface/dynamixel_shield/#dynamixelshieldv010-or-above - dxl.writeControlTableItem(PROFILE_ACCELERATION, DXL_ID, 50); - dxl.writeControlTableItem(PROFILE_VELOCITY, DXL_ID, 300); - break; - case '2': - // Max Profile Acceleration, Low Profile Velocity - dxl.writeControlTableItem(PROFILE_ACCELERATION, DXL_ID, 0); - dxl.writeControlTableItem(PROFILE_VELOCITY, DXL_ID, 100); - break; - case '3': - // Max Profile Acceleration, Max Profile Velocity - // WARNING : Please BE AWARE that this option will make a vibrant motion for PRO(A) or PRO+ series that requires high current supply. - dxl.writeControlTableItem(PROFILE_ACCELERATION, DXL_ID, 0); - dxl.writeControlTableItem(PROFILE_VELOCITY, DXL_ID, 0); - break; - default: - break; - } - - // Set Goal Position in RAW value - dxl.setGoalPosition(DXL_ID, 0); - delay(500); - // Check if DYNAMIXEL is in motion - while(dxl.readControlTableItem(MOVING, DXL_ID)); - - // Set Goal Position in angle(degree) - dxl.setGoalPosition(DXL_ID, 179.0, UNIT_DEGREE); - delay(500); - // Check if DYNAMIXEL is in motion - while(dxl.readControlTableItem(MOVING, DXL_ID)); -} +/******************************************************************************* +* 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. +*******************************************************************************/ + +/* NOTE: Please check if your DYNAMIXEL supports Profile Control +* Supported Products : MX-28(2.0), MX-64(2.0), MX-106(2.0), All X series(except XL-320) +* PRO(A) series / PRO+ series +*/ + +#include + +// Please modify it to suit your hardware. +#if defined(ARDUINO_AVR_UNO) || defined(ARDUINO_AVR_MEGA2560) // When using DynamixelShield + #include + SoftwareSerial soft_serial(7, 8); // DYNAMIXELShield UART RX/TX + #define DXL_SERIAL Serial + #define DEBUG_SERIAL soft_serial + const int DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN +#elif defined(ARDUINO_SAM_DUE) // When using DynamixelShield + #define DXL_SERIAL Serial + #define DEBUG_SERIAL SerialUSB + const int DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN +#elif defined(ARDUINO_SAM_ZERO) // When using DynamixelShield + #define DXL_SERIAL Serial1 + #define DEBUG_SERIAL SerialUSB + const int DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN +#elif defined(ARDUINO_OpenCM904) // When using official ROBOTIS board with DXL circuit. + #define DXL_SERIAL Serial3 //OpenCM9.04 EXP Board's DXL port Serial. (Serial1 for the DXL port on the OpenCM 9.04 board) + #define DEBUG_SERIAL Serial + const int DXL_DIR_PIN = 22; //OpenCM9.04 EXP Board's DIR PIN. (28 for the DXL port on the OpenCM 9.04 board) +#elif defined(ARDUINO_OpenCR) // When using official ROBOTIS board with DXL circuit. + // For OpenCR, there is a DXL Power Enable pin, so you must initialize and control it. + // Reference link : https://github.com/ROBOTIS-GIT/OpenCR/blob/master/arduino/opencr_arduino/opencr/libraries/DynamixelSDK/src/dynamixel_sdk/port_handler_arduino.cpp#L78 + #define DXL_SERIAL Serial3 + #define DEBUG_SERIAL Serial + const int DXL_DIR_PIN = 84; // OpenCR Board's DIR PIN. +#elif defined(ARDUINO_OpenRB) // When using OpenRB-150 + //OpenRB does not require the DIR control pin. + #define DXL_SERIAL Serial1 + #define DEBUG_SERIAL Serial + const int DXL_DIR_PIN = -1; +#else // Other boards when using DynamixelShield + #define DXL_SERIAL Serial1 + #define DEBUG_SERIAL Serial + const int DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN +#endif + + +const uint8_t DXL_ID = 1; +const float DXL_PROTOCOL_VERSION = 2.0; + +Dynamixel2Arduino dxl(DXL_SERIAL, DXL_DIR_PIN); + +//This namespace is required to use Control table item names +using namespace ControlTableItem; + +void setup() { + // put your setup code here, to run once: + + // Use UART port of DYNAMIXEL Shield to debug. + DEBUG_SERIAL.begin(115200); + + // 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); + // Get DYNAMIXEL information + dxl.ping(DXL_ID); + + // Turn on the LED on DYNAMIXEL + dxl.ledOn(DXL_ID); + delay(500); + // Turn off the LED on DYNAMIXEL + dxl.ledOff(DXL_ID); + delay(500); + + // Turn off torque when configuring items in EEPROM area + dxl.torqueOff(DXL_ID); + dxl.setOperatingMode(DXL_ID, OP_POSITION); + dxl.torqueOn(DXL_ID); +} + +void loop() { + // put your main code here, to run repeatedly: + uint8_t key_input; + DEBUG_SERIAL.println("Select Profile Type :"); + DEBUG_SERIAL.println("[1] Low Accel / High Vel"); + DEBUG_SERIAL.println("[2] Max Accel / Low Vel"); + DEBUG_SERIAL.println("[3] Max Accel / Max Vel"); + + while(DEBUG_SERIAL.available()==0); + key_input = DEBUG_SERIAL.read(); + + switch(key_input) { + case '1': + // Low Profile Acceleration, High Profile Velocity + // Refer to API documentation for available parameters + // http://emanual.robotis.com/docs/en/parts/interface/dynamixel_shield/#dynamixelshieldv010-or-above + dxl.writeControlTableItem(PROFILE_ACCELERATION, DXL_ID, 50); + dxl.writeControlTableItem(PROFILE_VELOCITY, DXL_ID, 300); + break; + case '2': + // Max Profile Acceleration, Low Profile Velocity + dxl.writeControlTableItem(PROFILE_ACCELERATION, DXL_ID, 0); + dxl.writeControlTableItem(PROFILE_VELOCITY, DXL_ID, 100); + break; + case '3': + // Max Profile Acceleration, Max Profile Velocity + // WARNING : Please BE AWARE that this option will make a vibrant motion for PRO(A) or PRO+ series that requires high current supply. + dxl.writeControlTableItem(PROFILE_ACCELERATION, DXL_ID, 0); + dxl.writeControlTableItem(PROFILE_VELOCITY, DXL_ID, 0); + break; + default: + break; + } + + // Set Goal Position in RAW value + dxl.setGoalPosition(DXL_ID, 0); + delay(500); + // Check if DYNAMIXEL is in motion + while(dxl.readControlTableItem(MOVING, DXL_ID)); + + // Set Goal Position in angle(degree) + dxl.setGoalPosition(DXL_ID, 179.0, UNIT_DEGREE); + delay(500); + // Check if DYNAMIXEL is in motion + while(dxl.readControlTableItem(MOVING, DXL_ID)); +} diff --git a/examples/basic/pwm_mode/pwm_mode.ino b/examples/basic/pwm_mode/pwm_mode.ino index a5e3a59..49630fb 100644 --- a/examples/basic/pwm_mode/pwm_mode.ino +++ b/examples/basic/pwm_mode/pwm_mode.ino @@ -1,96 +1,101 @@ -/******************************************************************************* -* 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 - - -const uint8_t DXL_ID = 1; -const float DXL_PROTOCOL_VERSION = 2.0; - -Dynamixel2Arduino dxl(DXL_SERIAL, DXL_DIR_PIN); - -//This namespace is required to use Control table item names -using namespace ControlTableItem; - -void setup() { - // put your setup code here, to run once: - - // Use UART port of DYNAMIXEL Shield to debug. - DEBUG_SERIAL.begin(115200); - - // 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); - // Get DYNAMIXEL information - dxl.ping(DXL_ID); - - // Turn off torque when configuring items in EEPROM area - dxl.torqueOff(DXL_ID); - dxl.setOperatingMode(DXL_ID, OP_PWM); - dxl.torqueOn(DXL_ID); -} - -void loop() { - // put your main code here, to run repeatedly: - - // Please refer to e-Manual(http://emanual.robotis.com) for available range of value. - // Set Goal PWM using RAW unit - dxl.setGoalPWM(DXL_ID, 300); - delay(1000); - // Print present PWM - DEBUG_SERIAL.print("Present PWM(raw) : "); - DEBUG_SERIAL.println(dxl.getPresentPWM(DXL_ID)); - delay(1000); - - // Set Goal PWM using percentage (-100.0 [%] ~ 100.0 [%]) - dxl.setGoalPWM(DXL_ID, -40.8, UNIT_PERCENT); - delay(1000); - DEBUG_SERIAL.print("Present PWM(ratio) : "); - DEBUG_SERIAL.println(dxl.getPresentPWM(DXL_ID, UNIT_PERCENT)); - delay(1000); -} +/******************************************************************************* +* Copyright 2016 ROBOTIS CO., LTD. +* +* Licensed under the Apache License, Version 2.0 (the "License"); +* you may not use this file except in compliance with the License. +* You may obtain a copy of the License at +* +* http://www.apache.org/licenses/LICENSE-2.0 +* +* Unless required by applicable law or agreed to in writing, software +* distributed under the License is distributed on an "AS IS" BASIS, +* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +* See the License for the specific language governing permissions and +* limitations under the License. +*******************************************************************************/ + +#include + +// Please modify it to suit your hardware. +#if defined(ARDUINO_AVR_UNO) || defined(ARDUINO_AVR_MEGA2560) // When using DynamixelShield + #include + SoftwareSerial soft_serial(7, 8); // DYNAMIXELShield UART RX/TX + #define DXL_SERIAL Serial + #define DEBUG_SERIAL soft_serial + const int DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN +#elif defined(ARDUINO_SAM_DUE) // When using DynamixelShield + #define DXL_SERIAL Serial + #define DEBUG_SERIAL SerialUSB + const int DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN +#elif defined(ARDUINO_SAM_ZERO) // When using DynamixelShield + #define DXL_SERIAL Serial1 + #define DEBUG_SERIAL SerialUSB + const int DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN +#elif defined(ARDUINO_OpenCM904) // When using official ROBOTIS board with DXL circuit. + #define DXL_SERIAL Serial3 //OpenCM9.04 EXP Board's DXL port Serial. (Serial1 for the DXL port on the OpenCM 9.04 board) + #define DEBUG_SERIAL Serial + const int DXL_DIR_PIN = 22; //OpenCM9.04 EXP Board's DIR PIN. (28 for the DXL port on the OpenCM 9.04 board) +#elif defined(ARDUINO_OpenCR) // When using official ROBOTIS board with DXL circuit. + // For OpenCR, there is a DXL Power Enable pin, so you must initialize and control it. + // Reference link : https://github.com/ROBOTIS-GIT/OpenCR/blob/master/arduino/opencr_arduino/opencr/libraries/DynamixelSDK/src/dynamixel_sdk/port_handler_arduino.cpp#L78 + #define DXL_SERIAL Serial3 + #define DEBUG_SERIAL Serial + const int DXL_DIR_PIN = 84; // OpenCR Board's DIR PIN. +#elif defined(ARDUINO_OpenRB) // When using OpenRB-150 + //OpenRB does not require the DIR control pin. + #define DXL_SERIAL Serial1 + #define DEBUG_SERIAL Serial + const int DXL_DIR_PIN = -1; +#else // Other boards when using DynamixelShield + #define DXL_SERIAL Serial1 + #define DEBUG_SERIAL Serial + const int DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN +#endif + + +const uint8_t DXL_ID = 1; +const float DXL_PROTOCOL_VERSION = 2.0; + +Dynamixel2Arduino dxl(DXL_SERIAL, DXL_DIR_PIN); + +//This namespace is required to use Control table item names +using namespace ControlTableItem; + +void setup() { + // put your setup code here, to run once: + + // Use UART port of DYNAMIXEL Shield to debug. + DEBUG_SERIAL.begin(115200); + + // 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); + // Get DYNAMIXEL information + dxl.ping(DXL_ID); + + // Turn off torque when configuring items in EEPROM area + dxl.torqueOff(DXL_ID); + dxl.setOperatingMode(DXL_ID, OP_PWM); + dxl.torqueOn(DXL_ID); +} + +void loop() { + // put your main code here, to run repeatedly: + + // Please refer to e-Manual(http://emanual.robotis.com) for available range of value. + // Set Goal PWM using RAW unit + dxl.setGoalPWM(DXL_ID, 300); + delay(1000); + // Print present PWM + DEBUG_SERIAL.print("Present PWM(raw) : "); + DEBUG_SERIAL.println(dxl.getPresentPWM(DXL_ID)); + delay(1000); + + // Set Goal PWM using percentage (-100.0 [%] ~ 100.0 [%]) + dxl.setGoalPWM(DXL_ID, -40.8, UNIT_PERCENT); + delay(1000); + DEBUG_SERIAL.print("Present PWM(ratio) : "); + DEBUG_SERIAL.println(dxl.getPresentPWM(DXL_ID, UNIT_PERCENT)); + delay(1000); +} diff --git a/examples/basic/scan_dynamixel/scan_dynamixel.ino b/examples/basic/scan_dynamixel/scan_dynamixel.ino index 01dfaf4..1418daa 100644 --- a/examples/basic/scan_dynamixel/scan_dynamixel.ino +++ b/examples/basic/scan_dynamixel/scan_dynamixel.ino @@ -1,99 +1,104 @@ -/******************************************************************************* -* 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 int32_t buad[MAX_BAUD] = {57600, 115200, 1000000, 2000000, 3000000}; - -Dynamixel2Arduino dxl(DXL_SERIAL, DXL_DIR_PIN); - -//This namespace is required to use Control table item names -using namespace ControlTableItem; - -void setup() { - // put your setup code here, to run once: - int8_t index = 0; - int8_t found_dynamixel = 0; - - // 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 - - for(int8_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("SCAN PROTOCOL "); - DEBUG_SERIAL.println(protocol); - - for(index = 0; index < MAX_BAUD; index++) { - // Set Port baudrate. - DEBUG_SERIAL.print("SCAN BAUDRATE "); - DEBUG_SERIAL.println(buad[index]); - dxl.begin(buad[index]); - for(int id = 0; id < DXL_BROADCAST_ID; id++) { - //iterate until all ID in each buadrate is scanned. - if(dxl.ping(id)) { - DEBUG_SERIAL.print("ID : "); - DEBUG_SERIAL.print(id); - DEBUG_SERIAL.print(", Model Number: "); - DEBUG_SERIAL.println(dxl.getModelNumber(id)); - found_dynamixel++; - } - } - } - } - - DEBUG_SERIAL.print("Total "); - DEBUG_SERIAL.print(found_dynamixel); - DEBUG_SERIAL.println(" DYNAMIXEL(s) found!"); -} - -void loop() { - // put your main code here, to run repeatedly: +/******************************************************************************* +* Copyright 2016 ROBOTIS CO., LTD. +* +* Licensed under the Apache License, Version 2.0 (the "License"); +* you may not use this file except in compliance with the License. +* You may obtain a copy of the License at +* +* http://www.apache.org/licenses/LICENSE-2.0 +* +* Unless required by applicable law or agreed to in writing, software +* distributed under the License is distributed on an "AS IS" BASIS, +* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +* See the License for the specific language governing permissions and +* limitations under the License. +*******************************************************************************/ + +#include + +// Please modify it to suit your hardware. +#if defined(ARDUINO_AVR_UNO) || defined(ARDUINO_AVR_MEGA2560) // When using DynamixelShield + #include + SoftwareSerial soft_serial(7, 8); // DYNAMIXELShield UART RX/TX + #define DXL_SERIAL Serial + #define DEBUG_SERIAL soft_serial + const int DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN +#elif defined(ARDUINO_SAM_DUE) // When using DynamixelShield + #define DXL_SERIAL Serial + #define DEBUG_SERIAL SerialUSB + const int DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN +#elif defined(ARDUINO_SAM_ZERO) // When using DynamixelShield + #define DXL_SERIAL Serial1 + #define DEBUG_SERIAL SerialUSB + const int DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN +#elif defined(ARDUINO_OpenCM904) // When using official ROBOTIS board with DXL circuit. + #define DXL_SERIAL Serial3 //OpenCM9.04 EXP Board's DXL port Serial. (Serial1 for the DXL port on the OpenCM 9.04 board) + #define DEBUG_SERIAL Serial + const int DXL_DIR_PIN = 22; //OpenCM9.04 EXP Board's DIR PIN. (28 for the DXL port on the OpenCM 9.04 board) +#elif defined(ARDUINO_OpenCR) // When using official ROBOTIS board with DXL circuit. + // For OpenCR, there is a DXL Power Enable pin, so you must initialize and control it. + // Reference link : https://github.com/ROBOTIS-GIT/OpenCR/blob/master/arduino/opencr_arduino/opencr/libraries/DynamixelSDK/src/dynamixel_sdk/port_handler_arduino.cpp#L78 + #define DXL_SERIAL Serial3 + #define DEBUG_SERIAL Serial + const int DXL_DIR_PIN = 84; // OpenCR Board's DIR PIN. +#elif defined(ARDUINO_OpenRB) // When using OpenRB-150 + //OpenRB does not require the DIR control pin. + #define DXL_SERIAL Serial1 + #define DEBUG_SERIAL Serial + const int DXL_DIR_PIN = -1; +#else // Other boards when using DynamixelShield + #define DXL_SERIAL Serial1 + #define DEBUG_SERIAL Serial + const int DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN +#endif + + +#define MAX_BAUD 5 +const int32_t buad[MAX_BAUD] = {57600, 115200, 1000000, 2000000, 3000000}; + +Dynamixel2Arduino dxl(DXL_SERIAL, DXL_DIR_PIN); + +//This namespace is required to use Control table item names +using namespace ControlTableItem; + +void setup() { + // put your setup code here, to run once: + int8_t index = 0; + int8_t found_dynamixel = 0; + + // 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 + + for(int8_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("SCAN PROTOCOL "); + DEBUG_SERIAL.println(protocol); + + for(index = 0; index < MAX_BAUD; index++) { + // Set Port baudrate. + DEBUG_SERIAL.print("SCAN BAUDRATE "); + DEBUG_SERIAL.println(buad[index]); + dxl.begin(buad[index]); + for(int id = 0; id < DXL_BROADCAST_ID; id++) { + //iterate until all ID in each buadrate is scanned. + if(dxl.ping(id)) { + DEBUG_SERIAL.print("ID : "); + DEBUG_SERIAL.print(id); + DEBUG_SERIAL.print(", Model Number: "); + DEBUG_SERIAL.println(dxl.getModelNumber(id)); + found_dynamixel++; + } + } + } + } + + DEBUG_SERIAL.print("Total "); + DEBUG_SERIAL.print(found_dynamixel); + DEBUG_SERIAL.println(" DYNAMIXEL(s) found!"); +} + +void loop() { + // put your main code here, to run repeatedly: } \ No newline at end of file diff --git a/examples/basic/torque/torque.ino b/examples/basic/torque/torque.ino index b3d842f..67cabc8 100644 --- a/examples/basic/torque/torque.ino +++ b/examples/basic/torque/torque.ino @@ -1,84 +1,89 @@ -/******************************************************************************* -* 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. -*******************************************************************************/ - -/* -* Please refer to each DYNAMIXEL eManual(http://emanual.robotis.com/docs/en/dxl/) for more information regarding Torque. -*/ - -#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 - - -const uint8_t DXL_ID = 1; -const float DXL_PROTOCOL_VERSION = 2.0; - -Dynamixel2Arduino dxl(DXL_SERIAL, DXL_DIR_PIN); - -//This namespace is required to use Control table item names -using namespace ControlTableItem; - -void setup() { - // put your setup code here, to run once: - - // 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); - // Get DYNAMIXEL information - dxl.ping(DXL_ID); -} - -void loop() { - // put your main code here, to run repeatedly: - - // Turn on the output Torque. - dxl.torqueOn(DXL_ID); - delay(2000); - - // Turn off the output Torque. - dxl.torqueOff(DXL_ID); - delay(2000); -} +/******************************************************************************* +* 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. +*******************************************************************************/ + +/* +* Please refer to each DYNAMIXEL eManual(http://emanual.robotis.com/docs/en/dxl/) for more information regarding Torque. +*/ + +#include + +// Please modify it to suit your hardware. +#if defined(ARDUINO_AVR_UNO) || defined(ARDUINO_AVR_MEGA2560) // When using DynamixelShield + #include + SoftwareSerial soft_serial(7, 8); // DYNAMIXELShield UART RX/TX + #define DXL_SERIAL Serial + #define DEBUG_SERIAL soft_serial + const int DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN +#elif defined(ARDUINO_SAM_DUE) // When using DynamixelShield + #define DXL_SERIAL Serial + #define DEBUG_SERIAL SerialUSB + const int DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN +#elif defined(ARDUINO_SAM_ZERO) // When using DynamixelShield + #define DXL_SERIAL Serial1 + #define DEBUG_SERIAL SerialUSB + const int DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN +#elif defined(ARDUINO_OpenCM904) // When using official ROBOTIS board with DXL circuit. + #define DXL_SERIAL Serial3 //OpenCM9.04 EXP Board's DXL port Serial. (Serial1 for the DXL port on the OpenCM 9.04 board) + #define DEBUG_SERIAL Serial + const int DXL_DIR_PIN = 22; //OpenCM9.04 EXP Board's DIR PIN. (28 for the DXL port on the OpenCM 9.04 board) +#elif defined(ARDUINO_OpenCR) // When using official ROBOTIS board with DXL circuit. + // For OpenCR, there is a DXL Power Enable pin, so you must initialize and control it. + // Reference link : https://github.com/ROBOTIS-GIT/OpenCR/blob/master/arduino/opencr_arduino/opencr/libraries/DynamixelSDK/src/dynamixel_sdk/port_handler_arduino.cpp#L78 + #define DXL_SERIAL Serial3 + #define DEBUG_SERIAL Serial + const int DXL_DIR_PIN = 84; // OpenCR Board's DIR PIN. +#elif defined(ARDUINO_OpenRB) // When using OpenRB-150 + //OpenRB does not require the DIR control pin. + #define DXL_SERIAL Serial1 + #define DEBUG_SERIAL Serial + const int DXL_DIR_PIN = -1; +#else // Other boards when using DynamixelShield + #define DXL_SERIAL Serial1 + #define DEBUG_SERIAL Serial + const int DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN +#endif + + +const uint8_t DXL_ID = 1; +const float DXL_PROTOCOL_VERSION = 2.0; + +Dynamixel2Arduino dxl(DXL_SERIAL, DXL_DIR_PIN); + +//This namespace is required to use Control table item names +using namespace ControlTableItem; + +void setup() { + // put your setup code here, to run once: + + // 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); + // Get DYNAMIXEL information + dxl.ping(DXL_ID); +} + +void loop() { + // put your main code here, to run repeatedly: + + // Turn on the output Torque. + dxl.torqueOn(DXL_ID); + delay(2000); + + // Turn off the output Torque. + dxl.torqueOff(DXL_ID); + delay(2000); +} diff --git a/examples/basic/velocity_mode/velocity_mode.ino b/examples/basic/velocity_mode/velocity_mode.ino index e6b1fe4..e6a174c 100644 --- a/examples/basic/velocity_mode/velocity_mode.ino +++ b/examples/basic/velocity_mode/velocity_mode.ino @@ -1,103 +1,108 @@ -/******************************************************************************* -* 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 - - -const uint8_t DXL_ID = 1; -const float DXL_PROTOCOL_VERSION = 2.0; - -Dynamixel2Arduino dxl(DXL_SERIAL, DXL_DIR_PIN); - -//This namespace is required to use Control table item names -using namespace ControlTableItem; - -void setup() { - // put your setup code here, to run once: - - // Use UART port of DYNAMIXEL Shield to debug. - DEBUG_SERIAL.begin(115200); - - // 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); - // Get DYNAMIXEL information - dxl.ping(DXL_ID); - - // Turn off torque when configuring items in EEPROM area - dxl.torqueOff(DXL_ID); - dxl.setOperatingMode(DXL_ID, OP_VELOCITY); - dxl.torqueOn(DXL_ID); -} - -void loop() { - // put your main code here, to run repeatedly: - - // Please refer to e-Manual(http://emanual.robotis.com) for available range of value. - // Set Goal Velocity using RAW unit - dxl.setGoalVelocity(DXL_ID, 200); - delay(1000); - // Print present velocity - DEBUG_SERIAL.print("Present Velocity(raw) : "); - DEBUG_SERIAL.println(dxl.getPresentVelocity(DXL_ID)); - delay(1000); - - // Set Goal Velocity using RPM - dxl.setGoalVelocity(DXL_ID, 25.8, UNIT_RPM); - delay(1000); - DEBUG_SERIAL.print("Present Velocity(rpm) : "); - DEBUG_SERIAL.println(dxl.getPresentVelocity(DXL_ID, UNIT_RPM)); - delay(1000); - - // Set Goal Velocity using percentage (-100.0 [%] ~ 100.0 [%]) - dxl.setGoalVelocity(DXL_ID, -10.2, UNIT_PERCENT); - delay(1000); - DEBUG_SERIAL.print("Present Velocity(ratio) : "); - DEBUG_SERIAL.println(dxl.getPresentVelocity(DXL_ID, UNIT_PERCENT)); - delay(1000); -} +/******************************************************************************* +* Copyright 2016 ROBOTIS CO., LTD. +* +* Licensed under the Apache License, Version 2.0 (the "License"); +* you may not use this file except in compliance with the License. +* You may obtain a copy of the License at +* +* http://www.apache.org/licenses/LICENSE-2.0 +* +* Unless required by applicable law or agreed to in writing, software +* distributed under the License is distributed on an "AS IS" BASIS, +* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +* See the License for the specific language governing permissions and +* limitations under the License. +*******************************************************************************/ + +#include + +// Please modify it to suit your hardware. +#if defined(ARDUINO_AVR_UNO) || defined(ARDUINO_AVR_MEGA2560) // When using DynamixelShield + #include + SoftwareSerial soft_serial(7, 8); // DYNAMIXELShield UART RX/TX + #define DXL_SERIAL Serial + #define DEBUG_SERIAL soft_serial + const int DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN +#elif defined(ARDUINO_SAM_DUE) // When using DynamixelShield + #define DXL_SERIAL Serial + #define DEBUG_SERIAL SerialUSB + const int DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN +#elif defined(ARDUINO_SAM_ZERO) // When using DynamixelShield + #define DXL_SERIAL Serial1 + #define DEBUG_SERIAL SerialUSB + const int DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN +#elif defined(ARDUINO_OpenCM904) // When using official ROBOTIS board with DXL circuit. + #define DXL_SERIAL Serial3 //OpenCM9.04 EXP Board's DXL port Serial. (Serial1 for the DXL port on the OpenCM 9.04 board) + #define DEBUG_SERIAL Serial + const int DXL_DIR_PIN = 22; //OpenCM9.04 EXP Board's DIR PIN. (28 for the DXL port on the OpenCM 9.04 board) +#elif defined(ARDUINO_OpenCR) // When using official ROBOTIS board with DXL circuit. + // For OpenCR, there is a DXL Power Enable pin, so you must initialize and control it. + // Reference link : https://github.com/ROBOTIS-GIT/OpenCR/blob/master/arduino/opencr_arduino/opencr/libraries/DynamixelSDK/src/dynamixel_sdk/port_handler_arduino.cpp#L78 + #define DXL_SERIAL Serial3 + #define DEBUG_SERIAL Serial + const int DXL_DIR_PIN = 84; // OpenCR Board's DIR PIN. +#elif defined(ARDUINO_OpenRB) // When using OpenRB-150 + //OpenRB does not require the DIR control pin. + #define DXL_SERIAL Serial1 + #define DEBUG_SERIAL Serial + const int DXL_DIR_PIN = -1; +#else // Other boards when using DynamixelShield + #define DXL_SERIAL Serial1 + #define DEBUG_SERIAL Serial + const int DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN +#endif + + +const uint8_t DXL_ID = 1; +const float DXL_PROTOCOL_VERSION = 2.0; + +Dynamixel2Arduino dxl(DXL_SERIAL, DXL_DIR_PIN); + +//This namespace is required to use Control table item names +using namespace ControlTableItem; + +void setup() { + // put your setup code here, to run once: + + // Use UART port of DYNAMIXEL Shield to debug. + DEBUG_SERIAL.begin(115200); + + // 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); + // Get DYNAMIXEL information + dxl.ping(DXL_ID); + + // Turn off torque when configuring items in EEPROM area + dxl.torqueOff(DXL_ID); + dxl.setOperatingMode(DXL_ID, OP_VELOCITY); + dxl.torqueOn(DXL_ID); +} + +void loop() { + // put your main code here, to run repeatedly: + + // Please refer to e-Manual(http://emanual.robotis.com) for available range of value. + // Set Goal Velocity using RAW unit + dxl.setGoalVelocity(DXL_ID, 200); + delay(1000); + // Print present velocity + DEBUG_SERIAL.print("Present Velocity(raw) : "); + DEBUG_SERIAL.println(dxl.getPresentVelocity(DXL_ID)); + delay(1000); + + // Set Goal Velocity using RPM + dxl.setGoalVelocity(DXL_ID, 25.8, UNIT_RPM); + delay(1000); + DEBUG_SERIAL.print("Present Velocity(rpm) : "); + DEBUG_SERIAL.println(dxl.getPresentVelocity(DXL_ID, UNIT_RPM)); + delay(1000); + + // Set Goal Velocity using percentage (-100.0 [%] ~ 100.0 [%]) + dxl.setGoalVelocity(DXL_ID, -10.2, UNIT_PERCENT); + delay(1000); + DEBUG_SERIAL.print("Present Velocity(ratio) : "); + DEBUG_SERIAL.println(dxl.getPresentVelocity(DXL_ID, UNIT_PERCENT)); + delay(1000); +} diff --git a/examples/blender/openmanipulator_x/openmanipulator_x.ino b/examples/blender/openmanipulator_x/openmanipulator_x.ino index cd009c6..48d1a58 100644 --- a/examples/blender/openmanipulator_x/openmanipulator_x.ino +++ b/examples/blender/openmanipulator_x/openmanipulator_x.ino @@ -1,892 +1,897 @@ -// Copyright 2021 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. - -// Example Environment -// -// - DYNAMIXEL: X series (except XL-320) -// ID = 11, 12, 13, 14, 15, Baudrate = 4000000bps, Protocol 2.0 -// - Controller: OpenCR 1.0 -// - Library: DYNAMIXEL2Arduino -// - Software: Blender 2.93.1 -// Arduino IDE -// -// Author: David Park - -#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 - - -Dynamixel2Arduino dxl(DXL_SERIAL, DXL_DIR_PIN); - -// This namespace is required to use Control table item names -using namespace ControlTableItem; - -// DYNAMIXEL Configuration -const float DYNAMIXEL_PROTOCOL_VERSION = 2.0; -// OpenMANIPULATOR has ID of 11 ~ 15 by default -const uint8_t DYNAMIXEL_ID[] = {1, 2, 3, 4, 5}; -const uint8_t NUMBER_OF_JOINT = sizeof(DYNAMIXEL_ID)/sizeof(uint8_t); - - -// Data struct for SyncRead -typedef struct sync_read_data { - int32_t present_position; -} __attribute__((packed)) sync_read_data_t; - -// Sync Read Present Position -const uint16_t PRESENT_POSITION_ADDRESS = 132; -const uint16_t LENGTH_TO_SYNC_READ = 4; - -// Sync Read -sync_read_data_t sync_read_data[NUMBER_OF_JOINT]; -DYNAMIXEL::InfoSyncReadInst_t sync_read_information; -DYNAMIXEL::XELInfoSyncRead_t sync_read_dynamixel_info[NUMBER_OF_JOINT]; - - -// Data struct for SyncWrite -typedef struct sync_write_data { - int32_t goal_position; -} __attribute__((packed)) sync_write_data_t; - -// Sync Write Goal Position -const uint16_t GOAL_POSITION_ADDRESS = 116; -const uint16_t LENGTH_TO_SYNC_WRITE = 4; - -// Sync Write -sync_write_data_t sync_write_data[NUMBER_OF_JOINT]; -DYNAMIXEL::InfoSyncWriteInst_t sync_write_information; -DYNAMIXEL::XELInfoSyncWrite_t sync_write_dynamixel_info[NUMBER_OF_JOINT]; - - -const uint16_t MAX_PACKET_BUFFER_LENGTH = 128; -uint8_t packet_buffer[MAX_PACKET_BUFFER_LENGTH]; - -int loop_counts = 2; // Number of times to play the motion -int frame_time = 0; -unsigned long current_time = 0; -unsigned long saved_time = 0; - -// The number of information in each frame. (Time + Position of each Joint) -const int MAX_MOTION_DATA_COLUMN_INDEX = 1 + NUMBER_OF_JOINT; - -// Paste all keyframes. -const int MOTION_DATA [][MAX_MOTION_DATA_COLUMN_INDEX]{ -//Time[ms],ID1,ID2,ID3,ID4,Hand, -{100,2047,1535,2957,1649,2039}, -{200,2047,1535,2957,1650,2039}, -{300,2047,1534,2956,1652,2039}, -{400,2047,1533,2956,1655,2039}, -{500,2047,1531,2954,1660,2039}, -{600,2047,1529,2953,1666,2039}, -{700,2047,1526,2951,1674,2039}, -{800,2047,1523,2949,1682,2039}, -{900,2047,1520,2947,1692,2039}, -{1000,2047,1516,2944,1702,2039}, -{1100,2047,1511,2941,1714,2039}, -{1200,2047,1507,2938,1726,2039}, -{1300,2047,1502,2934,1740,2039}, -{1400,2047,1497,2930,1754,2039}, -{1500,2047,1492,2926,1769,2039}, -{1600,2047,1486,2921,1784,2039}, -{1700,2047,1480,2916,1800,2039}, -{1800,2047,1474,2911,1817,2039}, -{1900,2047,1468,2905,1834,2039}, -{2000,2047,1462,2899,1851,2039}, -{2100,2047,1455,2893,1869,2039}, -{2200,2047,1448,2886,1887,2039}, -{2300,2047,1442,2879,1906,2039}, -{2400,2047,1435,2872,1924,2039}, -{2500,2047,1428,2865,1943,2039}, -{2600,2047,1421,2857,1962,2039}, -{2700,2047,1415,2849,1980,2039}, -{2800,2047,1408,2840,1999,2039}, -{2900,2047,1401,2831,2018,2039}, -{3000,2047,1394,2822,2036,2039}, -{3100,2047,1388,2813,2054,2039}, -{3200,2047,1381,2803,2072,2039}, -{3300,2047,1375,2793,2090,2039}, -{3400,2047,1369,2783,2107,2039}, -{3500,2047,1363,2772,2123,2039}, -{3600,2047,1357,2761,2139,2039}, -{3700,2047,1351,2750,2155,2039}, -{3800,2047,1346,2738,2170,2039}, -{3900,2047,1341,2726,2184,2039}, -{4000,2047,1336,2714,2197,2039}, -{4100,2047,1331,2702,2209,2039}, -{4200,2047,1327,2689,2221,2039}, -{4300,2047,1323,2676,2232,2039}, -{4400,2047,1320,2662,2241,2039}, -{4500,2047,1317,2648,2250,2039}, -{4600,2047,1314,2634,2257,2039}, -{4700,2047,1312,2620,2263,2039}, -{4800,2047,1310,2605,2268,2039}, -{4900,2047,1309,2590,2272,2039}, -{5000,2047,1308,2575,2274,2039}, -{5100,2047,1308,2559,2274,2039}, -{5200,2048,1308,2543,2274,2039}, -{5300,2049,1308,2527,2274,2039}, -{5400,2052,1308,2510,2274,2039}, -{5500,2056,1308,2493,2274,2039}, -{5600,2061,1308,2476,2274,2039}, -{5700,2067,1308,2459,2274,2039}, -{5800,2074,1308,2442,2274,2039}, -{5900,2082,1308,2424,2274,2039}, -{6000,2091,1308,2407,2274,2039}, -{6100,2100,1308,2389,2274,2039}, -{6200,2110,1308,2372,2274,2039}, -{6300,2121,1308,2354,2274,2039}, -{6400,2133,1308,2336,2274,2039}, -{6500,2145,1308,2318,2274,2039}, -{6600,2158,1308,2301,2274,2039}, -{6700,2171,1308,2283,2274,2039}, -{6800,2184,1308,2265,2274,2039}, -{6900,2198,1308,2248,2274,2039}, -{7000,2213,1308,2231,2274,2039}, -{7100,2227,1308,2214,2274,2039}, -{7200,2242,1308,2197,2274,2039}, -{7300,2257,1308,2180,2274,2039}, -{7400,2272,1308,2164,2274,2039}, -{7500,2288,1308,2147,2274,2039}, -{7600,2303,1308,2131,2274,2039}, -{7700,2318,1308,2116,2274,2039}, -{7800,2334,1308,2101,2274,2039}, -{7900,2349,1308,2086,2274,2039}, -{8000,2364,1308,2071,2274,2039}, -{8100,2379,1308,2057,2274,2039}, -{8200,2393,1308,2044,2274,2039}, -{8300,2408,1308,2030,2274,2039}, -{8400,2422,1308,2018,2274,2039}, -{8500,2435,1308,2006,2274,2039}, -{8600,2448,1308,1994,2274,2039}, -{8700,2461,1308,1983,2274,2039}, -{8800,2473,1308,1972,2274,2039}, -{8900,2485,1308,1963,2274,2039}, -{9000,2495,1308,1953,2274,2039}, -{9100,2506,1308,1945,2274,2039}, -{9200,2515,1308,1937,2274,2039}, -{9300,2524,1308,1930,2274,2039}, -{9400,2532,1308,1924,2274,2039}, -{9500,2539,1308,1918,2274,2039}, -{9600,2545,1308,1913,2274,2039}, -{9700,2550,1308,1909,2274,2039}, -{9800,2554,1308,1906,2274,2039}, -{9900,2556,1308,1904,2274,2039}, -{10000,2558,1308,1902,2274,2039}, -{10100,2559,1308,1902,2274,2039}, -{10200,2559,1308,1902,2274,2039}, -{10300,2559,1309,1903,2274,2039}, -{10400,2559,1312,1903,2274,2039}, -{10500,2559,1315,1904,2273,2039}, -{10600,2559,1319,1906,2273,2039}, -{10700,2559,1323,1908,2272,2039}, -{10800,2559,1329,1910,2271,2039}, -{10900,2559,1335,1912,2271,2039}, -{11000,2559,1342,1914,2270,2039}, -{11100,2559,1349,1917,2269,2039}, -{11200,2559,1357,1920,2267,2039}, -{11300,2559,1365,1923,2266,2039}, -{11400,2559,1374,1926,2265,2039}, -{11500,2559,1384,1930,2264,2039}, -{11600,2559,1394,1933,2262,2039}, -{11700,2559,1404,1937,2261,2039}, -{11800,2559,1414,1941,2259,2039}, -{11900,2559,1425,1945,2258,2039}, -{12000,2559,1436,1949,2256,2039}, -{12100,2559,1448,1953,2254,2039}, -{12200,2559,1459,1957,2253,2039}, -{12300,2559,1471,1961,2251,2039}, -{12400,2559,1483,1966,2249,2039}, -{12500,2559,1495,1970,2248,2039}, -{12600,2559,1507,1974,2246,2039}, -{12700,2559,1519,1979,2244,2039}, -{12800,2559,1531,1983,2243,2039}, -{12900,2559,1542,1987,2241,2039}, -{13000,2559,1554,1992,2239,2039}, -{13100,2559,1566,1996,2238,2039}, -{13200,2559,1577,2000,2236,2039}, -{13300,2559,1588,2004,2234,2039}, -{13400,2559,1599,2008,2233,2039}, -{13500,2559,1610,2012,2231,2039}, -{13600,2559,1620,2016,2230,2039}, -{13700,2559,1630,2019,2229,2039}, -{13800,2559,1639,2023,2227,2039}, -{13900,2559,1648,2026,2226,2039}, -{14000,2559,1656,2029,2225,2039}, -{14100,2559,1664,2032,2224,2039}, -{14200,2559,1672,2035,2222,2039}, -{14300,2559,1678,2037,2222,2039}, -{14400,2559,1685,2039,2221,2039}, -{14500,2559,1690,2041,2220,2039}, -{14600,2559,1695,2043,2219,2039}, -{14700,2559,1699,2044,2219,2039}, -{14800,2559,1702,2045,2218,2039}, -{14900,2559,1704,2046,2218,2039}, -{15000,2559,1705,2047,2218,2039}, -{15100,2559,1706,2047,2218,2039}, -{15200,2559,1706,2047,2218,2038}, -{15300,2559,1706,2047,2218,2033}, -{15400,2559,1706,2047,2218,2024}, -{15500,2559,1706,2047,2218,2013}, -{15600,2559,1706,2047,2218,1998}, -{15700,2559,1706,2047,2218,1981}, -{15800,2559,1706,2047,2218,1961}, -{15900,2559,1706,2047,2218,1939}, -{16000,2559,1706,2047,2218,1914}, -{16100,2559,1706,2047,2218,1887}, -{16200,2559,1706,2047,2218,1857}, -{16300,2559,1706,2047,2218,1826}, -{16400,2559,1706,2047,2218,1793}, -{16500,2559,1706,2047,2218,1758}, -{16600,2559,1706,2047,2218,1722}, -{16700,2559,1706,2047,2218,1684}, -{16800,2559,1706,2047,2218,1645}, -{16900,2559,1706,2047,2218,1605}, -{17000,2559,1706,2047,2218,1564}, -{17100,2559,1706,2047,2218,1522}, -{17200,2559,1706,2047,2218,1479}, -{17300,2559,1706,2047,2218,1436}, -{17400,2559,1706,2047,2218,1393}, -{17500,2559,1706,2047,2218,1349}, -{17600,2559,1706,2047,2218,1304}, -{17700,2559,1706,2047,2218,1260}, -{17800,2559,1706,2047,2218,1216}, -{17900,2559,1706,2047,2218,1173}, -{18000,2559,1706,2047,2218,1130}, -{18100,2559,1706,2047,2218,1087}, -{18200,2559,1706,2047,2218,1045}, -{18300,2559,1706,2047,2218,1004}, -{18400,2559,1706,2047,2218,964}, -{18500,2559,1706,2047,2218,925}, -{18600,2559,1706,2047,2218,887}, -{18700,2559,1706,2047,2218,851}, -{18800,2559,1706,2047,2218,816}, -{18900,2559,1706,2047,2218,783}, -{19000,2559,1706,2047,2218,752}, -{19100,2559,1706,2047,2218,722}, -{19200,2559,1706,2047,2218,695}, -{19300,2559,1706,2047,2218,670}, -{19400,2559,1706,2047,2218,648}, -{19500,2559,1706,2047,2218,628}, -{19600,2559,1706,2047,2218,611}, -{19700,2559,1706,2047,2218,596}, -{19800,2559,1706,2047,2218,585}, -{19900,2559,1706,2047,2218,576}, -{20000,2559,1706,2047,2218,571}, -{20100,2559,1706,2047,2218,570}, -{20200,2559,1705,2047,2218,570}, -{20300,2559,1704,2047,2220,570}, -{20400,2559,1702,2047,2224,570}, -{20500,2559,1699,2047,2228,570}, -{20600,2559,1695,2047,2234,570}, -{20700,2559,1690,2047,2240,570}, -{20800,2559,1685,2047,2248,570}, -{20900,2559,1678,2047,2257,570}, -{21000,2559,1672,2047,2266,570}, -{21100,2559,1664,2047,2277,570}, -{21200,2559,1656,2047,2288,570}, -{21300,2559,1648,2047,2300,570}, -{21400,2559,1639,2047,2313,570}, -{21500,2559,1630,2047,2326,570}, -{21600,2559,1620,2047,2340,570}, -{21700,2559,1610,2047,2355,570}, -{21800,2559,1599,2047,2370,570}, -{21900,2559,1588,2047,2386,570}, -{22000,2559,1577,2047,2402,570}, -{22100,2559,1566,2047,2418,570}, -{22200,2559,1554,2047,2434,570}, -{22300,2559,1542,2047,2451,570}, -{22400,2559,1531,2047,2468,570}, -{22500,2559,1519,2047,2485,570}, -{22600,2559,1507,2047,2502,570}, -{22700,2559,1495,2047,2519,570}, -{22800,2559,1483,2047,2536,570}, -{22900,2559,1471,2047,2553,570}, -{23000,2559,1459,2047,2570,570}, -{23100,2559,1448,2047,2586,570}, -{23200,2559,1436,2047,2602,570}, -{23300,2559,1425,2047,2618,570}, -{23400,2559,1414,2047,2634,570}, -{23500,2559,1404,2047,2649,570}, -{23600,2559,1394,2047,2664,570}, -{23700,2559,1384,2047,2678,570}, -{23800,2559,1374,2047,2691,570}, -{23900,2559,1365,2047,2704,570}, -{24000,2559,1357,2047,2716,570}, -{24100,2559,1349,2047,2727,570}, -{24200,2559,1342,2047,2738,570}, -{24300,2559,1335,2047,2747,570}, -{24400,2559,1329,2047,2756,570}, -{24500,2559,1323,2047,2764,570}, -{24600,2559,1319,2047,2770,570}, -{24700,2559,1315,2047,2776,570}, -{24800,2559,1312,2047,2780,570}, -{24900,2559,1309,2047,2784,570}, -{25000,2559,1308,2047,2786,570}, -{25100,2559,1308,2047,2786,570}, -{25200,2558,1308,2047,2786,570}, -{25300,2554,1308,2047,2786,570}, -{25400,2548,1308,2047,2786,570}, -{25500,2540,1308,2047,2786,570}, -{25600,2530,1308,2047,2786,570}, -{25700,2518,1308,2047,2786,570}, -{25800,2504,1308,2047,2786,570}, -{25900,2489,1308,2047,2786,570}, -{26000,2471,1308,2047,2786,570}, -{26100,2452,1308,2047,2786,570}, -{26200,2432,1308,2047,2786,570}, -{26300,2410,1308,2047,2786,570}, -{26400,2387,1308,2047,2786,570}, -{26500,2363,1308,2047,2786,570}, -{26600,2338,1308,2047,2786,570}, -{26700,2311,1308,2047,2786,570}, -{26800,2284,1308,2047,2786,570}, -{26900,2256,1308,2047,2786,570}, -{27000,2228,1308,2047,2786,570}, -{27100,2199,1308,2047,2786,570}, -{27200,2169,1308,2047,2786,570}, -{27300,2139,1308,2047,2786,570}, -{27400,2108,1308,2047,2786,570}, -{27500,2078,1308,2047,2786,570}, -{27600,2047,1308,2047,2786,570}, -{27700,2016,1308,2047,2786,570}, -{27800,1986,1308,2047,2786,570}, -{27900,1955,1308,2047,2786,570}, -{28000,1925,1308,2047,2786,570}, -{28100,1895,1308,2047,2786,570}, -{28200,1866,1308,2047,2786,570}, -{28300,1838,1308,2047,2786,570}, -{28400,1810,1308,2047,2786,570}, -{28500,1783,1308,2047,2786,570}, -{28600,1756,1308,2047,2786,570}, -{28700,1731,1308,2047,2786,570}, -{28800,1707,1308,2047,2786,570}, -{28900,1684,1308,2047,2786,570}, -{29000,1662,1308,2047,2786,570}, -{29100,1642,1308,2047,2786,570}, -{29200,1623,1308,2047,2786,570}, -{29300,1605,1308,2047,2786,570}, -{29400,1590,1308,2047,2786,570}, -{29500,1576,1308,2047,2786,570}, -{29600,1564,1308,2047,2786,570}, -{29700,1554,1308,2047,2786,570}, -{29800,1546,1308,2047,2786,570}, -{29900,1540,1308,2047,2786,570}, -{30000,1536,1308,2047,2786,570}, -{30100,1535,1308,2047,2786,570}, -{30200,1535,1308,2047,2786,570}, -{30300,1535,1309,2047,2784,570}, -{30400,1535,1312,2047,2782,570}, -{30500,1535,1315,2047,2778,570}, -{30600,1535,1319,2047,2774,570}, -{30700,1535,1323,2047,2768,570}, -{30800,1535,1329,2047,2762,570}, -{30900,1535,1335,2047,2755,570}, -{31000,1535,1342,2047,2747,570}, -{31100,1535,1349,2047,2739,570}, -{31200,1535,1357,2047,2730,570}, -{31300,1535,1365,2047,2720,570}, -{31400,1535,1374,2047,2710,570}, -{31500,1535,1384,2047,2699,570}, -{31600,1535,1394,2047,2688,570}, -{31700,1535,1404,2047,2676,570}, -{31800,1535,1414,2047,2664,570}, -{31900,1535,1425,2047,2652,570}, -{32000,1535,1436,2047,2639,570}, -{32100,1535,1448,2047,2626,570}, -{32200,1535,1459,2047,2613,570}, -{32300,1535,1471,2047,2600,570}, -{32400,1535,1483,2047,2586,570}, -{32500,1535,1495,2047,2573,570}, -{32600,1535,1507,2047,2559,570}, -{32700,1535,1519,2047,2545,570}, -{32800,1535,1531,2047,2532,570}, -{32900,1535,1542,2047,2518,570}, -{33000,1535,1554,2047,2505,570}, -{33100,1535,1566,2047,2492,570}, -{33200,1535,1577,2047,2479,570}, -{33300,1535,1588,2047,2466,570}, -{33400,1535,1599,2047,2453,570}, -{33500,1535,1610,2047,2441,570}, -{33600,1535,1620,2047,2430,570}, -{33700,1535,1630,2047,2418,570}, -{33800,1535,1639,2047,2408,570}, -{33900,1535,1648,2047,2397,570}, -{34000,1535,1656,2047,2388,570}, -{34100,1535,1664,2047,2379,570}, -{34200,1535,1672,2047,2370,570}, -{34300,1535,1678,2047,2363,570}, -{34400,1535,1685,2047,2356,570}, -{34500,1535,1690,2047,2349,570}, -{34600,1535,1695,2047,2344,570}, -{34700,1535,1699,2047,2340,570}, -{34800,1535,1702,2047,2336,570}, -{34900,1535,1704,2047,2334,570}, -{35000,1535,1705,2047,2332,570}, -{35100,1535,1706,2047,2331,570}, -{35200,1535,1706,2047,2331,571}, -{35300,1535,1706,2047,2331,576}, -{35400,1535,1706,2047,2331,585}, -{35500,1535,1706,2047,2331,596}, -{35600,1535,1706,2047,2331,611}, -{35700,1535,1706,2047,2331,628}, -{35800,1535,1706,2047,2331,648}, -{35900,1535,1706,2047,2331,670}, -{36000,1535,1706,2047,2331,695}, -{36100,1535,1706,2047,2331,722}, -{36200,1535,1706,2047,2331,752}, -{36300,1535,1706,2047,2331,783}, -{36400,1535,1706,2047,2331,816}, -{36500,1535,1706,2047,2331,851}, -{36600,1535,1706,2047,2331,887}, -{36700,1535,1706,2047,2331,925}, -{36800,1535,1706,2047,2331,964}, -{36900,1535,1706,2047,2331,1004}, -{37000,1535,1706,2047,2331,1045}, -{37100,1535,1706,2047,2331,1087}, -{37200,1535,1706,2047,2331,1130}, -{37300,1535,1706,2047,2331,1173}, -{37400,1535,1706,2047,2331,1216}, -{37500,1535,1706,2047,2331,1260}, -{37600,1535,1706,2047,2331,1305}, -{37700,1535,1706,2047,2331,1349}, -{37800,1535,1706,2047,2331,1393}, -{37900,1535,1706,2047,2331,1436}, -{38000,1535,1706,2047,2331,1479}, -{38100,1535,1706,2047,2331,1522}, -{38200,1535,1706,2047,2331,1564}, -{38300,1535,1706,2047,2331,1605}, -{38400,1535,1706,2047,2331,1645}, -{38500,1535,1706,2047,2331,1684}, -{38600,1535,1706,2047,2331,1722}, -{38700,1535,1706,2047,2331,1758}, -{38800,1535,1706,2047,2331,1793}, -{38900,1535,1706,2047,2331,1826}, -{39000,1535,1706,2047,2331,1857}, -{39100,1535,1706,2047,2331,1887}, -{39200,1535,1706,2047,2331,1914}, -{39300,1535,1706,2047,2331,1939}, -{39400,1535,1706,2047,2331,1961}, -{39500,1535,1706,2047,2331,1981}, -{39600,1535,1706,2047,2331,1998}, -{39700,1535,1706,2047,2331,2013}, -{39800,1535,1706,2047,2331,2024}, -{39900,1535,1706,2047,2331,2033}, -{40000,1535,1706,2047,2331,2038}, -{40100,1535,1706,2047,2331,2040}, -{40200,1536,1705,2047,2331,2040}, -{40300,1537,1704,2048,2331,2040}, -{40400,1540,1702,2050,2331,2040}, -{40500,1544,1699,2052,2331,2040}, -{40600,1549,1695,2055,2331,2039}, -{40700,1555,1691,2058,2331,2039}, -{40800,1561,1685,2062,2331,2039}, -{40900,1569,1679,2066,2331,2039}, -{41000,1577,1673,2071,2331,2039}, -{41100,1586,1666,2077,2331,2039}, -{41200,1596,1658,2083,2331,2039}, -{41300,1607,1650,2089,2331,2039}, -{41400,1618,1641,2096,2330,2039}, -{41500,1630,1632,2104,2330,2039}, -{41600,1642,1623,2111,2330,2039}, -{41700,1655,1613,2120,2330,2039}, -{41800,1668,1603,2128,2329,2039}, -{41900,1681,1592,2137,2329,2039}, -{42000,1695,1581,2147,2328,2039}, -{42100,1710,1570,2157,2328,2039}, -{42200,1724,1559,2167,2327,2039}, -{42300,1739,1547,2177,2327,2039}, -{42400,1754,1536,2188,2326,2039}, -{42500,1769,1524,2199,2325,2039}, -{42600,1784,1513,2210,2325,2039}, -{42700,1799,1501,2222,2324,2039}, -{42800,1814,1489,2234,2323,2039}, -{42900,1829,1478,2246,2322,2039}, -{43000,1843,1466,2259,2321,2039}, -{43100,1858,1455,2271,2320,2039}, -{43200,1873,1443,2284,2319,2039}, -{43300,1887,1432,2297,2317,2039}, -{43400,1901,1421,2310,2316,2039}, -{43500,1914,1411,2323,2315,2039}, -{43600,1927,1401,2337,2313,2039}, -{43700,1940,1391,2350,2311,2039}, -{43800,1952,1381,2364,2310,2039}, -{43900,1964,1372,2378,2308,2039}, -{44000,1975,1363,2392,2306,2039}, -{44100,1986,1355,2406,2304,2039}, -{44200,1996,1348,2420,2302,2039}, -{44300,2005,1340,2434,2300,2039}, -{44400,2013,1334,2448,2297,2039}, -{44500,2021,1328,2462,2295,2039}, -{44600,2027,1323,2476,2292,2039}, -{44700,2033,1318,2490,2290,2039}, -{44800,2038,1315,2504,2287,2039}, -{44900,2042,1312,2518,2284,2039}, -{45000,2045,1309,2531,2281,2039}, -{45100,2046,1308,2545,2278,2039}, -{45200,2047,1308,2559,2274,2039}, -{45300,2047,1308,2572,2271,2039}, -{45400,2047,1309,2586,2265,2039}, -{45500,2047,1310,2599,2259,2039}, -{45600,2047,1312,2612,2251,2039}, -{45700,2047,1314,2625,2243,2039}, -{45800,2047,1317,2638,2233,2039}, -{45900,2047,1320,2651,2223,2039}, -{46000,2047,1324,2663,2211,2039}, -{46100,2047,1328,2676,2199,2039}, -{46200,2047,1332,2688,2186,2039}, -{46300,2047,1337,2700,2172,2039}, -{46400,2047,1342,2712,2157,2039}, -{46500,2047,1347,2723,2142,2039}, -{46600,2047,1353,2735,2127,2039}, -{46700,2047,1359,2746,2110,2039}, -{46800,2047,1365,2757,2094,2039}, -{46900,2047,1371,2768,2077,2039}, -{47000,2047,1377,2779,2059,2039}, -{47100,2047,1384,2789,2041,2039}, -{47200,2047,1390,2799,2023,2039}, -{47300,2047,1397,2809,2005,2039}, -{47400,2047,1404,2818,1987,2039}, -{47500,2047,1411,2828,1969,2039}, -{47600,2047,1418,2837,1950,2039}, -{47700,2047,1425,2846,1932,2039}, -{47800,2047,1432,2854,1914,2039}, -{47900,2047,1439,2862,1896,2039}, -{48000,2047,1446,2870,1878,2039}, -{48100,2047,1452,2878,1860,2039}, -{48200,2047,1459,2885,1843,2039}, -{48300,2047,1466,2892,1826,2039}, -{48400,2047,1472,2899,1810,2039}, -{48500,2047,1478,2905,1794,2039}, -{48600,2047,1484,2911,1778,2039}, -{48700,2047,1490,2917,1763,2039}, -{48800,2047,1496,2922,1749,2039}, -{48900,2047,1501,2927,1736,2039}, -{49000,2047,1506,2932,1723,2039}, -{49100,2047,1511,2936,1711,2039}, -{49200,2047,1515,2940,1700,2039}, -{49300,2047,1519,2943,1690,2039}, -{49400,2047,1523,2947,1681,2039}, -{49500,2047,1526,2949,1673,2039}, -{49600,2047,1529,2952,1666,2039}, -{49700,2047,1531,2954,1660,2039}, -{49800,2047,1533,2955,1655,2039}, -{49900,2047,1534,2956,1652,2039}, -{50000,2047,1535,2957,1650,2039}, -{50100,2047,1535,2957,1649,2039} -}; - -// The number of key frames. -const int TOTAL_FRAMES = sizeof(MOTION_DATA) / sizeof(MOTION_DATA[0]); - -void setup() -{ - int time_between_frame = (MOTION_DATA[2][0] - MOTION_DATA[1][0]) * 2; - - // Open serial port for debugging - DEBUG_SERIAL.begin(115200); - while(!DEBUG_SERIAL); - - // Port Baudrate to communicate with connected DYNAMIXELs. If not matched, you will meet communication fail. - dxl.begin(4000000); - - delay(100); // Allow time for DYNAMIXEL to finish boot up - - // Recommended Protocol Version is "2.0" Note that DYNAMIXEL Protocol for each product may differ depending on your model in use. - dxl.setPortProtocolVersion(DYNAMIXEL_PROTOCOL_VERSION); - - InitDXL(time_between_frame); // Initialize the DYNAMIXEL - InitSyncRead(); - InitSyncWrite(); - delay(2000); - InitPose(MOTION_DATA, time_between_frame); // Pass a pose -} - -void loop() -{ - // Pass the motion data - PlayMotion(MOTION_DATA); -} - -void InitDXL(int frame_time) -{ - uint8_t index = 0; - dxl.torqueOff(DXL_BROADCAST_ID); - - // Ping DYNAMIXELs. If failed, check your Baudrate and pysical wiring connection. - for (index = 0; index < NUMBER_OF_JOINT; index++) - { - if (!dxl.ping(DYNAMIXEL_ID[index])) { - DEBUG_SERIAL.print("[ERROR] Failed to connect DYNAMIXEL "); - DEBUG_SERIAL.println(DYNAMIXEL_ID[index]); - } else { - // Set the Drive Mode as Time-based mode. - dxl.writeControlTableItem(DRIVE_MODE, DYNAMIXEL_ID[index], 4); - // Lower Return Delay Time enhances the responsiveness. - dxl.writeControlTableItem(RETURN_DELAY_TIME, DYNAMIXEL_ID[index], 0); - dxl.setOperatingMode(DYNAMIXEL_ID[index], OP_POSITION); - // Time-based should be larger than frame time. - dxl.writeControlTableItem(PROFILE_VELOCITY, DYNAMIXEL_ID[index], frame_time); - } - } - dxl.torqueOn(DXL_BROADCAST_ID); - delay(100); -} - -// Initialize SyncRead packet in the structure -void InitSyncRead() -{ - uint8_t index = 0; - - sync_read_information.packet.p_buf = packet_buffer; - sync_read_information.packet.buf_capacity = MAX_PACKET_BUFFER_LENGTH; - sync_read_information.packet.is_completed = false; - sync_read_information.addr = PRESENT_POSITION_ADDRESS; - sync_read_information.addr_length = LENGTH_TO_SYNC_READ; - sync_read_information.p_xels = sync_read_dynamixel_info; - sync_read_information.xel_count = 0; - - for (index = 0; index < NUMBER_OF_JOINT ; index++) { - sync_read_dynamixel_info[index].id = DYNAMIXEL_ID[index]; - sync_read_dynamixel_info[index].p_recv_buf = (uint8_t*)&sync_read_data[index]; - sync_read_information.xel_count++; - } - sync_read_information.is_info_changed = true; - - ReadPresentPosition(); -} - -// Initialize SyncWrite packet frame in the structure -void InitSyncWrite() -{ - uint8_t index = 0; - - sync_write_information.packet.p_buf = nullptr; - sync_write_information.packet.is_completed = false; - sync_write_information.addr = GOAL_POSITION_ADDRESS; - sync_write_information.addr_length = LENGTH_TO_SYNC_WRITE; - sync_write_information.p_xels = sync_write_dynamixel_info; - sync_write_information.xel_count = 0; - - for (index = 0; index < NUMBER_OF_JOINT; index++) { - sync_write_dynamixel_info[index].id = DYNAMIXEL_ID[index]; - sync_write_dynamixel_info[index].p_data = (uint8_t*)&sync_write_data[index].goal_position; - sync_write_information.xel_count++; - } -} - -void InitPose(const int init_pose[][MAX_MOTION_DATA_COLUMN_INDEX], int frame_time) -{ - uint8_t index = 0; - // Store margin between Goal and Present Position. - // This determines the DYNAMIXEL where to rotate in either CW or CCW. - int arr_position_margin[NUMBER_OF_JOINT] = {}; - int arr_dxl_ready_state[NUMBER_OF_JOINT] = {}; - int init_steps = 1; - int dxl_ready_count = 0; - int position_tolerance = 5; - - for (index = 0; index < NUMBER_OF_JOINT; index++){ - if (index == 0) { - DEBUG_SERIAL.println("[Present Position] "); - } - - arr_position_margin[index] = init_pose[0][index+1] - sync_read_data[index].present_position; - DEBUG_SERIAL.print("[Present Position_Setup] ID"); - DEBUG_SERIAL.print(sync_read_information.p_xels[index].id); - DEBUG_SERIAL.print(": "); - DEBUG_SERIAL.println(sync_read_data[index].present_position); - DEBUG_SERIAL.print("[Goal Position - Present Position] ID"); - DEBUG_SERIAL.print(sync_read_information.p_xels[index].id); - DEBUG_SERIAL.print(": "); - DEBUG_SERIAL.println(arr_position_margin[index]); - } - - DEBUG_SERIAL.println(""); - - while(true) { - // Count the number of ready DYNAMIXEL. - for (index = 0; index < NUMBER_OF_JOINT; index++) { - if (arr_dxl_ready_state[index] == 1) { - dxl_ready_count++; - } else { - dxl_ready_count = 0; - break; - } - } - - if(dxl_ready_count == NUMBER_OF_JOINT) { - delay(1000); - - for (index = 0; index < NUMBER_OF_JOINT; index++) { - sync_write_data[index].goal_position = init_pose[0][index+1]; - dxl.ledOff(DYNAMIXEL_ID[index]); - } - - sync_write_information.is_info_changed = true; - dxl.syncWrite(&sync_write_information); - frame_time +=1; - DEBUG_SERIAL.println("=== [Play Motion] ==="); - break; - } - - if ( ElapsedTime() >= (unsigned long)(frame_time - 10)) { - for (index=0; index < NUMBER_OF_JOINT; index++) { - if (index == 0) { - DEBUG_SERIAL.println("[Moving to the init pose...]"); - } - - // If result in Goal Position - Present Position is positive number, movement direction is in CCW. - if (arr_position_margin[index] >= position_tolerance) { - DEBUG_SERIAL.print("DIR :CCW, "); - DEBUG_SERIAL.print("ID: "); - DEBUG_SERIAL.print(sync_read_information.p_xels[index].id); - //set position every 20 ms - arr_position_margin[index] = arr_position_margin[index] - init_steps ; - DEBUG_SERIAL.print("\t Target Pose: "); - DEBUG_SERIAL.print(init_pose[0][index+1]); - DEBUG_SERIAL.print("\t Present Pose: "); - DEBUG_SERIAL.println(init_pose[0][index+1] - arr_position_margin[index]); - } else if (arr_position_margin[index] <= -position_tolerance ) { - // If result in Goal Position - Present Position is negative number, movement direction is in CW. - DEBUG_SERIAL.print("DIR : CW, "); - DEBUG_SERIAL.print("ID: "); - DEBUG_SERIAL.print(sync_read_information.p_xels[index].id); - arr_position_margin[index] = arr_position_margin[index] + init_steps ; - DEBUG_SERIAL.print("\t Target Pose: "); - DEBUG_SERIAL.print(init_pose[0][index+1]); - DEBUG_SERIAL.print("\t Present Pose: "); - DEBUG_SERIAL.println(init_pose[0][index+1] - arr_position_margin[index]); - } else { - arr_dxl_ready_state[index] = 1; - dxl.ledOn(DYNAMIXEL_ID[index]); - DEBUG_SERIAL.print("ID: "); - DEBUG_SERIAL.print(sync_read_information.p_xels[index].id); - DEBUG_SERIAL.println(" is ready to go."); - } - - sync_write_data[index].goal_position = init_pose[0][index+1] - arr_position_margin[index]; - } - } - - if ( ElapsedTime() >= (unsigned long)(frame_time)) { - sync_write_information.is_info_changed = true; - dxl.syncWrite(&sync_write_information); - DEBUG_SERIAL.println(" SyncWrite."); - saved_time = current_time; - } - } -} - -void PlayMotion(const int MOTION_DATA[][MAX_MOTION_DATA_COLUMN_INDEX]) -{ - // Time difference between the saved_time and current time. - if ( ElapsedTime() >= (unsigned long)(MOTION_DATA[frame_time + 1][0] - MOTION_DATA[frame_time][0]) ) { - // DisplayTime(); - // ReadPresentPosition(); - - uint8_t index = 0; - - DEBUG_SERIAL.print("[Goal Position] : "); - for (index = 0; index < NUMBER_OF_JOINT; index++) { - sync_write_data[index].goal_position = MOTION_DATA[frame_time + 1][index + 1]; - DEBUG_SERIAL.print(sync_write_data[index].goal_position); - DEBUG_SERIAL.print("\t"); - } - DEBUG_SERIAL.println(""); - - sync_write_information.is_info_changed = true; - - if(!dxl.syncWrite(&sync_write_information)) { - DEBUG_SERIAL.println("[ERROR] Failed to Sync Write."); - } - - saved_time = current_time; - frame_time += 1; - - // When reached at the end of the motion frame array - if (frame_time >= TOTAL_FRAMES -1 ) { - DEBUG_SERIAL.println("=== [Motion Play Completed] ==="); - DEBUG_SERIAL.print("Total Played Frames = "); - DEBUG_SERIAL.println(frame_time + 1); - frame_time = 1; - loop_counts--; - } - } -} - -void DisplayTime() -{ - // Display current time, saved_time time, and elapsed time in two trajectory points. If not needed, comment out. - DEBUG_SERIAL.println("=== [Print Time (ms)] ==="); - DEBUG_SERIAL.print("Current: "); - DEBUG_SERIAL.print(current_time); - DEBUG_SERIAL.print("\t Saved: "); - DEBUG_SERIAL.print(saved_time); - DEBUG_SERIAL.print("\t Elapsed:"); - DEBUG_SERIAL.print(current_time - saved_time); - DEBUG_SERIAL.print("\t Motion Frame:"); - DEBUG_SERIAL.println(MOTION_DATA[frame_time+1][0]); -} - -void ReadPresentPosition() -{ - uint8_t index = 0; - - if(dxl.syncRead(&sync_read_information) > 0) { - DEBUG_SERIAL.print("[Present Position] : "); - for (index = 0; index < NUMBER_OF_JOINT; index++) { - DEBUG_SERIAL.print(sync_read_data[index].present_position); - DEBUG_SERIAL.print("\t"); - } - DEBUG_SERIAL.println(""); - } else { - DEBUG_SERIAL.println("[ERROR] Sync Read Failed"); - } -} - -unsigned long ElapsedTime() -{ - current_time = millis(); - return (current_time - saved_time); +// Copyright 2021 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. + +// Example Environment +// +// - DYNAMIXEL: X series (except XL-320) +// ID = 11, 12, 13, 14, 15, Baudrate = 4000000bps, Protocol 2.0 +// - Controller: OpenCR 1.0 +// - Library: DYNAMIXEL2Arduino +// - Software: Blender 2.93.1 +// Arduino IDE +// +// Author: David Park + +#include + +// Please modify it to suit your hardware. +#if defined(ARDUINO_AVR_UNO) || defined(ARDUINO_AVR_MEGA2560) // When using DynamixelShield + #include + SoftwareSerial soft_serial(7, 8); // DYNAMIXELShield UART RX/TX + #define DXL_SERIAL Serial + #define DEBUG_SERIAL soft_serial + const int DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN +#elif defined(ARDUINO_SAM_DUE) // When using DynamixelShield + #define DXL_SERIAL Serial + #define DEBUG_SERIAL SerialUSB + const int DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN +#elif defined(ARDUINO_SAM_ZERO) // When using DynamixelShield + #define DXL_SERIAL Serial1 + #define DEBUG_SERIAL SerialUSB + const int DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN +#elif defined(ARDUINO_OpenCM904) // When using official ROBOTIS board with DXL circuit. + #define DXL_SERIAL Serial3 //OpenCM9.04 EXP Board's DXL port Serial. (Serial1 for the DXL port on the OpenCM 9.04 board) + #define DEBUG_SERIAL Serial + const int DXL_DIR_PIN = 22; //OpenCM9.04 EXP Board's DIR PIN. (28 for the DXL port on the OpenCM 9.04 board) +#elif defined(ARDUINO_OpenCR) // When using official ROBOTIS board with DXL circuit. + // For OpenCR, there is a DXL Power Enable pin, so you must initialize and control it. + // Reference link : https://github.com/ROBOTIS-GIT/OpenCR/blob/master/arduino/opencr_arduino/opencr/libraries/DynamixelSDK/src/dynamixel_sdk/port_handler_arduino.cpp#L78 + #define DXL_SERIAL Serial3 + #define DEBUG_SERIAL Serial + const int DXL_DIR_PIN = 84; // OpenCR Board's DIR PIN. +#elif defined(ARDUINO_OpenRB) // When using OpenRB-150 + //OpenRB does not require the DIR control pin. + #define DXL_SERIAL Serial1 + #define DEBUG_SERIAL Serial + const int DXL_DIR_PIN = -1; +#else // Other boards when using DynamixelShield + #define DXL_SERIAL Serial1 + #define DEBUG_SERIAL Serial + const int DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN +#endif + + +Dynamixel2Arduino dxl(DXL_SERIAL, DXL_DIR_PIN); + +// This namespace is required to use Control table item names +using namespace ControlTableItem; + +// DYNAMIXEL Configuration +const float DYNAMIXEL_PROTOCOL_VERSION = 2.0; +// OpenMANIPULATOR has ID of 11 ~ 15 by default +const uint8_t DYNAMIXEL_ID[] = {1, 2, 3, 4, 5}; +const uint8_t NUMBER_OF_JOINT = sizeof(DYNAMIXEL_ID)/sizeof(uint8_t); + + +// Data struct for SyncRead +typedef struct sync_read_data { + int32_t present_position; +} __attribute__((packed)) sync_read_data_t; + +// Sync Read Present Position +const uint16_t PRESENT_POSITION_ADDRESS = 132; +const uint16_t LENGTH_TO_SYNC_READ = 4; + +// Sync Read +sync_read_data_t sync_read_data[NUMBER_OF_JOINT]; +DYNAMIXEL::InfoSyncReadInst_t sync_read_information; +DYNAMIXEL::XELInfoSyncRead_t sync_read_dynamixel_info[NUMBER_OF_JOINT]; + + +// Data struct for SyncWrite +typedef struct sync_write_data { + int32_t goal_position; +} __attribute__((packed)) sync_write_data_t; + +// Sync Write Goal Position +const uint16_t GOAL_POSITION_ADDRESS = 116; +const uint16_t LENGTH_TO_SYNC_WRITE = 4; + +// Sync Write +sync_write_data_t sync_write_data[NUMBER_OF_JOINT]; +DYNAMIXEL::InfoSyncWriteInst_t sync_write_information; +DYNAMIXEL::XELInfoSyncWrite_t sync_write_dynamixel_info[NUMBER_OF_JOINT]; + + +const uint16_t MAX_PACKET_BUFFER_LENGTH = 128; +uint8_t packet_buffer[MAX_PACKET_BUFFER_LENGTH]; + +int loop_counts = 2; // Number of times to play the motion +int frame_time = 0; +unsigned long current_time = 0; +unsigned long saved_time = 0; + +// The number of information in each frame. (Time + Position of each Joint) +const int MAX_MOTION_DATA_COLUMN_INDEX = 1 + NUMBER_OF_JOINT; + +// Paste all keyframes. +const int MOTION_DATA [][MAX_MOTION_DATA_COLUMN_INDEX]{ +//Time[ms],ID1,ID2,ID3,ID4,Hand, +{100,2047,1535,2957,1649,2039}, +{200,2047,1535,2957,1650,2039}, +{300,2047,1534,2956,1652,2039}, +{400,2047,1533,2956,1655,2039}, +{500,2047,1531,2954,1660,2039}, +{600,2047,1529,2953,1666,2039}, +{700,2047,1526,2951,1674,2039}, +{800,2047,1523,2949,1682,2039}, +{900,2047,1520,2947,1692,2039}, +{1000,2047,1516,2944,1702,2039}, +{1100,2047,1511,2941,1714,2039}, +{1200,2047,1507,2938,1726,2039}, +{1300,2047,1502,2934,1740,2039}, +{1400,2047,1497,2930,1754,2039}, +{1500,2047,1492,2926,1769,2039}, +{1600,2047,1486,2921,1784,2039}, +{1700,2047,1480,2916,1800,2039}, +{1800,2047,1474,2911,1817,2039}, +{1900,2047,1468,2905,1834,2039}, +{2000,2047,1462,2899,1851,2039}, +{2100,2047,1455,2893,1869,2039}, +{2200,2047,1448,2886,1887,2039}, +{2300,2047,1442,2879,1906,2039}, +{2400,2047,1435,2872,1924,2039}, +{2500,2047,1428,2865,1943,2039}, +{2600,2047,1421,2857,1962,2039}, +{2700,2047,1415,2849,1980,2039}, +{2800,2047,1408,2840,1999,2039}, +{2900,2047,1401,2831,2018,2039}, +{3000,2047,1394,2822,2036,2039}, +{3100,2047,1388,2813,2054,2039}, +{3200,2047,1381,2803,2072,2039}, +{3300,2047,1375,2793,2090,2039}, +{3400,2047,1369,2783,2107,2039}, +{3500,2047,1363,2772,2123,2039}, +{3600,2047,1357,2761,2139,2039}, +{3700,2047,1351,2750,2155,2039}, +{3800,2047,1346,2738,2170,2039}, +{3900,2047,1341,2726,2184,2039}, +{4000,2047,1336,2714,2197,2039}, +{4100,2047,1331,2702,2209,2039}, +{4200,2047,1327,2689,2221,2039}, +{4300,2047,1323,2676,2232,2039}, +{4400,2047,1320,2662,2241,2039}, +{4500,2047,1317,2648,2250,2039}, +{4600,2047,1314,2634,2257,2039}, +{4700,2047,1312,2620,2263,2039}, +{4800,2047,1310,2605,2268,2039}, +{4900,2047,1309,2590,2272,2039}, +{5000,2047,1308,2575,2274,2039}, +{5100,2047,1308,2559,2274,2039}, +{5200,2048,1308,2543,2274,2039}, +{5300,2049,1308,2527,2274,2039}, +{5400,2052,1308,2510,2274,2039}, +{5500,2056,1308,2493,2274,2039}, +{5600,2061,1308,2476,2274,2039}, +{5700,2067,1308,2459,2274,2039}, +{5800,2074,1308,2442,2274,2039}, +{5900,2082,1308,2424,2274,2039}, +{6000,2091,1308,2407,2274,2039}, +{6100,2100,1308,2389,2274,2039}, +{6200,2110,1308,2372,2274,2039}, +{6300,2121,1308,2354,2274,2039}, +{6400,2133,1308,2336,2274,2039}, +{6500,2145,1308,2318,2274,2039}, +{6600,2158,1308,2301,2274,2039}, +{6700,2171,1308,2283,2274,2039}, +{6800,2184,1308,2265,2274,2039}, +{6900,2198,1308,2248,2274,2039}, +{7000,2213,1308,2231,2274,2039}, +{7100,2227,1308,2214,2274,2039}, +{7200,2242,1308,2197,2274,2039}, +{7300,2257,1308,2180,2274,2039}, +{7400,2272,1308,2164,2274,2039}, +{7500,2288,1308,2147,2274,2039}, +{7600,2303,1308,2131,2274,2039}, +{7700,2318,1308,2116,2274,2039}, +{7800,2334,1308,2101,2274,2039}, +{7900,2349,1308,2086,2274,2039}, +{8000,2364,1308,2071,2274,2039}, +{8100,2379,1308,2057,2274,2039}, +{8200,2393,1308,2044,2274,2039}, +{8300,2408,1308,2030,2274,2039}, +{8400,2422,1308,2018,2274,2039}, +{8500,2435,1308,2006,2274,2039}, +{8600,2448,1308,1994,2274,2039}, +{8700,2461,1308,1983,2274,2039}, +{8800,2473,1308,1972,2274,2039}, +{8900,2485,1308,1963,2274,2039}, +{9000,2495,1308,1953,2274,2039}, +{9100,2506,1308,1945,2274,2039}, +{9200,2515,1308,1937,2274,2039}, +{9300,2524,1308,1930,2274,2039}, +{9400,2532,1308,1924,2274,2039}, +{9500,2539,1308,1918,2274,2039}, +{9600,2545,1308,1913,2274,2039}, +{9700,2550,1308,1909,2274,2039}, +{9800,2554,1308,1906,2274,2039}, +{9900,2556,1308,1904,2274,2039}, +{10000,2558,1308,1902,2274,2039}, +{10100,2559,1308,1902,2274,2039}, +{10200,2559,1308,1902,2274,2039}, +{10300,2559,1309,1903,2274,2039}, +{10400,2559,1312,1903,2274,2039}, +{10500,2559,1315,1904,2273,2039}, +{10600,2559,1319,1906,2273,2039}, +{10700,2559,1323,1908,2272,2039}, +{10800,2559,1329,1910,2271,2039}, +{10900,2559,1335,1912,2271,2039}, +{11000,2559,1342,1914,2270,2039}, +{11100,2559,1349,1917,2269,2039}, +{11200,2559,1357,1920,2267,2039}, +{11300,2559,1365,1923,2266,2039}, +{11400,2559,1374,1926,2265,2039}, +{11500,2559,1384,1930,2264,2039}, +{11600,2559,1394,1933,2262,2039}, +{11700,2559,1404,1937,2261,2039}, +{11800,2559,1414,1941,2259,2039}, +{11900,2559,1425,1945,2258,2039}, +{12000,2559,1436,1949,2256,2039}, +{12100,2559,1448,1953,2254,2039}, +{12200,2559,1459,1957,2253,2039}, +{12300,2559,1471,1961,2251,2039}, +{12400,2559,1483,1966,2249,2039}, +{12500,2559,1495,1970,2248,2039}, +{12600,2559,1507,1974,2246,2039}, +{12700,2559,1519,1979,2244,2039}, +{12800,2559,1531,1983,2243,2039}, +{12900,2559,1542,1987,2241,2039}, +{13000,2559,1554,1992,2239,2039}, +{13100,2559,1566,1996,2238,2039}, +{13200,2559,1577,2000,2236,2039}, +{13300,2559,1588,2004,2234,2039}, +{13400,2559,1599,2008,2233,2039}, +{13500,2559,1610,2012,2231,2039}, +{13600,2559,1620,2016,2230,2039}, +{13700,2559,1630,2019,2229,2039}, +{13800,2559,1639,2023,2227,2039}, +{13900,2559,1648,2026,2226,2039}, +{14000,2559,1656,2029,2225,2039}, +{14100,2559,1664,2032,2224,2039}, +{14200,2559,1672,2035,2222,2039}, +{14300,2559,1678,2037,2222,2039}, +{14400,2559,1685,2039,2221,2039}, +{14500,2559,1690,2041,2220,2039}, +{14600,2559,1695,2043,2219,2039}, +{14700,2559,1699,2044,2219,2039}, +{14800,2559,1702,2045,2218,2039}, +{14900,2559,1704,2046,2218,2039}, +{15000,2559,1705,2047,2218,2039}, +{15100,2559,1706,2047,2218,2039}, +{15200,2559,1706,2047,2218,2038}, +{15300,2559,1706,2047,2218,2033}, +{15400,2559,1706,2047,2218,2024}, +{15500,2559,1706,2047,2218,2013}, +{15600,2559,1706,2047,2218,1998}, +{15700,2559,1706,2047,2218,1981}, +{15800,2559,1706,2047,2218,1961}, +{15900,2559,1706,2047,2218,1939}, +{16000,2559,1706,2047,2218,1914}, +{16100,2559,1706,2047,2218,1887}, +{16200,2559,1706,2047,2218,1857}, +{16300,2559,1706,2047,2218,1826}, +{16400,2559,1706,2047,2218,1793}, +{16500,2559,1706,2047,2218,1758}, +{16600,2559,1706,2047,2218,1722}, +{16700,2559,1706,2047,2218,1684}, +{16800,2559,1706,2047,2218,1645}, +{16900,2559,1706,2047,2218,1605}, +{17000,2559,1706,2047,2218,1564}, +{17100,2559,1706,2047,2218,1522}, +{17200,2559,1706,2047,2218,1479}, +{17300,2559,1706,2047,2218,1436}, +{17400,2559,1706,2047,2218,1393}, +{17500,2559,1706,2047,2218,1349}, +{17600,2559,1706,2047,2218,1304}, +{17700,2559,1706,2047,2218,1260}, +{17800,2559,1706,2047,2218,1216}, +{17900,2559,1706,2047,2218,1173}, +{18000,2559,1706,2047,2218,1130}, +{18100,2559,1706,2047,2218,1087}, +{18200,2559,1706,2047,2218,1045}, +{18300,2559,1706,2047,2218,1004}, +{18400,2559,1706,2047,2218,964}, +{18500,2559,1706,2047,2218,925}, +{18600,2559,1706,2047,2218,887}, +{18700,2559,1706,2047,2218,851}, +{18800,2559,1706,2047,2218,816}, +{18900,2559,1706,2047,2218,783}, +{19000,2559,1706,2047,2218,752}, +{19100,2559,1706,2047,2218,722}, +{19200,2559,1706,2047,2218,695}, +{19300,2559,1706,2047,2218,670}, +{19400,2559,1706,2047,2218,648}, +{19500,2559,1706,2047,2218,628}, +{19600,2559,1706,2047,2218,611}, +{19700,2559,1706,2047,2218,596}, +{19800,2559,1706,2047,2218,585}, +{19900,2559,1706,2047,2218,576}, +{20000,2559,1706,2047,2218,571}, +{20100,2559,1706,2047,2218,570}, +{20200,2559,1705,2047,2218,570}, +{20300,2559,1704,2047,2220,570}, +{20400,2559,1702,2047,2224,570}, +{20500,2559,1699,2047,2228,570}, +{20600,2559,1695,2047,2234,570}, +{20700,2559,1690,2047,2240,570}, +{20800,2559,1685,2047,2248,570}, +{20900,2559,1678,2047,2257,570}, +{21000,2559,1672,2047,2266,570}, +{21100,2559,1664,2047,2277,570}, +{21200,2559,1656,2047,2288,570}, +{21300,2559,1648,2047,2300,570}, +{21400,2559,1639,2047,2313,570}, +{21500,2559,1630,2047,2326,570}, +{21600,2559,1620,2047,2340,570}, +{21700,2559,1610,2047,2355,570}, +{21800,2559,1599,2047,2370,570}, +{21900,2559,1588,2047,2386,570}, +{22000,2559,1577,2047,2402,570}, +{22100,2559,1566,2047,2418,570}, +{22200,2559,1554,2047,2434,570}, +{22300,2559,1542,2047,2451,570}, +{22400,2559,1531,2047,2468,570}, +{22500,2559,1519,2047,2485,570}, +{22600,2559,1507,2047,2502,570}, +{22700,2559,1495,2047,2519,570}, +{22800,2559,1483,2047,2536,570}, +{22900,2559,1471,2047,2553,570}, +{23000,2559,1459,2047,2570,570}, +{23100,2559,1448,2047,2586,570}, +{23200,2559,1436,2047,2602,570}, +{23300,2559,1425,2047,2618,570}, +{23400,2559,1414,2047,2634,570}, +{23500,2559,1404,2047,2649,570}, +{23600,2559,1394,2047,2664,570}, +{23700,2559,1384,2047,2678,570}, +{23800,2559,1374,2047,2691,570}, +{23900,2559,1365,2047,2704,570}, +{24000,2559,1357,2047,2716,570}, +{24100,2559,1349,2047,2727,570}, +{24200,2559,1342,2047,2738,570}, +{24300,2559,1335,2047,2747,570}, +{24400,2559,1329,2047,2756,570}, +{24500,2559,1323,2047,2764,570}, +{24600,2559,1319,2047,2770,570}, +{24700,2559,1315,2047,2776,570}, +{24800,2559,1312,2047,2780,570}, +{24900,2559,1309,2047,2784,570}, +{25000,2559,1308,2047,2786,570}, +{25100,2559,1308,2047,2786,570}, +{25200,2558,1308,2047,2786,570}, +{25300,2554,1308,2047,2786,570}, +{25400,2548,1308,2047,2786,570}, +{25500,2540,1308,2047,2786,570}, +{25600,2530,1308,2047,2786,570}, +{25700,2518,1308,2047,2786,570}, +{25800,2504,1308,2047,2786,570}, +{25900,2489,1308,2047,2786,570}, +{26000,2471,1308,2047,2786,570}, +{26100,2452,1308,2047,2786,570}, +{26200,2432,1308,2047,2786,570}, +{26300,2410,1308,2047,2786,570}, +{26400,2387,1308,2047,2786,570}, +{26500,2363,1308,2047,2786,570}, +{26600,2338,1308,2047,2786,570}, +{26700,2311,1308,2047,2786,570}, +{26800,2284,1308,2047,2786,570}, +{26900,2256,1308,2047,2786,570}, +{27000,2228,1308,2047,2786,570}, +{27100,2199,1308,2047,2786,570}, +{27200,2169,1308,2047,2786,570}, +{27300,2139,1308,2047,2786,570}, +{27400,2108,1308,2047,2786,570}, +{27500,2078,1308,2047,2786,570}, +{27600,2047,1308,2047,2786,570}, +{27700,2016,1308,2047,2786,570}, +{27800,1986,1308,2047,2786,570}, +{27900,1955,1308,2047,2786,570}, +{28000,1925,1308,2047,2786,570}, +{28100,1895,1308,2047,2786,570}, +{28200,1866,1308,2047,2786,570}, +{28300,1838,1308,2047,2786,570}, +{28400,1810,1308,2047,2786,570}, +{28500,1783,1308,2047,2786,570}, +{28600,1756,1308,2047,2786,570}, +{28700,1731,1308,2047,2786,570}, +{28800,1707,1308,2047,2786,570}, +{28900,1684,1308,2047,2786,570}, +{29000,1662,1308,2047,2786,570}, +{29100,1642,1308,2047,2786,570}, +{29200,1623,1308,2047,2786,570}, +{29300,1605,1308,2047,2786,570}, +{29400,1590,1308,2047,2786,570}, +{29500,1576,1308,2047,2786,570}, +{29600,1564,1308,2047,2786,570}, +{29700,1554,1308,2047,2786,570}, +{29800,1546,1308,2047,2786,570}, +{29900,1540,1308,2047,2786,570}, +{30000,1536,1308,2047,2786,570}, +{30100,1535,1308,2047,2786,570}, +{30200,1535,1308,2047,2786,570}, +{30300,1535,1309,2047,2784,570}, +{30400,1535,1312,2047,2782,570}, +{30500,1535,1315,2047,2778,570}, +{30600,1535,1319,2047,2774,570}, +{30700,1535,1323,2047,2768,570}, +{30800,1535,1329,2047,2762,570}, +{30900,1535,1335,2047,2755,570}, +{31000,1535,1342,2047,2747,570}, +{31100,1535,1349,2047,2739,570}, +{31200,1535,1357,2047,2730,570}, +{31300,1535,1365,2047,2720,570}, +{31400,1535,1374,2047,2710,570}, +{31500,1535,1384,2047,2699,570}, +{31600,1535,1394,2047,2688,570}, +{31700,1535,1404,2047,2676,570}, +{31800,1535,1414,2047,2664,570}, +{31900,1535,1425,2047,2652,570}, +{32000,1535,1436,2047,2639,570}, +{32100,1535,1448,2047,2626,570}, +{32200,1535,1459,2047,2613,570}, +{32300,1535,1471,2047,2600,570}, +{32400,1535,1483,2047,2586,570}, +{32500,1535,1495,2047,2573,570}, +{32600,1535,1507,2047,2559,570}, +{32700,1535,1519,2047,2545,570}, +{32800,1535,1531,2047,2532,570}, +{32900,1535,1542,2047,2518,570}, +{33000,1535,1554,2047,2505,570}, +{33100,1535,1566,2047,2492,570}, +{33200,1535,1577,2047,2479,570}, +{33300,1535,1588,2047,2466,570}, +{33400,1535,1599,2047,2453,570}, +{33500,1535,1610,2047,2441,570}, +{33600,1535,1620,2047,2430,570}, +{33700,1535,1630,2047,2418,570}, +{33800,1535,1639,2047,2408,570}, +{33900,1535,1648,2047,2397,570}, +{34000,1535,1656,2047,2388,570}, +{34100,1535,1664,2047,2379,570}, +{34200,1535,1672,2047,2370,570}, +{34300,1535,1678,2047,2363,570}, +{34400,1535,1685,2047,2356,570}, +{34500,1535,1690,2047,2349,570}, +{34600,1535,1695,2047,2344,570}, +{34700,1535,1699,2047,2340,570}, +{34800,1535,1702,2047,2336,570}, +{34900,1535,1704,2047,2334,570}, +{35000,1535,1705,2047,2332,570}, +{35100,1535,1706,2047,2331,570}, +{35200,1535,1706,2047,2331,571}, +{35300,1535,1706,2047,2331,576}, +{35400,1535,1706,2047,2331,585}, +{35500,1535,1706,2047,2331,596}, +{35600,1535,1706,2047,2331,611}, +{35700,1535,1706,2047,2331,628}, +{35800,1535,1706,2047,2331,648}, +{35900,1535,1706,2047,2331,670}, +{36000,1535,1706,2047,2331,695}, +{36100,1535,1706,2047,2331,722}, +{36200,1535,1706,2047,2331,752}, +{36300,1535,1706,2047,2331,783}, +{36400,1535,1706,2047,2331,816}, +{36500,1535,1706,2047,2331,851}, +{36600,1535,1706,2047,2331,887}, +{36700,1535,1706,2047,2331,925}, +{36800,1535,1706,2047,2331,964}, +{36900,1535,1706,2047,2331,1004}, +{37000,1535,1706,2047,2331,1045}, +{37100,1535,1706,2047,2331,1087}, +{37200,1535,1706,2047,2331,1130}, +{37300,1535,1706,2047,2331,1173}, +{37400,1535,1706,2047,2331,1216}, +{37500,1535,1706,2047,2331,1260}, +{37600,1535,1706,2047,2331,1305}, +{37700,1535,1706,2047,2331,1349}, +{37800,1535,1706,2047,2331,1393}, +{37900,1535,1706,2047,2331,1436}, +{38000,1535,1706,2047,2331,1479}, +{38100,1535,1706,2047,2331,1522}, +{38200,1535,1706,2047,2331,1564}, +{38300,1535,1706,2047,2331,1605}, +{38400,1535,1706,2047,2331,1645}, +{38500,1535,1706,2047,2331,1684}, +{38600,1535,1706,2047,2331,1722}, +{38700,1535,1706,2047,2331,1758}, +{38800,1535,1706,2047,2331,1793}, +{38900,1535,1706,2047,2331,1826}, +{39000,1535,1706,2047,2331,1857}, +{39100,1535,1706,2047,2331,1887}, +{39200,1535,1706,2047,2331,1914}, +{39300,1535,1706,2047,2331,1939}, +{39400,1535,1706,2047,2331,1961}, +{39500,1535,1706,2047,2331,1981}, +{39600,1535,1706,2047,2331,1998}, +{39700,1535,1706,2047,2331,2013}, +{39800,1535,1706,2047,2331,2024}, +{39900,1535,1706,2047,2331,2033}, +{40000,1535,1706,2047,2331,2038}, +{40100,1535,1706,2047,2331,2040}, +{40200,1536,1705,2047,2331,2040}, +{40300,1537,1704,2048,2331,2040}, +{40400,1540,1702,2050,2331,2040}, +{40500,1544,1699,2052,2331,2040}, +{40600,1549,1695,2055,2331,2039}, +{40700,1555,1691,2058,2331,2039}, +{40800,1561,1685,2062,2331,2039}, +{40900,1569,1679,2066,2331,2039}, +{41000,1577,1673,2071,2331,2039}, +{41100,1586,1666,2077,2331,2039}, +{41200,1596,1658,2083,2331,2039}, +{41300,1607,1650,2089,2331,2039}, +{41400,1618,1641,2096,2330,2039}, +{41500,1630,1632,2104,2330,2039}, +{41600,1642,1623,2111,2330,2039}, +{41700,1655,1613,2120,2330,2039}, +{41800,1668,1603,2128,2329,2039}, +{41900,1681,1592,2137,2329,2039}, +{42000,1695,1581,2147,2328,2039}, +{42100,1710,1570,2157,2328,2039}, +{42200,1724,1559,2167,2327,2039}, +{42300,1739,1547,2177,2327,2039}, +{42400,1754,1536,2188,2326,2039}, +{42500,1769,1524,2199,2325,2039}, +{42600,1784,1513,2210,2325,2039}, +{42700,1799,1501,2222,2324,2039}, +{42800,1814,1489,2234,2323,2039}, +{42900,1829,1478,2246,2322,2039}, +{43000,1843,1466,2259,2321,2039}, +{43100,1858,1455,2271,2320,2039}, +{43200,1873,1443,2284,2319,2039}, +{43300,1887,1432,2297,2317,2039}, +{43400,1901,1421,2310,2316,2039}, +{43500,1914,1411,2323,2315,2039}, +{43600,1927,1401,2337,2313,2039}, +{43700,1940,1391,2350,2311,2039}, +{43800,1952,1381,2364,2310,2039}, +{43900,1964,1372,2378,2308,2039}, +{44000,1975,1363,2392,2306,2039}, +{44100,1986,1355,2406,2304,2039}, +{44200,1996,1348,2420,2302,2039}, +{44300,2005,1340,2434,2300,2039}, +{44400,2013,1334,2448,2297,2039}, +{44500,2021,1328,2462,2295,2039}, +{44600,2027,1323,2476,2292,2039}, +{44700,2033,1318,2490,2290,2039}, +{44800,2038,1315,2504,2287,2039}, +{44900,2042,1312,2518,2284,2039}, +{45000,2045,1309,2531,2281,2039}, +{45100,2046,1308,2545,2278,2039}, +{45200,2047,1308,2559,2274,2039}, +{45300,2047,1308,2572,2271,2039}, +{45400,2047,1309,2586,2265,2039}, +{45500,2047,1310,2599,2259,2039}, +{45600,2047,1312,2612,2251,2039}, +{45700,2047,1314,2625,2243,2039}, +{45800,2047,1317,2638,2233,2039}, +{45900,2047,1320,2651,2223,2039}, +{46000,2047,1324,2663,2211,2039}, +{46100,2047,1328,2676,2199,2039}, +{46200,2047,1332,2688,2186,2039}, +{46300,2047,1337,2700,2172,2039}, +{46400,2047,1342,2712,2157,2039}, +{46500,2047,1347,2723,2142,2039}, +{46600,2047,1353,2735,2127,2039}, +{46700,2047,1359,2746,2110,2039}, +{46800,2047,1365,2757,2094,2039}, +{46900,2047,1371,2768,2077,2039}, +{47000,2047,1377,2779,2059,2039}, +{47100,2047,1384,2789,2041,2039}, +{47200,2047,1390,2799,2023,2039}, +{47300,2047,1397,2809,2005,2039}, +{47400,2047,1404,2818,1987,2039}, +{47500,2047,1411,2828,1969,2039}, +{47600,2047,1418,2837,1950,2039}, +{47700,2047,1425,2846,1932,2039}, +{47800,2047,1432,2854,1914,2039}, +{47900,2047,1439,2862,1896,2039}, +{48000,2047,1446,2870,1878,2039}, +{48100,2047,1452,2878,1860,2039}, +{48200,2047,1459,2885,1843,2039}, +{48300,2047,1466,2892,1826,2039}, +{48400,2047,1472,2899,1810,2039}, +{48500,2047,1478,2905,1794,2039}, +{48600,2047,1484,2911,1778,2039}, +{48700,2047,1490,2917,1763,2039}, +{48800,2047,1496,2922,1749,2039}, +{48900,2047,1501,2927,1736,2039}, +{49000,2047,1506,2932,1723,2039}, +{49100,2047,1511,2936,1711,2039}, +{49200,2047,1515,2940,1700,2039}, +{49300,2047,1519,2943,1690,2039}, +{49400,2047,1523,2947,1681,2039}, +{49500,2047,1526,2949,1673,2039}, +{49600,2047,1529,2952,1666,2039}, +{49700,2047,1531,2954,1660,2039}, +{49800,2047,1533,2955,1655,2039}, +{49900,2047,1534,2956,1652,2039}, +{50000,2047,1535,2957,1650,2039}, +{50100,2047,1535,2957,1649,2039} +}; + +// The number of key frames. +const int TOTAL_FRAMES = sizeof(MOTION_DATA) / sizeof(MOTION_DATA[0]); + +void setup() +{ + int time_between_frame = (MOTION_DATA[2][0] - MOTION_DATA[1][0]) * 2; + + // Open serial port for debugging + DEBUG_SERIAL.begin(115200); + while(!DEBUG_SERIAL); + + // Port Baudrate to communicate with connected DYNAMIXELs. If not matched, you will meet communication fail. + dxl.begin(4000000); + + delay(100); // Allow time for DYNAMIXEL to finish boot up + + // Recommended Protocol Version is "2.0" Note that DYNAMIXEL Protocol for each product may differ depending on your model in use. + dxl.setPortProtocolVersion(DYNAMIXEL_PROTOCOL_VERSION); + + InitDXL(time_between_frame); // Initialize the DYNAMIXEL + InitSyncRead(); + InitSyncWrite(); + delay(2000); + InitPose(MOTION_DATA, time_between_frame); // Pass a pose +} + +void loop() +{ + // Pass the motion data + PlayMotion(MOTION_DATA); +} + +void InitDXL(int frame_time) +{ + uint8_t index = 0; + dxl.torqueOff(DXL_BROADCAST_ID); + + // Ping DYNAMIXELs. If failed, check your Baudrate and pysical wiring connection. + for (index = 0; index < NUMBER_OF_JOINT; index++) + { + if (!dxl.ping(DYNAMIXEL_ID[index])) { + DEBUG_SERIAL.print("[ERROR] Failed to connect DYNAMIXEL "); + DEBUG_SERIAL.println(DYNAMIXEL_ID[index]); + } else { + // Set the Drive Mode as Time-based mode. + dxl.writeControlTableItem(DRIVE_MODE, DYNAMIXEL_ID[index], 4); + // Lower Return Delay Time enhances the responsiveness. + dxl.writeControlTableItem(RETURN_DELAY_TIME, DYNAMIXEL_ID[index], 0); + dxl.setOperatingMode(DYNAMIXEL_ID[index], OP_POSITION); + // Time-based should be larger than frame time. + dxl.writeControlTableItem(PROFILE_VELOCITY, DYNAMIXEL_ID[index], frame_time); + } + } + dxl.torqueOn(DXL_BROADCAST_ID); + delay(100); +} + +// Initialize SyncRead packet in the structure +void InitSyncRead() +{ + uint8_t index = 0; + + sync_read_information.packet.p_buf = packet_buffer; + sync_read_information.packet.buf_capacity = MAX_PACKET_BUFFER_LENGTH; + sync_read_information.packet.is_completed = false; + sync_read_information.addr = PRESENT_POSITION_ADDRESS; + sync_read_information.addr_length = LENGTH_TO_SYNC_READ; + sync_read_information.p_xels = sync_read_dynamixel_info; + sync_read_information.xel_count = 0; + + for (index = 0; index < NUMBER_OF_JOINT ; index++) { + sync_read_dynamixel_info[index].id = DYNAMIXEL_ID[index]; + sync_read_dynamixel_info[index].p_recv_buf = (uint8_t*)&sync_read_data[index]; + sync_read_information.xel_count++; + } + sync_read_information.is_info_changed = true; + + ReadPresentPosition(); +} + +// Initialize SyncWrite packet frame in the structure +void InitSyncWrite() +{ + uint8_t index = 0; + + sync_write_information.packet.p_buf = nullptr; + sync_write_information.packet.is_completed = false; + sync_write_information.addr = GOAL_POSITION_ADDRESS; + sync_write_information.addr_length = LENGTH_TO_SYNC_WRITE; + sync_write_information.p_xels = sync_write_dynamixel_info; + sync_write_information.xel_count = 0; + + for (index = 0; index < NUMBER_OF_JOINT; index++) { + sync_write_dynamixel_info[index].id = DYNAMIXEL_ID[index]; + sync_write_dynamixel_info[index].p_data = (uint8_t*)&sync_write_data[index].goal_position; + sync_write_information.xel_count++; + } +} + +void InitPose(const int init_pose[][MAX_MOTION_DATA_COLUMN_INDEX], int frame_time) +{ + uint8_t index = 0; + // Store margin between Goal and Present Position. + // This determines the DYNAMIXEL where to rotate in either CW or CCW. + int arr_position_margin[NUMBER_OF_JOINT] = {}; + int arr_dxl_ready_state[NUMBER_OF_JOINT] = {}; + int init_steps = 1; + int dxl_ready_count = 0; + int position_tolerance = 5; + + for (index = 0; index < NUMBER_OF_JOINT; index++){ + if (index == 0) { + DEBUG_SERIAL.println("[Present Position] "); + } + + arr_position_margin[index] = init_pose[0][index+1] - sync_read_data[index].present_position; + DEBUG_SERIAL.print("[Present Position_Setup] ID"); + DEBUG_SERIAL.print(sync_read_information.p_xels[index].id); + DEBUG_SERIAL.print(": "); + DEBUG_SERIAL.println(sync_read_data[index].present_position); + DEBUG_SERIAL.print("[Goal Position - Present Position] ID"); + DEBUG_SERIAL.print(sync_read_information.p_xels[index].id); + DEBUG_SERIAL.print(": "); + DEBUG_SERIAL.println(arr_position_margin[index]); + } + + DEBUG_SERIAL.println(""); + + while(true) { + // Count the number of ready DYNAMIXEL. + for (index = 0; index < NUMBER_OF_JOINT; index++) { + if (arr_dxl_ready_state[index] == 1) { + dxl_ready_count++; + } else { + dxl_ready_count = 0; + break; + } + } + + if(dxl_ready_count == NUMBER_OF_JOINT) { + delay(1000); + + for (index = 0; index < NUMBER_OF_JOINT; index++) { + sync_write_data[index].goal_position = init_pose[0][index+1]; + dxl.ledOff(DYNAMIXEL_ID[index]); + } + + sync_write_information.is_info_changed = true; + dxl.syncWrite(&sync_write_information); + frame_time +=1; + DEBUG_SERIAL.println("=== [Play Motion] ==="); + break; + } + + if ( ElapsedTime() >= (unsigned long)(frame_time - 10)) { + for (index=0; index < NUMBER_OF_JOINT; index++) { + if (index == 0) { + DEBUG_SERIAL.println("[Moving to the init pose...]"); + } + + // If result in Goal Position - Present Position is positive number, movement direction is in CCW. + if (arr_position_margin[index] >= position_tolerance) { + DEBUG_SERIAL.print("DIR :CCW, "); + DEBUG_SERIAL.print("ID: "); + DEBUG_SERIAL.print(sync_read_information.p_xels[index].id); + //set position every 20 ms + arr_position_margin[index] = arr_position_margin[index] - init_steps ; + DEBUG_SERIAL.print("\t Target Pose: "); + DEBUG_SERIAL.print(init_pose[0][index+1]); + DEBUG_SERIAL.print("\t Present Pose: "); + DEBUG_SERIAL.println(init_pose[0][index+1] - arr_position_margin[index]); + } else if (arr_position_margin[index] <= -position_tolerance ) { + // If result in Goal Position - Present Position is negative number, movement direction is in CW. + DEBUG_SERIAL.print("DIR : CW, "); + DEBUG_SERIAL.print("ID: "); + DEBUG_SERIAL.print(sync_read_information.p_xels[index].id); + arr_position_margin[index] = arr_position_margin[index] + init_steps ; + DEBUG_SERIAL.print("\t Target Pose: "); + DEBUG_SERIAL.print(init_pose[0][index+1]); + DEBUG_SERIAL.print("\t Present Pose: "); + DEBUG_SERIAL.println(init_pose[0][index+1] - arr_position_margin[index]); + } else { + arr_dxl_ready_state[index] = 1; + dxl.ledOn(DYNAMIXEL_ID[index]); + DEBUG_SERIAL.print("ID: "); + DEBUG_SERIAL.print(sync_read_information.p_xels[index].id); + DEBUG_SERIAL.println(" is ready to go."); + } + + sync_write_data[index].goal_position = init_pose[0][index+1] - arr_position_margin[index]; + } + } + + if ( ElapsedTime() >= (unsigned long)(frame_time)) { + sync_write_information.is_info_changed = true; + dxl.syncWrite(&sync_write_information); + DEBUG_SERIAL.println(" SyncWrite."); + saved_time = current_time; + } + } +} + +void PlayMotion(const int MOTION_DATA[][MAX_MOTION_DATA_COLUMN_INDEX]) +{ + // Time difference between the saved_time and current time. + if ( ElapsedTime() >= (unsigned long)(MOTION_DATA[frame_time + 1][0] - MOTION_DATA[frame_time][0]) ) { + // DisplayTime(); + // ReadPresentPosition(); + + uint8_t index = 0; + + DEBUG_SERIAL.print("[Goal Position] : "); + for (index = 0; index < NUMBER_OF_JOINT; index++) { + sync_write_data[index].goal_position = MOTION_DATA[frame_time + 1][index + 1]; + DEBUG_SERIAL.print(sync_write_data[index].goal_position); + DEBUG_SERIAL.print("\t"); + } + DEBUG_SERIAL.println(""); + + sync_write_information.is_info_changed = true; + + if(!dxl.syncWrite(&sync_write_information)) { + DEBUG_SERIAL.println("[ERROR] Failed to Sync Write."); + } + + saved_time = current_time; + frame_time += 1; + + // When reached at the end of the motion frame array + if (frame_time >= TOTAL_FRAMES -1 ) { + DEBUG_SERIAL.println("=== [Motion Play Completed] ==="); + DEBUG_SERIAL.print("Total Played Frames = "); + DEBUG_SERIAL.println(frame_time + 1); + frame_time = 1; + loop_counts--; + } + } +} + +void DisplayTime() +{ + // Display current time, saved_time time, and elapsed time in two trajectory points. If not needed, comment out. + DEBUG_SERIAL.println("=== [Print Time (ms)] ==="); + DEBUG_SERIAL.print("Current: "); + DEBUG_SERIAL.print(current_time); + DEBUG_SERIAL.print("\t Saved: "); + DEBUG_SERIAL.print(saved_time); + DEBUG_SERIAL.print("\t Elapsed:"); + DEBUG_SERIAL.print(current_time - saved_time); + DEBUG_SERIAL.print("\t Motion Frame:"); + DEBUG_SERIAL.println(MOTION_DATA[frame_time+1][0]); +} + +void ReadPresentPosition() +{ + uint8_t index = 0; + + if(dxl.syncRead(&sync_read_information) > 0) { + DEBUG_SERIAL.print("[Present Position] : "); + for (index = 0; index < NUMBER_OF_JOINT; index++) { + DEBUG_SERIAL.print(sync_read_data[index].present_position); + DEBUG_SERIAL.print("\t"); + } + DEBUG_SERIAL.println(""); + } else { + DEBUG_SERIAL.println("[ERROR] Sync Read Failed"); + } +} + +unsigned long ElapsedTime() +{ + current_time = millis(); + return (current_time - saved_time); } \ No newline at end of file diff --git a/examples/dynamixel_protocol/factory_reset/factory_reset.ino b/examples/dynamixel_protocol/factory_reset/factory_reset.ino index df1c2cd..a357ae8 100644 --- a/examples/dynamixel_protocol/factory_reset/factory_reset.ino +++ b/examples/dynamixel_protocol/factory_reset/factory_reset.ino @@ -1,129 +1,134 @@ -/******************************************************************************* -* 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 TIMEOUT 10 //default communication timeout 10ms -const uint8_t DXL_ID = 1; -const float DXL_PROTOCOL_VERSION = 2.0; - -bool ret = false; -uint8_t option = 0; - -Dynamixel2Arduino dxl(DXL_SERIAL, DXL_DIR_PIN); - -void setup() { - // put your setup code here, to run once: - - // 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 for terminal is opened - - // 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: - ret = false; - - // DYNAMIXEL protocol version 2.0 instruction - if(DXL_PROTOCOL_VERSION == 2.0) { - DEBUG_SERIAL.println("Select Reset Option:"); - DEBUG_SERIAL.println("[1] Reset all value"); - DEBUG_SERIAL.println("[2] Reset all value except ID"); - DEBUG_SERIAL.println("[3] Reset all value except ID and Baudrate"); - - DEBUG_SERIAL.read(); - while(DEBUG_SERIAL.available()==0); - option = DEBUG_SERIAL.read(); - - switch(option) { - case '1': // reset all value - // if DXL_ID is Broadcast ID, this instruction will not work - ret = dxl.factoryReset(DXL_ID, 0xFF, TIMEOUT); - break; - case '2': // reset all value except ID - ret = dxl.factoryReset(DXL_ID, 0x01, TIMEOUT); - break; - case '3': // reset all value except ID and Baudrate - ret = dxl.factoryReset(DXL_ID, 0x02, TIMEOUT); - break; - default: - break; - } - } - //DYNAMIXEL protocol version 1.0 instruction - else { - // if DXL_ID is Broadcast ID, this instruction will not work - DEBUG_SERIAL.println(); - DEBUG_SERIAL.println("Proceed Factory Reset? [y/n]"); - - DEBUG_SERIAL.read(); - while(DEBUG_SERIAL.available()==0); - option = DEBUG_SERIAL.read(); - - switch(option) { - case 'y': - case 'Y': - ret = dxl.factoryReset(DXL_ID, 0xFF, TIMEOUT); - break; - default: - break; - } - } - - if(ret) { - DEBUG_SERIAL.println("factory reset succeeded!"); - } else { - DEBUG_SERIAL.println("factory reset failed!"); - } - - delay(1000); +/******************************************************************************* +* Copyright 2016 ROBOTIS CO., LTD. +* +* Licensed under the Apache License, Version 2.0 (the "License"); +* you may not use this file except in compliance with the License. +* You may obtain a copy of the License at +* +* http://www.apache.org/licenses/LICENSE-2.0 +* +* Unless required by applicable law or agreed to in writing, software +* distributed under the License is distributed on an "AS IS" BASIS, +* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +* See the License for the specific language governing permissions and +* limitations under the License. +*******************************************************************************/ + +#include + +// Please modify it to suit your hardware. +#if defined(ARDUINO_AVR_UNO) || defined(ARDUINO_AVR_MEGA2560) // When using DynamixelShield + #include + SoftwareSerial soft_serial(7, 8); // DYNAMIXELShield UART RX/TX + #define DXL_SERIAL Serial + #define DEBUG_SERIAL soft_serial + const int DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN +#elif defined(ARDUINO_SAM_DUE) // When using DynamixelShield + #define DXL_SERIAL Serial + #define DEBUG_SERIAL SerialUSB + const int DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN +#elif defined(ARDUINO_SAM_ZERO) // When using DynamixelShield + #define DXL_SERIAL Serial1 + #define DEBUG_SERIAL SerialUSB + const int DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN +#elif defined(ARDUINO_OpenCM904) // When using official ROBOTIS board with DXL circuit. + #define DXL_SERIAL Serial3 //OpenCM9.04 EXP Board's DXL port Serial. (Serial1 for the DXL port on the OpenCM 9.04 board) + #define DEBUG_SERIAL Serial + const int DXL_DIR_PIN = 22; //OpenCM9.04 EXP Board's DIR PIN. (28 for the DXL port on the OpenCM 9.04 board) +#elif defined(ARDUINO_OpenCR) // When using official ROBOTIS board with DXL circuit. + // For OpenCR, there is a DXL Power Enable pin, so you must initialize and control it. + // Reference link : https://github.com/ROBOTIS-GIT/OpenCR/blob/master/arduino/opencr_arduino/opencr/libraries/DynamixelSDK/src/dynamixel_sdk/port_handler_arduino.cpp#L78 + #define DXL_SERIAL Serial3 + #define DEBUG_SERIAL Serial + const int DXL_DIR_PIN = 84; // OpenCR Board's DIR PIN. +#elif defined(ARDUINO_OpenRB) // When using OpenRB-150 + //OpenRB does not require the DIR control pin. + #define DXL_SERIAL Serial1 + #define DEBUG_SERIAL Serial + const int DXL_DIR_PIN = -1; +#else // Other boards when using DynamixelShield + #define DXL_SERIAL Serial1 + #define DEBUG_SERIAL Serial + const int DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN +#endif + +#define TIMEOUT 10 //default communication timeout 10ms +const uint8_t DXL_ID = 1; +const float DXL_PROTOCOL_VERSION = 2.0; + +bool ret = false; +uint8_t option = 0; + +Dynamixel2Arduino dxl(DXL_SERIAL, DXL_DIR_PIN); + +void setup() { + // put your setup code here, to run once: + + // 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 for terminal is opened + + // 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: + ret = false; + + // DYNAMIXEL protocol version 2.0 instruction + if(DXL_PROTOCOL_VERSION == 2.0) { + DEBUG_SERIAL.println("Select Reset Option:"); + DEBUG_SERIAL.println("[1] Reset all value"); + DEBUG_SERIAL.println("[2] Reset all value except ID"); + DEBUG_SERIAL.println("[3] Reset all value except ID and Baudrate"); + + DEBUG_SERIAL.read(); + while(DEBUG_SERIAL.available()==0); + option = DEBUG_SERIAL.read(); + + switch(option) { + case '1': // reset all value + // if DXL_ID is Broadcast ID, this instruction will not work + ret = dxl.factoryReset(DXL_ID, 0xFF, TIMEOUT); + break; + case '2': // reset all value except ID + ret = dxl.factoryReset(DXL_ID, 0x01, TIMEOUT); + break; + case '3': // reset all value except ID and Baudrate + ret = dxl.factoryReset(DXL_ID, 0x02, TIMEOUT); + break; + default: + break; + } + } + //DYNAMIXEL protocol version 1.0 instruction + else { + // if DXL_ID is Broadcast ID, this instruction will not work + DEBUG_SERIAL.println(); + DEBUG_SERIAL.println("Proceed Factory Reset? [y/n]"); + + DEBUG_SERIAL.read(); + while(DEBUG_SERIAL.available()==0); + option = DEBUG_SERIAL.read(); + + switch(option) { + case 'y': + case 'Y': + ret = dxl.factoryReset(DXL_ID, 0xFF, TIMEOUT); + break; + default: + break; + } + } + + if(ret) { + DEBUG_SERIAL.println("factory reset succeeded!"); + } else { + DEBUG_SERIAL.println("factory reset failed!"); + } + + delay(1000); } \ No newline at end of file diff --git a/examples/dynamixel_protocol/ping/ping.ino b/examples/dynamixel_protocol/ping/ping.ino index 281c73d..e291130 100644 --- a/examples/dynamixel_protocol/ping/ping.ino +++ b/examples/dynamixel_protocol/ping/ping.ino @@ -1,102 +1,107 @@ -/******************************************************************************* -* 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 TIMEOUT 10 //default communication timeout 10ms -#define BROADCAST_ID 254 -#define MODEL_NUMBER_ADDR 0 -#define MODEL_NUMBER_LENGTH 2 - -DYNAMIXEL::InfoFromPing_t recv_info[32]; //Set the maximum DYNAMIXEL in the network to 32 - -uint16_t model_num = 0; -uint8_t ret = 0; -uint8_t recv_count = 0; - -const uint8_t DXL_ID = BROADCAST_ID; -const float DXL_PROTOCOL_VERSION = 2.0; - -Dynamixel2Arduino dxl(DXL_SERIAL, DXL_DIR_PIN); - -void setup() { - // put your setup code here, to run once: - - // 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 for terminal is opened - - // 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); - - DEBUG_SERIAL.print("Ping for PROTOCOL "); - DEBUG_SERIAL.print(DXL_PROTOCOL_VERSION, 1); - DEBUG_SERIAL.print(", ID "); - DEBUG_SERIAL.println(DXL_ID); - - ret = dxl.ping(DXL_ID, recv_info, sizeof(recv_info), sizeof(recv_info)*3); - - if(ret > 0) { - while (recv_count < ret) { - DEBUG_SERIAL.print("DYNAMIXEL Detected!"); - dxl.read(recv_info[recv_count].id, MODEL_NUMBER_ADDR, MODEL_NUMBER_LENGTH, (uint8_t*)&model_num, sizeof(model_num), TIMEOUT); - DEBUG_SERIAL.print(", ID: "); - DEBUG_SERIAL.print(recv_info[recv_count].id); - DEBUG_SERIAL.print(" Model Number: "); - DEBUG_SERIAL.println(model_num); - recv_count++; - } - } else { - DEBUG_SERIAL.println("ping failed!"); - } -} - -void loop() { - // put your main code here, to run repeatedly: +/******************************************************************************* +* Copyright 2016 ROBOTIS CO., LTD. +* +* Licensed under the Apache License, Version 2.0 (the "License"); +* you may not use this file except in compliance with the License. +* You may obtain a copy of the License at +* +* http://www.apache.org/licenses/LICENSE-2.0 +* +* Unless required by applicable law or agreed to in writing, software +* distributed under the License is distributed on an "AS IS" BASIS, +* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +* See the License for the specific language governing permissions and +* limitations under the License. +*******************************************************************************/ + +#include + +// Please modify it to suit your hardware. +#if defined(ARDUINO_AVR_UNO) || defined(ARDUINO_AVR_MEGA2560) // When using DynamixelShield + #include + SoftwareSerial soft_serial(7, 8); // DYNAMIXELShield UART RX/TX + #define DXL_SERIAL Serial + #define DEBUG_SERIAL soft_serial + const int DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN +#elif defined(ARDUINO_SAM_DUE) // When using DynamixelShield + #define DXL_SERIAL Serial + #define DEBUG_SERIAL SerialUSB + const int DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN +#elif defined(ARDUINO_SAM_ZERO) // When using DynamixelShield + #define DXL_SERIAL Serial1 + #define DEBUG_SERIAL SerialUSB + const int DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN +#elif defined(ARDUINO_OpenCM904) // When using official ROBOTIS board with DXL circuit. + #define DXL_SERIAL Serial3 //OpenCM9.04 EXP Board's DXL port Serial. (Serial1 for the DXL port on the OpenCM 9.04 board) + #define DEBUG_SERIAL Serial + const int DXL_DIR_PIN = 22; //OpenCM9.04 EXP Board's DIR PIN. (28 for the DXL port on the OpenCM 9.04 board) +#elif defined(ARDUINO_OpenCR) // When using official ROBOTIS board with DXL circuit. + // For OpenCR, there is a DXL Power Enable pin, so you must initialize and control it. + // Reference link : https://github.com/ROBOTIS-GIT/OpenCR/blob/master/arduino/opencr_arduino/opencr/libraries/DynamixelSDK/src/dynamixel_sdk/port_handler_arduino.cpp#L78 + #define DXL_SERIAL Serial3 + #define DEBUG_SERIAL Serial + const int DXL_DIR_PIN = 84; // OpenCR Board's DIR PIN. +#elif defined(ARDUINO_OpenRB) // When using OpenRB-150 + //OpenRB does not require the DIR control pin. + #define DXL_SERIAL Serial1 + #define DEBUG_SERIAL Serial + const int DXL_DIR_PIN = -1; +#else // Other boards when using DynamixelShield + #define DXL_SERIAL Serial1 + #define DEBUG_SERIAL Serial + const int DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN +#endif + +#define TIMEOUT 10 //default communication timeout 10ms +#define BROADCAST_ID 254 +#define MODEL_NUMBER_ADDR 0 +#define MODEL_NUMBER_LENGTH 2 + +DYNAMIXEL::InfoFromPing_t recv_info[32]; //Set the maximum DYNAMIXEL in the network to 32 + +uint16_t model_num = 0; +uint8_t ret = 0; +uint8_t recv_count = 0; + +const uint8_t DXL_ID = BROADCAST_ID; +const float DXL_PROTOCOL_VERSION = 2.0; + +Dynamixel2Arduino dxl(DXL_SERIAL, DXL_DIR_PIN); + +void setup() { + // put your setup code here, to run once: + + // 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 for terminal is opened + + // 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); + + DEBUG_SERIAL.print("Ping for PROTOCOL "); + DEBUG_SERIAL.print(DXL_PROTOCOL_VERSION, 1); + DEBUG_SERIAL.print(", ID "); + DEBUG_SERIAL.println(DXL_ID); + + ret = dxl.ping(DXL_ID, recv_info, sizeof(recv_info), sizeof(recv_info)*3); + + if(ret > 0) { + while (recv_count < ret) { + DEBUG_SERIAL.print("DYNAMIXEL Detected!"); + dxl.read(recv_info[recv_count].id, MODEL_NUMBER_ADDR, MODEL_NUMBER_LENGTH, (uint8_t*)&model_num, sizeof(model_num), TIMEOUT); + DEBUG_SERIAL.print(", ID: "); + DEBUG_SERIAL.print(recv_info[recv_count].id); + DEBUG_SERIAL.print(" Model Number: "); + DEBUG_SERIAL.println(model_num); + recv_count++; + } + } else { + DEBUG_SERIAL.println("ping failed!"); + } +} + +void loop() { + // put your main code here, to run repeatedly: } \ No newline at end of file diff --git a/examples/dynamixel_protocol/read_ax_mx/read_ax_mx.ino b/examples/dynamixel_protocol/read_ax_mx/read_ax_mx.ino index fe6ddea..7c365f5 100644 --- a/examples/dynamixel_protocol/read_ax_mx/read_ax_mx.ino +++ b/examples/dynamixel_protocol/read_ax_mx/read_ax_mx.ino @@ -1,107 +1,112 @@ -/******************************************************************************* -* 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 - -//Please see eManual Control Table section of your DYNAMIXEL. -//This example is written for DYNAMIXEL AX & MX series with Protocol 1.0. -//For MX 2.0 with Protocol 2.0, refer to write_x.ino example. -#define ID_ADDR 3 -#define ID_ADDR_LEN 1 -#define BAUDRATE_ADDR 4 -#define BAUDRATE_ADDR_LEN 1 -#define PRESENT_POSITION_ADDR 36 -#define PRESENT_POSITION_ADDR_LEN 2 -#define TIMEOUT 10 //default communication timeout 10ms - -uint8_t returned_id = 0; -uint8_t returned_baudrate = 0; -uint16_t present_position = 0; - -const uint8_t DXL_ID = 1; -const float DXL_PROTOCOL_VERSION = 1.0; - -Dynamixel2Arduino dxl(DXL_SERIAL, DXL_DIR_PIN); - -void setup() { - // put your setup code here, to run once: - - // For Uno, Nano, Mini, and Mega, 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 for terminal is opened - - // 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); - - DEBUG_SERIAL.println("Refer to eManual for more details."); - DEBUG_SERIAL.println("https://emanual.robotis.com/docs/en/dxl/"); - DEBUG_SERIAL.print("Read for PROTOCOL "); - DEBUG_SERIAL.print(DXL_PROTOCOL_VERSION, 1); - DEBUG_SERIAL.print(", ID "); - DEBUG_SERIAL.println(DXL_ID); - - // Read DYNAMIXEL ID - dxl.read(DXL_ID, ID_ADDR, ID_ADDR_LEN, (uint8_t*)&returned_id, sizeof(returned_id), TIMEOUT); - DEBUG_SERIAL.print("ID : "); - DEBUG_SERIAL.println(returned_id); - delay(100); - // Read DYNAMIXEL Baudrate - dxl.read(DXL_ID, BAUDRATE_ADDR, BAUDRATE_ADDR_LEN, (uint8_t*)&returned_baudrate, sizeof(returned_baudrate), TIMEOUT); - DEBUG_SERIAL.print("Baud Rate : "); - DEBUG_SERIAL.println(returned_baudrate); - delay(100); - // Read DYNAMIXEL Present Position - dxl.read(DXL_ID, PRESENT_POSITION_ADDR, PRESENT_POSITION_ADDR_LEN, (uint8_t*)&present_position, sizeof(present_position), TIMEOUT); - DEBUG_SERIAL.print("Present Position : "); - DEBUG_SERIAL.println(present_position); -} - -void loop() { - // put your main code here, to run repeatedly: -} +/******************************************************************************* +* Copyright 2016 ROBOTIS CO., LTD. +* +* Licensed under the Apache License, Version 2.0 (the "License"); +* you may not use this file except in compliance with the License. +* You may obtain a copy of the License at +* +* http://www.apache.org/licenses/LICENSE-2.0 +* +* Unless required by applicable law or agreed to in writing, software +* distributed under the License is distributed on an "AS IS" BASIS, +* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +* See the License for the specific language governing permissions and +* limitations under the License. +*******************************************************************************/ + +#include + +// Please modify it to suit your hardware. +#if defined(ARDUINO_AVR_UNO) || defined(ARDUINO_AVR_MEGA2560) // When using DynamixelShield + #include + SoftwareSerial soft_serial(7, 8); // DYNAMIXELShield UART RX/TX + #define DXL_SERIAL Serial + #define DEBUG_SERIAL soft_serial + const int DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN +#elif defined(ARDUINO_SAM_DUE) // When using DynamixelShield + #define DXL_SERIAL Serial + #define DEBUG_SERIAL SerialUSB + const int DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN +#elif defined(ARDUINO_SAM_ZERO) // When using DynamixelShield + #define DXL_SERIAL Serial1 + #define DEBUG_SERIAL SerialUSB + const int DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN +#elif defined(ARDUINO_OpenCM904) // When using official ROBOTIS board with DXL circuit. + #define DXL_SERIAL Serial3 //OpenCM9.04 EXP Board's DXL port Serial. (Serial1 for the DXL port on the OpenCM 9.04 board) + #define DEBUG_SERIAL Serial + const int DXL_DIR_PIN = 22; //OpenCM9.04 EXP Board's DIR PIN. (28 for the DXL port on the OpenCM 9.04 board) +#elif defined(ARDUINO_OpenCR) // When using official ROBOTIS board with DXL circuit. + // For OpenCR, there is a DXL Power Enable pin, so you must initialize and control it. + // Reference link : https://github.com/ROBOTIS-GIT/OpenCR/blob/master/arduino/opencr_arduino/opencr/libraries/DynamixelSDK/src/dynamixel_sdk/port_handler_arduino.cpp#L78 + #define DXL_SERIAL Serial3 + #define DEBUG_SERIAL Serial + const int DXL_DIR_PIN = 84; // OpenCR Board's DIR PIN. +#elif defined(ARDUINO_OpenRB) // When using OpenRB-150 + //OpenRB does not require the DIR control pin. + #define DXL_SERIAL Serial1 + #define DEBUG_SERIAL Serial + const int DXL_DIR_PIN = -1; +#else // Other boards when using DynamixelShield + #define DXL_SERIAL Serial1 + #define DEBUG_SERIAL Serial + const int DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN +#endif + +//Please see eManual Control Table section of your DYNAMIXEL. +//This example is written for DYNAMIXEL AX & MX series with Protocol 1.0. +//For MX 2.0 with Protocol 2.0, refer to write_x.ino example. +#define ID_ADDR 3 +#define ID_ADDR_LEN 1 +#define BAUDRATE_ADDR 4 +#define BAUDRATE_ADDR_LEN 1 +#define PRESENT_POSITION_ADDR 36 +#define PRESENT_POSITION_ADDR_LEN 2 +#define TIMEOUT 10 //default communication timeout 10ms + +uint8_t returned_id = 0; +uint8_t returned_baudrate = 0; +uint16_t present_position = 0; + +const uint8_t DXL_ID = 1; +const float DXL_PROTOCOL_VERSION = 1.0; + +Dynamixel2Arduino dxl(DXL_SERIAL, DXL_DIR_PIN); + +void setup() { + // put your setup code here, to run once: + + // For Uno, Nano, Mini, and Mega, 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 for terminal is opened + + // 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); + + DEBUG_SERIAL.println("Refer to eManual for more details."); + DEBUG_SERIAL.println("https://emanual.robotis.com/docs/en/dxl/"); + DEBUG_SERIAL.print("Read for PROTOCOL "); + DEBUG_SERIAL.print(DXL_PROTOCOL_VERSION, 1); + DEBUG_SERIAL.print(", ID "); + DEBUG_SERIAL.println(DXL_ID); + + // Read DYNAMIXEL ID + dxl.read(DXL_ID, ID_ADDR, ID_ADDR_LEN, (uint8_t*)&returned_id, sizeof(returned_id), TIMEOUT); + DEBUG_SERIAL.print("ID : "); + DEBUG_SERIAL.println(returned_id); + delay(100); + // Read DYNAMIXEL Baudrate + dxl.read(DXL_ID, BAUDRATE_ADDR, BAUDRATE_ADDR_LEN, (uint8_t*)&returned_baudrate, sizeof(returned_baudrate), TIMEOUT); + DEBUG_SERIAL.print("Baud Rate : "); + DEBUG_SERIAL.println(returned_baudrate); + delay(100); + // Read DYNAMIXEL Present Position + dxl.read(DXL_ID, PRESENT_POSITION_ADDR, PRESENT_POSITION_ADDR_LEN, (uint8_t*)&present_position, sizeof(present_position), TIMEOUT); + DEBUG_SERIAL.print("Present Position : "); + DEBUG_SERIAL.println(present_position); +} + +void loop() { + // put your main code here, to run repeatedly: +} diff --git a/examples/dynamixel_protocol/read_x/read_x.ino b/examples/dynamixel_protocol/read_x/read_x.ino index d4ddae9..70b9a39 100644 --- a/examples/dynamixel_protocol/read_x/read_x.ino +++ b/examples/dynamixel_protocol/read_x/read_x.ino @@ -1,106 +1,111 @@ -/******************************************************************************* -* 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 - -//Please see eManual Control Table section of your DYNAMIXEL. -//This example is written based on DYNAMIXEL X series(excluding XL-320) -#define ID_ADDR 7 -#define ID_ADDR_LEN 1 -#define BAUDRATE_ADDR 8 -#define BAUDRATE_ADDR_LEN 1 -#define PROTOCOL_TYPE_ADDR 13 -#define PROTOCOL_TYPE_ADDR_LEN 1 -#define TIMEOUT 10 //default communication timeout 10ms - -uint8_t returned_id = 0; -uint8_t returned_baudrate = 0; -uint8_t returned_protocol = 0; - -const uint8_t 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: - - // 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 for terminal is opened - - // 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); - - DEBUG_SERIAL.println("Refer to eManual for more details."); - DEBUG_SERIAL.println("https://emanual.robotis.com/docs/en/dxl/"); - DEBUG_SERIAL.print("Read for PROTOCOL "); - DEBUG_SERIAL.print(DXL_PROTOCOL_VERSION, 1); - DEBUG_SERIAL.print(", ID "); - DEBUG_SERIAL.println(DXL_ID); - - // Read DYNAMIXEL ID - dxl.read(DXL_ID, ID_ADDR, ID_ADDR_LEN, (uint8_t*)&returned_id, sizeof(returned_id), TIMEOUT); - DEBUG_SERIAL.print("ID : "); - DEBUG_SERIAL.println(returned_id); - delay(100); - // Read DYNAMIXEL Baudrate - dxl.read(DXL_ID, BAUDRATE_ADDR, BAUDRATE_ADDR_LEN, (uint8_t*)&returned_baudrate, sizeof(returned_baudrate), TIMEOUT); - DEBUG_SERIAL.print("Baud Rate : "); - DEBUG_SERIAL.println(returned_baudrate); - delay(100); - // Read DYNAMIXEL Protocol type - dxl.read(DXL_ID, PROTOCOL_TYPE_ADDR, PROTOCOL_TYPE_ADDR_LEN, (uint8_t*)&returned_protocol, sizeof(returned_protocol), TIMEOUT); - DEBUG_SERIAL.print("Protocol Type : "); - DEBUG_SERIAL.println(returned_protocol); -} - -void loop() { - // put your main code here, to run repeatedly: -} +/******************************************************************************* +* Copyright 2016 ROBOTIS CO., LTD. +* +* Licensed under the Apache License, Version 2.0 (the "License"); +* you may not use this file except in compliance with the License. +* You may obtain a copy of the License at +* +* http://www.apache.org/licenses/LICENSE-2.0 +* +* Unless required by applicable law or agreed to in writing, software +* distributed under the License is distributed on an "AS IS" BASIS, +* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +* See the License for the specific language governing permissions and +* limitations under the License. +*******************************************************************************/ + +#include + +// Please modify it to suit your hardware. +#if defined(ARDUINO_AVR_UNO) || defined(ARDUINO_AVR_MEGA2560) // When using DynamixelShield + #include + SoftwareSerial soft_serial(7, 8); // DYNAMIXELShield UART RX/TX + #define DXL_SERIAL Serial + #define DEBUG_SERIAL soft_serial + const int DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN +#elif defined(ARDUINO_SAM_DUE) // When using DynamixelShield + #define DXL_SERIAL Serial + #define DEBUG_SERIAL SerialUSB + const int DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN +#elif defined(ARDUINO_SAM_ZERO) // When using DynamixelShield + #define DXL_SERIAL Serial1 + #define DEBUG_SERIAL SerialUSB + const int DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN +#elif defined(ARDUINO_OpenCM904) // When using official ROBOTIS board with DXL circuit. + #define DXL_SERIAL Serial3 //OpenCM9.04 EXP Board's DXL port Serial. (Serial1 for the DXL port on the OpenCM 9.04 board) + #define DEBUG_SERIAL Serial + const int DXL_DIR_PIN = 22; //OpenCM9.04 EXP Board's DIR PIN. (28 for the DXL port on the OpenCM 9.04 board) +#elif defined(ARDUINO_OpenCR) // When using official ROBOTIS board with DXL circuit. + // For OpenCR, there is a DXL Power Enable pin, so you must initialize and control it. + // Reference link : https://github.com/ROBOTIS-GIT/OpenCR/blob/master/arduino/opencr_arduino/opencr/libraries/DynamixelSDK/src/dynamixel_sdk/port_handler_arduino.cpp#L78 + #define DXL_SERIAL Serial3 + #define DEBUG_SERIAL Serial + const int DXL_DIR_PIN = 84; // OpenCR Board's DIR PIN. +#elif defined(ARDUINO_OpenRB) // When using OpenRB-150 + //OpenRB does not require the DIR control pin. + #define DXL_SERIAL Serial1 + #define DEBUG_SERIAL Serial + const int DXL_DIR_PIN = -1; +#else // Other boards when using DynamixelShield + #define DXL_SERIAL Serial1 + #define DEBUG_SERIAL Serial + const int DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN +#endif + +//Please see eManual Control Table section of your DYNAMIXEL. +//This example is written based on DYNAMIXEL X series(excluding XL-320) +#define ID_ADDR 7 +#define ID_ADDR_LEN 1 +#define BAUDRATE_ADDR 8 +#define BAUDRATE_ADDR_LEN 1 +#define PROTOCOL_TYPE_ADDR 13 +#define PROTOCOL_TYPE_ADDR_LEN 1 +#define TIMEOUT 10 //default communication timeout 10ms + +uint8_t returned_id = 0; +uint8_t returned_baudrate = 0; +uint8_t returned_protocol = 0; + +const uint8_t 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: + + // 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 for terminal is opened + + // 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); + + DEBUG_SERIAL.println("Refer to eManual for more details."); + DEBUG_SERIAL.println("https://emanual.robotis.com/docs/en/dxl/"); + DEBUG_SERIAL.print("Read for PROTOCOL "); + DEBUG_SERIAL.print(DXL_PROTOCOL_VERSION, 1); + DEBUG_SERIAL.print(", ID "); + DEBUG_SERIAL.println(DXL_ID); + + // Read DYNAMIXEL ID + dxl.read(DXL_ID, ID_ADDR, ID_ADDR_LEN, (uint8_t*)&returned_id, sizeof(returned_id), TIMEOUT); + DEBUG_SERIAL.print("ID : "); + DEBUG_SERIAL.println(returned_id); + delay(100); + // Read DYNAMIXEL Baudrate + dxl.read(DXL_ID, BAUDRATE_ADDR, BAUDRATE_ADDR_LEN, (uint8_t*)&returned_baudrate, sizeof(returned_baudrate), TIMEOUT); + DEBUG_SERIAL.print("Baud Rate : "); + DEBUG_SERIAL.println(returned_baudrate); + delay(100); + // Read DYNAMIXEL Protocol type + dxl.read(DXL_ID, PROTOCOL_TYPE_ADDR, PROTOCOL_TYPE_ADDR_LEN, (uint8_t*)&returned_protocol, sizeof(returned_protocol), TIMEOUT); + DEBUG_SERIAL.print("Protocol Type : "); + DEBUG_SERIAL.println(returned_protocol); +} + +void loop() { + // put your main code here, to run repeatedly: +} diff --git a/examples/dynamixel_protocol/reboot/reboot.ino b/examples/dynamixel_protocol/reboot/reboot.ino index 8af4e5a..2d8b1d0 100644 --- a/examples/dynamixel_protocol/reboot/reboot.ino +++ b/examples/dynamixel_protocol/reboot/reboot.ino @@ -1,96 +1,101 @@ -/******************************************************************************* -* 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 TIMEOUT 10 //default communication timeout 10ms -const uint8_t DXL_ID = 1; -const float DXL_PROTOCOL_VERSION = 2.0; -uint8_t option = 0; - -Dynamixel2Arduino dxl(DXL_SERIAL, DXL_DIR_PIN); - -void setup() { - // put your setup code here, to run once: - - // 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 for terminal is opened - - // 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: - - bool ret = false; - DEBUG_SERIAL.println(); - DEBUG_SERIAL.println("Reboot DYNAMIXEL? [y/n]"); - - DEBUG_SERIAL.read(); - while(DEBUG_SERIAL.available()==0); - option = DEBUG_SERIAL.read(); - - switch(option) { - case 'y': - case 'Y': - ret = dxl.reboot(DXL_ID, TIMEOUT); - break; - default: - break; - } - if(ret) { - DEBUG_SERIAL.println("Reboot Succeeded!"); - } else { - DEBUG_SERIAL.println("Reboot Failed!"); - } - - delay(1000); +/******************************************************************************* +* Copyright 2016 ROBOTIS CO., LTD. +* +* Licensed under the Apache License, Version 2.0 (the "License"); +* you may not use this file except in compliance with the License. +* You may obtain a copy of the License at +* +* http://www.apache.org/licenses/LICENSE-2.0 +* +* Unless required by applicable law or agreed to in writing, software +* distributed under the License is distributed on an "AS IS" BASIS, +* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +* See the License for the specific language governing permissions and +* limitations under the License. +*******************************************************************************/ + +#include + +// Please modify it to suit your hardware. +#if defined(ARDUINO_AVR_UNO) || defined(ARDUINO_AVR_MEGA2560) // When using DynamixelShield + #include + SoftwareSerial soft_serial(7, 8); // DYNAMIXELShield UART RX/TX + #define DXL_SERIAL Serial + #define DEBUG_SERIAL soft_serial + const int DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN +#elif defined(ARDUINO_SAM_DUE) // When using DynamixelShield + #define DXL_SERIAL Serial + #define DEBUG_SERIAL SerialUSB + const int DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN +#elif defined(ARDUINO_SAM_ZERO) // When using DynamixelShield + #define DXL_SERIAL Serial1 + #define DEBUG_SERIAL SerialUSB + const int DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN +#elif defined(ARDUINO_OpenCM904) // When using official ROBOTIS board with DXL circuit. + #define DXL_SERIAL Serial3 //OpenCM9.04 EXP Board's DXL port Serial. (Serial1 for the DXL port on the OpenCM 9.04 board) + #define DEBUG_SERIAL Serial + const int DXL_DIR_PIN = 22; //OpenCM9.04 EXP Board's DIR PIN. (28 for the DXL port on the OpenCM 9.04 board) +#elif defined(ARDUINO_OpenCR) // When using official ROBOTIS board with DXL circuit. + // For OpenCR, there is a DXL Power Enable pin, so you must initialize and control it. + // Reference link : https://github.com/ROBOTIS-GIT/OpenCR/blob/master/arduino/opencr_arduino/opencr/libraries/DynamixelSDK/src/dynamixel_sdk/port_handler_arduino.cpp#L78 + #define DXL_SERIAL Serial3 + #define DEBUG_SERIAL Serial + const int DXL_DIR_PIN = 84; // OpenCR Board's DIR PIN. +#elif defined(ARDUINO_OpenRB) // When using OpenRB-150 + //OpenRB does not require the DIR control pin. + #define DXL_SERIAL Serial1 + #define DEBUG_SERIAL Serial + const int DXL_DIR_PIN = -1; +#else // Other boards when using DynamixelShield + #define DXL_SERIAL Serial1 + #define DEBUG_SERIAL Serial + const int DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN +#endif + +#define TIMEOUT 10 //default communication timeout 10ms +const uint8_t DXL_ID = 1; +const float DXL_PROTOCOL_VERSION = 2.0; +uint8_t option = 0; + +Dynamixel2Arduino dxl(DXL_SERIAL, DXL_DIR_PIN); + +void setup() { + // put your setup code here, to run once: + + // 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 for terminal is opened + + // 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: + + bool ret = false; + DEBUG_SERIAL.println(); + DEBUG_SERIAL.println("Reboot DYNAMIXEL? [y/n]"); + + DEBUG_SERIAL.read(); + while(DEBUG_SERIAL.available()==0); + option = DEBUG_SERIAL.read(); + + switch(option) { + case 'y': + case 'Y': + ret = dxl.reboot(DXL_ID, TIMEOUT); + break; + default: + break; + } + if(ret) { + DEBUG_SERIAL.println("Reboot Succeeded!"); + } else { + DEBUG_SERIAL.println("Reboot Failed!"); + } + + delay(1000); } \ No newline at end of file diff --git a/examples/dynamixel_protocol/write_ax_mx/write_ax_mx.ino b/examples/dynamixel_protocol/write_ax_mx/write_ax_mx.ino index 15f6b54..5072d5f 100644 --- a/examples/dynamixel_protocol/write_ax_mx/write_ax_mx.ino +++ b/examples/dynamixel_protocol/write_ax_mx/write_ax_mx.ino @@ -1,135 +1,140 @@ -/******************************************************************************* -* 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 - -//Please see eManual Control Table section of your DYNAMIXEL. -//This example is written for DYNAMIXEL AX & MX series with Protocol 1.0. -//For MX 2.0 with Protocol 2.0, refer to write_x.ino example. -#define CW_ANGLE_LIMIT_ADDR 6 -#define CCW_ANGLE_LIMIT_ADDR 8 -#define ANGLE_LIMIT_ADDR_LEN 2 -#define OPERATING_MODE_ADDR_LEN 2 -#define TORQUE_ENABLE_ADDR 24 -#define TORQUE_ENABLE_ADDR_LEN 1 -#define LED_ADDR 25 -#define LED_ADDR_LEN 1 -#define GOAL_POSITION_ADDR 30 -#define GOAL_POSITION_ADDR_LEN 2 -#define PRESENT_POSITION_ADDR 36 -#define PRESENT_POSITION_ADDR_LEN 2 -#define TIMEOUT 10 //default communication timeout 10ms - -uint8_t turn_on = 1; -uint8_t turn_off = 0; - -const uint8_t DXL_ID = 1; -const float DXL_PROTOCOL_VERSION = 1.0; - -uint16_t goalPosition1 = 0; -uint16_t goalPosition2 = 1023; - -Dynamixel2Arduino dxl(DXL_SERIAL, DXL_DIR_PIN); - -void setup() { - // put your setup code here, to run once: - - // For Uno, Nano, Mini, and Mega, 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 for terminal is opened - - // 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); - - // Turn off torque when configuring items in EEPROM area - if(dxl.write(DXL_ID, TORQUE_ENABLE_ADDR, (uint8_t*)&turn_off , TORQUE_ENABLE_ADDR_LEN, TIMEOUT)) - DEBUG_SERIAL.println("DYNAMIXEL Torque off"); - else - DEBUG_SERIAL.println("Error: Torque off failed"); - - // Set to Joint Mode - if(dxl.write(DXL_ID, CW_ANGLE_LIMIT_ADDR, (uint8_t*)&goalPosition1, ANGLE_LIMIT_ADDR_LEN, TIMEOUT) - && dxl.write(DXL_ID, CCW_ANGLE_LIMIT_ADDR, (uint8_t*)&goalPosition2, ANGLE_LIMIT_ADDR_LEN, TIMEOUT)) - DEBUG_SERIAL.println("Set operating mode"); - else - DEBUG_SERIAL.println("Error: Set operating mode failed"); - - // Turn on torque - if(dxl.write(DXL_ID, TORQUE_ENABLE_ADDR, (uint8_t*)&turn_on, TORQUE_ENABLE_ADDR_LEN, TIMEOUT)) - DEBUG_SERIAL.println("Torque on"); - else - DEBUG_SERIAL.println("Error: Torque on failed"); -} - -void loop() { - // put your main code here, to run repeatedly: - - // LED On - DEBUG_SERIAL.println("LED ON"); - dxl.write(DXL_ID, LED_ADDR, (uint8_t*)&turn_on, LED_ADDR_LEN, TIMEOUT); - delay(500); - - // Please refer to e-Manual(http://emanual.robotis.com/docs/en/parts/interface/dynamixel_shield/) for available range of value. - // Set Goal Position - DEBUG_SERIAL.print("Goal Position : "); - DEBUG_SERIAL.println(goalPosition1); - dxl.write(DXL_ID, GOAL_POSITION_ADDR, (uint8_t*)&goalPosition1, GOAL_POSITION_ADDR_LEN, TIMEOUT); - delay(1000); - - // LED Off - DEBUG_SERIAL.println("LED OFF"); - dxl.write(DXL_ID, LED_ADDR, (uint8_t*)&turn_off, LED_ADDR_LEN, TIMEOUT); - delay(500); - - // Set Goal Position - DEBUG_SERIAL.print("Goal Position : "); - DEBUG_SERIAL.println(goalPosition2); - dxl.write(DXL_ID, GOAL_POSITION_ADDR, (uint8_t*)&goalPosition2, GOAL_POSITION_ADDR_LEN, TIMEOUT); - delay(1000); -} +/******************************************************************************* +* Copyright 2016 ROBOTIS CO., LTD. +* +* Licensed under the Apache License, Version 2.0 (the "License"); +* you may not use this file except in compliance with the License. +* You may obtain a copy of the License at +* +* http://www.apache.org/licenses/LICENSE-2.0 +* +* Unless required by applicable law or agreed to in writing, software +* distributed under the License is distributed on an "AS IS" BASIS, +* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +* See the License for the specific language governing permissions and +* limitations under the License. +*******************************************************************************/ + +#include + +// Please modify it to suit your hardware. +#if defined(ARDUINO_AVR_UNO) || defined(ARDUINO_AVR_MEGA2560) // When using DynamixelShield + #include + SoftwareSerial soft_serial(7, 8); // DYNAMIXELShield UART RX/TX + #define DXL_SERIAL Serial + #define DEBUG_SERIAL soft_serial + const int DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN +#elif defined(ARDUINO_SAM_DUE) // When using DynamixelShield + #define DXL_SERIAL Serial + #define DEBUG_SERIAL SerialUSB + const int DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN +#elif defined(ARDUINO_SAM_ZERO) // When using DynamixelShield + #define DXL_SERIAL Serial1 + #define DEBUG_SERIAL SerialUSB + const int DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN +#elif defined(ARDUINO_OpenCM904) // When using official ROBOTIS board with DXL circuit. + #define DXL_SERIAL Serial3 //OpenCM9.04 EXP Board's DXL port Serial. (Serial1 for the DXL port on the OpenCM 9.04 board) + #define DEBUG_SERIAL Serial + const int DXL_DIR_PIN = 22; //OpenCM9.04 EXP Board's DIR PIN. (28 for the DXL port on the OpenCM 9.04 board) +#elif defined(ARDUINO_OpenCR) // When using official ROBOTIS board with DXL circuit. + // For OpenCR, there is a DXL Power Enable pin, so you must initialize and control it. + // Reference link : https://github.com/ROBOTIS-GIT/OpenCR/blob/master/arduino/opencr_arduino/opencr/libraries/DynamixelSDK/src/dynamixel_sdk/port_handler_arduino.cpp#L78 + #define DXL_SERIAL Serial3 + #define DEBUG_SERIAL Serial + const int DXL_DIR_PIN = 84; // OpenCR Board's DIR PIN. +#elif defined(ARDUINO_OpenRB) // When using OpenRB-150 + //OpenRB does not require the DIR control pin. + #define DXL_SERIAL Serial1 + #define DEBUG_SERIAL Serial + const int DXL_DIR_PIN = -1; +#else // Other boards when using DynamixelShield + #define DXL_SERIAL Serial1 + #define DEBUG_SERIAL Serial + const int DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN +#endif + +//Please see eManual Control Table section of your DYNAMIXEL. +//This example is written for DYNAMIXEL AX & MX series with Protocol 1.0. +//For MX 2.0 with Protocol 2.0, refer to write_x.ino example. +#define CW_ANGLE_LIMIT_ADDR 6 +#define CCW_ANGLE_LIMIT_ADDR 8 +#define ANGLE_LIMIT_ADDR_LEN 2 +#define OPERATING_MODE_ADDR_LEN 2 +#define TORQUE_ENABLE_ADDR 24 +#define TORQUE_ENABLE_ADDR_LEN 1 +#define LED_ADDR 25 +#define LED_ADDR_LEN 1 +#define GOAL_POSITION_ADDR 30 +#define GOAL_POSITION_ADDR_LEN 2 +#define PRESENT_POSITION_ADDR 36 +#define PRESENT_POSITION_ADDR_LEN 2 +#define TIMEOUT 10 //default communication timeout 10ms + +uint8_t turn_on = 1; +uint8_t turn_off = 0; + +const uint8_t DXL_ID = 1; +const float DXL_PROTOCOL_VERSION = 1.0; + +uint16_t goalPosition1 = 0; +uint16_t goalPosition2 = 1023; + +Dynamixel2Arduino dxl(DXL_SERIAL, DXL_DIR_PIN); + +void setup() { + // put your setup code here, to run once: + + // For Uno, Nano, Mini, and Mega, 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 for terminal is opened + + // 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); + + // Turn off torque when configuring items in EEPROM area + if(dxl.write(DXL_ID, TORQUE_ENABLE_ADDR, (uint8_t*)&turn_off , TORQUE_ENABLE_ADDR_LEN, TIMEOUT)) + DEBUG_SERIAL.println("DYNAMIXEL Torque off"); + else + DEBUG_SERIAL.println("Error: Torque off failed"); + + // Set to Joint Mode + if(dxl.write(DXL_ID, CW_ANGLE_LIMIT_ADDR, (uint8_t*)&goalPosition1, ANGLE_LIMIT_ADDR_LEN, TIMEOUT) + && dxl.write(DXL_ID, CCW_ANGLE_LIMIT_ADDR, (uint8_t*)&goalPosition2, ANGLE_LIMIT_ADDR_LEN, TIMEOUT)) + DEBUG_SERIAL.println("Set operating mode"); + else + DEBUG_SERIAL.println("Error: Set operating mode failed"); + + // Turn on torque + if(dxl.write(DXL_ID, TORQUE_ENABLE_ADDR, (uint8_t*)&turn_on, TORQUE_ENABLE_ADDR_LEN, TIMEOUT)) + DEBUG_SERIAL.println("Torque on"); + else + DEBUG_SERIAL.println("Error: Torque on failed"); +} + +void loop() { + // put your main code here, to run repeatedly: + + // LED On + DEBUG_SERIAL.println("LED ON"); + dxl.write(DXL_ID, LED_ADDR, (uint8_t*)&turn_on, LED_ADDR_LEN, TIMEOUT); + delay(500); + + // Please refer to e-Manual(http://emanual.robotis.com/docs/en/parts/interface/dynamixel_shield/) for available range of value. + // Set Goal Position + DEBUG_SERIAL.print("Goal Position : "); + DEBUG_SERIAL.println(goalPosition1); + dxl.write(DXL_ID, GOAL_POSITION_ADDR, (uint8_t*)&goalPosition1, GOAL_POSITION_ADDR_LEN, TIMEOUT); + delay(1000); + + // LED Off + DEBUG_SERIAL.println("LED OFF"); + dxl.write(DXL_ID, LED_ADDR, (uint8_t*)&turn_off, LED_ADDR_LEN, TIMEOUT); + delay(500); + + // Set Goal Position + DEBUG_SERIAL.print("Goal Position : "); + DEBUG_SERIAL.println(goalPosition2); + dxl.write(DXL_ID, GOAL_POSITION_ADDR, (uint8_t*)&goalPosition2, GOAL_POSITION_ADDR_LEN, TIMEOUT); + delay(1000); +} diff --git a/examples/dynamixel_protocol/write_x/write_x.ino b/examples/dynamixel_protocol/write_x/write_x.ino index 360261e..5da050d 100644 --- a/examples/dynamixel_protocol/write_x/write_x.ino +++ b/examples/dynamixel_protocol/write_x/write_x.ino @@ -1,133 +1,138 @@ -/******************************************************************************* -* 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 - -//Please see eManual Control Table section of your DYNAMIXEL. -//This example is written for DYNAMIXEL X series(excluding XL-320) -#define OPERATING_MODE_ADDR 11 -#define OPERATING_MODE_ADDR_LEN 1 -#define TORQUE_ENABLE_ADDR 64 -#define TORQUE_ENABLE_ADDR_LEN 1 -#define LED_ADDR 65 -#define LED_ADDR_LEN 1 -#define GOAL_POSITION_ADDR 116 -#define GOAL_POSITION_ADDR_LEN 4 -#define PRESENT_POSITION_ADDR 132 -#define PRESENT_POSITION_ADDR_LEN 4 -#define POSITION_CONTROL_MODE 3 -#define TIMEOUT 10 //default communication timeout 10ms - -uint8_t turn_on = 1; -uint8_t turn_off = 0; - -const uint8_t DXL_ID = 1; -const float DXL_PROTOCOL_VERSION = 2.0; - -uint8_t operatingMode = POSITION_CONTROL_MODE; -uint32_t goalPosition1 = 512; -uint32_t goalPosition2 = 2048; - -Dynamixel2Arduino dxl(DXL_SERIAL, DXL_DIR_PIN); - -void setup() { - // put your setup code here, to run once: - - // 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 for terminal is opened - - // 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); - - // Turn off torque when configuring items in EEPROM area - if(dxl.write(DXL_ID, TORQUE_ENABLE_ADDR, (uint8_t*)&turn_off , TORQUE_ENABLE_ADDR_LEN, TIMEOUT)) - DEBUG_SERIAL.println("DYNAMIXEL Torque off"); - else - DEBUG_SERIAL.println("Error: Torque off failed"); - - // Set Operating Mode - if(dxl.write(DXL_ID, OPERATING_MODE_ADDR, (uint8_t*)&operatingMode, OPERATING_MODE_ADDR_LEN, TIMEOUT)) - DEBUG_SERIAL.println("Set operating mode"); - else - DEBUG_SERIAL.println("Error: Set operating mode failed"); - - // Turn on torque - if(dxl.write(DXL_ID, TORQUE_ENABLE_ADDR, (uint8_t*)&turn_on, TORQUE_ENABLE_ADDR_LEN, TIMEOUT)) - DEBUG_SERIAL.println("Torque on"); - else - DEBUG_SERIAL.println("Error: Torque on failed"); -} - -void loop() { - // put your main code here, to run repeatedly: - - // LED On - DEBUG_SERIAL.println("LED ON"); - dxl.write(DXL_ID, LED_ADDR, (uint8_t*)&turn_on, LED_ADDR_LEN, TIMEOUT); - delay(500); - - // Please refer to e-Manual(http://emanual.robotis.com/docs/en/parts/interface/dynamixel_shield/) for available range of value. - // Set Goal Position - DEBUG_SERIAL.print("Goal Position : "); - DEBUG_SERIAL.println(goalPosition1); - dxl.write(DXL_ID, GOAL_POSITION_ADDR, (uint8_t*)&goalPosition1, GOAL_POSITION_ADDR_LEN, TIMEOUT); - delay(1000); - - // LED Off - DEBUG_SERIAL.println("LED OFF"); - dxl.write(DXL_ID, LED_ADDR, (uint8_t*)&turn_off, LED_ADDR_LEN, TIMEOUT); - delay(500); - - // Set Goal Position - DEBUG_SERIAL.print("Goal Position : "); - DEBUG_SERIAL.println(goalPosition2); - dxl.write(DXL_ID, GOAL_POSITION_ADDR, (uint8_t*)&goalPosition2, GOAL_POSITION_ADDR_LEN, TIMEOUT); - delay(1000); -} +/******************************************************************************* +* Copyright 2016 ROBOTIS CO., LTD. +* +* Licensed under the Apache License, Version 2.0 (the "License"); +* you may not use this file except in compliance with the License. +* You may obtain a copy of the License at +* +* http://www.apache.org/licenses/LICENSE-2.0 +* +* Unless required by applicable law or agreed to in writing, software +* distributed under the License is distributed on an "AS IS" BASIS, +* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +* See the License for the specific language governing permissions and +* limitations under the License. +*******************************************************************************/ + +#include + +// Please modify it to suit your hardware. +#if defined(ARDUINO_AVR_UNO) || defined(ARDUINO_AVR_MEGA2560) // When using DynamixelShield + #include + SoftwareSerial soft_serial(7, 8); // DYNAMIXELShield UART RX/TX + #define DXL_SERIAL Serial + #define DEBUG_SERIAL soft_serial + const int DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN +#elif defined(ARDUINO_SAM_DUE) // When using DynamixelShield + #define DXL_SERIAL Serial + #define DEBUG_SERIAL SerialUSB + const int DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN +#elif defined(ARDUINO_SAM_ZERO) // When using DynamixelShield + #define DXL_SERIAL Serial1 + #define DEBUG_SERIAL SerialUSB + const int DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN +#elif defined(ARDUINO_OpenCM904) // When using official ROBOTIS board with DXL circuit. + #define DXL_SERIAL Serial3 //OpenCM9.04 EXP Board's DXL port Serial. (Serial1 for the DXL port on the OpenCM 9.04 board) + #define DEBUG_SERIAL Serial + const int DXL_DIR_PIN = 22; //OpenCM9.04 EXP Board's DIR PIN. (28 for the DXL port on the OpenCM 9.04 board) +#elif defined(ARDUINO_OpenCR) // When using official ROBOTIS board with DXL circuit. + // For OpenCR, there is a DXL Power Enable pin, so you must initialize and control it. + // Reference link : https://github.com/ROBOTIS-GIT/OpenCR/blob/master/arduino/opencr_arduino/opencr/libraries/DynamixelSDK/src/dynamixel_sdk/port_handler_arduino.cpp#L78 + #define DXL_SERIAL Serial3 + #define DEBUG_SERIAL Serial + const int DXL_DIR_PIN = 84; // OpenCR Board's DIR PIN. +#elif defined(ARDUINO_OpenRB) // When using OpenRB-150 + //OpenRB does not require the DIR control pin. + #define DXL_SERIAL Serial1 + #define DEBUG_SERIAL Serial + const int DXL_DIR_PIN = -1; +#else // Other boards when using DynamixelShield + #define DXL_SERIAL Serial1 + #define DEBUG_SERIAL Serial + const int DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN +#endif + +//Please see eManual Control Table section of your DYNAMIXEL. +//This example is written for DYNAMIXEL X series(excluding XL-320) +#define OPERATING_MODE_ADDR 11 +#define OPERATING_MODE_ADDR_LEN 1 +#define TORQUE_ENABLE_ADDR 64 +#define TORQUE_ENABLE_ADDR_LEN 1 +#define LED_ADDR 65 +#define LED_ADDR_LEN 1 +#define GOAL_POSITION_ADDR 116 +#define GOAL_POSITION_ADDR_LEN 4 +#define PRESENT_POSITION_ADDR 132 +#define PRESENT_POSITION_ADDR_LEN 4 +#define POSITION_CONTROL_MODE 3 +#define TIMEOUT 10 //default communication timeout 10ms + +uint8_t turn_on = 1; +uint8_t turn_off = 0; + +const uint8_t DXL_ID = 1; +const float DXL_PROTOCOL_VERSION = 2.0; + +uint8_t operatingMode = POSITION_CONTROL_MODE; +uint32_t goalPosition1 = 512; +uint32_t goalPosition2 = 2048; + +Dynamixel2Arduino dxl(DXL_SERIAL, DXL_DIR_PIN); + +void setup() { + // put your setup code here, to run once: + + // 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 for terminal is opened + + // 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); + + // Turn off torque when configuring items in EEPROM area + if(dxl.write(DXL_ID, TORQUE_ENABLE_ADDR, (uint8_t*)&turn_off , TORQUE_ENABLE_ADDR_LEN, TIMEOUT)) + DEBUG_SERIAL.println("DYNAMIXEL Torque off"); + else + DEBUG_SERIAL.println("Error: Torque off failed"); + + // Set Operating Mode + if(dxl.write(DXL_ID, OPERATING_MODE_ADDR, (uint8_t*)&operatingMode, OPERATING_MODE_ADDR_LEN, TIMEOUT)) + DEBUG_SERIAL.println("Set operating mode"); + else + DEBUG_SERIAL.println("Error: Set operating mode failed"); + + // Turn on torque + if(dxl.write(DXL_ID, TORQUE_ENABLE_ADDR, (uint8_t*)&turn_on, TORQUE_ENABLE_ADDR_LEN, TIMEOUT)) + DEBUG_SERIAL.println("Torque on"); + else + DEBUG_SERIAL.println("Error: Torque on failed"); +} + +void loop() { + // put your main code here, to run repeatedly: + + // LED On + DEBUG_SERIAL.println("LED ON"); + dxl.write(DXL_ID, LED_ADDR, (uint8_t*)&turn_on, LED_ADDR_LEN, TIMEOUT); + delay(500); + + // Please refer to e-Manual(http://emanual.robotis.com/docs/en/parts/interface/dynamixel_shield/) for available range of value. + // Set Goal Position + DEBUG_SERIAL.print("Goal Position : "); + DEBUG_SERIAL.println(goalPosition1); + dxl.write(DXL_ID, GOAL_POSITION_ADDR, (uint8_t*)&goalPosition1, GOAL_POSITION_ADDR_LEN, TIMEOUT); + delay(1000); + + // LED Off + DEBUG_SERIAL.println("LED OFF"); + dxl.write(DXL_ID, LED_ADDR, (uint8_t*)&turn_off, LED_ADDR_LEN, TIMEOUT); + delay(500); + + // Set Goal Position + DEBUG_SERIAL.print("Goal Position : "); + DEBUG_SERIAL.println(goalPosition2); + dxl.write(DXL_ID, GOAL_POSITION_ADDR, (uint8_t*)&goalPosition2, GOAL_POSITION_ADDR_LEN, TIMEOUT); + delay(1000); +} diff --git a/install.sh b/install.sh index 72bf0e5..d1ddbb5 100644 --- a/install.sh +++ b/install.sh @@ -10,7 +10,7 @@ fi # this will be eval'd in the functions below because arrays can't be exported # For creating a new main platform, please refer to https://arduino.github.io/arduino-cli/latest/platform-specification/#hardware-folders-structure # and https://arduino.github.io/arduino-cli/latest/platform-specification/#boardstxt -export MAIN_PLATFORMS='declare -A main_platforms=([uno]="arduino:avr:uno" [mega2560]="arduino:avr:mega:cpu=atmega2560" [leonardo]="arduino:avr:leonardo" [due]="arduino:sam:arduino_due_x" [zero]="arduino:samd:arduino_zero_native" [mzero]="arduino:samd:mzero_bl" [mzeropro]="arduino:samd:mzero_pro_bl" [mkrzero]="arduino:samd:mkrzero" [mkr1000]="arduino:samd:mkr1000" [mkrwifi1010]="arduino:samd:mkrwifi1010" [opencr]="OpenCR:OpenCR:OpenCR" [portenta]="arduino:mbed_portenta:envie_m7")' +export MAIN_PLATFORMS='declare -A main_platforms=([uno]="arduino:avr:uno" [mega2560]="arduino:avr:mega:cpu=atmega2560" [leonardo]="arduino:avr:leonardo" [due]="arduino:sam:arduino_due_x" [zero]="arduino:samd:arduino_zero_native" [mzero]="arduino:samd:mzero_bl" [mzeropro]="arduino:samd:mzero_pro_bl" [mkrzero]="arduino:samd:mkrzero" [mkr1000]="arduino:samd:mkr1000" [mkrwifi1010]="arduino:samd:mkrwifi1010" [opencr]="OpenCR:OpenCR:OpenCR" [openrb-150]="OpenRB-150:samd:OpenRB-150" [portenta]="arduino:mbed_portenta:envie_m7")' # make display available for arduino CLI /sbin/start-stop-daemon --start --quiet --pidfile /tmp/custom_xvfb_1.pid --make-pidfile --background --exec /usr/bin/Xvfb -- :1 -ac -screen 0 1280x1024x16 @@ -38,14 +38,18 @@ sudo apt-get update sudo apt-get install libc6:i386 echo -n "ADD PACKAGE INDEX: " -DEPENDENCY_OUTPUT=$(arduino --pref "boardsmanager.additional.urls=https://raw.githubusercontent.com/ROBOTIS-GIT/OpenCR/master/arduino/opencr_release/package_opencr_index.json,https://raw.githubusercontent.com/ROBOTIS-GIT/OpenCM9.04/master/arduino/opencm_release/package_opencm9.04_index.json,https://github.com/espressif/arduino-esp32/releases/download/1.0.1/package_esp32_index.json" --save-prefs 2>&1) +DEPENDENCY_OUTPUT=$(arduino --pref "boardsmanager.additional.urls=https://github.com/espressif/arduino-esp32/releases/download/1.0.1/package_esp32_index.json" --save-prefs 2>&1) if [ $? -ne 0 ]; then echo -e "\xe2\x9c\x96"; else echo -e "\xe2\x9c\x93"; fi # install other board packages -echo -n "ADD OpenCR PACKAGE INDEX: " -DEPENDENCY_OUTPUT=$(arduino --pref "boardsmanager.additional.urls=https://raw.githubusercontent.com/ROBOTIS-GIT/OpenCR/master/arduino/opencr_release/package_opencr_index.json" --save-prefs 2>&1) +echo -n "ADD OpenRB-150 PACKAGE INDEX: " +DEPENDENCY_OUTPUT=$(arduino --pref "boardsmanager.additional.urls=https://raw.githubusercontent.com/ROBOTIS-GIT/OpenRB-150/master/package_openrb_index.json" --save-prefs 2>&1) if [ $? -ne 0 ]; then echo -e "\xe2\x9c\x96"; else echo -e "\xe2\x9c\x93"; fi +# echo -n "ADD OpenCR PACKAGE INDEX: " +# DEPENDENCY_OUTPUT=$(arduino --pref "boardsmanager.additional.urls=https://raw.githubusercontent.com/ROBOTIS-GIT/OpenCR/master/arduino/opencr_release/package_opencr_index.json" --save-prefs 2>&1) +# if [ $? -ne 0 ]; then echo -e "\xe2\x9c\x96"; else echo -e "\xe2\x9c\x93"; fi + echo -n "INSTALL DUE(sam): " DEPENDENCY_OUTPUT=$(arduino --install-boards arduino:sam 2>&1) if [ $? -ne 0 ]; then echo -e "\xe2\x9c\x96"; else echo -e "\xe2\x9c\x93"; fi @@ -58,8 +62,12 @@ echo -n "INSTALL Portenta H7: " DEPENDENCY_OUTPUT=$(arduino --install-boards arduino:mbed_portenta 2>&1) if [ $? -ne 0 ]; then echo -e "\xe2\x9c\x96"; else echo -e "\xe2\x9c\x93"; fi -echo -n "INSTALL OpenCR: " -DEPENDENCY_OUTPUT=$(arduino --install-boards OpenCR:OpenCR 2>&1) +# echo -n "INSTALL OpenCR: " +# DEPENDENCY_OUTPUT=$(arduino --install-boards OpenCR:OpenCR 2>&1) +# if [ $? -ne 0 ]; then echo -e "\xe2\x9c\x96"; else echo -e "\xe2\x9c\x93"; fi + +echo -n "INSTALL OpenRB-150: " +DEPENDENCY_OUTPUT=$(arduino --install-boards OpenRB-150:samd 2>&1) if [ $? -ne 0 ]; then echo -e "\xe2\x9c\x96"; else echo -e "\xe2\x9c\x93"; fi # install random lib so the arduino IDE grabs a new library index diff --git a/library.properties b/library.properties index 855aba2..8801ca1 100644 --- a/library.properties +++ b/library.properties @@ -1,5 +1,5 @@ name=Dynamixel2Arduino -version=0.5.3 +version=0.6.0 author=ROBOTIS license=Apache-2.0 maintainer=Will Son(willson@robotis.com) diff --git a/src/Dynamixel2Arduino.cpp b/src/Dynamixel2Arduino.cpp index 7ce2e71..dc53fdf 100644 --- a/src/Dynamixel2Arduino.cpp +++ b/src/Dynamixel2Arduino.cpp @@ -557,6 +557,10 @@ bool Dynamixel2Arduino::setLedState(uint8_t id, bool state) case PRO_M42P_010_S260_R: case PRO_M54P_040_S250_R: case PRO_M54P_060_S250_R: + if (state == false) { + writeControlTableItem(ControlTableItem::LED_GREEN, id, state); + writeControlTableItem(ControlTableItem::LED_BLUE, id, state); + } ret = writeControlTableItem(ControlTableItem::LED_RED, id, state); break; diff --git a/src/dxl_c/protocol.c b/src/dxl_c/protocol.c index 95de6f2..1bc939d 100644 --- a/src/dxl_c/protocol.c +++ b/src/dxl_c/protocol.c @@ -48,6 +48,7 @@ // 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 fast_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); @@ -327,8 +328,6 @@ static DXLLibErrorCode_t end_make_dxl2_0_packet(InfoToMakeDXLPacket_t* p_make_pa 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) { @@ -350,6 +349,26 @@ DXLLibErrorCode_t begin_parse_dxl_packet(InfoToParseDXLPacket_t* p_parse_packet, return DXL_LIB_OK; } +DXLLibErrorCode_t fast_begin_parse_dxl_packet(InfoToParseDXLPacket_t* p_parse_packet, + uint8_t protocol_ver, uint8_t* p_param_buf, uint16_t param_buf_capacity, uint8_t xel_count) +{ + 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->xel_count = xel_count; + 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; @@ -371,6 +390,27 @@ DXLLibErrorCode_t parse_dxl_packet(InfoToParseDXLPacket_t* p_parse_packet, uint8 return ret; } +DXLLibErrorCode_t fast_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 = fast_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; @@ -655,6 +695,175 @@ static DXLLibErrorCode_t parse_dxl2_0_packet(InfoToParseDXLPacket_t* p_parse_pac return ret; } +//fast_parse_dxl2_0_packet +static DXLLibErrorCode_t fast_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; + uint8_t size = ((p_parse_packet->param_buf_capacity+4) * p_parse_packet->xel_count) - 3; // 4 = Instruction(1)+Error(1)+CRC(2) + uint8_t* param_array = (uint8_t *)malloc(sizeof(uint8_t) * size); + + 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->err_idx = 0; + p_parse_packet->recv_param_len = 0; + p_parse_packet->param_count = 0; + p_parse_packet->check_xel_count = 0; + + if(recv_data == DXL_INST_STATUS){ + p_parse_packet->parse_state = DXL2_0_PACKET_PARSING_STATE_ERROR; + 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) * p_parse_packet->xel_count) + 1 ){ // 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; + } + + param_array[p_parse_packet->recv_param_len] = recv_data; + update_dxl_crc(&p_parse_packet->calculated_crc, recv_data); + + + /////TODO///// + // https://emanual.robotis.com/docs/en/dxl/protocol2/#parameter + for(p_parse_packet->check_xel_count=0; p_parse_packet->check_xel_count < p_parse_packet->xel_count; p_parse_packet->check_xel_count++){ + if(p_parse_packet->check_xel_count == 0) { + if(0 < p_parse_packet->recv_param_len && p_parse_packet->recv_param_len <= p_parse_packet->param_buf_capacity) { + p_parse_packet->p_param_buf[p_parse_packet->param_count] = recv_data; + p_parse_packet->param_count += 1; + } + } else { + if((p_parse_packet->check_xel_count * (p_parse_packet->param_buf_capacity + 4)) < p_parse_packet->recv_param_len + && p_parse_packet->recv_param_len <= ((p_parse_packet->check_xel_count + 1) * p_parse_packet->param_buf_capacity) + 4) { + p_parse_packet->p_param_buf[p_parse_packet->param_count] = recv_data; + p_parse_packet->param_count += 1; + } + } + } + + p_parse_packet->recv_param_len += 1; + + if(p_parse_packet->recv_param_len == size) { + 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, diff --git a/src/dxl_c/protocol.h b/src/dxl_c/protocol.h index b586b56..03882ad 100644 --- a/src/dxl_c/protocol.h +++ b/src/dxl_c/protocol.h @@ -40,6 +40,7 @@ enum DXLInstruction{ DXL_INST_CLEAR = 0x10, //ONLY Protocol2.0 DXL_INST_STATUS = 0x55, //ONLY Protocol2.0 DXL_INST_SYNC_READ = 0x82, //ONLY Protocol2.0 + DXL_INST_FAST_SYNC_READ = 0x8A, //ONLY Protocol2.0 DXL_INST_SYNC_WRITE = 0x83, DXL_INST_BULK_READ = 0x92, DXL_INST_BULK_WRITE = 0x93 //ONLY Protocol2.0 @@ -134,6 +135,9 @@ typedef struct InfoToParseDXLPacket{ uint8_t recv_check_sum; uint8_t reserved; uint8_t parse_state; + uint8_t param_count; + uint8_t xel_count; + uint8_t check_xel_count; bool is_init; }InfoToParseDXLPacket_t; @@ -162,8 +166,10 @@ 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 fast_begin_parse_dxl_packet(InfoToParseDXLPacket_t* p_parse_packet, + uint8_t protocol_ver, uint8_t* p_param_buf, uint16_t param_buf_cap, uint8_t xel_count); DXLLibErrorCode_t parse_dxl_packet(InfoToParseDXLPacket_t* p_parse_packet, uint8_t recv_data); - +DXLLibErrorCode_t fast_parse_dxl_packet(InfoToParseDXLPacket_t* p_parse_packet, uint8_t recv_data); #ifdef __cplusplus } diff --git a/src/utility/master.cpp b/src/utility/master.cpp index f5bfe7e..2b7a0ba 100644 --- a/src/utility/master.cpp +++ b/src/utility/master.cpp @@ -666,6 +666,84 @@ Master::syncRead(InfoSyncReadInst_t* p_info, uint32_t timeout_ms) return recv_cnt; } +// FastSyncRead +uint8_t +Master::fastSyncRead(InfoFastSyncReadInst_t* p_info, uint32_t timeout_ms) +{ + uint8_t i, recv_cnt = 0; + DXLLibErrorCode_t err = DXL_LIB_OK; + uint8_t tx_param[4], param_len = 0; + uint8_t* p_packet_buf = nullptr; + uint16_t packet_buf_cap; + XELInfoFastSyncRead_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(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_FAST_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_FAST_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; + } + } + } + } + } + + if(err == DXL_LIB_OK){ + p_port_->write(p_packet_buf, p_info->packet.gen_length); + if(fastRxStatusPacket(p_info->p_xels, p_info->addr_length, p_info->xel_count, timeout_ms) != nullptr){ + recv_cnt = p_info->xel_count; + }else{ + err = DXL_LIB_ERROR_NULLPTR; + } + }else{ + p_info->packet.is_completed = false; + } + last_lib_err_ = err; + return recv_cnt; +} // (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 @@ -1056,6 +1134,65 @@ Master::rxStatusPacket(uint8_t* p_param_buf, uint16_t param_buf_cap, uint32_t ti +//fastRxStatusPacket +const InfoToParseDXLPacket_t* +Master::fastRxStatusPacket(XELInfoFastSyncRead_t* p_xel_info, uint16_t param_buf_cap, uint8_t xel_count, uint32_t timeout_ms) +{ + InfoToParseDXLPacket_t *p_ret = nullptr; + DXLLibErrorCode_t err = DXL_LIB_OK; + uint32_t pre_time_ms; + uint8_t* p_recv_buf_array[xel_count]; + + // Parameter exception handling + for(uint8_t index=0; index 0 && p_xel_info[index].p_recv_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; + } + + for(uint8_t index=0; indexavailable() > 0){ + err = fast_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; + } + } + } + + last_lib_err_ = err; + + return p_ret; +} + + + // >> Legacy (Deprecated since v0.4.0) bool Master::syncRead(const ParamForSyncReadInst_t ¶m_info, RecvInfoFromStatusInst_t &recv_info, uint32_t timeout_ms) diff --git a/src/utility/master.h b/src/utility/master.h index b44c22e..5fa819e 100644 --- a/src/utility/master.h +++ b/src/utility/master.h @@ -120,6 +120,21 @@ typedef struct InfoSyncReadInst{ InfoSyncBulkBuffer_t packet; } __attribute__((packed)) InfoSyncReadInst_t; +typedef struct XELInfoFastSyncRead{ + uint8_t *p_recv_buf; + uint8_t id; + uint8_t error; +} __attribute__((packed)) XELInfoFastSyncRead_t; + +typedef struct InfoFastSyncReadInst{ + uint16_t addr; + uint16_t addr_length; + XELInfoFastSyncRead_t* p_xels; + uint8_t xel_count; + bool is_info_changed; + InfoSyncBulkBuffer_t packet; +} __attribute__((packed)) InfoFastSyncReadInst_t; + typedef struct XELInfoSyncWrite{ uint8_t* p_data; uint8_t id; @@ -134,6 +149,7 @@ typedef struct InfoSyncWriteInst{ InfoSyncBulkBuffer_t packet; } __attribute__((packed)) InfoSyncWriteInst_t; + /* Bulk Instructions */ typedef struct XELInfoBulkRead{ uint16_t addr; @@ -206,6 +222,7 @@ class Master bool setPort(DXLPortHandler *p_port); DXLPortHandler* getPort() const; + /* Instructions */ uint8_t ping(uint8_t id, uint8_t *p_recv_id_array, uint8_t recv_array_capacity, uint32_t timeout_ms = 10); @@ -224,6 +241,7 @@ class Master 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); + uint8_t fastSyncRead(InfoFastSyncReadInst_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); @@ -236,6 +254,7 @@ class Master // 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); + const InfoToParseDXLPacket_t* fastRxStatusPacket(XELInfoFastSyncRead_t* p_xel_info, uint16_t param_buf_cap, uint8_t xel_count, uint32_t timeout_ms = 10); // >> Legacy (Deprecated since v0.4.0) bool syncRead(const ParamForSyncReadInst_t ¶m_info, RecvInfoFromStatusInst_t &recv_info, uint32_t timeout_ms = 100); @@ -269,4 +288,4 @@ class Master typedef DYNAMIXEL::InfoFromPing_t XelInfoFromPing_t; -#endif /* DYNAMIXEL_MASTER_HPP_ */ \ No newline at end of file +#endif /* DYNAMIXEL_MASTER_HPP_ */ diff --git a/src/utility/port_handler.cpp b/src/utility/port_handler.cpp index 16016ab..b20bcad 100644 --- a/src/utility/port_handler.cpp +++ b/src/utility/port_handler.cpp @@ -35,6 +35,12 @@ void SerialPortHandler::begin(unsigned long baud) if(port_ == Serial1 && getOpenState() == false){ Serial1.setDxlMode(true); } +#elif defined(ARDUINO_OpenRB) + if(port_ == Serial1 && getOpenState() == false){ + pinMode(BDPIN_DXL_PWR_EN, OUTPUT); + digitalWrite(BDPIN_DXL_PWR_EN, HIGH); + delay(300); // Wait for the FET to turn on. + } #elif defined(ARDUINO_OpenCR) if(port_ == Serial3 && getOpenState() == false){ pinMode(BDPIN_DXL_PWR_EN, OUTPUT); @@ -45,6 +51,7 @@ void SerialPortHandler::begin(unsigned long baud) baud_ = baud; port_.begin(baud_); + mbedTXdelayus = 24000000 / baud; if(dir_pin_ != -1){ pinMode(dir_pin_, OUTPUT); @@ -107,6 +114,9 @@ size_t SerialPortHandler::write(uint8_t *buf, size_t len) if(dir_pin_ != -1){ port_.flush(); +#if defined(ARDUINO_ARCH_MBED) + delayMicroseconds(mbedTXdelayus); +#endif digitalWrite(dir_pin_, LOW); while(digitalRead(dir_pin_) != LOW); } diff --git a/src/utility/port_handler.h b/src/utility/port_handler.h index 6a7e6aa..2256d63 100644 --- a/src/utility/port_handler.h +++ b/src/utility/port_handler.h @@ -61,6 +61,7 @@ class SerialPortHandler : public DXLPortHandler HardwareSerial& port_; const int dir_pin_; unsigned long baud_; + unsigned int mbedTXdelayus; }; diff --git a/src/utility/slave.cpp b/src/utility/slave.cpp index fe663e7..58596be 100644 --- a/src/utility/slave.cpp +++ b/src/utility/slave.cpp @@ -119,6 +119,12 @@ Slave::getPortProtocolVersion() const return (float)protocol_ver_idx_; } +void +Slave::begin(void) +{ + p_port_->begin(); +} + uint8_t Slave::getPortProtocolVersionIndex() const { diff --git a/src/utility/slave.h b/src/utility/slave.h index 1cbd207..be0c7f7 100644 --- a/src/utility/slave.h +++ b/src/utility/slave.h @@ -54,6 +54,7 @@ class Slave void setFirmwareVersion(uint8_t version); uint8_t getFirmwareVersion() const; + void begin(); bool setPort(DXLPortHandler &port); bool setPort(DXLPortHandler *p_port); DXLPortHandler* getPort() const;