Submitted version of the thesis - Airlab, the Artificial Intelligence ...

130 Appendix B. Documentation of the programming
if (v L ˜= 0)
line ([0 v2] ,[0 0] , ’Color ’ ,[1 0 0]);
if (v L < 0)
fill ([v2 v2+0.12∗p1 v2+0.12∗p1] ,[0 p1∗0.05 −p1∗0.05] , ’r ’)
else
fill ([v2 v2−0.12∗p1 v2−0.12∗p1] ,[0 p1∗0.05 −p1∗0.05] , ’r ’)
end
text(v2,0.1∗v2, ’v L ’ );
end
if (omega ˜= 0)
if (omega > 0)
theta = linspace(−pi/3,pi/2 ,100);
fill ([−0.12∗p1 0 0] ,[ R robot/6 R robot/6+p1∗0.05 R robot/6−p1
∗0.05] , ’b’ );
text(−0.14∗p1, R robot/6+p1∗0.05 , ’\omega ’)
else
theta = linspace(pi/2,4∗pi/3 ,100);
fill ([0.12∗p1 0 0] ,[ R robot/6 R robot/6+p1∗0.05 R robot/6−p1∗
0.05] , ’b’ );
text(0.12∗p1,R robot/6+p1∗0.05 , ’\omega ’ );
end
rho = ones(1 ,100)∗ R robot/6;
[xr , yr] = pol2cart(theta , rho );
line(xr , yr );
end
% ruota 1
if (vt(1) ˜= 0)
line ([ R robot∗cosA , R robot∗cosA+vt(1)∗s1∗sinA ] ,[ R robot∗sinA , R robo
∗sinA−vt(1)∗s1∗cosA] , ’Color ’ ,[0 1 0]);
offset1 = [ R robot∗cosA+vt(1)∗s1∗sinA; R robot∗sinA−vt(1)∗s1∗cosA ];
if (vt(1)∗s1∗sinA < 0)
M = m rot(alpha );
else
M = m rot(−pi + alpha );
end
m arr1 = M∗m arr + [ offset1 offset1 offset1 ];
v txt1 = M∗v txt + offset1 ;