function [N,posArray,t,XYZi,XYZf,PeeArray] = Seq_ContMov_2(H,seq)

globalVar;

XYZ = zeros(3);
disp(' ** Leg - XYZ **');
for i = 1:3
    XYZ(1,i) = seq(i);
end
disp(['XYZ_f: ', num2str(XYZ(1,:))]);
XYZ(1,1) = (XYZ(1,1)+200)*10;
XYZ(1,2) = (XYZ(1,2)+200)*10;
XYZ(1,3) = XYZ(1,3)*10;

XYZ(2,1) = seq(4);
disp(['Tx: ', num2str(XYZ(2,1))]);
XYZ(2,1) = XYZ(2,1)*100;

ord = seq(5);
XYZ(2,2) = ord;

Tramas = 2;
for i = 1:Tramas
    param1 = bitand(XYZ(i,1),MASK_HIGH) / 256;
    param2 = bitand(XYZ(i,1),MASK_LOW);
    param3 = bitand(XYZ(i,2),MASK_HIGH) / 256;
    param4 = bitand(XYZ(i,2),MASK_LOW);
    param5 = bitand(XYZ(i,3),MASK_HIGH) / 256;
    param6 = bitand(XYZ(i,3),MASK_LOW);
    sendServo(H,Tramas,22,param1,param2,param3,param4,param5,param6);
end

[command,return1,return2,return3] = readServo(H);
q(1) = return1*256 + return2;
temp_ret = return3;
[command,return1,return2,return3] = readServo(H);
q(2) = temp_ret*256 + return1;
q(3) = return2*256 + return3;
q = (((q-VAL_ANGLE_MIN)*ANGLE_RANGE)/VAL_ANGLE_RANGE)+ANGLE_MIN;
P = DirKin3dof(q);
XYZi = P';
XYZf(1,1) = XYZ(1,1)/10-200;
XYZf(1,2) = XYZ(1,2)/10-200;
XYZf(1,3) = XYZ(1,3)/10;

disp(' ** RESULTS **');
[command,return1,return2,return3] = readServo(H);
if(command ~= 22)
    disp('OPERATION FAILED: Testing Inverse Kinetic');
end
isVal = return1;
switch (isVal)
    case 0
        disp('OUT of Limits');
    case 1
        disp('Inverse Kinetic is Working');
    case 2
        disp('ERRO: OUT1 of WorkVolume');
    case 3
        disp('ERRO: OUT2 of WorkVolume');
    case 4
        disp('ERRO: OUT3 of WorkVolume');
    case 5
        disp('ERRO: OUT4 of WorkVolume');
    case 6
        disp('Not Used');
end

if (isVal == 1)
    return1 = 0;        % apenas para limpar o registo
    i=0;
    while (return1 ~= 1 || return2 ~= 2 || return3 ~= 3)
        i = i+1;
        [command,return1,return2,return3] = readServo(H);
        posArray(1,i) = return1*256 + return2;
        temp_ret = return3;
        [command,return1,return2,return3] = readServo(H);
        posArray(2,i) = temp_ret*256 + return1;
        posArray(3,i) = return2*256 + return3;

        [command,return1,return2,return3] = readServo(H);
        t(1,i) = return1*256+return2;
    end

    [command,return1,return2,return3] = readServo(H);
    j = return1;
    N = i-1;
    disp(['Movement with ', num2str(N), ' values.']);
    disp(['Movement with ', num2str(j), ' invalid InvKin.']);
    time = t(1,N)/1000;
    disp(['Time from PIC: ', num2str(time), ' s']);

    for j=1:3;
        for i=1:N
            posArray(j,i) = (((posArray(j,i)-VAL_ANGLE_MIN)*ANGLE_RANGE)/VAL_ANGLE_RANGE)+ANGLE_MIN;
            
            % TENTAR diminuir o erro
            if (posArray(j,i)==-150)
                if (i-1 ~= 0)
                    posArray(j,i) = posArray(j,i-1);
                end
            end
        end
    end
    posArray = posArray(:,1:N);
    t = t(1,1:N);
    
	%-------------------------------------------------------%
    sendServo(H,1,17,0,0,0,0,0,0);
    [command,return1,return2,return3]=readServo(H);
    N = return1;
    for i=1:N
        [command,return1,return2,return3] = readServo(H);
        xt = (return1*256) + return2;
        temp_ret = return3;
        [command,return1,return2,return3] = readServo(H);
        yt = (temp_ret*256) + return1;
        zt = (return2*256) + return3;
%         PeeArray(1,i) = xt/10-200;
%         PeeArray(2,i) = yt/10-200;
%         PeeArray(3,i) = zt/10-200;
        PeeArray(1,i) = (((xt-VAL_ANGLE_MIN)*ANGLE_RANGE)/VAL_ANGLE_RANGE)+ANGLE_MIN;
        PeeArray(2,i) = (((yt-VAL_ANGLE_MIN)*ANGLE_RANGE)/VAL_ANGLE_RANGE)+ANGLE_MIN;
        PeeArray(3,i) = (((zt-VAL_ANGLE_MIN)*ANGLE_RANGE)/VAL_ANGLE_RANGE)+ANGLE_MIN;
    end
    %-------------------------------------------------------%
end

end