function Leg_Sens_simpl_2(H)

globalVar;

XYZ = zeros(1,3);
disp(' ');
disp(' ** Leg **');
XYZ(1,1) = input('Time [1 5](s): ');
while (XYZ(1,1) ~= [1:0.1:5])
    XYZ(1,1) = input('Time [1 5](s): ');
end
XYZ(1,1) = XYZ(1,1)*100;

Tramas = 1;
for i = 1:Tramas
    param1 = bitand(XYZ(i,1),MASK_HIGH) / 256;
    param2 = bitand(XYZ(i,1),MASK_LOW);
    param3 = bitand(XYZ(i,2),MASK_HIGH) / 256;
    param4 = bitand(XYZ(i,2),MASK_LOW);
    param5 = bitand(XYZ(i,3),MASK_HIGH) / 256;
    param6 = bitand(XYZ(i,3),MASK_LOW);
    sendServo(H,Tramas,32,param1,param2,param3,param4,param5,param6);
end



return1 = 0;        % apenas para limpar o registo
i=0;
while (return1 ~= 1 || return2 ~= 2 || return3 ~= 3)
    i = i+1;
    [command,return1,return2,return3] = readServo(H);
    COPx_ref(1,i) = (return1*256 + return2)-200.0;
    temp_ret = return3;
    [command,return1,return2,return3] = readServo(H);
    COPy_ref(1,i) = (temp_ret*256 + return1)-200.0;
    COGz_ref(1,i) = (return2*256 + return3)-200.0;

    [command,return1,return2,return3] = readServo(H);
    posArray(1,i) = return1*256 + return2;
    temp_ret = return3;
    [command,return1,return2,return3] = readServo(H);
    posArray(2,i) = temp_ret*256 + return1;
    posArray(3,i) = return2*256 + return3;

    [command,return1,return2,return3] = readServo(H);
    t(1,i) = return1*256+return2;
    
    [command,return1,return2,return3] = readServo(H);
    valSensor(1,i) = return1*256 + return2;
    temp_ret = return3;
    [command,return1,return2,return3] = readServo(H);
    valSensor(2,i) = temp_ret*256 + return1;
    valSensor(3,i) = return2*256 + return3;
    [command,return1,return2,return3] = readServo(H);
    valSensor(4,i) = return1*256+return2;
	[command,return1,return2,return3]=readServo(H);
    totalSensor(i) = return1*256+return2;
    
    [command,return1,return2,return3] = readServo(H);
    Qvel(1,i) = (return1*256 + return2)-200;
    temp_ret = return3;
    [command,return1,return2,return3] = readServo(H);
    Qvel(2,i) = (temp_ret*256 + return1)-200;
    Qvel(3,i) = (return2*256 + return3)-200;
    
    [command,return1,return2,return3] = readServo(H);
    Qvelp(1,i) = return1*256 + return2;
    temp_ret = return3;
    [command,return1,return2,return3] = readServo(H);
    Qvelp(2,i) = temp_ret*256 + return1;
    Qvelp(3,i) = return2*256 + return3;
    
    [command,return1,return2,return3] = readServo(H);
    posArray(4,i) = return1*256 + return2;
    temp_ret = return3;
    [command,return1,return2,return3] = readServo(H);
    posArray(5,i) = temp_ret*256 + return1;
    posArray(6,i) = return2*256 + return3;

    [command,return1,return2,return3]=readServo(H);
    COPx_p(i) = (return1*256+return2)-200;
end

N = i-1;
disp(' ');
disp(['Movement with ', num2str(N), ' values.']);
time = t(1,N)/1000;
disp(['Time from PIC: ', num2str(time), ' s']);

COPx_ref = COPx_ref(1,1:N);
COPy_ref = COPy_ref(1,1:N);
for j=1:6;
    for i=1:N
        posArray(j,i) = (((posArray(j,i)-VAL_ANGLE_MIN)*ANGLE_RANGE)/VAL_ANGLE_RANGE)+ANGLE_MIN;
    end
end
posArray = posArray(:,1:N);
t = t(1,1:N);
valSensor = valSensor(:,1:N);
totalSensor = totalSensor(1,1:N);
Qvel = Qvel(:,1:N);
Qvelp = Qvelp(:,1:N);
%errCOPx = errCOPx(:,1:N);
COPx_p = COPx_p(:,1:N);

save('saveVideo3','t','posArray','valSensor','totalSensor');

COPx=0;
COPy=0;
figure;
subplot(321)
hold on
xxl=-7;
xxr=7;
yyb=-4;
yyt=4;
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(COPx,COPy,'.','XDataSource','COPxp','YDataSource','COPyp');
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-')
for i=1:N
	COPx(i)=(-valSensor(1,i)-valSensor(2,i)+valSensor(3,i)+valSensor(4,i))*7/totalSensor(i);
    COPy(i)=(valSensor(1,i)-valSensor(2,i)-valSensor(3,i)+valSensor(4,i))*4/totalSensor(i);
end
plot(COPx,COPy,'.')
hold off

subplot(322)
hold on
plot(t',COPx, t',(COPx_p/10), t',(COPx_ref/10), '-','LineWidth',2)
title('Valor dos Sensores','FontWeight','bold','FontSize',12)
xlabel('Tempo (ms)','FontWeight','bold','FontSize',12)
ylabel('Valor (-10...10)','FontWeight','bold','FontSize',12)
legend('COPx','COPx p','COPx_ref','FontWeight','bold','FontSize',11);
axis([0 max(t) -2 2])
grid on
hold off

subplot(323)
hold on
plot(t',posArray(1:3,:)','.')
plot(t',posArray(4:6,:)','-')
title('Angulos dos Servos','FontWeight','bold','FontSize',12)
xlabel('Tempo (ms)','FontWeight','bold','FontSize',12)
ylabel('Posição (º)','FontWeight','bold','FontSize',12)
%legend('Servo1 - Ankle1','Servo2 - Ankle2','Servo3 - Knee','Servo1 - sem erros','Servo2 - sem erros','Servo3 - sem erros','FontWeight','bold','FontSize',11)
axis([0 max(t) -100 100])
grid on
hold off

subplot(324)
hold on
plot(t',COPy, t',(COPy_ref/10), '-','LineWidth',2)
title('Valor dos Sensores','FontWeight','bold','FontSize',12)
xlabel('Tempo (ms)','FontWeight','bold','FontSize',12)
ylabel('Valor (-10...10)','FontWeight','bold','FontSize',12)
legend('COPy','COPy_ref','FontWeight','bold','FontSize',11);
axis([0 max(t) -2 2])
grid on
hold off

subplot(325)
hold on
plot(t',Qvelp','-','LineWidth',2)
%plot(t',Qvelt','-','LineWidth',2)
title('Velocidades dos Servos','FontWeight','bold','FontSize',12)
xlabel('Tempo (ms)','FontWeight','bold','FontSize',12)
ylabel('Qvelp (%)','FontWeight','bold','FontSize',12)
%legend('Servo1 - Ankle1','Servo2 - Ankle2','Servo3 - Knee','FontWeight','bold','FontSize',11)
axis([0 max(t) 0 60])
grid on
hold off

Kgain = [10 20];
err_COPx = COPx_ref/10-COPx;
err_COPy = COPy_ref/10-COPy;

% subplot(326)
% hold on
% plot(t',Qvel',t',Qvelt','-','LineWidth',2)
% title('Velocidades dos Servos (os primeiros 150 valores)','FontWeight','bold','FontSize',12)
% xlabel('Tempo (ms)','FontWeight','bold','FontSize',12)
% ylabel('Qvel (º/s)','FontWeight','bold','FontSize',12)
% %legend('Servo1 - Ankle1','Servo2 - Ankle2','Servo3 - Knee','FontWeight','bold','FontSize',11)
% axis([0 max(t) -150 150])
% grid on
% hold off

subplot(326)
hold on
plot(t',err_COPx,t',err_COPy,'-','LineWidth',2)
title('erros COP teorico','FontWeight','bold','FontSize',12)
xlabel('Tempo (ms)','FontWeight','bold','FontSize',12)
ylabel('err','FontWeight','bold','FontSize',12)
legend('err COPx','err COPy','FontWeight','bold','FontSize',11)
axis([0 max(t) -2 2])
grid on
hold off

for i=1:N;
    XYZ(1:3,i) = DirKin3dof(posArray(4:6,i)');
end
figure
hold on
plot3(XYZ(1,:),XYZ(2,:),XYZ(3,:),'.b')
xlabel('pos X')
ylabel('pos Y')
zlabel('pos Z')
grid on
axis square
hold off

end