close all
H=initCom;

globalVar;

initMsg = [hex2dec('FF') hex2dec('00') hex2dec('00') hex2dec('FF')];
fwrite(H,initMsg,'uint8');

while(1)
    disp(' ');
    disp('Options: ');
    disp(' ');
    disp('0 - Read EEPROM');
    disp('1 - Write EEPROM');
    disp('2 - Read Memory');
    disp('3 - Write Memory');
    disp('4 - Read Position');
    disp('5 - Set Target Position (ALL)');
    disp('6 - Read Version & ID');
    disp('7 - Read PulseWidth & Voltage');
    disp('8 - Set Servo #id Speed & Read Position');
    disp('9 - Select Control Parameter Set');
    disp('10 - Go/Stop');
    disp('11 - Set Servo #id Target Position');  
    disp(' ');
    opt=input('Option: ');
    
    switch opt
        case 0
            addr = input('Address (Dec): ');
            data=readEeprom(H,addr);
            disp(['Data (hex): ', num2str(dec2hex(data))]);
                
        case 1
            addr = input('Address (Dec): ');
            data = input('Data (Dec): ');
            writeEeprom(H,addr,data);
                
        case 2
            addr = input('Address (Dec): ');
            data=readMem(H,addr);
            disp(['Data (hex): ', num2str(dec2hex(data))]);
                
        case 3
            addr = input('Address (Dec): ');
            data = input('Data (Dec): ');
            writeMem(H,addr,data);
                
        case 4
            pos = readPos(H);
            disp(['Position: ', num2str(pos), 'º']);
                
        case 5
            pos = input('Position [-90 ; 90]º: ');
            setPosAll(H,pos);
  
        case 6
            [version,id] = readVersionId(H);
            disp(['Version: ', num2str(version)]);  
            disp(['ID: ', num2str(id)]);                 
                
        case 7
            [pw,v] = readPwV(H);
            disp(['Pulsewidth: ', num2str(pw)]);  
            disp(['Voltage: ', num2str(v)]);  
                
        case 8
            id = input('ID: ');
            speed = input('Speed [0 ; 100]%: ');
            pos = setSpeedReadPos(H,id,speed);
            disp(['Position: ', num2str(pos), 'º']);
                
        case 9
            set = input('Control Parameter Set (1/2/3): ');
            ctrlParamSet(H,set);
                
        case 10
            set = input('Go/Stop (1/0): ');
            goStop(H,set);
                
        case 11
            id = input('ID: ');
            pos = input('Position [-90 ; 90]º: ');
            setPosId(H,id,pos);
            
        otherwise
            break;
            
        end
end
killCom(H);