%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))
   % disp('Gama de velocidades válidas: [1;100]%');
    if(speed > SPEED_MAX)
        speed=SPEED_MAX;
    else if(speed < SPEED_MIN)
            speed=SPEED_MIN;
        end
    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;
pos=((pos-VAL_ANGLE_MIN)*(ANGLE_MAX-ANGLE_MIN)/(VAL_ANGLE_MAX-VAL_ANGLE_MIN))+ANGLE_MIN;
