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);            

disp('  RESULTS  ');
[command,return1,return2,return3] = readServo(H);
if(command ~= INV_KIN)
    disp('OPERATION FAILED: Testing Inverse Kinetic');
end
switch (return1)
    case 0
        disp('Fora dos Limites');
    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');
end

pos = (return2*256) + return3;
qf1 = (((pos-VAL_ANGLE_MIN)*ANGLE_RANGE)/VAL_ANGLE_RANGE)+ANGLE_MIN;
[command,return1,return2,return3] = readServo(H);
pos = (return1*256) + return2;
qf2 = (((pos-VAL_ANGLE_MIN)*ANGLE_RANGE)/VAL_ANGLE_RANGE)+ANGLE_MIN;
temp_ret = return3;
[command,return1,return2,return3] = readServo(H);
pos = (temp_ret*256) + return1;
qf3 = (((pos-VAL_ANGLE_MIN)*ANGLE_RANGE)/VAL_ANGLE_RANGE)+ANGLE_MIN;
pos = (return2*256) + return3;
qf4 = (((pos-VAL_ANGLE_MIN)*ANGLE_RANGE)/VAL_ANGLE_RANGE)+ANGLE_MIN;

disp(['Ang Joint 1: ', num2str(qf1), 'º']);
disp(['Ang Joint 2: ', num2str(qf2), 'º']);
disp(['Ang Joint 3: ', num2str(qf3), 'º']);
disp(['Ang Joint 4: ', num2str(qf4), 'º']);

end


