Skip to content

Commit 608463b

Browse files
author
Kei
authored
Merge pull request #16 from ROBOTIS-GIT/add_slave_functions
Add DYNAMIXEL Slave function (Limited functionality)
2 parents f50deb6 + 60dc775 commit 608463b

File tree

7 files changed

+1020
-44
lines changed

7 files changed

+1020
-44
lines changed

‎examples/advanced/slave/slave.ino‎

Lines changed: 89 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,89 @@
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+
// The following definitions are examples when using the DYNAMIXEL Shield.
20+
// Please modify it to suit your hardware.
21+
#if defined(ARDUINO_AVR_UNO) || defined(ARDUINO_AVR_MEGA2560)
22+
#include <SoftwareSerial.h>
23+
SoftwareSerial soft_serial(7, 8); //RX,TX //UART Port on DYNAMIXEL Shield
24+
#define DXL_SERIAL Serial
25+
#define DEBUG_SERIAL soft_serial
26+
const uint8_t DXL_DIR_PIN = 2; //DYNAMIXEL Shield
27+
#else
28+
#define DXL_SERIAL Serial1
29+
#define DEBUG_SERIAL Serial
30+
const uint8_t DXL_DIR_PIN = 2; //DYNAMIXEL Shield
31+
#endif
32+
33+
// Create a port object for DYNAMIXEL communication.
34+
// The list of available classes is as follows.
35+
// 1) DYNAMIXEL::SerialPortHandler (HardwareSerial only)
36+
// -note: If you do not want to use half duplex communication, do not enter the second parameter.
37+
// 2) DYNAMIXEL::USBSerialPortHandler (Only USB port on each board)
38+
DYNAMIXEL::SerialPortHandler dxl_port(DXL_SERIAL, DXL_DIR_PIN);
39+
40+
// Create a Slave object to communicate with the master.
41+
const float DXL_PROTOCOL_VER_1_0 = 1.0;
42+
const float DXL_PROTOCOL_VER_2_0 = 2.0;
43+
const uint16_t DXL_MODEL_NUM = 0x5005; // Modify it to what you want.
44+
DYNAMIXEL::Slave dxl(dxl_port, DXL_MODEL_NUM);
45+
46+
// Declare the address of the Slave control item you want
47+
// to register and the variable (size is also important)
48+
// to store its data.
49+
const uint16_t ADDR_CONTROL_ITEM_LED = 10;
50+
const uint16_t ADDR_CONTROL_ITEM_ANALOG = 20;
51+
// note: 'int' is not supported because its size varies by system architecture.
52+
uint8_t control_item_led;
53+
int16_t control_item_analog;
54+
55+
56+
void setup() {
57+
// put your setup code here, to run once:
58+
DEBUG_SERIAL.begin(115200);
59+
60+
// Speed setting for communication (not necessary for USB)
61+
dxl_port.begin(1000000);
62+
63+
dxl.setPortProtocolVersion(DXL_PROTOCOL_VER_2_0);
64+
dxl.setFirmwareVersion(1);
65+
dxl.setID(1);
66+
67+
// Add control items.
68+
// Input the address(p1) and the variable(p2) to write/read the data as parameters.
69+
dxl.addControlItem(ADDR_CONTROL_ITEM_LED, control_item_led);
70+
dxl.addControlItem(ADDR_CONTROL_ITEM_ANALOG, control_item_analog);
71+
}
72+
73+
void loop() {
74+
// put your main code here, to run repeatedly:
75+
76+
// If there is a packet from the master, process it.
77+
if(dxl.processPacket() == false){
78+
DEBUG_SERIAL.print("Last lib err code: ");
79+
DEBUG_SERIAL.print(dxl.getLastLibErrCode());
80+
DEBUG_SERIAL.print(", ");
81+
DEBUG_SERIAL.print("Last status packet err code: ");
82+
DEBUG_SERIAL.println(dxl.getLastStatusPacketError());
83+
DEBUG_SERIAL.println();
84+
}
85+
86+
// Update or use variables for each control item.
87+
digitalWrite(LED_BUILTIN, control_item_led);
88+
control_item_analog = analogRead(A0);
89+
}

‎examples/advanced/slave_callback/slave_callback.ino‎

Lines changed: 112 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,112 @@
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+
// The following definitions are examples when using the DYNAMIXEL Shield.
20+
// Please modify it to suit your hardware.
21+
#if defined(ARDUINO_AVR_UNO) || defined(ARDUINO_AVR_MEGA2560)
22+
#include <SoftwareSerial.h>
23+
SoftwareSerial soft_serial(7, 8); //RX,TX //UART Port on DYNAMIXEL Shield
24+
#define DXL_SERIAL Serial
25+
#define DEBUG_SERIAL soft_serial
26+
const uint8_t DXL_DIR_PIN = 2; //DYNAMIXEL Shield
27+
#else
28+
#define DXL_SERIAL Serial1
29+
#define DEBUG_SERIAL Serial
30+
const uint8_t DXL_DIR_PIN = 2; //DYNAMIXEL Shield
31+
#endif
32+
33+
// Create a port object for DYNAMIXEL communication.
34+
// The list of available classes is as follows.
35+
// 1) DYNAMIXEL::SerialPortHandler (HardwareSerial only)
36+
// -note: If you do not want to use half duplex communication, do not enter the second parameter.
37+
// 2) DYNAMIXEL::USBSerialPortHandler (Only USB port on each board)
38+
DYNAMIXEL::SerialPortHandler dxl_port(DXL_SERIAL, DXL_DIR_PIN);
39+
40+
// Create a Slave object to communicate with the master.
41+
const float DXL_PROTOCOL_VER_1_0 = 1.0;
42+
const float DXL_PROTOCOL_VER_2_0 = 2.0;
43+
const uint16_t DXL_MODEL_NUM = 0x5005; // Modify it to what you want.
44+
DYNAMIXEL::Slave dxl(dxl_port, DXL_MODEL_NUM);
45+
46+
// Declare the address of the Slave control item you want
47+
// to register and the variable (size is also important)
48+
// to store its data.
49+
const uint16_t ADDR_CONTROL_ITEM_LED = 10;
50+
const uint16_t ADDR_CONTROL_ITEM_ANALOG = 20;
51+
// note: 'int' is not supported because its size varies by system architecture.
52+
uint8_t control_item_led;
53+
int16_t control_item_analog;
54+
55+
56+
void setup() {
57+
// put your setup code here, to run once:
58+
DEBUG_SERIAL.begin(115200);
59+
60+
// Speed setting for communication (not necessary for USB)
61+
dxl_port.begin(1000000);
62+
63+
dxl.setPortProtocolVersion(DXL_PROTOCOL_VER_2_0);
64+
dxl.setFirmwareVersion(1);
65+
dxl.setID(1);
66+
67+
// Add control items.
68+
// Input the address(p1) and the variable(p2) to write/read the data as parameters.
69+
dxl.addControlItem(ADDR_CONTROL_ITEM_LED, control_item_led);
70+
dxl.addControlItem(ADDR_CONTROL_ITEM_ANALOG, control_item_analog);
71+
72+
// Add interrupt callback functions to process read/write packet.
73+
dxl.setReadCallbackFunc(read_callback_func);
74+
dxl.setWriteCallbackFunc(write_callback_func);
75+
}
76+
77+
void loop() {
78+
// put your main code here, to run repeatedly:
79+
80+
// If there is a packet from the master, process it.
81+
if(dxl.processPacket() == false){
82+
DEBUG_SERIAL.print("Last lib err code: ");
83+
DEBUG_SERIAL.print(dxl.getLastLibErrCode());
84+
DEBUG_SERIAL.print(", ");
85+
DEBUG_SERIAL.print("Last status packet err code: ");
86+
DEBUG_SERIAL.println(dxl.getLastStatusPacketError());
87+
DEBUG_SERIAL.println();
88+
}
89+
}
90+
91+
92+
// Function to update data according to master's read request.
93+
// If you use dxl_err_code, you can send the packet's error code to the Master.
94+
// See the DYNAMIXEL Protocol documentation for this.
95+
// (http://emanual.robotis.com/docs/kr/dxl/protocol2/)
96+
void read_callback_func(uint16_t item_addr, uint8_t &dxl_err_code)
97+
{
98+
if(item_addr == ADDR_CONTROL_ITEM_ANALOG){
99+
control_item_analog = analogRead(A0);
100+
}
101+
}
102+
103+
// Function to update data according to master write request.
104+
// If you use dxl_err_code, you can send the packet's error code to the Master.
105+
// See the DYNAMIXEL Protocol documentation for this.
106+
// (http://emanual.robotis.com/docs/kr/dxl/protocol2/)
107+
void write_callback_func(uint16_t item_addr, uint8_t &dxl_err_code)
108+
{
109+
if(item_addr == ADDR_CONTROL_ITEM_LED){
110+
digitalWrite(LED_BUILTIN, control_item_led);
111+
}
112+
}

‎keywords.txt‎

Lines changed: 22 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -8,7 +8,9 @@
88

99
Dynamixel2Arduino KEYWORD1
1010
Master KEYWORD1
11+
Slave KEYWORD1
1112
SerialPortHandler KEYWORD1
13+
USBSerialPortHandler KEYWORD1
1214

1315
###########################################
1416
# Methods and Functions (KEYWORD2)
@@ -51,8 +53,27 @@ factoryReset KEYWORD2
5153
reboot KEYWORD2
5254
syncWrite KEYWORD2
5355
syncRead KEYWORD2
56+
bulkWrite KEYWORD2
57+
bulkRead KEYWORD2
58+
getLastStatusPacketError KEYWORD2
59+
getLastLibErrCode KEYWORD2
60+
setLastLibErrCode KEYWORD2
5461

55-
# Slave Class (TODO)
62+
# Slave Class
63+
getModelNumber KEYWORD2
64+
setID KEYWORD2
65+
getID KEYWORD2
66+
setFirmwareVersion KEYWORD2
67+
getFirmwareVersion KEYWORD2
68+
setPort KEYWORD2
69+
setPortProtocolVersion KEYWORD2
70+
getPortProtocolVersion KEYWORD2
71+
setWriteCallbackFunc KEYWORD2
72+
setReadCallbackFunc KEYWORD2
73+
processPacket KEYWORD2
74+
getLastStatusPacketError KEYWORD2
75+
getLastLibErrCode KEYWORD2
76+
addControlItem KEYWORD2
5677

5778
###########################################
5879
# Structures (KEYWORD3)

‎src/utility/port_handler.cpp‎

Lines changed: 47 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -102,4 +102,50 @@ size_t SerialPortHandler::write(uint8_t *buf, size_t len)
102102
unsigned long SerialPortHandler::getBaud() const
103103
{
104104
return baud_;
105-
}
105+
}
106+
107+
108+
/* USBSerialPortHandler */
109+
USBSerialPortHandler::USBSerialPortHandler(USB_SERIAL_CLASS& port)
110+
: PortHandler(), port_(port)
111+
{}
112+
113+
void USBSerialPortHandler::begin()
114+
{
115+
port_.begin(1000000);
116+
setOpenState(true);
117+
}
118+
119+
void USBSerialPortHandler::end(void)
120+
{
121+
port_.end();
122+
setOpenState(false);
123+
}
124+
125+
int USBSerialPortHandler::available(void)
126+
{
127+
return port_.available();
128+
}
129+
130+
int USBSerialPortHandler::read()
131+
{
132+
return port_.read();
133+
}
134+
135+
size_t USBSerialPortHandler::write(uint8_t c)
136+
{
137+
size_t ret = 0;
138+
139+
ret = port_.write(c);
140+
141+
return ret;
142+
}
143+
144+
size_t USBSerialPortHandler::write(uint8_t *buf, size_t len)
145+
{
146+
size_t ret;
147+
148+
ret = port_.write(buf, len);
149+
150+
return ret;
151+
}

0 commit comments

Comments
 (0)