function [Nlength,Nmoves,tlast] = storeData(H,Nlength,Nmoves,tlast)

globalVar;
    
% 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

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

% 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);

end