function PtPMov(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,:) = (XYZ(1,:)+200)*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;

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,14,param1,param2,param3,param4,param5,param6);
end

disp(' ');
disp(' ** RESULTS **');
[command,return1,return2,return3] = readServo(H);
if(command ~= 14)
    disp('OPERATION FAILED: Testing Inverse Kinetic');
end
switch (return1)
    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 (return1 == 1)
    pause(XYZ(2,1)/100-0.2);
    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;
        [command,return1,return2,return3] = readServo(H);
        t(1,i) = return1*256+return2;
    end
    
    disp(' ');
    N = i-1;
    disp(['Movement with ', num2str(N), ' values.']);
    time = t(1,N)/1000;
    disp(['Time from PIC: ', num2str(time), ' s']);

    for i=1:N
    	posArray(1,i) = (((posArray(1,i)-VAL_ANGLE_MIN)*ANGLE_RANGE)/VAL_ANGLE_RANGE)+ANGLE_MIN;
    end
    posArray = posArray(1,1:N);
    t = t(1,1:N);

    figure
    hold on
    plot(t',posArray','*')
    xlabel('Tempo (ms)')
    ylabel('Posição (º)')
    legend('Servo 4')
    axis([t(1,1) max(t) -100 100])
    grid on
    hold off 
end
    
end