function setPosId(H,id,pos)

globalVar;

pos = round((((pos+ANGLE_MAX)*VAL_ANGLE_RANGE)/ANGLE_RANGE)+VAL_ANGLE_MIN);
param1 = bitand(pos,MASK_HIGH)/256;  
param2 = bitand(pos,MASK_LOW);

sendServo(H,1,SET_POS_ID,id,param1,param2,0,0,0);     % alteração IMPORTANTE na orden de entrada de parametros
[command,return1] = readServo(H);

if(~(command == SET_POS_ID && return1 == 0))
    disp(['OPERATION FAILED: Set Position on servo ', num2str(id)]);
end

