%Set servo #id speed & read position
%
%This command sets the speed of a single servo on the bus and reads
%its current position. 
%
%Valid speed range is 1..255.

function pos=setSpeedReadPos(H,ID,speed)

globalVar;

%if((speed<SPEED_MIN) || (speed>SPEED_MAX))
    %error('Gama de velocidades válidas: [0;100]%');
%end
if(speed<SPEED_MIN)
    speed=1;
else if(speed>SPEED_MAX)
        speed=100;
    end
end

command = SET_SPEED_READ_POS;
param1 = ID;
param2 = speed;
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;
