%function phua(flag)

    clear all, close all
    delete(instrfind);
    H=initCom;

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

    middleX=7;
    totalX=14;
    middleY=4;
    totalY=8;

    val=zeros(Nsensors,1);
    calibSensor=zeros(Nsensors,1);
    weight=zeros(1,5);
    
    nMoves=0;
    Nmoves=0;
    tlast=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
        
        displayOptions;         %%%% DISPLAY OPTIONS %%%%
        
    while (1)
        disp(' ');
        disp('                                     Display Options Again - 55');
        opt=input('Option: ');
    
        switch opt
            case 55
                displayOptions;
                
            case -1
                initialization(H);
            
            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);
                
%%%%%%    Kinematic   %%%%%%
            case 13         % Inverse Kinematic
                posX = input('X position: ');
                sign = input('Positive/Negative (0/1): ');
                if(sign)
                    posX=-posX;
                end
                posX=400+posX;      % porque somar 400 ao posX ????
                
                posY = input('Y position: ');
                sign = input('Positive/Negative (0/1): ');
                if(sign)
                    posY=-posY;
                end
                posY=400+posY;      % porque somar 400 ao posY ????
                
                posZ = input('Z position: ');
                invKin(H,posX,posY,posZ);
                
                sendServo(H,24,0,0,0,0,0,0);
                [command,return1,return2] = readServo(H);
                N = (return1*256) + return2;
                disp(' ');
                disp(['Movement with ', num2str(N), ' values.']);
                
            case 14         % Regula a velocidade maxima do motor que faz o maior angulo
                            % os outros vão trabalhar a uma velocidade menor mas relativa ao primeiro motor
                maxSpeed = input('Maximum Speed: ');
                sendServo(H,23,maxSpeed,0,0,0,0,0);
                
            case 15         % Direct Kinematic
                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: ');
                dirKin(H,posX,posY,posZ);

%%%%%%   Recolha de Dados   %%%%%%
            case 16
                % Leitura da posição (posArray) e tempo (t)
                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

                % Tratamento dos dados recolhidos (posArray)
                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);
                        end
                        if(j==1)
                            posArray(j,i)=-posArray(j,i)/4;
                        end
                        if(j==2)
                            posArray(j,i)=posArray(j,i)/4;
                        end
                    end
                end
       
                % Grafico da posição para cada servo (GRAF3)
                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

%                 % Leitura dos sensores (valSensor)
%                 for j=1:N
%                     for i=1:4
%                         [command,return1,return2,return3]=readServo(H);
%                         valSensor(j,i)=return1*256+return2;
%                     end
%                 end
%
%                 % Tratamento dos dados recolhidos (valSensor)
%                 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
%                 
%                 % Grafico do centro de pressão x,y (GRAF2)
%                 figure
%                 hold on
%                 for j=1:N
%                     plot(COPyp(j),COPxp(j),'*','Color',[j/N 1/(1+0.1*j) 0])
%                 end
%                 hold off
                
            case 17
                % Incrementa o movimento
                nMoves=nMoves+1;
                if(nMoves==6)       % apenas guarda 5 movimentos; depois volta ao 1
                    nMoves=1;
                end
                % Guarda os novos valores nos vectores posArray, time e valSensor
                for i=(nMoves*N-N+1):nMoves*N       % se nMove=2 então i[201,400]; se nMove=3 então i[401,600]
                    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),:);      % isto não faz nada aqui
                end
                % Guarda o test (os 3 vectores) no ficheiro x
                save .\finalTests2\agacharSpeed25 test

            case 18
                % Faz o load dos dados e ajusta o tempo
                S=load('.\finalTests2\agacharSpeed25');
                %S=load('.\graf_will\mov');
                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
                
                % Desenha o grafico (GRAF4)
                figure
                plot(timeAdjust,S.test.posArray,'*');
                hold on
                % vline - draws a vertical line on the location x
                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
                if(length(timeAdjust) >= 1000)
                    h1000 = vline(timeAdjust(1,1000),'m','Fim do 5º 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 19
                sendServo(H,24,0,0,0,0,0,0);
                [command,return1,return2] = readServo(H);
                N = (return1*256) + return2;
                disp(['Movement with ', num2str(N), ' values.']);

%%%%%%   Sensores    %%%%%%
            case 20        % read all sensors - command 30
                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         % calibr all sensors - command 31
                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
                % Leitura de todos os sensores e calculo do Centro de Massa
                x = 1:nSampPlot;
                COPxp=0;
                COPyp=0;

                subplot(211)
                hold on    

                xxl=-middleX;
                xxr=middleX;
                yyb=-middleY;
                yyt=middleY;
                axis([xxl-0.5 xxr+0.5 yyb-0.5 yyt+0.5]);
                plot(xxl,yyt,'r.',xxr,yyt,'g.',xxl,yyb,'c.',xxr,yyb,'m.','MarkerSize',30);
                hPlot(5)=plot(COPxp,COPyp,'.','XDataSource','COPxp','YDataSource','COPyp','MarkerSize',30);
                title('Centro de Pressão','FontWeight','bold','FontSize',12);
                text(xxl+0.1,yyt,'Sensor 1','FontWeight','bold','FontSize',12);
                text(xxl+0.1,yyb,'Sensor 2','FontWeight','bold','FontSize',12);
                text(xxr-1.1,yyb,'Sensor 3','FontWeight','bold','FontSize',12);
                text(xxr-1.1,yyt,'Sensor 4','FontWeight','bold','FontSize',12);
                xlabel('Distância (cm)','FontWeight','bold','FontSize',12);
                ylabel('Distância (cm)','FontWeight','bold','FontSize',12);
                h = vline(0,'k');
                xplot=xxl-0.5:1:xxr+0.5;
                yplot=0*xplot;
                plot(xplot,yplot,'k-')
                hold off

                subplot(212)
                hold on
                hPlot(1) = plot(x,y(1,:),'r','MarkerSize',20);
                hPlot(2) = plot(x,y(2,:),'c');
                hPlot(3) = plot(x,y(3,:),'m');
                hPlot(4) = plot(x,y(4,:),'g');
                title('Valor dos Sensores','FontWeight','bold','FontSize',12)
                ylabel('Valor (0...1023)','FontWeight','bold','FontSize',12)
                legend('Sensor 1','Sensor 2','Sensor 3','Sensor 4','FontWeight','bold','FontSize',11);
                
                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);    % read all sensors
                for i=1:1:4
                    [command,return1,return2,return3]=readServo(H);
                    calibSensor(i,1)=return1*256+return2;
                end
                
            case 25
                nServosMove(H);

%%%%%%   Movimentos    %%%%%%
            case 26     % Movimentos para utilzar no GARF3
                invKin(H,400,400,312);
                invKin(H,400,400,331);

            case 27     % Movimentos para utilzar no GARF4
                maxSpeed = input('Maximum Speed: ');
                sendServo(H,23,maxSpeed,0,0,0,0,0);
                % Inicio de uma sequencia de movimentos
                invKin(H,400,400,331);
                invKin(H,400+95,400+35,312);
                invKin(H,400+95,400-39,312);
                invKin(H,400-150,400-39,270);
                invKin(H,400-150,400+35,270);
                invKin(H,400+95,400+20,312);
                invKin(H,400,400,331);
                
            case 28
                disp('Maximum Speed: 50');
                sendServo(H,23,50,0,0,0,0,0);

                S = load('.\graf_will\sequencia');
                nValores=10;
                for i= 1:nValores
                    invKin(H, 400+S.testar(i,1), 400+S.testar(i,2), 331-S.testar(i,3));
                end

            case 29
                sendServo(H,27,0,0,0,0,0,0);
                for i=1:10
                    [command,return1,return2,return3] = readServo(H);
                    disp([' Initiate movement ' num2str(return1)]);
                end
                
            case 30
                disp(' All Servo Positions');
                id = 0;
                speed = 100;
                pos = 0;
                while (pos ~= -150)
                    pos = setSpeedReadPos(H,id,speed);
                    if (pos ~= -150)
                        disp(['  Servo   : ', num2str(id)]);
                        disp([' Position : ', num2str(pos), 'º']);
                    end
                    id = id+1;
                end
                
                disp(' ');
                disp(' All Servos Speeds are at 100%');
                disp(' ');
                disp(' All Sensors');
                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 31
                disp('Maximum Speed: 50');
                sendServo(H,23,50,0,0,0,0,0);
                disp(' ');
                disp('Do you want to use the sequence file?');
                ans = input(' y/n: ','s');

                if (ans == 'y')
                    disp(' ');
                    disp(' Start Sequence');
                    S = load('.\graf_will\sequencia');
                    nValores=10;
                    for i= 1:nValores
                        invKin2(H, S.testar(i,1), S.testar(i,2), S.testar(i,3), S.testar(i,4));
                    end
                    disp(' Sequence End');
                else
                    disp(' ');
                    disp('   Chose one option');
                    disp(' 1 - Inverse Kinematic Support');
                    disp(' 2 - Inverse Kinematic Free');
                    disp(' 3 - Inverse Kinematic Arm');
                    ans = input(' 1/2/3: ');
                    select = ans-1;
                    disp(' ');
                    posX = input(' X position: ');
                    posY = input(' Y position: ');
                    posZ = input(' Z position: ');
                    invKin2(H, posX, posY, posZ, select);
                end

            case 32             % porque é que não trabalha com o servo 1??
                id = input(' ID Servo to test 0/1: ');
                sendServo(H,29,id,0,0,0,0,0);
                
                [command,return1,return2] = readServo(H);
                N1 = (return1*256) + return2;
                disp(['Movement 1 with ', num2str(N1), ' values.']);
                
                posArray = zeros(2,500);
                t = zeros(1,500);
                
                % Leitura da posição (posArray) e tempo (t)
                for i=1:N1
                	[command,return1,return2,return3]=readServo(H);
                    pos = (return1*256) + return2;
                    posArray(1,i) = (((pos-VAL_ANGLE_MIN)*ANGLE_RANGE)/VAL_ANGLE_RANGE)+ANGLE_MIN;
                    %posArray(1,i) = -pos/4;       % Tratamento dos dados recolhidos (posArray)
                    posArray(2,i)=return3;
                end
                for i=1:N1
                    [command,return1,return2,return3]=readServo(H);
                    t(1,i)=return1*256+return2;
                end
                
             %%%%  Repetição  %%%%
                [command,return1,return2] = readServo(H);
                N2 = (return1*256) + return2;
                disp(['Movement 2 with ', num2str(N2), ' values.']);
                NT= N1+N2;
                
                % Leitura da posição (posArray) e tempo (t)
                for i=(N1+1):NT
                	[command,return1,return2,return3]=readServo(H);
                    pos = (return1*256) + return2;
                    posArray(1,i) = (((pos-VAL_ANGLE_MIN)*ANGLE_RANGE)/VAL_ANGLE_RANGE)+ANGLE_MIN;
                    %posArray(1,i) = -pos/4;       % Tratamento dos dados recolhidos (posArray)
                    posArray(2,i)=return3;
                end
                for i=(N1+1):NT
                    [command,return1,return2,return3]=readServo(H);
                    t(1,i)=return1*256+return2;
                end
             %%%%  %%%%  %%%%  %%%%
                
                [command,return1,return2] = readServo(H);
                if (~(command == 29 && ~return1 && ~return2))
                    disp('A leitura de dados falhou.');
                end
                
                % Grafico
                figure
                hold on
                subplot(2,1,1)
                plot(t(1:NT)',posArray(1,1:NT)','*')
                xlabel('Tempo (ms)')
                ylabel('Posição (º)')
                axis([0 max(t) -100 100])
                grid on
                
                t_aux = t(2:NT);
                posArray_aux = posArray(1,2:NT);
                velArray = (posArray_aux-posArray(1,1:NT-1))./(3*1e-3*(t_aux-t(1:NT-1)));
                
                subplot(2,1,2)
                plot(t(1:NT)',posArray(2,1:NT)','*', t(1:NT-1),velArray,'r')
                xlabel('Tempo (ms)')
                ylabel('Velocidade (%)')
                axis([0 max(t) 0 100])
                grid on
                hold off
                              
            case 40         % Inverse Kinematic Point-to-Point
                disp(' Inverse Kinematic Point-to-Point');
                posX = input(' X position: ');
                posY = input(' Y position: ');
                posZ = input(' Z position: ');
                [posArray,t] = invKinPtP(H,posX,posY,posZ);
                
                figure
                hold on
                plot(t',posArray','*')
                xlabel('Tempo (ms)')
                ylabel('Posição (º)')
                legend('Servo 0','Servo 1','Servo 2')
                axis([0 max(t) -160 100])
                grid on
                hold off
                
                [posArray,t] = invKinPtP(H,0,0,0);
                
            case 41
                maxSpeed = input('Maximum Speed: ');
                sendServo(H,23,maxSpeed,0,0,0,0,0);

                S = load('.\graf_will\sequencia');
                nValores = 5;       % apartir do 6 tem erro devido ao ciclo na PIC não terminar
                tlast = 0;
                figure
                hold on
                for i= 1:nValores
                    [posArray,t] = invKinPtP(H, S.testar(i,1), S.testar(i,2), S.testar(i,3));
                    t = t + tlast;
                    plot(t',posArray','.')
                    tlast = max(t);
                end
                
                [posArray,t] = invKinPtP(H, 0, 0, 0);   % posição final
                t = t + tlast;
                plot(t',posArray','.')
                
                grid on
                xlabel('Tempo (ms)')
                ylabel('Posição (º)')
                legend('Servo 0','Servo 1','Servo 2')
                axis([0 max(t) -160 100])
                hold off

%%%%%%   A Testar   %%%%%%
            case 43
                disp(' Inverse Kinematic Version1    vs');
                disp(' Inverse Kinematic Point-to-Point');
                posX = input(' X position: ');
                posY = input(' Y position: ');
                posZ = input(' Z position: ');
                
                [posArray,t] = invKinPtP(H,posX,posY,posZ);
                invKinPtP(H,0,0,0);                 % posição inicial
                invKin(H,400+posX,400+posY,331-posZ);
                
                sendServo(H,24,0,0,0,0,0,0);
                [command,return1,return2] = readServo(H);
                N = (return1*256) + return2;
                disp(['Movement with ', num2str(N), ' values.']);
                
                % Leitura da posição (posArray) e tempo (t)
                sendServo(H,25,0,0,0,0,0,0);
                for i=1:N
                    for j=1:3
                        [command,return1,return2,return3]=readServo(H);
                        posArray2(j,i)=return1*256+return2;
                    end
                end
                for i=1:N
                    [command,return1,return2,return3]=readServo(H);
                    t2(1,i)=return1*256+return2;
                end
                
                % Leitura dos sensores (valSensor)          % isto é necessário apenas para a PIC terminar o programa (se não fica a espera)
                for j=1:N
                    for i=1:4
                        [command,return1,return2,return3]=readServo(H);
                        valSensor(j,i)=return1*256+return2;
                    end
                end

                % Tratamento dos dados recolhidos (posArray)
                for i=1:N
                    for j=1:3
                        posArray2(j,i) = (((posArray2(j,i)-VAL_ANGLE_MIN)*ANGLE_RANGE)/VAL_ANGLE_RANGE)+ANGLE_MIN;
%                         if(j==3)
%                             posArray(j,i)=(-posArray(j,i)-90)/(30/22);
%                         end
%                         if(j==1)
%                             posArray(j,i)=-posArray(j,i)/4;
%                         end
%                         if(j==2)
%                             posArray(j,i)=posArray(j,i)/4;
%                         end
                    end
                end
                
                figure
                hold on
                plot(t',posArray','*',t2',posArray2','.')
                xlabel('Tempo (ms)')
                ylabel('Posição (º)')
                legend('InvKinPtP Serv0','InvKinPtP Serv1','InvKinPtP Serv2',' InvKin    Serv0',' InvKin    Serv1',' InvKin    Serv2')
                axis([0 max(t) -160 100])
                grid on
                hold off
                
                invKinPtP(H,0,0,0);                     % posição inicial
                
                aux = t(1)-t2(1);
                disp(['Time difference between t(1) and t2(1): ', num2str(aux), ' ms']);
                aux = t(2)-t2(2);
                disp(['Time difference between t(2) and t2(2): ', num2str(aux), ' ms']);
                aux = t(3)-t2(3);
                disp(['Time difference between t(3) and t2(3): ', num2str(aux), ' ms']);

            case 42
                maxSpeed = input('Maximum Speed: ');
                sendServo(H,23,maxSpeed,0,0,0,0,0);

                S = load('.\graf_will\sequencia');
                nValores = 5;       % apartir do 6 tem erro devido ao ciclo na PIC não terminar
                tlast = 0;
                figure
                hold on
                for i= 1:nValores
                    [posArray,t] = invKinPtP(H, S.testar(i,1), S.testar(i,2), S.testar(i,3));
                    t = t + tlast;
                    plot(t(1,:)',posArray(1,:)','b.')
                    plot(t(2,:)',posArray(2,:)','g.')
                    plot(t(3,:)',posArray(3,:)','r.')
                    tlast = max(t(3,:));
                end
                
                [posArray,t] = invKinPtP(H, 0, 0, 0);   % posição final
                t = t + tlast;
                plot(t(1,:)',posArray(1,:)','b.')
                plot(t(2,:)',posArray(2,:)','g.')
                plot(t(3,:)',posArray(3,:)','r.')
                
                grid on
                xlabel('Tempo (ms)')
                ylabel('Posição (º)')
                legend('Servo 0','Servo 1','Servo 2')
                axis([0 max(t(3,:)) -100 100])
                hold off
                
            case 36
                id = 0;
                speedx = [100 75 50 25 5];
                figure
                hold on
                
                for i=1:5
                    setPosId(H,id,-90);
                    pos = setSpeedReadPos(H,id,speedx(i));
                    pause(3);
                    setPosId(H,id,90);
                    pause(3);
                    
                    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
                    
                    j=1;
                    for i=1:N
                        posArray(j,i)= (((posArray(j,i)-VAL_ANGLE_MIN)*ANGLE_RANGE)/VAL_ANGLE_RANGE)+ANGLE_MIN;
                        posArray(j,i)= -posArray(j,i)/4;
                    end
                    
                    plot(t',posArray(j,:)','*')
                end
                
                xlabel('Tempo (ms)')
                ylabel('Posição (º)')
                legend('Servo 0')
                axis([0 max(t) -100 100])
                grid on
                hold off
                
                pos = setSpeedReadPos(H,id,100);
                setPosId(H,id,0);
                
            case 33
                id = 0;
                setPosId(H,id,-90);
                pos = setSpeedReadPos(H,id,50);
                pause(1);
                setPosId(H,id,90);
                for i=1:80
                    posAr80(1,i) = setSpeedReadPos(H,id,50);
                end
                figure
                hold on                    
                plot(1:80',posAr80(1,:)','*')
                xlabel('Amostras')
                ylabel('Posição (º)')
                legend('Servo 0')
                grid on
                hold off
                setPosId(H,id,0);
                
            case 34     % versão resumida da opção 35 com 4 movimentos
                invKin(H,400+95,400+35,312);
                [Nlength,Nmoves,tlast] = storeData(H,Nlength,Nmoves,tlast);
                invKin(H,400+95,400-39,312);
                [Nlength,Nmoves,tlast] = storeData(H,Nlength,Nmoves,tlast);
                invKin(H,400-150,400-39,270);
                [Nlength,Nmoves,tlast] = storeData(H,Nlength,Nmoves,tlast);
                invKin(H,400,400,331);
                [Nlength,Nmoves,tlast] = storeData(H,Nlength,Nmoves,tlast);
                
            case 35                
                % Inverse Kinematic, Leitura, Armazenamento dos Dados...
                if (Nmoves == 5)
                   Nmoves = 0;
                   Nlength = 0;
                   tlast = 0;
                   % e tenho de apagar o que tenho no ficheiro
                   % como?
                end
                
                posX = input('X position: ');%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
                sign = input('Positive/Negative (0/1): ');
                if(sign)
                    posX=-posX;     %% Ou seja o negativo não serve para nada. Posso por abs??
                end
                posX=400+posX;      % porque somar 400 ao posX ????
                
                posY = input('Y position: ');
                sign = input('Positive/Negative (0/1): ');
                if(sign)
                    posY=-posY;
                end
                posY=400+posY;      % porque somar 400 ao posY ????
                
                posZ = input('Z position: ');
                invKin(H,posX,posY,posZ);%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
                
                sendServo(H,24,0,0,0,0,0,0);
                [command,return1,return2] = readServo(H);
                N = (return1*256) + return2;
                Nlength = Nlength + N;
                Nmoves = Nmoves + 1;
                disp(' ');
                disp(['Movement ', num2str(Nmoves), ' with ', num2str(N), ' values.']);
                disp(['Total of ', num2str(Nmoves), ' Moves with ', num2str(Nlength), ' values.']);
                
                % Criar matrizes com N variaveis pra leiturra
                posArray = zeros(3,N);
                t = zeros(1,N);
                valSensor = zeros(N,4);
                
                % Leitura da posição (posArray) e tempo (t)
                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

                % Tratamento dos dados recolhidos (posArray)
                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==1)
                            posArray(j,i)=-posArray(j,i)/4;
                        end
                        if(j==2)
                            posArray(j,i)=posArray(j,i)/4;
                        end
                        if(j==3)
                            posArray(j,i)=(-posArray(j,i)-90)/(30/22);
                        end
                    end
                end
                
                % Grafico da posição deste Movimento
                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
                
%                 % Leitura dos sensores (valSensor)
%                 for j=1:N
%                     for i=1:4
%                         [command,return1,return2,return3]=readServo(H);
%                         valSensor(j,i)=return1*256+return2;
%                     end
%                 end
%
%                 % Tratamento dos dados recolhidos (valSensor)
%                 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
%                 
%                 % Grafico do centro de pressão x,y (GRAF2)
%                 figure
%                 hold on
%                 for j=1:N
%                     plot(COPyp(j),COPxp(j),'*','Color',[j/N 1/(1+0.1*j) 0])
%                 end
%                 hold off

                % Ajustar o tempo
                t = t + tlast;
                tlast = t(1:N);
                
                % Guarda os novos valores
                for i=(Nlength-N):Nlength
                    test.posArray(:,i) = posArray;                  
                    test.time(1,i) = t;
                    %test.valSensor(i,:) = valSensor;
                end
                % Guarda o test (os 3 vectores) no ficheiro x
                save .\graf_will\mov test
                
                % Grafico final com os 4 movimentos
                if (Nmoves == 4)
                    % Faz o load dos dados e ajusta o tempo
                    S = load('.\graf_will\mov');
                    
                    % Desenha o grafico (GRAF4)
                    figure
                    plot( S.test.time, S.test.posArray, '*');
                    hold on
                    % vline - draws a vertical line on the location x
%                     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
%                     if(length(timeAdjust) >= 1000)
%                         h1000 = vline(timeAdjust(1,1000),'m','Fim do 5º 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
                end
                
                % Limpeza das variaveis antes de mudar o N
                posArray = zeros(3,N);
                t = zeros(1,N);
                valSensor = zeros(N,4);
                
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% %%%%%%%%  MOVEMENTS  %%%%%%%%%
% %%%%%%%%%%%% LEG %%%%%%%%%%%%%            
%             case 20
%                 maxSpeed = input('Maximum Speed: ');
%                 sendServo(H,23,maxSpeed,0,0,0,0,0);
%                 move(H,-60,10,20);
%                 pause(1.5);
%                 move(H,-60,10,-90);
%                 pause(1.5);
%                 move(H,80,10,-90);
%                 pause(1.5);
%                 move(H,80,70,0);
%                 pause(2);
%                 move(H,80,-40,0);
%                 pause(1.5);
%                 move(H,80,10,0);     % Posição final
%                 pause(1.5);
%                 
%             case 21
%                 maxSpeed = input('Maximum Speed: ');
%                 sendServo(H,23,maxSpeed,0,0,0,0,0);
%                 for i=1:2
%                     move(H,-60,10,20);
%                     pause(3);                
%                     move(H,20,10,-80);
%                     pause(3);
%                     move(H,80,10,-80);
%                     pause(3);
%                     move(H,80,10,0);
%                     pause(2);
%                 end
%                 
%             case 22
%                 maxSpeed = input('Maximum Speed: ');
%                 sendServo(H,23,maxSpeed,0,0,0,0,0);
%                 move(H,-60,10,20);
%                 pause(2);                
%                 move(H,80,10,-90);
%                 pause(2);
%                 move(H,80,10,0);
%                 pause(2);
%                 
% %%%%%%%%%%%% Arm %%%%%%%%%%%%%            
%             case 30
%                 %maxSpeed = input('Maximum Speed: ');
%                 sendServo(H,23,30,0,0,0,0,0);
%                 move(H,0,85,-90);
%                 pause(2);
%                 move(H,0,90,-35);
%                 pause(2);
%                 move(H,0,-90,-35);
%                 pause(2);
%                 move(H,0,85,45);    % Posição final
%                 pause(2);
%                 for i=0:5
%                     move(H,75,90,-35);
%                     pause(0.3);                
%                     move(H,75,60,-35);
%                     pause(0.3);
%                 end
%                 move(H,0,85,45);    % Posição final
%                 pause(2);
%                 
% %%%%%%%%%%%% Body %%%%%%%%%%%%%            
%             case 40
%                 maxSpeed = input('Maximum Speed: ');
%                 sendServo(H,23,maxSpeed,0,0,0,0,0);
%                 move(H,0,0,0);
%                 pause(4);
%                 move(H,35,0,0);     % Motor 0
%                 pause(4);
%                 move(H,-35,0,0);
%                 pause(4);
%                 move(H,0,-50,0);    % Motor 1
%                 pause(4);
%                 move(H,0,20,0);
%                 pause(4);
%                 move(H,0,0,10);     % Motor 2
%                 pause(4);
%                 move(H,0,0,-10);
%                 pause(4);
%                 move(H,0,0,0);      % Posição final
%                 pause(4);
%                 
%             case 41
%                 maxSpeed = input('Maximum Speed: ');
%                 sendServo(H,23,maxSpeed,0,0,0,0,0);
%                 for i=1:3
%                     move(H,35,-50,0);
%                     pause(4);
%                     move(H,0,0,0);
%                     pause(4);
%                     move(H,-35,-50,0);
%                     pause(4);
%                     move(H,0,0,0);
%                     pause(4);
%                 end
%                 
% %%%%%%%%%%%% 2 Legs %%%%%%%%%%%%%            
%             case 25
%                 maxSpeed = input('Maximum Speed (30): ');
%                 sendServo(H,23,maxSpeed,0,0,0,0,0);
%               % move2(H,anca1,pé1,joelho1,anca2,pé2,joelho2);
%                 for i=1:2
%                     move2(H,-45,0,0,-60,-80,80);
%                     pause(2);
%                     move2(H,60,80,-90,45,0,0);
%                     pause(2);
%                 end
%                 move2(H,-10,0,-90,20,10,80);    % Posição final
%                 pause(1);
% 
%             case 26
%                 maxSpeed = input('Maximum Speed (60): ');
%                 sendServo(H,23,maxSpeed,0,0,0,0,0);
%               % move2(H,anca1,pé1,joelho1,anca2,pé2,joelho2);
%                 for i=1:2
%                     move2(H,60,80,45,-80,-80,-45);
%                     pause(2);
%                     move2(H,-10,-80,-90,20,80,80);
%                     pause(2);
%                 end
%                 move2(H,-10,0,-90,20,10,80);    % Posição final
%                 pause(1);
% 
% %%%%%%%%%%%% Legs and Arms %%%%%%%%%%%%%            
%             case 35
%                 maxSpeed = input('Maximum Speed (30): ');
%                 sendServo(H,23,maxSpeed,0,0,0,0,0);
%                 % move4(H,  perna1,    perna2,    braço1,    braço2);
%                 move4(H,  -10,90,-90,  20,-90,80,  0,0,-45,  0,0,45);
%                 pause(2);
%                 move4(H,  -10,0,-90,  20,10,80,  -90,-90,45,  90,85,-45);
%                 pause(2);
%                 move4(H,  -10,0,0,  20,10,-10,  0,0,-45,  0,0,45);
%                 pause(2);
%                 move4(H,  -10,0,-90,  20,10,80,  -5,-85,-45,  0,85,45);
%                 pause(1);       % Posição final
%                 
%             case 36
%                 maxSpeed = input('Maximum Speed (30): ');
%                 sendServo(H,23,maxSpeed,0,0,0,0,0);
%                 for i=1:2
%                     move4(H,  -45,0,0,  -60,-80,80,  -45,-85,0,  0,85,90);
%                     pause(2);
%                     move4(H,  60,80,-90,  45,0,0,  -5,-85,-90,  45,85,0);
%                     pause(2);
%                 end
%                 move4(H,  -10,0,-90,  20,10,80,  -5,-85,-45,  0,85,45);    % Posição final
%                 pause(1);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

            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