function Leg_Sens(H)

globalVar;
% nSampPlot=50;
% Nsensors=4;
% hPlot=zeros(Nsensors+1);
% y=zeros(Nsensors,nSampPlot);

XYZ = zeros(1,3);
disp(' ');
disp(' ** Leg **');
XYZ(1,1) = input('Time [5 20](s): ');
while (XYZ(1,1) ~= [5:0.1:20])
    XYZ(1,1) = input('Time [5 20](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,25,param1,param2,param3,param4,param5,param6);
end

%-------------------------------------------------------------------
% x = 1:nSampPlot;
% COPxp=0;
% COPyp=0;
% figure
% subplot(211)
% 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(COPxp,COPyp,'.','XDataSource','COPxp','YDataSource','COPyp','MarkerSize',30);
% 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-')
% hold off
% 
% subplot(212)
% hold on
% hPlot(1) = plot(x,y(1,:),'r','MarkerSize',20);
% hPlot(2) = plot(x,y(2,:),'c');
% hPlot(3) = plot(x,y(3,:),'m');
% hPlot(4) = plot(x,y(4,:),'g');
% title('Valor dos Sensores','FontWeight','bold','FontSize',12)
% ylabel('Valor (0...1023)','FontWeight','bold','FontSize',12)
% legend('Sensor 1','Sensor 2','Sensor 3','Sensor 4','FontWeight','bold','FontSize',11);
% StripChart('Initialize',gca)
% hold off
%-------------------------------------------------------------------

[command,return1,return2,return3] = readServo(H);
COPx_ref = (return1*256 + return2)-200.0;
temp_ret = return3;
[command,return1,return2,return3] = readServo(H);
COPy_ref = (temp_ret*256 + return1)-200.0;
COGz_ref = (return2*256 + return3)-200.0;

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);
    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.0;
    temp_ret = return3;
    [command,return1,return2,return3] = readServo(H);
    Qvel(2,i) = (temp_ret*256 + return1)-200.0;
    Qvel(3,i) = (return2*256 + return3)-200.0;
    
    [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;
    
    [command,return1,return2,return3]=readServo(H);
    COPx_p(i) = (return1*256+return2)-200;
%-------------------------------------------------------------------
%     for k=1:4
%         y(k,i) = valSensor(k,i);
%         StripChart('Update',hPlot(k),y(k,i));
%     end
%     xx=[y(1,i) y(2,i) y(3,i) y(4,i)];
%     if xx == 0
%         COPxp= 0;
%         COPyp= 0;
%     else
%        COPxp=(-y(1,i)-y(2,i)+y(3,i)+y(4,i))*7/sum(xx);
%        COPyp=(y(1,i)-y(2,i)-y(3,i)+y(4,i))*4/sum(xx);
%     end
%    refreshdata(hPlot(5),'caller');
%    drawnow
%-------------------------------------------------------------------
end

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

for j=1:4;
    for i=1:N
        posArray(j,i) = (((posArray(j,i)-VAL_ANGLE_MIN)*ANGLE_RANGE)/VAL_ANGLE_RANGE)+ANGLE_MIN;

%         % TENTAR diminuir o erro
%         if (posArray(j,i)==-150)
%             if (i-1 ~= 0)
%             	posArray(j,i) = posArray(j,i-1);
%             end
%         end
    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);
COPx_p = COPx_p(:,1:N);

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',valSensor(1,:)','r-')
plot(t',valSensor(2,:)','c-')
plot(t',valSensor(3,:)','m-')
plot(t',valSensor(4,:)','g-')
title('Valor dos Sensores','FontWeight','bold','FontSize',12)
xlabel('Tempo (ms)','FontWeight','bold','FontSize',12)
ylabel('Valor (0...1023)','FontWeight','bold','FontSize',12)
%legend('Sensor 1','Sensor 2','Sensor 3','Sensor 4','FontWeight','bold','FontSize',11);
axis([0 max(t) -10 390])
grid on
hold off

subplot(323)
hold on
plot(t',posArray(1:3,:)','.')
plot(t',posArray(4,:)','-')
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','Servo2 - corrigido pelo controlador ','FontWeight','bold','FontSize',11)
axis([0 max(t) -100 100])
grid on
hold off

subplot(324)
hold on
plot(t',COPx,t',COPy,t',(COPx_p/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','COPy','COPx p','FontWeight','bold','FontSize',11);
axis([0 max(t) -8 8])
grid on
hold off

subplot(325)
hold on
plot(t',Qvelp','-','LineWidth',2)
title('Velocidades dos Servos (os primeiros 150 valores)','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

subplot(326)
hold on
plot(t',Qvel','-','LineWidth',2)
title('Velocidades dos Servos','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


% Apenas para TESTES -----------------------------------------------------%
COPx_ref;
COPy_ref;
COGz_ref;
COPx_ref = 0;
COPy_ref = 0;
COGz_ref = 0;
Gain = [32/8 32/8 30/22];
Kgain = [100 100 100];
err(1,1:N) = COPx_ref-(COPx*10);
err(2,1:N) = 0;
err(3,1:N) = 0;

q(1,:) = -(posArray(1,:)+0)*Gain(1);
q(2,:) = -(posArray(2,:)-20)*Gain(2);
q(3,:) = (posArray(3,:)+90)*Gain(3);

% Modo do prof no Matlab
for (k=1:N)
    inv_J = InverseJacobianCOG(q(:,k)',-1);
    Qvelt1(:,k) = inv_J * [Kgain(1)*err(1,k) Kgain(2)*err(2,k) Kgain(3)*err(3,k)]';
end
Qvelt1(1,:) = -Qvelt1(1,:);
Qvelt1(2,:) = -Qvelt1(2,:);
% Modo do william no PIC
for (k=1:N)
	inv_J = InverseJacobianCOG(q(:,k)',-1);
    for (i=1:3)
        Qvelt2(i,k) = inv_J(i,1)*Kgain(1)*err(1,k) + inv_J(i,2)*Kgain(2)*err(2,k) + inv_J(i,3)*Kgain(3)*err(3,k);
    end
end
Qvelt2(1,:) = -Qvelt2(1,:);
Qvelt2(2,:) = -Qvelt2(2,:);

figure
subplot(221)
hold on
plot(t',Qvel','-','LineWidth',2)
title('Velocidades calculadas no PIC','FontWeight','bold','FontSize',12)
xlabel('Tempo (ms)','FontWeight','bold','FontSize',12)
ylabel('Qvel do PIC (º/s)','FontWeight','bold','FontSize',12)
%legend('Servo1 - Ankle1','Servo2 - Ankle2','Servo3 - Knee','FontWeight','bold','FontSize',11)
axis([0 max(t) -200 200])
grid on
hold off
subplot(222)
hold on
plot(t',Qvelt1','-','LineWidth',2)
title('Velocidades calculadas no Matlab (modo 1)','FontWeight','bold','FontSize',12)
xlabel('Tempo (ms)','FontWeight','bold','FontSize',12)
ylabel('Qvel teórico 1 (º/s)','FontWeight','bold','FontSize',12)
%legend('Servo1 - Ankle1','Servo2 - Ankle2','Servo3 - Knee','FontWeight','bold','FontSize',11)
axis([0 max(t) -200 200])
grid on
hold off
subplot(224)
hold on
plot(t',Qvelt2','-','LineWidth',2)
title('Velocidades calculadas no Matlab (modo 2)','FontWeight','bold','FontSize',12)
xlabel('Tempo (ms)','FontWeight','bold','FontSize',12)
ylabel('Qvel teórico 2 (º/s)','FontWeight','bold','FontSize',12)
%legend('Servo1 - Ankle1','Servo2 - Ankle2','Servo3 - Knee','FontWeight','bold','FontSize',11)
axis([0 max(t) -200 200])
grid on
hold off
subplot(223)
hold on
plot(t',(Qvel-Qvelt1)','-')
title('Diferença entre Velocidades','FontWeight','bold','FontSize',12)
xlabel('Tempo (ms)','FontWeight','bold','FontSize',12)
ylabel('Diferença entre Qvel do PIC e do Matlab (º/s)','FontWeight','bold','FontSize',12)
%legend('Servo1 - Ankle1','Servo2 - Ankle2','Servo3 - Knee','FontWeight','bold','FontSize',11)
axis([0 max(t) -200 200])
grid on
hold off
%-------------------------------------------------------------------------%

end