function phua(flag)

    close all
    H=initCom;

    globalVar;
    N=200;
    Nsensors=4;
    Nsamples=1000;
    hPlot=zeros(Nsensors+1);
    nSampPlot=200;
    y=zeros(Nsensors,nSampPlot);

    middleX=4.5;
    totalX=9;
    middleY=2.3;
    totalY=4.6;

    val=zeros(Nsensors,1);
    calibSensor=zeros(Nsensors,1);
    weight=zeros(1,5);
    
    nMoves=0;
    posArray=zeros(3,N);
    posArraySaved=zeros(3,Nsamples);
    t=zeros(1,N);
    tSaved=zeros(nMoves,Nsamples);
    timeAdjust=zeros(1,Nsamples);
    valSensor=zeros(N,4);
     calibSensor=zeros(4,1);
    
    if(flag)
        initMsg = [hex2dec('FF') hex2dec('00') hex2dec('00') hex2dec('FF') hex2dec('00') hex2dec('00') hex2dec('FF')];
        fwrite(H,initMsg,'uint8');
    end

    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 - Release');
        disp('12 - Set Servo #id Target Position');  
        disp('13 - Test Inverse Kinetic');
        disp('14 - Set Maximum Speed');
        disp('15 - Test Direct Kinetic');
        disp('16 - Get Results');
        disp('17 - Save Results');
        disp('20 - Read Sensor All');
        disp('21 - Calibrate Sensors');
        disp('22 - CoP Live');
        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
                release(H);
                
            case 12
                id = input('ID: ');
                pos = input('Position [-90 ; 90]º: ');
                setPosId(H,id,pos);

            case 13
                posX = input('X position: ');
                sign = input('Positive/Negative (0/1): ');
                if(sign)
                    posX=-posX;
                end
                posX=400+posX;
                
                posY = input('Y position: ');
                sign = input('Positive/Negative (0/1): ');
                if(sign)
                    posY=-posY;
                end
                posY=400+posY;
                
                posZ = input('Z position: ');
                invKin(H,posX,posY,posZ);
                
            case 14
                maxSpeed = input('Maximum Speed: ');
                sendServo(H,23,maxSpeed,0,0,0,0,0);
                
            case 15
                posX = input('X position: ');
                posY = input('Y position: ');
                posZ = input('Z position: ');
                dirKin(H,posX,posY,posZ);
                
            case 16
                sendServo(H,25,0,0,0,0,0,0);
                for i=1:N
                    for j=1:3
                        [command,return1,return2,return3]=readServo(H);
                        posArray(j,i)=return1*256+return2;
                    end
                end
                for i=1:N
                    [command,return1,return2,return3]=readServo(H);
                    t(1,i)=return1*256+return2;
                end
                for j=1:N
                    for i=1:4
                        [command,return1,return2,return3]=readServo(H);
                        valSensor(j,i)=return1*256+return2;
                    end
                end

                for i=1:N
                    for j=1:3
                        posArray(j,i) = (((posArray(j,i)-VAL_ANGLE_MIN)*ANGLE_RANGE)/VAL_ANGLE_RANGE)+ANGLE_MIN;
                        if(j==3)
                            posArray(j,i)=(-posArray(j,i)-90)/(30/22);
                        else
                            posArray(j,i)=posArray(j,i)/4;
                        end
                    end
                end

                for j=1:N
                   COPxp(j)=(valSensor(j,1)*middleX-valSensor(j,3)*middleX-valSensor(j,2)*middleX+valSensor(j,4)*middleX)/totalX;
                   COPyp(j)=(-valSensor(j,1)*middleY+valSensor(j,4)*middleY-valSensor(j,2)*middleY+valSensor(j,3)*middleY)/totalY;
                end
                
%                 figure
%                 hold on
%                 for j=1:N
%                     plot(COPyp(j),COPxp(j),'*','Color',[j/N 1/(1+0.1*j) 0])
%                 end
%                 hold off
%                 
%                 figure
%                 plot(t',posArray','*')
%                 hold on
%                 xlabel('Tempo (ms)')
%                 ylabel('Posição (º)')
%                 legend('Servo 0','Servo 1','Servo 2')
%                 axis([0 max(t) -160 100])
%                 grid on
%                 hold off
                
            case 17     
                nMoves=nMoves+1;
                if(nMoves==6)
                    nMoves=1;
                end
                
                for i=(nMoves*N-N+1):nMoves*N
                    test.posArray(:,i)=posArray(:,i-((nMoves-1)*N));                  
                    test.time(1,i)=t(1,i-((nMoves-1)*N));
                    test.valSensor(i,:)=valSensor(i-((nMoves-1)*N),:);
                end

               save .\finalTests\SomeTrajectories12 test

            case 18
                S=load('.\finalTests\SomeTrajectories12.mat');
                timeAdjust=S.test.time;
                j=1;
                for i=201:length(S.test.time)
                    timeAdjust(1,i)=timeAdjust(1,i)+timeAdjust(1,j*N);
                    if(mod(i,200)==0)
                        j=j+1;
                    end
                end
                for i=1:length(timeAdjust)
                    timeAdjust(1,i)=timeAdjust(1,i)/1000;
                end
                
                figure
                plot(timeAdjust,S.test.posArray,'*');
                hold on
                if(length(timeAdjust) >= 200)
                    h200 = vline(timeAdjust(1,200),'m','Fim do 1º Movimento');
                end
                if(length(timeAdjust) >= 400)
                    h400 = vline(timeAdjust(1,400),'m','Fim do 2º Movimento');
                end
                if(length(timeAdjust) >= 600)
                    h600 = vline(timeAdjust(1,600),'m','Fim do 3º Movimento');
                end
                if(length(timeAdjust) >= 800)
                    h800 = vline(timeAdjust(1,800),'m','Fim do 4º Movimento');
                end
                grid on
                xlabel('Tempo (s)')
                ylabel('Posição (º)')
                legend('Servo 0','Servo 1','Servo 2')
                hold off
                
                figure
                calibSensor(1)=746;
                calibSensor(2)=728;
                calibSensor(3)=748;
                calibSensor(4)=776;
                
                for i=1:1:4
                    for j=1:1:length(timeAdjust)
                        valSensor(j,i)=S.test.valSensor(j,i)-calibSensor(i);
                    end
                end
                COPxp=zeros(length(timeAdjust));
                COPyp=zeros(length(timeAdjust));
                for j=1:length(timeAdjust)
                    hold on
                    plot(timeAdjust(j),valSensor(j,:),'*','MarkerSize',6)
                    hold off
                   sumValS=sum([valSensor(j,1) valSensor(j,2) valSensor(j,3) valSensor(j,4)]);
                   if(sumValS==0)
                       COPxp(j)=0;
                       COPyp(j)=0;
                   else
                       COPxp(j)=(valSensor(j,1)*middleX-valSensor(j,3)*middleX-valSensor(j,2)*middleX+valSensor(j,4)*middleX)/sumValS;
                       COPyp(j)=(-valSensor(j,1)*middleY+valSensor(j,4)*middleY-valSensor(j,2)*middleY+valSensor(j,3)*middleY)/sumValS;
                   end
                end

                hold on
                legend('1','2','3','4');
                hold off
                
                l=-1;
                for j=1:length(timeAdjust)
                    if(mod(j,200)==1)
                        l=l+1;
                        hold off
                        figure
                        xxl=-middleX;
                        xxr=middleX;
                        yyb=-middleY;
                        yyt=middleY;
                        axis([-5 5 -3 3])
                        plot(xxl,yyt,'ro',xxr,yyt,'go',xxl,yyb,'co',xxr,yyb,'mo');
                        hold on
                    end
                    plot(COPyp(j),COPxp(j),'*','Color',[(j-(l*N))/N 1/(1+0.1*(j-(l*N))) 0])
                end
                hold off
                
            case 20
                sendServo(H,30,0,0,0,0,0,0);
                for i=1:4
                    [command,return1,return2,return3]=readServo(H);
                    valSensor(i,1)=return1*256+return2;
                    disp(['Sensor ' num2str(i) ': ' num2str(valSensor(i,1))]);
                end
                
            case 21
                sendServo(H,31,0,0,0,0,0,0);
                for i=1:4
                    [command,return1,return2,return3]=readServo(H);
                    valSensor(i,1)=return1*256+return2;
                    disp(['Calibration Sensor ' num2str(i) ': ' num2str(valSensor(i,1))]);
                end
                
            case 22
                x = 1:nSampPlot;
                
                COPxp=0;
                COPyp=0;
            
                subplot(211)
                hold on
                axis([-5 5 -3 3])
                xxl=-middleX;
                xxr=middleX;
                yyb=-middleY;
                yyt=middleY;
                plot(xxl,yyt,'r*',xxr,yyt,'g*',xxl,yyb,'c*',xxr,yyb,'m*');
                hPlot(5)=plot(COPxp,COPyp,'*','XDataSource','COPxp','YDataSource','COPyp');
                title('Center of Pressure')
                legend('Sensor 1 (top left)','Sensor 2 (top right)','Sensor 3 (bottom left)','Sensor 4 (bottom right)','Center of Pressure');
                xlabel('Distance (cm)')
                ylabel('Distance (cm)')
                grid on
                hold off

                subplot(212)
                hold on
                hPlot(1) = plot(x,y(1,:),'b');
                hPlot(2) = plot(x,y(2,:),'r');
                hPlot(3) = plot(x,y(3,:),'g');
                hPlot(4) = plot(x,y(4,:),'c');
                title('Sensor Values')
                ylabel('Value (0...1023)')
                legend('Sensor 1 (top left)','Sensor 2 (top right)','Sensor 3 (bottom left)','Sensor 4 (bottom right)');
                stripchart('Initialize',gca)
                hold off

                while(ishandle(hPlot(1)))
                    sendServo(H,30,0,0,0,0,0,0);
                    for i=1:1:4
                        [command,return1,return2,return3]=readServo(H);
                        y(i,N)=return1*256+return2-calibSensor(i);
                       StripChart('Update',hPlot(i),y(i,N));
                    end
                    
                    xx=[y(1,N) y(2,N) y(3,N) y(4,N)];
                   COPxp=(y(1,N)-y(2,N)-y(3,N)+y(4,N))*middleX/sum(xx);
                   COPyp=(-y(1,N)-y(2,N)+y(3,N)+y(4,N))*middleY/sum(xx);
                   refreshdata(hPlot(5),'caller');
                   drawnow
                end
                
            case 23
                sendServo(H,30,0,0,0,0,0,0);
                for i=1:1:4
                    [command,return1,return2,return3]=readServo(H);
                    calibSensor(i,1)=return1*256+return2;
                end
                
            otherwise
                break;

        end
    end
    
killCom(H);
end

%             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