function ContMov_v3(H)

globalVar;

XYZ = zeros(3);
disp(' ');
disp(' ** Leg - XYZ **');
XYZ(1,1) = input('Pos Xf [ ](mm): ');
% while (XYZ(1,1) ~= [-234:234])
%     XYZ(1,1) = input('Pos Xf [-234 234](mm): ');
% end
XYZ(1,2) = input('Pos Yf [ ](mm): ');
% while (XYZ(1,2) ~= [140:374])
%     XYZ(1,2) = input('Pos Yf [140 374](mm): ');
% end
XYZ(1,3) = input('Pos Zf [ ] (mm): ');
% while (XYZ(1,3) ~= [-84:384])
%     XYZ(1,3) = input('Pos Zf [-84 384](mm): ');
% end

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) = input('Time [0 5](s): ');
while (XYZ(2,1) ~= [0:0.01:5])
    XYZ(2,1) = input('Time [0 5](s): ');
end
XYZ(2,1) = XYZ(2,1)*100;

ord = input('Tipe of Order (3 or 5): ');
if isempty(ord)
    ord = 3;
end
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(' ');
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(' ');
    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);
    
    for i=1:N;
    	XYZ(:,i) = DirKin3dof(posArray(:,i)');
    end
%     for i=1:N;
%         if (posArray(1,i)==-150 || posArray(2,i)==-150 || posArray(3,i)==-150)
%         	XYZ(:,i) = XYZf-0.1;      % FILTRO
%         else
%             XYZ(:,i) = DirKin3dof(posArray(:,i)');
%         end
%     end
    
    % desenhar XYZi, XYZf e os XYZ
    figure
    hold on
    plot3(XYZi(1),XYZi(2),XYZi(3),'.r')
    plot3(XYZ(1,:),XYZ(2,:),XYZ(3,:),'.b')
    plot3(XYZf(1),XYZf(2),XYZf(3),'.g')
    xlabel('pos X')
    ylabel('pos Y')
    zlabel('pos Z')
    grid on
    axis square
    hold off

    figure
%    subplot(2,1,1)
    hold on
    plot(t',posArray','*')
    xlabel('Tempo (ms)')
    ylabel('Posição (º)')
%   legend('Servo1 - Ankle1','Servo2 - Ankle2','Servo3 - Knee')
    axis([0 max(t) -100 100])
    grid on
%    hold off

%-------------------------------------------------------%
    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;
        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
    
%     figure
%     hold on
%     plot3(PeeArray(1,:),PeeArray(2,:),PeeArray(3,:),'.b')
%     xlabel('pos X')
%     ylabel('pos Y')
%     zlabel('pos Z')
%     grid on
%     axis square
%     hold off

%     subplot(2,1,2)
    hold on
    plot(t',PeeArray','.')
%     xlabel('Tempo (ms)')
%     ylabel('Qvel (%)')
%     legend('Servo1 - Ankle1','Servo2 - Ankle2','Servo3 - Knee')
%     axis([0 max(t) -100 100])
%     grid on
    legend('Real Servo1 - Ankle1','Real Servo2 - Ankle2','Real Servo3 - Knee','Imag Servo1 - Ankle1','Imag Servo2 - Ankle2','Imag Servo3 - Knee')
    hold off
%-------------------------------------------------------%

end

end