/
add_custom_SerialPortHandler.ino
140 lines (116 loc) · 4.65 KB
/
add_custom_SerialPortHandler.ino
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
/*******************************************************************************
* 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 <Dynamixel2Arduino.h>
// Please modify it to suit your hardware.
#if defined(ARDUINO_AVR_UNO) || defined(ARDUINO_AVR_MEGA2560) // When using DynamixelShield
#include <SoftwareSerial.h>
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
class NewSerialPortHandler : public DYNAMIXEL::SerialPortHandler
{
public:
NewSerialPortHandler(HardwareSerial& port, const int dir_pin = -1)
: SerialPortHandler(port, dir_pin), port_(port), dir_pin_(dir_pin)
{}
virtual size_t write(uint8_t c) override
{
size_t ret = 0;
digitalWrite(dir_pin_, HIGH);
while(digitalRead(dir_pin_) != HIGH);
ret = port_.write(c);
port_.flush();
digitalWrite(dir_pin_, LOW);
while(digitalRead(dir_pin_) != LOW);
return ret;
}
virtual size_t write(uint8_t *buf, size_t len) override
{
size_t ret;
digitalWrite(dir_pin_, HIGH);
while(digitalRead(dir_pin_) != HIGH);
ret = port_.write(buf, len);
port_.flush();
digitalWrite(dir_pin_, LOW);
while(digitalRead(dir_pin_) != LOW);
return ret;
}
private:
HardwareSerial& port_;
const int dir_pin_;
};
const uint8_t DXL_ID = 1;
const float DXL_PROTOCOL_VERSION = 2.0;
Dynamixel2Arduino dxl;
NewSerialPortHandler dxl_port(DXL_SERIAL, DXL_DIR_PIN);
//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 instance
dxl.setPort(dxl_port);
// Set Port baudrate to 57600bps. This has to match with DYNAMIXEL baudrate.
dxl.begin(57600);
// Set Port Protocol Version. This has to match with DYNAMIXEL protocol version.
dxl.setPortProtocolVersion(DXL_PROTOCOL_VERSION);
}
void loop() {
// put your main code here, to run repeatedly:
DEBUG_SERIAL.print("PROTOCOL ");
DEBUG_SERIAL.print(DXL_PROTOCOL_VERSION, 1);
DEBUG_SERIAL.print(", ID ");
DEBUG_SERIAL.print(DXL_ID);
DEBUG_SERIAL.print(": ");
if(dxl.ping(DXL_ID) == true){
DEBUG_SERIAL.print("ping succeeded!");
DEBUG_SERIAL.print(", Model Number: ");
DEBUG_SERIAL.println(dxl.getModelNumber(DXL_ID));
}else{
DEBUG_SERIAL.println("ping failed!");
}
delay(500);
}