-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathiRobotPose.m
55 lines (47 loc) · 1.46 KB
/
iRobotPose.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
function pose = iRobotPose(u)
%iRobotPose(u) Request the position (x,y) and orientation of iRobot Create
% Inputs:
% u = BLE object
% Outputs:
% pose = A 1x3 vector containing the position and orientation of the
% robot (x,y,yaw). Position is mm while orientation is in deci-degrees
%
% Author: Prof. E. Rodriguez-Seda
% Date: November 30, 2022
decMessage = zeros(1,20);
decMessage(1) = 1; %Motor device
decMessage(2) = 16; %Command
decMessage(3) = u.packetID;
tempString = dec2bin(decMessage(1:19),8);
inputMsg = zeros(19*8,1);
for i = 1:19
for j = 1:8
inputMsg(8*(i-1)+j) = str2double(tempString(i,j));
end
end
codeword = u.crc8(inputMsg);
checksum = codeword(end-7:end)';
decMessage(20) = bin2dec(num2str(checksum));
getPose = 1;
tStart = tic;
while getPose
write(u.dataTx,decMessage)
dataRead = read(u.dataRx);
if dataRead(2) == 16
getPose = 0;
x = typecast(fliplr(uint8(dataRead(8:11))), 'int32'); %in mm
y = typecast(fliplr(uint8(dataRead(12:15))), 'int32'); %in mm
yaw = typecast(fliplr(uint8(dataRead(16:17))), 'int16'); %in deci-degrees
pose = [x,y,yaw];
end
if toc(tStart) > 1
warning('Timeout, took longer than 1 seconds to get pose.')
break;
end
end
packetID = u.packetID + 1;
if packetID > 255
packetID = 0;
end
u.packetID = packetID;
end