%
%   storeData                       William Lage
%   

function storeData

    globalVar;
    delete(instrfind);          % william
    H=initCom;
    N=200;

    nMoves=0;
    posArray=zeros(3,N);
    t=zeros(1,N);
    %timeAdjust=zeros(1,Nsamples);
    %valSensor=zeros(N,4);
    %calibSensor=zeros(4,1);
    
    % 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
                
    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 .\graf_will\mov test
                
end