-
Notifications
You must be signed in to change notification settings - Fork 0
/
Read_Positions.m
74 lines (61 loc) · 2.53 KB
/
Read_Positions.m
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
%% Script to read the current positions of a pincher robot arm
% r.siddall@surrey.ac.uk, created 1/11/2022 using DynamixelSDK
%% Initialise serial port and motor(s)
clc;
clear all;
DEVICENAME = 'COM20'; % Check 'Device Manager' to see which COM port your device is using
run DynamixelStartup
% Open port
if (openPort(BaseMotor.port_num))
fprintf('Serial port opened.\n');
else
unloadlibrary(lib_name);
fprintf('Failed to open the port!\n');
input('Press any key to terminate...\n');
return;
end
% Set port baudrate
if (setBaudRate(BaseMotor.port_num, BaseMotor.BAUDRATE))
fprintf('Baudrate set.\n');
else
unloadlibrary(lib_name);
fprintf('Failed to change the baudrate!\n');
input('Press any key to terminate...\n');
return;
end
%% Read data and print
armPositions = [getData(BaseMotor,'position'),...
getData(ShoulderMotor,'position'),...
getData(ElbowMotor,'position'),...
getData(WristMotor,'position')];
disp(['Arm joint positions are: ']);
disp(['Base Joint: ' num2str(round(armPositions(1)*360/4095)) '° / ' num2str(armPositions(1)) ' bits'])
disp(['Shoulder Joint: ' num2str(round(armPositions(2)*360/4095)) '° / ' num2str(armPositions(2)) ' bits'])
disp(['Elbow Joint: ' num2str(round(armPositions(3)*360/4095)) '° / ' num2str(armPositions(3)) ' bits'])
disp(['Wrist Joint: ' num2str(round(armPositions(4)*360/4095)) '° / ' num2str(armPositions(4)) ' bits'])
disp(['For copy/paste: [' num2str(armPositions) ']']); disp([' '])
%% Close port
% Close port
closePort(BaseMotor.port_num);
% Unload Library
unloadlibrary(lib_name);
fprintf('Serial port closed.\n');
close all;
clear all;
%% Functions for motor operations
function [dataRead] = getData(MotorID,dataType)
switch(dataType)
case 'position'
data_address = MotorID.ADDR_PRESENT_POSITION;
case 'velocity'
data_address = MotorID.ADDR_PRESENT_VELOCITY;
end
dataRead = read4ByteTxRx(MotorID.port_num, MotorID.PROTOCOL_VERSION, MotorID.DXL_ID,data_address);
dxl_comm_result = getLastTxRxResult(MotorID.port_num, MotorID.PROTOCOL_VERSION);
dxl_error = getLastRxPacketError(MotorID.port_num, MotorID.PROTOCOL_VERSION);
if dxl_comm_result ~= MotorID.COMM_SUCCESS
fprintf('%s\n', getTxRxResult(MotorID.PROTOCOL_VERSION, dxl_comm_result));
elseif dxl_error ~= 0
fprintf('%s\n', getRxPacketError(MotorID.PROTOCOL_VERSION, dxl_error));
end
end