function servo

clear all
clc

globalVar;

id0=8;
id1=1;
id2=2;

ssrp0=zeros(1,1000);
ssrp1=zeros(1,1000);
ssrp2=zeros(1,1000);
rp=zeros(1,1000);
rm=zeros(1,1000);
demand=zeros(1,1000);
H=initCom;
%pos=setSpeedReadPos(H,8,100);
while(1)
    opt=input('\nOption: ');
    
    switch opt
        case 0
            pos=input('Position: ');
           % for i=1:1:1000
                setPosAll(H,pos);
            %end
            
        case 1
           % id=input('ID: ');
            for i=1:1:10
                pos=setSpeedReadPos(H,id0,100);
                disp(['Position 0: ' num2str(pos)]);
                pos=setSpeedReadPos(H,id1,100);
                disp(['Position 1: ' num2str(pos)]);
                pos=setSpeedReadPos(H,id2,100);
                disp(['Position 2: ' num2str(pos)]);
%                 pos=readPos(H);
%                 disp(['Position (readPos): ' num2str(pos)]);
%                 data0=readMem(H,hex2dec('07'));
%                 data1=readMem(H,hex2dec('06'));
%                 pos=data0*256+data1;                
%                 disp(['Position (read EEPROM): ' num2str(pos)]); 
%                 data0=readMem(H,hex2dec('A7'));
%                 data1=readMem(H,hex2dec('A8'));
%                 pos=data0*256+data1;                
%                 disp(['Position (read EEPROM): ' num2str(pos)]);
            end   
            
        case 2
            id=input('ID: ');
            writeEeprom(H,hex2dec('29'),id);
            
        case 3
            id = readEeprom(H,hex2dec('29'));
            disp(['ID: ' num2str(id)]);  
        
        case 4
            clear ssrp0 ssrp1 ssrp2 rp rm t0 t1 t2 i j k l
            pos=-80;
            speed=25;
            Texe=5;
            i=1;
            setSpeedReadPos(H,id0,speed);
            setSpeedReadPos(H,id1,speed);
            setSpeedReadPos(H,id2,speed);
            setPosId(H,id0,pos);
            setPosId(H,id1,pos);
            setPosId(H,id2,pos);
            tic;
            while(toc<Texe)
                ssrp0(i)=setSpeedReadPos(H,id0,speed);
                ssrp1(i)=setSpeedReadPos(H,id1,speed);
                ssrp2(i)=setSpeedReadPos(H,id2,speed);
                t0(i)=toc;
                demand(i)=pos;
                i=i+1;
            end
            pos=0;
            setPosId(H,id0,pos);
            setPosId(H,id1,pos);
            setPosId(H,id2,pos);
            while(toc<2*Texe)
                ssrp0(i)=setSpeedReadPos(H,id0,speed);
                ssrp1(i)=setSpeedReadPos(H,id1,speed);
                ssrp2(i)=setSpeedReadPos(H,id2,speed);
                t0(i)=toc;
                demand(i)=pos;
                i=i+1;
            end
            
            test.ssrp0=ssrp0;
            test.ssrp1=ssrp1;
            test.ssrp2=ssrp2;
            test.time=t0;
            test.demand=demand;
            save .\provas\Servos3Target80Speed25 test

            figure
            subplot(311)
            plot(t0,ssrp0(1,1:length(t0)),'.',t0,demand(1,1:length(t0)),'r-')
            hold on
            xlabel('Time (s)');
            ylabel('Position (º)');
            axis([0 2*Texe min(ssrp0)-10 max(ssrp0)+10]);
            legend('Servo Response','Target Position')
            hold off
            subplot(312)
            plot(t0,ssrp1(1,1:length(t0)),'.',t0,demand(1,1:length(t0)),'r-')
            hold on
            xlabel('Time (s)');
            ylabel('Position (º)');
            axis([0 2*Texe min(ssrp1)-10 max(ssrp1)+10]);
            legend('Servo Response','Target Position')
            hold off
            subplot(313)
            plot(t0,ssrp2(1,1:length(t0)),'.',t0,demand(1,1:length(t0)),'r-')
           % title('One Servo Response To Command "Set Speed & Read Position" When Alone On a Bus / Speed = 10%')
            hold on
            xlabel('Time (s)');
            ylabel('Position (º)');
            axis([0 2*Texe min(ssrp2)-10 max(ssrp2)+10]);
            legend('Servo Response','Target Position')
            hold off
%             pause;
%             j=1;
%             setPosId(H,id,pos);
%             tic;
%             while(toc<5)
%                 rp(j)=readPos(H);
%                 t1(j)=toc;
%                 j=j+1;
%             end
%             setPosId(H,id,0);
%              pause;
%             k=1;
%             setPosId(H,id,pos);
%             tic;
%             while(toc<5)
%                 data0=readMem(H,hex2dec('A7'));
%                 data1=readMem(H,hex2dec('A8'));
%                 rm(k)=data0*256+data1; 
%                 t2(k)=toc;
%                 k=k+1;
%             end
%              setPosId(H,id,0);           
%             while(toc<10)
%                 data0=readMem(H,hex2dec('A7'));
%                 data1=readMem(H,hex2dec('A8'));
%                 rm(k)=data0*256+data1; 
%                 t2(k)=toc;
%                 k=k+1;
%             end
%             for l=1:1:k-1
%                 rm(l)=(rm(l)-600)*180/1800-90;
%             end
%            plot(t0,ssrp(1,1:length(t0)),'.',t2,rm(1,1:length(t2)),'r.')
%             plot(t0,ssrp(1,1:length(t0)),t1,rp(1,1:length(t1)),t2,rm(1,1:length(t2)))
%             legend('SSRP','RP','RM')
        case 5 
            flag=0;
            for i=1:1:10000
                if(flag)
                    %goStop(H,flag);
                    errn=writeEeprom(H,41,8)
                    flag=0;
                else
                    %goStop(H,flag);
                    errn=writeEeprom(H,41,9)
                    flag=1;
                end
            end
        otherwise
            break;
           
    end
end

killCom(H);

end