maxtime=30;        % Tempo total de medição

reaction=[];
servo=[];
current=[];

time=0;
timeprev=0;
tic
while (time<maxtime)

    time=toc;
    
    % Leitura dos sensores de força
    reaction=[reaction; readreaction(H,0)];

    % Leitura das posições das juntas
    servo=[servo; readjoint(H,2,0)];
    
    % Leitura da corrente consumida pelas juntas
    current=[current; readjoint(H,2,2)];

    if (time-timeprev>=1)
        fprintf('%d ',floor(time))
        timeprev=floor(time);
    end
end

delta=time/size(reaction,1);
t=[0:delta:time-delta];

% Previsão da inclinação do plano
plan=[servo(2) -servo(1)];

% Cálculo do centro de pressao
CoP=[];
for i=1:length(t)
    CoP=[CoP; calccop(reaction(i,:))];
end
    
save cop_control CoP reaction servo current t

fprintf('\n');