From 52ec2f8ef36054748e9ed4faae50699926a38119 Mon Sep 17 00:00:00 2001 From: Will Son Date: Tue, 22 Dec 2020 17:25:44 +0900 Subject: [PATCH 1/2] prepare to release --- README.md | 5 +-- .../basic/position_mode/position_mode.ino | 35 +++++++++++++------ library.properties | 2 +- src/utility/port_handler.cpp | 2 +- 4 files changed, 30 insertions(+), 14 deletions(-) diff --git a/README.md b/README.md index 7f02b7e..4fdd586 100644 --- a/README.md +++ b/README.md @@ -1,13 +1,14 @@ # Dynamixel2Arduino [![Build Status](https://travis-ci.org/ROBOTIS-GIT/Dynamixel2Arduino.svg?branch=master)](https://travis-ci.org/ROBOTIS-GIT/Dynamixel2Arduino/branches) ## Serial and Direction Pin definitions by board - - The examples default to pins based on DynamixelShield. Therefore, when using hardware other than DynamixelShield (eg OpenCM9.04, OpenCR, Custom DXL boards), you need to change the Serial and Direction Pin. + - The examples defines GPIO pins based on the use with DYNAMIXEL Shields. + - 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 EXP|Serial3|22|| + |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))| diff --git a/examples/basic/position_mode/position_mode.ino b/examples/basic/position_mode/position_mode.ino index 85fb52d..c274831 100644 --- a/examples/basic/position_mode/position_mode.ino +++ b/examples/basic/position_mode/position_mode.ino @@ -61,6 +61,7 @@ void setup() { // 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); @@ -73,6 +74,9 @@ void setup() { 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() { @@ -81,17 +85,28 @@ void loop() { // 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); - delay(1000); - // Print present position in raw value - DEBUG_SERIAL.print("Present Position(raw) : "); - DEBUG_SERIAL.println(dxl.getPresentPosition(DXL_ID)); - delay(1000); + + 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); - delay(1000); - // Print present position in degree value - DEBUG_SERIAL.print("Present Position(degree) : "); - DEBUG_SERIAL.println(dxl.getPresentPosition(DXL_ID, UNIT_DEGREE)); - delay(1000); + + 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); } diff --git a/library.properties b/library.properties index e2012fd..c50c228 100644 --- a/library.properties +++ b/library.properties @@ -1,5 +1,5 @@ name=Dynamixel2Arduino -version=0.4.5 +version=0.4.6 author=ROBOTIS license=Apache-2.0 maintainer=Will Son(willson@robotis.com) diff --git a/src/utility/port_handler.cpp b/src/utility/port_handler.cpp index 28b806d..16016ab 100644 --- a/src/utility/port_handler.cpp +++ b/src/utility/port_handler.cpp @@ -40,7 +40,7 @@ void SerialPortHandler::begin(unsigned long baud) pinMode(BDPIN_DXL_PWR_EN, OUTPUT); digitalWrite(BDPIN_DXL_PWR_EN, HIGH); } - delay(200); // Wait for the DYNAMIXEL to power up normally. + delay(300); // Wait for the DYNAMIXEL to power up normally. #endif baud_ = baud; From 60e9b86f4bfa82673a93591b82e3e5cfaffa9b33 Mon Sep 17 00:00:00 2001 From: Will Son Date: Tue, 22 Dec 2020 18:18:04 +0900 Subject: [PATCH 2/2] header definition resolved --- src/dxl_c/protocol.c | 2 +- src/utility/config.h | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/dxl_c/protocol.c b/src/dxl_c/protocol.c index 1f32488..d8d9999 100644 --- a/src/dxl_c/protocol.c +++ b/src/dxl_c/protocol.c @@ -2,7 +2,7 @@ #if defined(ARDUINO) #include - #if !defined(ESP_PLATFORM) && !defined(ARDUINO_ARCH_MBED) + #if !defined(ESP_PLATFORM) && !defined(ARDUINO_ARCH_MBED) && !defined(ARDUINO_ARCH_SAMD) #include #endif #endif diff --git a/src/utility/config.h b/src/utility/config.h index bc28166..333d63e 100644 --- a/src/utility/config.h +++ b/src/utility/config.h @@ -83,7 +83,7 @@ #if defined(ARDUINO) #include - #if !defined(ESP_PLATFORM) && !defined(ARDUINO_ARCH_MBED) + #if !defined(ESP_PLATFORM) && !defined(ARDUINO_ARCH_MBED) && !defined(ARDUINO_ARCH_SAMD) #include #endif #endif