Skip to content

Commit 16547c8

Browse files
geoand-robotdesigngeoand-robotdesign
authored
Initial Commit
1 parent dfdf463 commit 16547c8

59 files changed

Lines changed: 3887 additions & 0 deletions

Some content is hidden

Large Commits have some content hidden by default. Use the searchbox below for content that may be hidden.

‎README.md‎

Lines changed: 357 additions & 0 deletions
Large diffs are not rendered by default.

‎examples/matlab/protocol 1.0/ReadSyncWrite.m‎

Lines changed: 105 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,105 @@
1+
% READSYNCWRITE Example file executing read and sync write functions for 1
2+
% or more Dynamixel motors under Protocol 1.0 communication
3+
%
4+
% Depending on the example code, the motor(s) need to be pre-set to joint
5+
% or wheel mode via the Dynamixel Wizard or equivalent software
6+
%
7+
% Part of the Dynamixel library for Matlab and Simulink
8+
% Author: Georgios Andrikopoulos (geoand@kth.se), 2022
9+
% Mechatronics & Embedded Control Systems Unit, KTH, Stockholm
10+
11+
clear
12+
close all
13+
clc
14+
15+
% Set COM port
16+
com_port = 'COM8';
17+
18+
% Set baudrate
19+
baud_rate = 1000000;
20+
21+
% Set protocol Version
22+
protocol_version = 1;
23+
24+
% Initialize library and port number
25+
[lib_name, port_num] = initDxl(com_port);
26+
27+
% Open port
28+
openPortDxl(lib_name, port_num);
29+
30+
% Set Baudrate
31+
setBaudDxl(lib_name, port_num, baud_rate);
32+
33+
% Find dynamixels
34+
[motor_IDs, motor_models] = findDxl(lib_name, port_num, protocol_version);
35+
36+
% Enable torque
37+
torqueDxl(lib_name, port_num, protocol_version, motor_IDs, ...
38+
motor_models, 'Enable');
39+
40+
% Check error
41+
checkErrorDxl(lib_name, port_num, protocol_version)
42+
43+
% Initialize sync write
44+
group_num_write = initSyncWriteDxl(lib_name, port_num, protocol_version,...
45+
motor_models, ["Goal Position"; "Moving Speed"]);
46+
47+
%% Joint Mode Example
48+
clc
49+
% Data to write
50+
data = 500; % Example goal position for one motor
51+
% data = [1000 2000]; % Example goal position for two motors
52+
53+
% Sync write function
54+
tic
55+
syncWriteDxl(lib_name, group_num_write(1), motor_IDs, motor_models, ...
56+
data, 'Goal Position')
57+
toc
58+
59+
% Read functions
60+
tic
61+
readDxl(lib_name, port_num, protocol_version, motor_IDs, motor_models, ...
62+
'Present Position')
63+
readDxl(lib_name, port_num, protocol_version, motor_IDs, motor_models, ...
64+
'Present Speed')
65+
readDxl(lib_name, port_num, protocol_version, motor_IDs, motor_models, ...
66+
'Present Load')
67+
readDxl(lib_name, port_num, protocol_version, motor_IDs, motor_models, ...
68+
'Present Temperature')
69+
toc
70+
71+
%% Wheel Mode Example
72+
clc
73+
% Data to write
74+
data = 500; % Example moving speed for one motor
75+
% data = [400 300]; % Example moving speeds for two motors
76+
77+
% Sync write function
78+
tic
79+
syncWriteDxl(lib_name, group_num_write(2), motor_IDs, motor_models, ...
80+
data, 'Moving Speed')
81+
toc
82+
83+
% Read functions
84+
tic
85+
readDxl(lib_name, port_num, protocol_version, motor_IDs, motor_models, ...
86+
'Present Position')
87+
readDxl(lib_name, port_num, protocol_version, motor_IDs, motor_models, ...
88+
'Present Speed')
89+
readDxl(lib_name, port_num, protocol_version, motor_IDs, motor_models, ...
90+
'Present Load')
91+
readDxl(lib_name, port_num, protocol_version, motor_IDs, motor_models, ...
92+
'Present Temperature')
93+
toc
94+
%% End Session
95+
clc
96+
97+
% Disable torque
98+
torqueDxl(lib_name, port_num, protocol_version, motor_IDs, ...
99+
motor_models, 'Disable');
100+
101+
% Check error
102+
checkErrorDxl(lib_name, port_num, protocol_version)
103+
104+
% Close port
105+
closePortDxl(lib_name,port_num);

‎examples/matlab/protocol 1.0/ReadWrite.m‎

Lines changed: 108 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,108 @@
1+
% READWRITE Example file executing read and write functions for 1 or more
2+
% Dynamixel motors under Protocol 1.0 communication
3+
%
4+
% Depending on the example code, the motor(s) need to be pre-set to joint
5+
% or wheel mode via the Dynamixel Wizard or equivalent software
6+
%
7+
% Part of the Dynamixel library for Matlab and Simulink
8+
% Author: Georgios Andrikopoulos (geoand@kth.se), 2022
9+
% Mechatronics & Embedded Control Systems Unit, KTH, Stockholm
10+
11+
clear
12+
close all
13+
clc
14+
15+
% Set COM port
16+
com_port = 'COM8';
17+
18+
% Set baudrate
19+
baud_rate = 1000000;
20+
21+
% Set protocol Version
22+
protocol_version = 1;
23+
24+
% Initialize library and port number
25+
[lib_name, port_num] = initDxl(com_port);
26+
27+
% Open port
28+
openPortDxl(lib_name, port_num);
29+
30+
% Set Baudrate
31+
setBaudDxl(lib_name, port_num, baud_rate);
32+
33+
% Find dynamixels
34+
[motor_IDs, motor_models] = findDxl(lib_name, port_num, protocol_version);
35+
36+
% Enable torque
37+
torqueDxl(lib_name, port_num, protocol_version, motor_IDs, ...
38+
motor_models, 'Enable');
39+
40+
% Check error
41+
checkErrorDxl(lib_name, port_num, protocol_version)
42+
43+
%% Joint Mode Example
44+
% AX Motor: Make sure that the motor is set to joint mode before using
45+
% this example
46+
clc
47+
48+
% Data to write
49+
data = 500; % Example goal position for one motor
50+
% data = [1000 500]; % Example goal position for two motors
51+
52+
% Write function
53+
tic
54+
writeDxl(lib_name, port_num, protocol_version, motor_IDs, motor_models, ...
55+
data, 'Goal Position')
56+
toc
57+
58+
% Read functions
59+
tic
60+
readDxl(lib_name, port_num, protocol_version, motor_IDs, motor_models, ...
61+
'Present Position')
62+
readDxl(lib_name, port_num, protocol_version, motor_IDs, motor_models, ...
63+
'Present Speed')
64+
readDxl(lib_name, port_num, protocol_version, motor_IDs, motor_models, ...
65+
'Present Load')
66+
readDxl(lib_name, port_num, protocol_version, motor_IDs, motor_models, ...
67+
'Present Temperature')
68+
toc
69+
70+
%% Wheel Mode Examples
71+
% AX Motor: Make sure that the motor is set to wheel mode before using
72+
% this example
73+
clc
74+
75+
% Data to write
76+
data = 300; % Example moving speed for one motor
77+
% data = [500 1000]; % Example moving speeds for two motors
78+
79+
% Write function
80+
tic
81+
writeDxl(lib_name, port_num, protocol_version, motor_IDs, motor_models, ...
82+
data, 'Moving Speed')
83+
toc
84+
85+
% Read functions
86+
tic
87+
readDxl(lib_name, port_num, protocol_version, motor_IDs, motor_models, ...
88+
'Present Position')
89+
readDxl(lib_name, port_num, protocol_version, motor_IDs, motor_models, ...
90+
'Present Speed')
91+
readDxl(lib_name, port_num, protocol_version, motor_IDs, motor_models, ...
92+
'Present Load')
93+
readDxl(lib_name, port_num, protocol_version, motor_IDs, motor_models, ...
94+
'Present Temperature')
95+
toc
96+
97+
%% End Session
98+
clc
99+
100+
% Disable torque
101+
torqueDxl(lib_name, port_num, protocol_version, motor_IDs, motor_models, ...
102+
'Disable');
103+
104+
% Check error
105+
checkErrorDxl(lib_name, port_num, protocol_version)
106+
107+
% Close port
108+
closePortDxl(lib_name,port_num);

‎examples/matlab/protocol 2.0/FastSyncReadFastSyncWrite.m‎

Lines changed: 128 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,128 @@
1+
% FASTSYNCREADFASTSYNCWRITE Example file executing fast sync read and fast
2+
% sync write functions for 2 Dynamixel motors under Protocol 2.0
3+
% communication and using indirect addresses.
4+
%
5+
% During the initialization phase, the motor(s) are set to 'Extended
6+
% Position Control' operating mode, while their drive mode is set to
7+
% 'Velocity-based Profile'.
8+
%
9+
% Part of the Dynamixel library for Matlab and Simulink
10+
% Author: Georgios Andrikopoulos (geoand@kth.se), 2022
11+
% Mechatronics & Embedded Control Systems Unit, KTH, Stockholm
12+
13+
clear
14+
close all
15+
clc
16+
17+
% Set COM port
18+
com_port = 'COM8';
19+
20+
% Set baudrate
21+
baud_rate = 4000000;
22+
23+
% Set protocol Version
24+
protocol_version = 2;
25+
26+
% Initialize library and port number
27+
[lib_name, port_num] = initDxl(com_port);
28+
29+
% Open port
30+
openPortDxl(lib_name, port_num);
31+
32+
% Set Baudrate
33+
setBaudDxl(lib_name, port_num, baud_rate);
34+
35+
% Find dynamixels
36+
[motor_IDs, motor_models] = findDxl(lib_name, port_num, protocol_version);
37+
38+
% Operating modes
39+
operatingModeDxl(lib_name, port_num, protocol_version, motor_IDs, ...
40+
motor_models, 'Extended Position Control');
41+
42+
% Drive modes
43+
driveModeDxl(lib_name, port_num, protocol_version, motor_IDs, ...
44+
motor_models, 'Velocity-based Profile');
45+
46+
% Indirect addresses are accessible only when the torque is disabled
47+
torqueDxl(lib_name, port_num, protocol_version, motor_IDs, ...
48+
motor_models, 'Disable');
49+
50+
% Define the read and write quantities
51+
select_sync_write = ["Goal Position", "LED"];
52+
select_sync_read = ["Present Position", "Present Velocity", ...
53+
"Present Current/Load", "Present Temperature"];
54+
55+
% Set indirect addresses for sync write
56+
[start_address_write_indirect, data_length_write_indirect] = ...
57+
setIndirectWriteDxl(lib_name, port_num, protocol_version, motor_IDs, ...
58+
motor_models, select_sync_write);
59+
60+
% Set indirect addresses for fast sync read
61+
start_address_read_indirect = start_address_write_indirect + ...
62+
data_length_write_indirect;
63+
[start_address_read_indirect, data_length_read_indirect] = ...
64+
setIndirectReadDxl(lib_name, port_num, protocol_version, motor_IDs, ...
65+
motor_models, start_address_read_indirect, select_sync_read);
66+
67+
% Enable torque
68+
torqueDxl(lib_name, port_num, protocol_version, motor_IDs, motor_models, ...
69+
'Enable');
70+
% Check error
71+
checkErrorDxl(lib_name, port_num, protocol_version)
72+
73+
% Initialize sync write
74+
group_num_write = initSyncWriteIndirectDxl(lib_name, port_num, protocol_version,...
75+
motor_models, start_address_write_indirect, data_length_write_indirect);
76+
77+
% Initialize sync read
78+
group_num_read = initSyncReadIndirectDxl(lib_name, port_num, protocol_version,...
79+
motor_models, start_address_read_indirect, data_length_read_indirect);
80+
81+
% Add parameters for the sync read
82+
syncReadAddParamDxl(lib_name, group_num_read, motor_IDs);
83+
84+
%% Example
85+
clc
86+
87+
% Goal positions for 2 motors
88+
goal_positions = [1000 1000];
89+
90+
% LED state for 2 motors
91+
LED = [0 0];
92+
93+
% Set write properties
94+
data_write = [goal_positions; LED];
95+
96+
% Fast sync write to indirect data
97+
tic
98+
fastSyncWriteDxl(lib_name, group_num_write, motor_IDs, motor_models, ...
99+
data_write, select_sync_write)
100+
toc
101+
102+
% Fast sync read from indirect data
103+
tic
104+
read_data = fastSyncReadDxl(lib_name, group_num_read, motor_IDs, ...
105+
motor_models, select_sync_read, start_address_read_indirect);
106+
toc
107+
108+
present_positions = read_data(1,:)
109+
present_velocities = read_data(2,:)
110+
present_currents = read_data(3,:)
111+
present_temperatures = read_data(4,:)
112+
113+
114+
%% End Session
115+
clc
116+
117+
% Read clear parameters
118+
syncReadClearParamDxl(lib_name, group_num_read);
119+
120+
% Disable torque
121+
torqueDxl(lib_name, port_num, protocol_version, motor_IDs, motor_models, ...
122+
'Disable');
123+
124+
% Check error
125+
checkErrorDxl(lib_name, port_num, protocol_version)
126+
127+
% Close port
128+
closePortDxl(lib_name,port_num);

0 commit comments

Comments
 (0)