function [t,current,pressure,error,errorstr]=choque_no_solo2(H,speed)

close all

% Inicialização das varáveis com as medidas
t=[];
current=[];
pressure=[];

scuid=[2 4];

% Comprimento dos elos
L1=9.5;
L2=9.5;
L3=6.0;
L=[L1 L2 L3];

% Coordenadas cartesianas iniciais
x0=0;
y0=0.4;

x1=0;
y1=2.0;

xf=0;
yf=0.1;

% Desactivação dos PWMs
disp 'PWMs desactivados.'
[error,errorstr]=killuc(H,scuid);
if (error==1), return, end

pause(1)

% Activação dos PWMs
[error,errorstr]=inituc(H,scuid);
if (error==1), return, end

% Esperar que a activação dos PWMs seja feito
disp 'Reactivando PWMs...'
pause(5);
disp 'Done!'

% Posicionamento da perna em altura
disp 'Inicializando a perna...'
[x,error,errorstr]=motioncartesian(H,[x1,y1],L,speed);
if (error==1), return, end

% Esperando pelo fim do movimento
[x,error,errsorstr]=waitmotionfin(H,scuid);
if (error==1), return, end

disp 'Done!'
disp 'Pressione ENTER para comecar as medições...'
pause

% Movendo a perna
disp 'Movendo a perna...'
[x,error,errorstr]=motioncartesian2(H,[xf,yf],L,speed);
if (error==1), return, end

% Espera que os motores se comecem a mover
% Espera que os motores se comecem a mover
[x,error,errorstr]=motionstart(H,scuid);
if (error~=0), return, end

% Espera que os motores terminem o movimento
tic
finall=0;
while (finall==0)
        
    % Instante temporal de amostragem
    t=[t; toc];

    % Leitura da corrente
    currentnow=zeros(1,3,length(scuid));
    for i=1:length(scuid)
        [currentnow(1,:,i),x,x,error,errorstr]=readjoint(H,scuid(i),2);
        if (error), return, end
    end
    current=[current; currentnow];
    
    % Leitura dos sensores de força
    [pressnow,x,error,errorstr]=readspecial(H,scuid(1));
    pressure=[pressure; pressnow];
    
    [finall,x,x,error,errorstr]=statmotionfin(H,scuid);
    if (error==1), return, end
end

disp 'Done!'
pause(1);

% Reposicionamento da perna no ponto inicial
disp 'Reciclagem da perna...'
[x,error,errorstr]=motioncartesian2(H,[x0,y0],L,speed);
if (error==1), return, end

% Esperando pelo fim do movimento
[x,error,errsorstr]=waitmotionfin(H,scuid);
if (error==1), return, end

% Desligar PWMs
[error,errorstr]=killuc(H,scuid);

disp 'Done!'
disp 'Programa terminado!'

figure(1)
plot(t,pressure(:,1),'b.',t,pressure(:,2),'g.',t,pressure(:,3),'m.',t,pressure(:,4),'r.')
axis([0,t(end),0,255])
xlabel('time');
ylabel('Amplitude');
legend('Sensor 1','Sensor 2','Sensor 3','Sensor 4');
grid on

figure(2)
plot(t,sum(pressure')'/size(pressure,2),'k.');
axis([0,t(end),0,255])
xlabel('time');
ylabel('Amplitude');
title('Mean')
grid on

for i=1:length(scuid)
    figure(2+i)
    plot(t,current(:,:,i));
    xlabel('time');
    ylabel('Current pulse');
    legend('Motor 1','Motor 2','Motor 3');
    grid on
end

save choque_descida_semobjectos1.mat t pressure current scuid