%Read position
%
%Read the current position.
%
%Use command 0xE9 to address multiple motors on one bus.

function pos=readPos(H)

globalVar;

command = READ_POS;
param1 = 0;
param2 = 0;
checksum = 256 - mod((STARTBYTE+command+param1+param2),256);

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

pos=return1*256+return2;    %256=2^8
pos=((pos-VAL_ANGLE_MIN)*(ANGLE_MAX-ANGLE_MIN)/(VAL_ANGLE_MAX-VAL_ANGLE_MIN))+ANGLE_MIN;

if((pos>(ANGLE_MAX+ANGLE_TOL)) || (pos<(ANGLE_MIN-ANGLE_TOL)))
    err=ERR_READ_POS;
    errfcn(H,err);
    pos=255;
end