function invKin(H,posX,posY,posZ)

globalVar;

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

sendServo(H,INV_KIN,param1,param2,param3,param4,param5,param6);            

[command,return1,return2,return3] = readServo(H);

% if (command == -2 && return1 == -2)               % testar isto mais tarde
%     while(command == -2 && return1 == -2)
%         [command,return1,return2,return3] = readServo(H);
%     end
% end

if(~(command == INV_KIN && ~return1 && ~return2 && ~return3))
    disp('OPERATION FAILED: Testing Inverse Kinetic');
% else
%     q = InvKin_3dof(posX,posY,posZ,L0,L1,L2);       % porque é que ele fazia isto?
end                                                 % devia ser para fazer mais tempo a PIC
                                                    % L0 L1 e L2 não parece estar a ser utilizado
end


