Initial Commit

geoand-robotdesign · Feb 28, 2023 · 16547c8 · 16547c8
1 parent dfdf463
commit 16547c8
Show file tree

Hide file tree

Showing 59 changed files with 3,887 additions and 0 deletions.
diff --git a/README.md b/README.md
diff --git a/examples/matlab/protocol 1.0/ReadSyncWrite.m b/examples/matlab/protocol 1.0/ReadSyncWrite.m
@@ -0,0 +1,105 @@
+% READSYNCWRITE Example file executing read and sync write functions for 1 
+% or more Dynamixel motors under Protocol 1.0 communication
+%
+% Depending on the example code, the motor(s) need to be pre-set to joint 
+% or wheel mode via the Dynamixel Wizard or equivalent software
+%
+%   Part of the Dynamixel library for Matlab and Simulink
+%   Author: Georgios Andrikopoulos (geoand@kth.se), 2022
+%   Mechatronics & Embedded Control Systems Unit, KTH, Stockholm
+
+clear
+close all
+clc
+
+% Set COM port
+com_port = 'COM8';
+
+% Set baudrate
+baud_rate = 1000000;
+
+% Set protocol Version
+protocol_version = 1;
+
+% Initialize library and port number
+[lib_name, port_num] = initDxl(com_port);
+
+% Open port
+openPortDxl(lib_name, port_num);
+
+% Set Baudrate
+setBaudDxl(lib_name, port_num, baud_rate);
+
+% Find dynamixels
+[motor_IDs, motor_models] = findDxl(lib_name, port_num, protocol_version);
+
+% Enable torque
+torqueDxl(lib_name, port_num, protocol_version, motor_IDs, ...
+    motor_models, 'Enable');
+
+% Check error
+checkErrorDxl(lib_name, port_num, protocol_version)
+
+% Initialize sync write
+group_num_write = initSyncWriteDxl(lib_name, port_num, protocol_version,...
+    motor_models, ["Goal Position"; "Moving Speed"]);
+
+%% Joint Mode Example
+clc
+% Data to write
+data = 500; % Example goal position for one motor
+% data = [1000 2000]; % Example goal position for two motors
+
+% Sync write function
+tic
+syncWriteDxl(lib_name, group_num_write(1), motor_IDs, motor_models, ...
+    data, 'Goal Position')
+toc
+
+% Read functions
+tic
+readDxl(lib_name, port_num, protocol_version, motor_IDs, motor_models, ...
+    'Present Position')
+readDxl(lib_name, port_num, protocol_version, motor_IDs, motor_models, ...
+    'Present Speed')
+readDxl(lib_name, port_num, protocol_version, motor_IDs, motor_models, ...
+    'Present Load')
+readDxl(lib_name, port_num, protocol_version, motor_IDs, motor_models, ...
+    'Present Temperature')
+toc
+
+%% Wheel Mode Example
+clc
+% Data to write
+data = 500; % Example moving speed for one motor
+% data = [400 300]; % Example moving speeds for two motors
+
+% Sync write function
+tic
+syncWriteDxl(lib_name, group_num_write(2), motor_IDs, motor_models, ...
+    data, 'Moving Speed')
+toc
+
+% Read functions
+tic
+readDxl(lib_name, port_num, protocol_version, motor_IDs, motor_models, ...
+    'Present Position')
+readDxl(lib_name, port_num, protocol_version, motor_IDs, motor_models, ...
+    'Present Speed')
+readDxl(lib_name, port_num, protocol_version, motor_IDs, motor_models, ...
+    'Present Load')
+readDxl(lib_name, port_num, protocol_version, motor_IDs, motor_models, ...
+    'Present Temperature')
+toc
+%% End Session
+clc 
+
+% Disable torque
+torqueDxl(lib_name, port_num, protocol_version, motor_IDs, ...
+    motor_models, 'Disable');
+
+% Check error
+checkErrorDxl(lib_name, port_num, protocol_version)
+
+% Close port 
+closePortDxl(lib_name,port_num);
diff --git a/examples/matlab/protocol 1.0/ReadWrite.m b/examples/matlab/protocol 1.0/ReadWrite.m
@@ -0,0 +1,108 @@
+% READWRITE Example file executing read and write functions for 1 or more 
+% Dynamixel motors under Protocol 1.0 communication
+%
+% Depending on the example code, the motor(s) need to be pre-set to joint 
+% or wheel mode via the Dynamixel Wizard or equivalent software
+%
+%   Part of the Dynamixel library for Matlab and Simulink
+%   Author: Georgios Andrikopoulos (geoand@kth.se), 2022
+%   Mechatronics & Embedded Control Systems Unit, KTH, Stockholm
+
+clear
+close all
+clc
+
+% Set COM port
+com_port = 'COM8';
+
+% Set baudrate
+baud_rate = 1000000;
+
+% Set protocol Version
+protocol_version = 1;
+
+% Initialize library and port number
+[lib_name, port_num] = initDxl(com_port);
+
+% Open port
+openPortDxl(lib_name, port_num);
+
+% Set Baudrate
+setBaudDxl(lib_name, port_num, baud_rate);
+
+% Find dynamixels
+[motor_IDs, motor_models] = findDxl(lib_name, port_num, protocol_version);
+
+% Enable torque
+torqueDxl(lib_name, port_num, protocol_version, motor_IDs, ...
+    motor_models, 'Enable');
+
+% Check error
+checkErrorDxl(lib_name, port_num, protocol_version)
+
+%% Joint Mode Example
+% AX Motor: Make sure that the motor is set to joint mode before using
+% this example
+clc 
+
+% Data to write
+data = 500; % Example goal position for one motor
+% data = [1000 500]; % Example goal position for two motors
+
+% Write function
+tic
+writeDxl(lib_name, port_num, protocol_version, motor_IDs, motor_models, ...
+    data, 'Goal Position')
+toc
+
+% Read functions
+tic
+readDxl(lib_name, port_num, protocol_version, motor_IDs, motor_models, ...
+    'Present Position')
+readDxl(lib_name, port_num, protocol_version, motor_IDs, motor_models, ...
+    'Present Speed')
+readDxl(lib_name, port_num, protocol_version, motor_IDs, motor_models, ...
+    'Present Load')
+readDxl(lib_name, port_num, protocol_version, motor_IDs, motor_models, ...
+    'Present Temperature')
+toc
+
+%% Wheel Mode Examples
+% AX Motor: Make sure that the motor is set to wheel mode before using
+% this example
+clc 
+
+% Data to write
+data = 300; % Example moving speed for one motor
+% data = [500 1000]; % Example moving speeds for two motors
+
+% Write function
+tic
+writeDxl(lib_name, port_num, protocol_version, motor_IDs, motor_models, ...
+    data, 'Moving Speed')
+toc
+
+% Read functions
+tic
+readDxl(lib_name, port_num, protocol_version, motor_IDs, motor_models, ...
+    'Present Position')
+readDxl(lib_name, port_num, protocol_version, motor_IDs, motor_models, ...
+    'Present Speed')
+readDxl(lib_name, port_num, protocol_version, motor_IDs, motor_models, ...
+    'Present Load')
+readDxl(lib_name, port_num, protocol_version, motor_IDs, motor_models, ...
+    'Present Temperature')
+toc
+
+%% End Session
+clc 
+
+% Disable torque
+torqueDxl(lib_name, port_num, protocol_version, motor_IDs, motor_models, ...
+    'Disable');
+
+% Check error
+checkErrorDxl(lib_name, port_num, protocol_version)
+
+% Close port 
+closePortDxl(lib_name,port_num);
diff --git a/examples/matlab/protocol 2.0/FastSyncReadFastSyncWrite.m b/examples/matlab/protocol 2.0/FastSyncReadFastSyncWrite.m
@@ -0,0 +1,128 @@
+% FASTSYNCREADFASTSYNCWRITE Example file executing fast sync read and fast 
+% sync write functions for 2 Dynamixel motors under Protocol 2.0 
+% communication and using indirect addresses.
+%
+% During the initialization phase, the motor(s) are set to 'Extended
+% Position Control' operating mode, while their drive mode is set to
+% 'Velocity-based Profile'.
+%
+%   Part of the Dynamixel library for Matlab and Simulink
+%   Author: Georgios Andrikopoulos (geoand@kth.se), 2022
+%   Mechatronics & Embedded Control Systems Unit, KTH, Stockholm
+
+clear
+close all
+clc
+
+% Set COM port
+com_port = 'COM8';
+
+% Set baudrate
+baud_rate = 4000000;
+
+% Set protocol Version
+protocol_version = 2;
+
+% Initialize library and port number
+[lib_name, port_num] = initDxl(com_port);
+
+% Open port
+openPortDxl(lib_name, port_num);
+
+% Set Baudrate
+setBaudDxl(lib_name, port_num, baud_rate);
+
+% Find dynamixels
+[motor_IDs, motor_models] = findDxl(lib_name, port_num, protocol_version);
+
+% Operating modes
+operatingModeDxl(lib_name, port_num, protocol_version, motor_IDs, ...
+    motor_models, 'Extended Position Control');
+
+% Drive modes
+driveModeDxl(lib_name, port_num, protocol_version, motor_IDs, ...
+    motor_models, 'Velocity-based Profile');
+
+% Indirect addresses are accessible only when the torque is disabled
+torqueDxl(lib_name, port_num, protocol_version, motor_IDs, ...
+    motor_models, 'Disable');
+
+% Define the read and write quantities
+select_sync_write = ["Goal Position", "LED"];
+select_sync_read = ["Present Position", "Present Velocity", ... 
+    "Present Current/Load", "Present Temperature"];
+
+% Set indirect addresses for sync write
+[start_address_write_indirect, data_length_write_indirect] = ...
+    setIndirectWriteDxl(lib_name, port_num, protocol_version, motor_IDs, ...
+    motor_models, select_sync_write);
+
+% Set indirect addresses for fast sync read
+start_address_read_indirect = start_address_write_indirect + ...
+    data_length_write_indirect;
+[start_address_read_indirect, data_length_read_indirect] = ...
+    setIndirectReadDxl(lib_name, port_num, protocol_version, motor_IDs, ...
+    motor_models, start_address_read_indirect, select_sync_read);
+
+% Enable torque
+torqueDxl(lib_name, port_num, protocol_version, motor_IDs, motor_models, ...
+    'Enable');
+% Check error
+checkErrorDxl(lib_name, port_num, protocol_version)
+
+% Initialize sync write
+group_num_write = initSyncWriteIndirectDxl(lib_name, port_num, protocol_version,...
+    motor_models, start_address_write_indirect, data_length_write_indirect);
+
+% Initialize sync read
+group_num_read = initSyncReadIndirectDxl(lib_name, port_num, protocol_version,...
+    motor_models, start_address_read_indirect, data_length_read_indirect);
+
+% Add parameters for the sync read
+syncReadAddParamDxl(lib_name, group_num_read, motor_IDs);
+
+%% Example
+clc
+
+% Goal positions for 2 motors
+goal_positions = [1000 1000];
+
+% LED state for 2 motors
+LED = [0 0];
+
+% Set write properties
+data_write = [goal_positions; LED];
+
+% Fast sync write to indirect data
+tic
+fastSyncWriteDxl(lib_name, group_num_write, motor_IDs, motor_models, ...
+    data_write, select_sync_write)
+toc
+
+% Fast sync read from indirect data
+tic
+read_data = fastSyncReadDxl(lib_name, group_num_read, motor_IDs, ...
+    motor_models, select_sync_read, start_address_read_indirect);
+toc
+
+present_positions = read_data(1,:)
+present_velocities = read_data(2,:)
+present_currents = read_data(3,:)
+present_temperatures = read_data(4,:)
+
+
+%% End Session
+clc 
+
+% Read clear parameters
+syncReadClearParamDxl(lib_name, group_num_read);
+
+% Disable torque
+torqueDxl(lib_name, port_num, protocol_version, motor_IDs, motor_models, ...
+    'Disable');
+
+% Check error
+checkErrorDxl(lib_name, port_num, protocol_version)
+
+% Close port 
+closePortDxl(lib_name,port_num);