Skip to content

Commit 3479924

Browse files
author
Kei
committed
Added macro in examples for DUE, ZERO.
1 parent b6f4914 commit 3479924

File tree

18 files changed

+256
-65
lines changed

18 files changed

+256
-65
lines changed

‎examples/advanced/add_custom_SerialPortHandler/add_custom_SerialPortHandler.ino

+13-4
Original file line numberDiff line numberDiff line change
@@ -23,21 +23,30 @@
2323
#define DXL_SERIAL Serial
2424
#define DEBUG_SERIAL soft_serial
2525
const uint8_t DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN
26-
#elif ARDUINO_OpenCM904 // Official ROBOTIS board with DXL circuit.
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.
2735
#define DXL_SERIAL Serial3 //OpenCM9.04 EXP Board's DXL port Serial. (Serial1 for the DXL port on the OpenCM 9.04 board)
2836
#define DEBUG_SERIAL Serial
2937
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)
30-
#elif ARDUINO_OpenCR // Official ROBOTIS board with DXL circuit.
38+
#elif defined(ARDUINO_OpenCR) // When using official ROBOTIS board with DXL circuit.
3139
// For OpenCR, there is a DXL Power Enable pin, so you must initialize and control it.
3240
// Reference link : https://github.com/ROBOTIS-GIT/OpenCR/blob/master/arduino/opencr_arduino/opencr/libraries/DynamixelSDK/src/dynamixel_sdk/port_handler_arduino.cpp#L78
3341
#define DXL_SERIAL Serial3
3442
#define DEBUG_SERIAL Serial
35-
const uint8_t DXL_DIR_PIN = 84; // OpenCR Board's DIR PIN.
36-
#else // When using DynamixelShield
43+
const uint8_t DXL_DIR_PIN = 84; // OpenCR Board's DIR PIN.
44+
#else // Other boards when using DynamixelShield
3745
#define DXL_SERIAL Serial1
3846
#define DEBUG_SERIAL Serial
3947
const uint8_t DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN
4048
#endif
49+
4150

4251
class NewSerialPortHandler : public DYNAMIXEL::SerialPortHandler
4352
{

‎examples/advanced/operating_mode_advanced/operating_mode_advanced.ino

+12-3
Original file line numberDiff line numberDiff line change
@@ -23,21 +23,30 @@
2323
#define DXL_SERIAL Serial
2424
#define DEBUG_SERIAL soft_serial
2525
const uint8_t DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN
26-
#elif ARDUINO_OpenCM904 // Official ROBOTIS board with DXL circuit.
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.
2735
#define DXL_SERIAL Serial3 //OpenCM9.04 EXP Board's DXL port Serial. (Serial1 for the DXL port on the OpenCM 9.04 board)
2836
#define DEBUG_SERIAL Serial
2937
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)
30-
#elif ARDUINO_OpenCR // Official ROBOTIS board with DXL circuit.
38+
#elif defined(ARDUINO_OpenCR) // When using official ROBOTIS board with DXL circuit.
3139
// For OpenCR, there is a DXL Power Enable pin, so you must initialize and control it.
3240
// Reference link : https://github.com/ROBOTIS-GIT/OpenCR/blob/master/arduino/opencr_arduino/opencr/libraries/DynamixelSDK/src/dynamixel_sdk/port_handler_arduino.cpp#L78
3341
#define DXL_SERIAL Serial3
3442
#define DEBUG_SERIAL Serial
3543
const uint8_t DXL_DIR_PIN = 84; // OpenCR Board's DIR PIN.
36-
#else // When using DynamixelShield
44+
#else // Other boards when using DynamixelShield
3745
#define DXL_SERIAL Serial1
3846
#define DEBUG_SERIAL Serial
3947
const uint8_t DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN
4048
#endif
49+
4150

4251
const uint8_t DXL_ID = 1;
4352

‎examples/advanced/read_write_ControlTableItem/read_write_ControlTableItem.ino

+12-3
Original file line numberDiff line numberDiff line change
@@ -122,22 +122,31 @@
122122
#define DXL_SERIAL Serial
123123
#define DEBUG_SERIAL soft_serial
124124
const uint8_t DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN
125-
#elif ARDUINO_OpenCM904 // Official ROBOTIS board with DXL circuit.
125+
#elif defined(ARDUINO_SAM_DUE) // When using DynamixelShield
126+
#define DXL_SERIAL Serial
127+
#define DEBUG_SERIAL SerialUSB
128+
const uint8_t DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN
129+
#elif defined(ARDUINO_SAM_ZERO) // When using DynamixelShield
130+
#define DXL_SERIAL Serial1
131+
#define DEBUG_SERIAL SerialUSB
132+
const uint8_t DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN
133+
#elif defined(ARDUINO_OpenCM904) // When using official ROBOTIS board with DXL circuit.
126134
#define DXL_SERIAL Serial3 //OpenCM9.04 EXP Board's DXL port Serial. (Serial1 for the DXL port on the OpenCM 9.04 board)
127135
#define DEBUG_SERIAL Serial
128136
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)
129-
#elif ARDUINO_OpenCR // Official ROBOTIS board with DXL circuit.
137+
#elif defined(ARDUINO_OpenCR) // When using official ROBOTIS board with DXL circuit.
130138
// For OpenCR, there is a DXL Power Enable pin, so you must initialize and control it.
131139
// Reference link : https://github.com/ROBOTIS-GIT/OpenCR/blob/master/arduino/opencr_arduino/opencr/libraries/DynamixelSDK/src/dynamixel_sdk/port_handler_arduino.cpp#L78
132140
#define DXL_SERIAL Serial3
133141
#define DEBUG_SERIAL Serial
134142
const uint8_t DXL_DIR_PIN = 84; // OpenCR Board's DIR PIN.
135-
#else // When using DynamixelShield
143+
#else // Other boards when using DynamixelShield
136144
#define DXL_SERIAL Serial1
137145
#define DEBUG_SERIAL Serial
138146
const uint8_t DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN
139147
#endif
140148

149+
141150
const uint8_t DXL_ID = 1;
142151

143152
Dynamixel2Arduino dxl(DXL_SERIAL, DXL_DIR_PIN);

‎examples/advanced/slave/slave.ino

+12-3
Original file line numberDiff line numberDiff line change
@@ -23,21 +23,30 @@
2323
#define DXL_SERIAL Serial
2424
#define DEBUG_SERIAL soft_serial
2525
const uint8_t DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN
26-
#elif ARDUINO_OpenCM904 // Official ROBOTIS board with DXL circuit.
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.
2735
#define DXL_SERIAL Serial3 //OpenCM9.04 EXP Board's DXL port Serial. (Serial1 for the DXL port on the OpenCM 9.04 board)
2836
#define DEBUG_SERIAL Serial
2937
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)
30-
#elif ARDUINO_OpenCR // Official ROBOTIS board with DXL circuit.
38+
#elif defined(ARDUINO_OpenCR) // When using official ROBOTIS board with DXL circuit.
3139
// For OpenCR, there is a DXL Power Enable pin, so you must initialize and control it.
3240
// Reference link : https://github.com/ROBOTIS-GIT/OpenCR/blob/master/arduino/opencr_arduino/opencr/libraries/DynamixelSDK/src/dynamixel_sdk/port_handler_arduino.cpp#L78
3341
#define DXL_SERIAL Serial3
3442
#define DEBUG_SERIAL Serial
3543
const uint8_t DXL_DIR_PIN = 84; // OpenCR Board's DIR PIN.
36-
#else // When using DynamixelShield
44+
#else // Other boards when using DynamixelShield
3745
#define DXL_SERIAL Serial1
3846
#define DEBUG_SERIAL Serial
3947
const uint8_t DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN
4048
#endif
49+
4150

4251
// Create a port object for DYNAMIXEL communication.
4352
// The list of available classes is as follows.

‎examples/advanced/slave_callback/slave_callback.ino

+12-3
Original file line numberDiff line numberDiff line change
@@ -23,21 +23,30 @@
2323
#define DXL_SERIAL Serial
2424
#define DEBUG_SERIAL soft_serial
2525
const uint8_t DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN
26-
#elif ARDUINO_OpenCM904 // Official ROBOTIS board with DXL circuit.
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.
2735
#define DXL_SERIAL Serial3 //OpenCM9.04 EXP Board's DXL port Serial. (Serial1 for the DXL port on the OpenCM 9.04 board)
2836
#define DEBUG_SERIAL Serial
2937
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)
30-
#elif ARDUINO_OpenCR // Official ROBOTIS board with DXL circuit.
38+
#elif defined(ARDUINO_OpenCR) // When using official ROBOTIS board with DXL circuit.
3139
// For OpenCR, there is a DXL Power Enable pin, so you must initialize and control it.
3240
// Reference link : https://github.com/ROBOTIS-GIT/OpenCR/blob/master/arduino/opencr_arduino/opencr/libraries/DynamixelSDK/src/dynamixel_sdk/port_handler_arduino.cpp#L78
3341
#define DXL_SERIAL Serial3
3442
#define DEBUG_SERIAL Serial
3543
const uint8_t DXL_DIR_PIN = 84; // OpenCR Board's DIR PIN.
36-
#else // When using DynamixelShield
44+
#else // Other boards when using DynamixelShield
3745
#define DXL_SERIAL Serial1
3846
#define DEBUG_SERIAL Serial
3947
const uint8_t DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN
4048
#endif
49+
4150

4251
// Create a port object for DYNAMIXEL communication.
4352
// The list of available classes is as follows.

‎examples/basic/baudrate/baudrate.ino

+12-3
Original file line numberDiff line numberDiff line change
@@ -23,21 +23,30 @@
2323
#define DXL_SERIAL Serial
2424
#define DEBUG_SERIAL soft_serial
2525
const uint8_t DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN
26-
#elif ARDUINO_OpenCM904 // Official ROBOTIS board with DXL circuit.
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.
2735
#define DXL_SERIAL Serial3 //OpenCM9.04 EXP Board's DXL port Serial. (Serial1 for the DXL port on the OpenCM 9.04 board)
2836
#define DEBUG_SERIAL Serial
2937
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)
30-
#elif ARDUINO_OpenCR // Official ROBOTIS board with DXL circuit.
38+
#elif defined(ARDUINO_OpenCR) // When using official ROBOTIS board with DXL circuit.
3139
// For OpenCR, there is a DXL Power Enable pin, so you must initialize and control it.
3240
// Reference link : https://github.com/ROBOTIS-GIT/OpenCR/blob/master/arduino/opencr_arduino/opencr/libraries/DynamixelSDK/src/dynamixel_sdk/port_handler_arduino.cpp#L78
3341
#define DXL_SERIAL Serial3
3442
#define DEBUG_SERIAL Serial
3543
const uint8_t DXL_DIR_PIN = 84; // OpenCR Board's DIR PIN.
36-
#else // When using DynamixelShield
44+
#else // Other boards when using DynamixelShield
3745
#define DXL_SERIAL Serial1
3846
#define DEBUG_SERIAL Serial
3947
const uint8_t DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN
4048
#endif
49+
4150

4251
const uint8_t DXL_ID = 1;
4352
const float DXL_PROTOCOL_VERSION = 2.0;

‎examples/basic/current_mode/current_mode.ino

+12-3
Original file line numberDiff line numberDiff line change
@@ -23,21 +23,30 @@
2323
#define DXL_SERIAL Serial
2424
#define DEBUG_SERIAL soft_serial
2525
const uint8_t DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN
26-
#elif ARDUINO_OpenCM904 // Official ROBOTIS board with DXL circuit.
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.
2735
#define DXL_SERIAL Serial3 //OpenCM9.04 EXP Board's DXL port Serial. (Serial1 for the DXL port on the OpenCM 9.04 board)
2836
#define DEBUG_SERIAL Serial
2937
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)
30-
#elif ARDUINO_OpenCR // Official ROBOTIS board with DXL circuit.
38+
#elif defined(ARDUINO_OpenCR) // When using official ROBOTIS board with DXL circuit.
3139
// For OpenCR, there is a DXL Power Enable pin, so you must initialize and control it.
3240
// Reference link : https://github.com/ROBOTIS-GIT/OpenCR/blob/master/arduino/opencr_arduino/opencr/libraries/DynamixelSDK/src/dynamixel_sdk/port_handler_arduino.cpp#L78
3341
#define DXL_SERIAL Serial3
3442
#define DEBUG_SERIAL Serial
3543
const uint8_t DXL_DIR_PIN = 84; // OpenCR Board's DIR PIN.
36-
#else // When using DynamixelShield
44+
#else // Other boards when using DynamixelShield
3745
#define DXL_SERIAL Serial1
3846
#define DEBUG_SERIAL Serial
3947
const uint8_t DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN
4048
#endif
49+
4150

4251
const uint8_t DXL_ID = 1;
4352
const float DXL_PROTOCOL_VERSION = 2.0;

‎examples/basic/current_position_mode/current_position_mode.ino

+12-3
Original file line numberDiff line numberDiff line change
@@ -27,21 +27,30 @@
2727
#define DXL_SERIAL Serial
2828
#define DEBUG_SERIAL soft_serial
2929
const uint8_t DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN
30-
#elif ARDUINO_OpenCM904 // Official ROBOTIS board with DXL circuit.
30+
#elif defined(ARDUINO_SAM_DUE) // When using DynamixelShield
31+
#define DXL_SERIAL Serial
32+
#define DEBUG_SERIAL SerialUSB
33+
const uint8_t DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN
34+
#elif defined(ARDUINO_SAM_ZERO) // When using DynamixelShield
35+
#define DXL_SERIAL Serial1
36+
#define DEBUG_SERIAL SerialUSB
37+
const uint8_t DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN
38+
#elif defined(ARDUINO_OpenCM904) // When using official ROBOTIS board with DXL circuit.
3139
#define DXL_SERIAL Serial3 //OpenCM9.04 EXP Board's DXL port Serial. (Serial1 for the DXL port on the OpenCM 9.04 board)
3240
#define DEBUG_SERIAL Serial
3341
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)
34-
#elif ARDUINO_OpenCR // Official ROBOTIS board with DXL circuit.
42+
#elif defined(ARDUINO_OpenCR) // When using official ROBOTIS board with DXL circuit.
3543
// For OpenCR, there is a DXL Power Enable pin, so you must initialize and control it.
3644
// Reference link : https://github.com/ROBOTIS-GIT/OpenCR/blob/master/arduino/opencr_arduino/opencr/libraries/DynamixelSDK/src/dynamixel_sdk/port_handler_arduino.cpp#L78
3745
#define DXL_SERIAL Serial3
3846
#define DEBUG_SERIAL Serial
3947
const uint8_t DXL_DIR_PIN = 84; // OpenCR Board's DIR PIN.
40-
#else // When using DynamixelShield
48+
#else // Other boards when using DynamixelShield
4149
#define DXL_SERIAL Serial1
4250
#define DEBUG_SERIAL Serial
4351
const uint8_t DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN
4452
#endif
53+
4554

4655
const uint8_t DXL_ID = 1;
4756
const float DXL_PROTOCOL_VERSION = 2.0;

‎examples/basic/id/id.ino

+38-12
Original file line numberDiff line numberDiff line change
@@ -23,32 +23,44 @@
2323
#define DXL_SERIAL Serial
2424
#define DEBUG_SERIAL soft_serial
2525
const uint8_t DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN
26-
#elif ARDUINO_OpenCM904 // Official ROBOTIS board with DXL circuit.
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.
2735
#define DXL_SERIAL Serial3 //OpenCM9.04 EXP Board's DXL port Serial. (Serial1 for the DXL port on the OpenCM 9.04 board)
2836
#define DEBUG_SERIAL Serial
2937
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)
30-
#elif ARDUINO_OpenCR // Official ROBOTIS board with DXL circuit.
38+
#elif defined(ARDUINO_OpenCR) // When using official ROBOTIS board with DXL circuit.
3139
// For OpenCR, there is a DXL Power Enable pin, so you must initialize and control it.
3240
// Reference link : https://github.com/ROBOTIS-GIT/OpenCR/blob/master/arduino/opencr_arduino/opencr/libraries/DynamixelSDK/src/dynamixel_sdk/port_handler_arduino.cpp#L78
3341
#define DXL_SERIAL Serial3
3442
#define DEBUG_SERIAL Serial
3543
const uint8_t DXL_DIR_PIN = 84; // OpenCR Board's DIR PIN.
36-
#else // When using DynamixelShield
44+
#else // Other boards when using DynamixelShield
3745
#define DXL_SERIAL Serial1
3846
#define DEBUG_SERIAL Serial
3947
const uint8_t DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN
4048
#endif
49+
4150

42-
const uint8_t DXL_ID = 1;
51+
const uint8_t DEFAULT_DXL_ID = 1;
4352
const float DXL_PROTOCOL_VERSION = 2.0;
4453

4554
Dynamixel2Arduino dxl(DXL_SERIAL, DXL_DIR_PIN);
4655

4756
void setup() {
4857
// put your setup code here, to run once:
58+
uint8_t present_id = DEFAULT_DXL_ID;
59+
uint8_t new_id = 0;
4960

5061
// Use UART port of DYNAMIXEL Shield to debug.
5162
DEBUG_SERIAL.begin(115200);
63+
while(!DEBUG_SERIAL);
5264

5365
// Set Port baudrate to 57600bps. This has to match with DYNAMIXEL baudrate.
5466
dxl.begin(57600);
@@ -58,22 +70,36 @@ void setup() {
5870
DEBUG_SERIAL.print("PROTOCOL ");
5971
DEBUG_SERIAL.print(DXL_PROTOCOL_VERSION, 1);
6072
DEBUG_SERIAL.print(", ID ");
61-
DEBUG_SERIAL.print(DXL_ID);
73+
DEBUG_SERIAL.print(present_id);
6274
DEBUG_SERIAL.print(": ");
63-
if(dxl.ping(DXL_ID) == true) {
75+
if(dxl.ping(present_id) == true) {
6476
DEBUG_SERIAL.print("ping succeeded!");
6577
DEBUG_SERIAL.print(", Model Number: ");
66-
DEBUG_SERIAL.println(dxl.getModelNumber(DXL_ID));
78+
DEBUG_SERIAL.println(dxl.getModelNumber(present_id));
6779

6880
// Turn off torque when configuring items in EEPROM area
69-
dxl.torqueOff(DXL_ID);
81+
dxl.torqueOff(present_id);
7082

7183
// set a new ID for DYNAMIXEL. Do not use ID 200
72-
dxl.setID(DXL_ID, 100);
73-
DEBUG_SERIAL.println("ID has been successfully changed to 100");
84+
new_id = 100;
85+
if(dxl.setID(present_id, new_id) == true){
86+
present_id = new_id;
87+
DEBUG_SERIAL.print("ID has been successfully changed to ");
88+
DEBUG_SERIAL.println(new_id);
7489

75-
dxl.setID(100, DXL_ID);
76-
DEBUG_SERIAL.println("ID has been successfully changed back to Original ID");
90+
new_id = DEFAULT_DXL_ID;
91+
if(dxl.setID(present_id, new_id) == true){
92+
present_id = new_id;
93+
DEBUG_SERIAL.print("ID has been successfully changed back to Original ID ");
94+
DEBUG_SERIAL.println(new_id);
95+
}else{
96+
DEBUG_SERIAL.print("Failed to change ID to ");
97+
DEBUG_SERIAL.println(new_id);
98+
}
99+
}else{
100+
DEBUG_SERIAL.print("Failed to change ID to ");
101+
DEBUG_SERIAL.println(new_id);
102+
}
77103
}
78104
else{
79105
DEBUG_SERIAL.println("ping failed!");

0 commit comments

Comments
 (0)