AUV-Coop-Assembly
Master Thesis for Robotics Engineering. Cooperative peg-in-hole assembly with two underwater vehicles guided by vision
plotCoopGeneric.m
1 function plotCoopGeneric(rootPath, coordName, file)
2 
3 legFontSize = 13;
4 titleFontSize = 16;
5 ylabFontSize = 15;
6 
7 fileName = strcat(file, '.txt');
8 xDot = importMatrices(strcat(rootPath, coordName, fileName));
9 nRow = size(xDot, 1);
10 nStep = size(xDot, 3);
11 
12 %millisecond indicated in missionManager
13 global sControlLoop
14 totSecondPassed = sControlLoop*(nStep-1);
15 seconds = 0:sControlLoop:totSecondPassed;
16 
17 %plot vel
18 figure
19 hold on;
20 for i = 1:nRow
21  a(:) = xDot(i,1,:);
22  plot(seconds, a);
23 end
24 xlabel('time [s]');
25 
26 tq = title(file);
27 
28 if nRow == 6
29  leg = legend('$\dot{x}$','$\dot{y}$', '$\dot{z}$', '$w_x$', '$w_y$', '$w_z$');
30  set(leg, 'Interpreter', 'latex', 'FontSize' , legFontSize);
31 elseif nRow == 5
32  leg = legend('$\dot{x}$','$\dot{y}$', '$\dot{z}$', '$w_y$', '$w_z$');
33  set(leg, 'Interpreter', 'latex', 'FontSize' , legFontSize);
34 
35 end
36 
37 set (tq, 'Interpreter', 'latex', 'FontSize' , titleFontSize);
38 hold off