%Set servo #id target position
%
%This allows to set the position of a single servo.
%
%Note: This command re-enables ALL released motors on the bus (see
%command 0xEF).
%
%Command = 0...0x7F (ID)
%
%Workaround: ( Single servo only! ) Before sending a new target position, read the actual joint
%position and write it to memory location 0x07:0x06 (this is where the actual trajectoy setpoint
%is stored).

function setPosId(H,ID,pos)

globalVar;
servoReturn1 = 0;
servoReturn2 = 0;

if((pos<ANGLE_MIN-1) || (pos>ANGLE_MAX+1))
    error('Gama de posições válidas: [-90;90]º');
end

%Leitura da posição actual
posActual=readPos(H);
posActual=round(((posActual+ANGLE_MAX)*(VAL_ANGLE_MAX-VAL_ANGLE_MIN)/(ANGLE_MAX-ANGLE_MIN))+VAL_ANGLE_MIN);
param1=bitand(posActual,MASK_HIGH)/256;
param2=bitand(posActual,MASK_LOW);

%Escrita da posição actual 
%na posição de memória 0x07:0x06
writeMem(H,CUR_TRAJ_HIGH_MEMADDR,param1);
writeMem(H,CUR_TRAJ_LOW_MEMADDR,param2);

%Cálculo e envio da posição desejada
pos=round(((pos+ANGLE_MAX)*(VAL_ANGLE_MAX-VAL_ANGLE_MIN)/(ANGLE_MAX-ANGLE_MIN))+VAL_ANGLE_MIN);

command = ID;
param1 = bitand(pos,MASK_HIGH)/256;     %divisao por 256 = shift(8)
param2 = bitand(pos,MASK_LOW);
checksum = 256 - mod((STARTBYTE+command+param1+param2),256);

sendMessage(H,command,param1,param2,checksum);
[return1,return2]=readMessage(H);

if((servoReturn1 ~= return1) || (servoReturn2 ~= return2))
    err = ERR_SET_POS_ID;
    errfcn(H,err);
end