You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
90 lines
2.1 KiB
90 lines
2.1 KiB
function [ ] = control(handles )
|
|
|
|
addpath('../kinematics/')
|
|
|
|
global do;
|
|
do = true;
|
|
|
|
% DH Parameter
|
|
a = [0,0,0,0,0,0,0];
|
|
alp = [-90,90,-90,90,-90,90,0];
|
|
d = [310.4,0,400.1,0,390,0,78];
|
|
|
|
L1 = Link('d',d(1),'a',a(1),'alpha',alp(1)/180*pi);
|
|
L1.isrevolute;
|
|
L2 = Link('d',d(2),'a',a(2),'alpha',alp(2)/180*pi);
|
|
L2.isrevolute;
|
|
L3 = Link('d',d(3),'a',a(3),'alpha',alp(3)/180*pi);
|
|
L3.isrevolute;
|
|
L4 = Link('d',d(4),'a',a(4),'alpha',alp(4)/180*pi);
|
|
L4.isrevolute;
|
|
L5 = Link('d',d(5),'a',a(5),'alpha',alp(5)/180*pi);
|
|
L5.isrevolute;
|
|
L6 = Link('d',d(6),'a',a(6),'alpha',alp(6)/180*pi);
|
|
L6.isrevolute;
|
|
L7 = Link('d',d(7),'a',a(7),'alpha',alp(7)/180*pi);
|
|
L7.isrevolute;
|
|
bot = SerialLink([L1 L2 L3 L4 L5 L6 L7], 'name', 'Kuka LBR4 +');
|
|
|
|
figure(1);
|
|
axis equal
|
|
|
|
tx_old = 0; ty_old = 0; tz_old = 0;
|
|
x_old = 0; y_old = 0; z_old = 0;
|
|
while (do)
|
|
%Calculating 'M'
|
|
tx = get(handles.slider1,'Value');
|
|
ty = get(handles.slider2,'Value');
|
|
tz = get(handles.slider3,'Value');
|
|
x = get(handles.slider4,'Value')/180*pi;
|
|
y = get(handles.slider5,'Value')/180*pi;
|
|
z = get(handles.slider6,'Value')/180*pi;
|
|
|
|
% only update if gui values have changed
|
|
values_unchanged = tx == tx_old && ty == ty_old && tz == tz_old && ...
|
|
x == x_old && y == y_old && z == z_old;
|
|
if (values_unchanged)
|
|
pause(0.01);
|
|
continue;
|
|
end
|
|
tx_old = tx;
|
|
ty_old = ty;
|
|
tz_old = tz;
|
|
x_old = x;
|
|
y_old = y;
|
|
z_old = z;
|
|
|
|
Rx = [ 1 0 0 ; 0 cos(x) -sin(x) ; 0 sin(x) cos(x) ];
|
|
Ry = [ cos(y) 0 sin(y) ; 0 1 0 ; -sin(y) 0 cos(y)];
|
|
Rz = [ cos(z) -sin(z) 0 ; sin(z) cos(z) 0 ; 0 0 1 ];
|
|
|
|
R = Rz*Ry*Rx;
|
|
|
|
M = [R, [tx;ty;tz]; 0,0,0,1];
|
|
|
|
args = [get(handles.arm,'Value'),get(handles.elbow,'Value'),get(handles.flip,'Value'),get(handles.slider9,'Value')*(pi/180)];
|
|
for i=1:3
|
|
if args(i) == 0
|
|
args(i) = -1;
|
|
end
|
|
end
|
|
|
|
set(handles.text8, 'String', [sprintf('%.4G',args(4)*180/pi),' °']);
|
|
|
|
try
|
|
J = LBRinvKin(M,args,'LBR4+');
|
|
J'
|
|
catch err
|
|
err.message
|
|
break;
|
|
end
|
|
|
|
try
|
|
bot.plot(J.*(pi/180));
|
|
catch err
|
|
err.message
|
|
break;
|
|
end
|
|
|
|
end
|
|
|