Skip to content

Commit 59c9304

Browse files
author
Kei
committed
Updated syncRead/Write class for testing. (#23)
1 parent e7d99e4 commit 59c9304

File tree

4 files changed

+421
-61
lines changed

4 files changed

+421
-61
lines changed

‎examples/advanced/bulk_read_write_raw/bulk_read_write_raw.ino

+1-1
Original file line numberDiff line numberDiff line change
@@ -222,7 +222,7 @@ void loop() {
222222
DEBUG_SERIAL.print("\t Present Position: ");DEBUG_SERIAL.println(br_data_xel_2.present_position);
223223
}else{
224224
DEBUG_SERIAL.print("[BulkRead] Fail, Lib error code: ");
225-
DEBUG_SERIAL.print(dxl.getLastLibErrCode());
225+
DEBUG_SERIAL.println(dxl.getLastLibErrCode());
226226
}
227227
DEBUG_SERIAL.println("=======================================================");
228228

‎examples/advanced/sync_read_write/sync_read_write.ino

+163
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,163 @@
1+
/*******************************************************************************
2+
* Copyright 2016 ROBOTIS CO., LTD.
3+
*
4+
* Licensed under the Apache License, Version 2.0 (the "License");
5+
* you may not use this file except in compliance with the License.
6+
* You may obtain a copy of the License at
7+
*
8+
* http://www.apache.org/licenses/LICENSE-2.0
9+
*
10+
* Unless required by applicable law or agreed to in writing, software
11+
* distributed under the License is distributed on an "AS IS" BASIS,
12+
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13+
* See the License for the specific language governing permissions and
14+
* limitations under the License.
15+
*******************************************************************************/
16+
17+
#include <Dynamixel2Arduino.h>
18+
19+
// Please modify it to suit your hardware.
20+
#if defined(ARDUINO_AVR_UNO) || defined(ARDUINO_AVR_MEGA2560) // When using DynamixelShield
21+
#include <SoftwareSerial.h>
22+
SoftwareSerial soft_serial(7, 8); // DYNAMIXELShield UART RX/TX
23+
#define DXL_SERIAL Serial
24+
#define DEBUG_SERIAL soft_serial
25+
const uint8_t DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN
26+
#elif defined(ARDUINO_SAM_DUE) // When using DynamixelShield
27+
#define DXL_SERIAL Serial
28+
#define DEBUG_SERIAL SerialUSB
29+
const uint8_t DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN
30+
#elif defined(ARDUINO_SAM_ZERO) // When using DynamixelShield
31+
#define DXL_SERIAL Serial1
32+
#define DEBUG_SERIAL SerialUSB
33+
const uint8_t DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN
34+
#elif defined(ARDUINO_OpenCM904) // When using official ROBOTIS board with DXL circuit.
35+
#define DXL_SERIAL Serial3 //OpenCM9.04 EXP Board's DXL port Serial. (Serial1 for the DXL port on the OpenCM 9.04 board)
36+
#define DEBUG_SERIAL Serial
37+
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)
38+
#elif defined(ARDUINO_OpenCR) // When using official ROBOTIS board with DXL circuit.
39+
// For OpenCR, there is a DXL Power Enable pin, so you must initialize and control it.
40+
// Reference link : https://github.com/ROBOTIS-GIT/OpenCR/blob/master/arduino/opencr_arduino/opencr/libraries/DynamixelSDK/src/dynamixel_sdk/port_handler_arduino.cpp#L78
41+
#define DXL_SERIAL Serial3
42+
#define DEBUG_SERIAL Serial
43+
const uint8_t DXL_DIR_PIN = 84; // OpenCR Board's DIR PIN.
44+
#else // Other boards when using DynamixelShield
45+
#define DXL_SERIAL Serial1
46+
#define DEBUG_SERIAL Serial
47+
const uint8_t DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN
48+
#endif
49+
50+
51+
52+
const uint8_t DXL_ID_CNT = 2;
53+
const uint8_t DXL_ID_LIST[DXL_ID_CNT] = {1, 2};
54+
const uint16_t user_pkt_buf_cap = 128;
55+
uint8_t user_pkt_buf[user_pkt_buf_cap];
56+
57+
const uint16_t SR_START_ADDR = 126;
58+
const uint16_t SR_ADDR_LEN = 10; //2+4+4
59+
const uint16_t SW_START_ADDR = 104; //Goal velocity
60+
const uint16_t SW_ADDR_LEN = 4;
61+
typedef struct sr_data{
62+
int16_t present_current;
63+
int32_t present_velocity;
64+
int32_t present_position;
65+
} __attribute__((packed)) sr_data_t;
66+
typedef struct sw_data{
67+
int32_t goal_velocity;
68+
} __attribute__((packed)) sw_data_t;
69+
70+
71+
sr_data_t sr_data[DXL_ID_CNT];
72+
sw_data_t sw_data[DXL_ID_CNT];
73+
74+
Dynamixel2Arduino dxl(DXL_SERIAL, DXL_DIR_PIN);
75+
DYNAMIXEL::SyncRead<SR_START_ADDR, sr_data_t, DXL_ID_CNT> dxl_sr(dxl);
76+
DYNAMIXEL::SyncWrite<SW_START_ADDR, sw_data_t, DXL_ID_CNT> dxl_sw(dxl);
77+
78+
79+
void setup() {
80+
// put your setup code here, to run once:
81+
uint8_t i;
82+
pinMode(LED_BUILTIN, OUTPUT);
83+
DEBUG_SERIAL.begin(115200);
84+
dxl.begin(57600);
85+
86+
for(i=0; i<DXL_ID_CNT; i++){
87+
dxl.torqueOff(DXL_ID_LIST[i]);
88+
dxl.setOperatingMode(DXL_ID_LIST[i], OP_VELOCITY);
89+
dxl.torqueOn(DXL_ID_LIST[i]);
90+
}
91+
92+
// SyncRead using user external buffer.
93+
dxl_sr.setPacketBuffer(user_pkt_buf, user_pkt_buf_cap);
94+
for(i=0; i<DXL_ID_CNT; i++){
95+
dxl_sr.addParam(DXL_ID_LIST[i], sr_data[i]);
96+
}
97+
98+
// SyncWrite using the internal buffer.
99+
sw_data[0].goal_velocity = 0;
100+
sw_data[1].goal_velocity = 100;
101+
for(i=0; i<DXL_ID_CNT; i++){
102+
dxl_sw.addParam(DXL_ID_LIST[i], sw_data[i]);
103+
}
104+
}
105+
106+
void loop() {
107+
// put your main code here, to run repeatedly:
108+
static uint32_t try_count = 0;
109+
uint8_t i, id, recv_cnt;
110+
111+
// Update parameter data for SyncWrite
112+
for(i=0; i<DXL_ID_CNT; i++){
113+
sw_data[i].goal_velocity+=5;
114+
if(sw_data[i].goal_velocity >= 200){
115+
sw_data[i].goal_velocity = 0;
116+
}
117+
}
118+
dxl_sw.updateParamData();
119+
120+
DEBUG_SERIAL.print("\n>>>>>> Sync Instruction Test : ");
121+
DEBUG_SERIAL.println(try_count++);
122+
// Send syncWrite
123+
if(dxl_sw.sendPacket() == true){
124+
DEBUG_SERIAL.println("[SyncWrite] Success");
125+
for(i=0; i<DXL_ID_CNT; i++){
126+
id = dxl_sw.getIDByIndex(i);
127+
DEBUG_SERIAL.print(" ID: ");DEBUG_SERIAL.print(id);
128+
DEBUG_SERIAL.print("\t Goal Velocity: ");DEBUG_SERIAL.println(dxl_sw.getDataPtr(id)->goal_velocity);
129+
}
130+
}else{
131+
DEBUG_SERIAL.print("[SyncWrite] Fail, Lib error code: ");
132+
DEBUG_SERIAL.print(dxl.getLastLibErrCode());
133+
}
134+
DEBUG_SERIAL.println();
135+
136+
delay(250);
137+
138+
// Send syncRead & Receive data
139+
recv_cnt = dxl_sr.sendPacket();
140+
if(recv_cnt > 0){
141+
DEBUG_SERIAL.print("[SyncRead] Success, Received ID Count: ");
142+
DEBUG_SERIAL.println(recv_cnt);
143+
for(i=0; i<recv_cnt; i++){
144+
id = dxl_sr.getIDByIndex(i);
145+
DEBUG_SERIAL.print(" ID: ");DEBUG_SERIAL.print(dxl_sr.getIDByIndex(i));
146+
DEBUG_SERIAL.print(", Error: ");DEBUG_SERIAL.println(dxl_sr.getError(id));
147+
DEBUG_SERIAL.print("\t Present Current: ");DEBUG_SERIAL.println(dxl_sr.getDataPtr(id)->present_current);
148+
DEBUG_SERIAL.print("\t Present Velocity: ");DEBUG_SERIAL.println(dxl_sr.getDataPtr(id)->present_velocity);
149+
DEBUG_SERIAL.print("\t Present Position: ");DEBUG_SERIAL.println(dxl_sr.getDataPtr(id)->present_position);
150+
}
151+
}else{
152+
DEBUG_SERIAL.print("[SyncRead] Fail, Lib error code: ");
153+
DEBUG_SERIAL.println(dxl.getLastLibErrCode());
154+
}
155+
DEBUG_SERIAL.println("=======================================================");
156+
157+
digitalWrite(LED_BUILTIN, !digitalRead(LED_BUILTIN));
158+
delay(750);
159+
}
160+
161+
162+
163+

‎examples/advanced/sync_read_write_raw/sync_read_write_raw.ino

+1-1
Original file line numberDiff line numberDiff line change
@@ -204,7 +204,7 @@ void loop() {
204204
}
205205
}else{
206206
DEBUG_SERIAL.print("[SyncRead] Fail, Lib error code: ");
207-
DEBUG_SERIAL.print(dxl.getLastLibErrCode());
207+
DEBUG_SERIAL.println(dxl.getLastLibErrCode());
208208
}
209209
DEBUG_SERIAL.println("=======================================================");
210210

0 commit comments

Comments
 (0)