TRC3200 MatLab Programming Assignment : Virtual Planar Rotating Extending Robotic arms

function rotextarmf(rot,ext)

x1 = [ 1 1 0 -1 -1];
y1 = [ -1 5 5 5 -1];

x2 = [ .5 .5 0 -.5 -.5];
y2 = [ 0 5 5 5 0 ];

b1 = [x1;y1];
b2 = [x2;y2];

if ext>max(b1)
warning('Extension limit exceeded');
end

%rot = 135;
%ext = 4;

[b1 b2] = stopmotionrotation(rot,b1,b2);

[b1 b2] = stopmotionextenstion(ext,b1,b2);

end
______________________________________________________

function varargout = stopmotionrotation(rot,b1,b2)

step = 1;

if rot<0
step = -step;
end

for i = 0:step:rot-step

pistep = step * pi/180;

b1 = rotation(b1,pistep);
b2 = rotation(b2,pistep);

draw2box(b1,b2);

pause(0.0001)

end

varargout{1} = b1;
varargout{2} = b2;

end
______________________________________________________
function m = rotation(b1,r)

c = cos(r);
s = sin(r);

z = size(b1);
S = z(1,2);

r1 = [ c -s 0 0; s c 0 0; 0 0 1 0; 0 0 0 1];

m = [b1; ones(1,S); ones(1,S)];

m1 = r1 * m;

m = m1(1:2,1:5);

end
______________________________________________________
function draw2box(b1,b2)

clf;hold on;

axis square
grid on

fill(b2(1,:),b2(2,:),'r');
fill(b1(1,:),b1(2,:),'b');

rectangle('Position',[-.5 -.5 1 1],'Curvature',[1 1],'FaceColor','k');

line([-10 10],[0 0], 'LineStyle', ':','Color','k');
line([0 0],[-10 10],'LineStyle', ':','Color','k');

theta = atan(b1(2,3)/b1(1,3));
text(-8, -8, ['\bfDegrees = ',num2str(theta*180/pi),' deg']);

x = b2(1,3) - b1(1,3);
y = b2(2,3) - b1(2,3);
text(-8, -9, ['\bfExtension = ',num2str(sqrt(x^2+y^2)),' unit']);

bx = b2(1,3);by = b2(2,3);

if sign(bx)<0
t1x = bx - 1;
t2x = bx - 1;
else
t1x = bx + 1;
t2x = bx - 1;
end

if sign(by)<0
t1y = by - 1;
t2y = by - 2;
else
t1y = by + 1;
t2y = by + 2;
end

text(t1x, t1y, ['\bf(',num2str(bx),',',num2str(by),')']);
text(t2x, t2y, ['\bfCoordinate:']);

end
______________________________________________________
function varargout = stopmotionextenstion(ext,b1,b2)

step = 0.1;

for i = 0:step:ext-step

b2 = extension(step,b2);

draw2box(b1,b2);

pause(0.0001)

end

varargout{1} = b1;
varargout{2} = b2;

end
______________________________________________________
function m = extension(ext,b1)

theta = atan(b1(2,3)/b1(1,3));

X = ext * cos(theta);
Y = ext * sin(theta);

if b1(1,3)<0
X = -X;
Y = -Y;
end

t = [1 0 0 X; 0 1 0 Y;0 0 1 0 ;0 0 0 1];

s = size(b1);
S = s(1,2);
m0 = [b1; ones(2,S)];

m = translation(m0,t);

end
______________________________________________________
function m = translation(b1,t);

m1 = t * b1;

m = m1(1:2,:);

end