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