function [posArray,t] = invKinPtP(H,posX,posY,posZ)

globalVar;

posX = posX+400;
param1 = bitand(posX,MASK_HIGH) / 256;
param2 = bitand(posX,MASK_LOW);
posY = posY+400;
param3 = bitand(posY,MASK_HIGH) / 256;
param4 = bitand(posY,MASK_LOW);
posZ = 331-posZ;
param5 = bitand(posZ,MASK_HIGH) / 256;
param6 = bitand(posZ,MASK_LOW);

sendServo(H,INV_KIN_PtP,param1,param2,param3,param4,param5,param6);
[command,return1,return2,return3] = readServo(H);
if(~(command == INV_KIN_PtP && ~return1 && ~return2 && ~return3))
    disp('OPERATION FAILED: Testing Inverse Kinetic PtP');
end

[posArray,t] = readAllPtP(H,return1,return2,return3);      % or readAllPtP2

end