Skip to content

Commit b7c779a

Browse files
author
Kei
committed
Updated bulk Classes, examples. (#23)
1 parent 59c9304 commit b7c779a

File tree

5 files changed

+482
-15
lines changed

5 files changed

+482
-15
lines changed

‎examples/advanced/bulk_read_write/bulk_read_write.ino‎

Lines changed: 168 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,168 @@
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+
const uint8_t DXL_1_ID = 1;
52+
const uint8_t DXL_2_ID = 2;
53+
const uint8_t DXL_ID_CNT = 2;
54+
const uint16_t user_pkt_buf_cap = 128;
55+
uint8_t user_pkt_buf[user_pkt_buf_cap];
56+
57+
struct br_data_xel_1{
58+
int16_t present_current;
59+
int32_t present_velocity;
60+
} __attribute__((packed));
61+
struct br_data_xel_2{
62+
int32_t present_position;
63+
} __attribute__((packed));
64+
struct bw_data_xel_1{
65+
int32_t goal_velocity;
66+
} __attribute__((packed));
67+
struct bw_data_xel_2{
68+
int32_t goal_position;
69+
} __attribute__((packed));
70+
71+
struct br_data_xel_1 br_data_xel_1;
72+
struct br_data_xel_2 br_data_xel_2;
73+
struct bw_data_xel_1 bw_data_xel_1;
74+
struct bw_data_xel_2 bw_data_xel_2;
75+
76+
Dynamixel2Arduino dxl(DXL_SERIAL, DXL_DIR_PIN);
77+
DYNAMIXEL::BulkRead<DXL_ID_CNT> dxl_br(dxl);
78+
DYNAMIXEL::BulkWrite<DXL_ID_CNT> dxl_bw(dxl);
79+
80+
void setup() {
81+
// put your setup code here, to run once:
82+
pinMode(LED_BUILTIN, OUTPUT);
83+
DEBUG_SERIAL.begin(115200);
84+
dxl.begin(57600);
85+
86+
dxl.torqueOff(DXL_1_ID);
87+
dxl.torqueOff(DXL_2_ID);
88+
dxl.setOperatingMode(DXL_1_ID, OP_VELOCITY);
89+
dxl.setOperatingMode(DXL_2_ID, OP_POSITION);
90+
dxl.torqueOn(DXL_1_ID);
91+
dxl.torqueOn(DXL_2_ID);
92+
93+
// BulkRead using user external packet buffer.
94+
dxl_br.setPacketBuffer(user_pkt_buf, user_pkt_buf_cap);
95+
dxl_br.addParam(DXL_1_ID, 126, br_data_xel_1); // Start Addr: Present Current of X serise.
96+
dxl_br.addParam(DXL_2_ID, 132, br_data_xel_2); // Start Addr: Present Position of X serise.
97+
98+
// BulkWrite using the internal packet buffer.
99+
bw_data_xel_1.goal_velocity = 0;
100+
bw_data_xel_2.goal_position = 0;
101+
dxl_bw.addParam(DXL_1_ID, 104, bw_data_xel_1); // Start Addr: Goal Velocity of X serise.
102+
dxl_bw.addParam(DXL_2_ID, 116, bw_data_xel_2); // Start Addr: Goal Position of X serise.
103+
}
104+
105+
void loop() {
106+
// put your main code here, to run repeatedly:
107+
static uint32_t try_count = 0;
108+
uint8_t recv_cnt;
109+
110+
DEBUG_SERIAL.print("\n>>>>>> Bulk Instruction Test : ");
111+
DEBUG_SERIAL.println(try_count++);
112+
113+
// Update parameter data for BulkWrite
114+
bw_data_xel_1.goal_velocity+=5;
115+
if(bw_data_xel_1.goal_velocity >= 200){
116+
bw_data_xel_1.goal_velocity = 0;
117+
}
118+
bw_data_xel_2.goal_position+=255;
119+
if(bw_data_xel_2.goal_position >= 1023){
120+
bw_data_xel_2.goal_position = 0;
121+
}
122+
dxl_bw.updateParamData();
123+
124+
// Send bulkWrite pakcet
125+
if(dxl_bw.sendPacket() == true){
126+
DEBUG_SERIAL.println("[BulkWrite] Success");
127+
128+
DEBUG_SERIAL.print(" ID: ");DEBUG_SERIAL.println(DXL_1_ID);
129+
DEBUG_SERIAL.print("\t Start Address: ");DEBUG_SERIAL.println(dxl_bw.getStartAddr(DXL_1_ID));
130+
DEBUG_SERIAL.print("\t Address Length: ");DEBUG_SERIAL.println(dxl_bw.getAddrLength(DXL_1_ID));
131+
DEBUG_SERIAL.print("\t Goal Velocity: ");DEBUG_SERIAL.println(bw_data_xel_1.goal_velocity);
132+
133+
DEBUG_SERIAL.print(" ID: ");DEBUG_SERIAL.println(DXL_2_ID);
134+
DEBUG_SERIAL.print("\t Start Address: ");DEBUG_SERIAL.println(dxl_bw.getStartAddr(DXL_2_ID));
135+
DEBUG_SERIAL.print("\t Address Length: ");DEBUG_SERIAL.println(dxl_bw.getAddrLength(DXL_2_ID));
136+
DEBUG_SERIAL.print("\t Goal Position: ");DEBUG_SERIAL.println(bw_data_xel_2.goal_position);
137+
}else{
138+
DEBUG_SERIAL.print("[BulkWrite] Fail, Lib error code: ");
139+
DEBUG_SERIAL.print(dxl.getLastLibErrCode());
140+
}
141+
DEBUG_SERIAL.println();
142+
143+
delay(250);
144+
145+
// Send bulkRead packet & Receive data
146+
recv_cnt = dxl_br.sendPacket();
147+
if(recv_cnt > 0){
148+
DEBUG_SERIAL.print("[BulkRead] Success, Received ID Count: ");
149+
DEBUG_SERIAL.println(recv_cnt);
150+
151+
DEBUG_SERIAL.print(" ID: ");DEBUG_SERIAL.print(DXL_1_ID);
152+
DEBUG_SERIAL.print(", Error: ");DEBUG_SERIAL.println(dxl_br.getError(DXL_1_ID));
153+
DEBUG_SERIAL.print("\t Present Current: ");DEBUG_SERIAL.println(br_data_xel_1.present_current);
154+
DEBUG_SERIAL.print("\t Present Velocity: ");DEBUG_SERIAL.println(br_data_xel_1.present_velocity);
155+
156+
DEBUG_SERIAL.print(" ID: ");DEBUG_SERIAL.print(DXL_2_ID);
157+
DEBUG_SERIAL.print(", Error: ");DEBUG_SERIAL.println(dxl_br.getError(DXL_2_ID));
158+
DEBUG_SERIAL.print("\t Present Position: ");DEBUG_SERIAL.println(br_data_xel_2.present_position);
159+
}else{
160+
DEBUG_SERIAL.print("[BulkRead] Fail, Lib error code: ");
161+
DEBUG_SERIAL.println(dxl.getLastLibErrCode());
162+
}
163+
DEBUG_SERIAL.println("=======================================================");
164+
165+
digitalWrite(LED_BUILTIN, !digitalRead(LED_BUILTIN));
166+
delay(750);
167+
}
168+

‎examples/advanced/bulk_read_write_raw/bulk_read_write_raw.ino‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -194,10 +194,10 @@ void loop() {
194194
DEBUG_SERIAL.println(try_count++);
195195
if(dxl.bulkWrite(&bw_infos) == true){
196196
DEBUG_SERIAL.println("[BulkWrite] Success");
197-
DEBUG_SERIAL.print(" ID: ");DEBUG_SERIAL.print(bw_infos.p_xels[0].id);
197+
DEBUG_SERIAL.print(" ID: ");DEBUG_SERIAL.println(bw_infos.p_xels[0].id);
198198
DEBUG_SERIAL.print("\t Goal Velocity: ");DEBUG_SERIAL.println(bw_data_xel_1.goal_velocity);
199199

200-
DEBUG_SERIAL.print(" ID: ");DEBUG_SERIAL.print(bw_infos.p_xels[1].id);
200+
DEBUG_SERIAL.print(" ID: ");DEBUG_SERIAL.println(bw_infos.p_xels[1].id);
201201
DEBUG_SERIAL.print("\t Goal Position: ");DEBUG_SERIAL.println(bw_data_xel_2.goal_position);
202202
}else{
203203
DEBUG_SERIAL.print("[BulkWrite] Fail, Lib error code: ");

‎examples/advanced/sync_read_write/sync_read_write.ino‎

Lines changed: 9 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -89,13 +89,13 @@ void setup() {
8989
dxl.torqueOn(DXL_ID_LIST[i]);
9090
}
9191

92-
// SyncRead using user external buffer.
92+
// SyncRead using user external packet buffer.
9393
dxl_sr.setPacketBuffer(user_pkt_buf, user_pkt_buf_cap);
9494
for(i=0; i<DXL_ID_CNT; i++){
9595
dxl_sr.addParam(DXL_ID_LIST[i], sr_data[i]);
9696
}
9797

98-
// SyncWrite using the internal buffer.
98+
// SyncWrite using the internal packet buffer.
9999
sw_data[0].goal_velocity = 0;
100100
sw_data[1].goal_velocity = 100;
101101
for(i=0; i<DXL_ID_CNT; i++){
@@ -108,6 +108,9 @@ void loop() {
108108
static uint32_t try_count = 0;
109109
uint8_t i, id, recv_cnt;
110110

111+
DEBUG_SERIAL.print("\n>>>>>> Sync Instruction Test : ");
112+
DEBUG_SERIAL.println(try_count++);
113+
111114
// Update parameter data for SyncWrite
112115
for(i=0; i<DXL_ID_CNT; i++){
113116
sw_data[i].goal_velocity+=5;
@@ -117,14 +120,12 @@ void loop() {
117120
}
118121
dxl_sw.updateParamData();
119122

120-
DEBUG_SERIAL.print("\n>>>>>> Sync Instruction Test : ");
121-
DEBUG_SERIAL.println(try_count++);
122-
// Send syncWrite
123+
// Send syncWrite pakcet
123124
if(dxl_sw.sendPacket() == true){
124125
DEBUG_SERIAL.println("[SyncWrite] Success");
125126
for(i=0; i<DXL_ID_CNT; i++){
126127
id = dxl_sw.getIDByIndex(i);
127-
DEBUG_SERIAL.print(" ID: ");DEBUG_SERIAL.print(id);
128+
DEBUG_SERIAL.print(" ID: ");DEBUG_SERIAL.println(id);
128129
DEBUG_SERIAL.print("\t Goal Velocity: ");DEBUG_SERIAL.println(dxl_sw.getDataPtr(id)->goal_velocity);
129130
}
130131
}else{
@@ -135,14 +136,14 @@ void loop() {
135136

136137
delay(250);
137138

138-
// Send syncRead & Receive data
139+
// Send syncRead packet & Receive data
139140
recv_cnt = dxl_sr.sendPacket();
140141
if(recv_cnt > 0){
141142
DEBUG_SERIAL.print("[SyncRead] Success, Received ID Count: ");
142143
DEBUG_SERIAL.println(recv_cnt);
143144
for(i=0; i<recv_cnt; i++){
144145
id = dxl_sr.getIDByIndex(i);
145-
DEBUG_SERIAL.print(" ID: ");DEBUG_SERIAL.print(dxl_sr.getIDByIndex(i));
146+
DEBUG_SERIAL.print(" ID: ");DEBUG_SERIAL.print(id);
146147
DEBUG_SERIAL.print(", Error: ");DEBUG_SERIAL.println(dxl_sr.getError(id));
147148
DEBUG_SERIAL.print("\t Present Current: ");DEBUG_SERIAL.println(dxl_sr.getDataPtr(id)->present_current);
148149
DEBUG_SERIAL.print("\t Present Velocity: ");DEBUG_SERIAL.println(dxl_sr.getDataPtr(id)->present_velocity);

‎examples/advanced/sync_read_write_raw/sync_read_write_raw.ino‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -180,7 +180,7 @@ void loop() {
180180
if(dxl.syncWrite(&sw_infos) == true){
181181
DEBUG_SERIAL.println("[SyncWrite] Success");
182182
for(i=0; i<sw_infos.xel_count; i++){
183-
DEBUG_SERIAL.print(" ID: ");DEBUG_SERIAL.print(sw_infos.p_xels[i].id);
183+
DEBUG_SERIAL.print(" ID: ");DEBUG_SERIAL.println(sw_infos.p_xels[i].id);
184184
DEBUG_SERIAL.print("\t Goal Velocity: ");DEBUG_SERIAL.println(sw_data[i].goal_velocity);
185185
}
186186
}else{

0 commit comments

Comments
 (0)