function ContMov_using_2Arms(H)

globalVar;

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,21,0,0,0,0,0,0);
end

XYZfL = [91 218 31];
XYZfR = [91 -218 31];

[command,return1,return2,return3] = readServo(H);
XYZiL(1) = return1*256 + return2;
temp_ret = return3;
[command,return1,return2,return3] = readServo(H);
XYZiL(2) = temp_ret*256 + return1;
XYZiL(3) = return2*256 + return3;
XYZiL = XYZiL-400;
[command,return1,return2,return3] = readServo(H);
XYZiR(1) = return1*256 + return2;
temp_ret = return3;
[command,return1,return2,return3] = readServo(H);
XYZiR(2) = temp_ret*256 + return1;
XYZiR(3) = return2*256 + return3;
XYZiR = XYZiR-400;

i=0;
while (return1 ~= 1 || return2 ~= 2 || return3 ~= 3)
    i = i+1;
    [command,return1,return2,return3] = readServo(H);
    posArray1(1,i) = return1*256 + return2;
    temp_ret = return3;
    [command,return1,return2,return3] = readServo(H);
    posArray1(2,i) = temp_ret*256 + return1;
    posArray1(3,i) = return2*256 + return3;

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

disp(' ');
disp(' ** RESULTS **');
% a= [XYZ(3,1) XYZ(3,2)];
% pause(max(a)/100);
[command,return1,return2,return3] = readServo(H);
if(command ~= 21)
    disp('OPERATION FAILED: Testing Inverse Kinetic');
end
disp('Lefth Arm :');
switch (return1)
    case 0
        disp('  OUT of Limits');
    case 1
        disp('  Inverse Kinetic is Working');
    case 2
        disp('  ERRO: OUT1 of WorkVolume');
    case 3
        disp('  ERRO: OUT2 of WorkVolume');
    case 4
        disp('  ERRO: OUT3 of WorkVolume');
    case 5
        disp('  ERRO: OUT4 of WorkVolume');
    case 6
        disp('  Not Used');
end
disp('Rigth Arm :');
switch (return2)
    case 0
        disp('  OUT of Limits');
    case 1
        disp('  Inverse Kinetic is Working');
    case 2
        disp('  ERRO: OUT1 of WorkVolume');
    case 3
        disp('  ERRO: OUT2 of WorkVolume');
    case 4
        disp('  ERRO: OUT3 of WorkVolume');
    case 5
        disp('  ERRO: OUT4 of WorkVolume');
    case 6
        disp('  Not Used');
end

disp(' ');
%k = return2;
N = return3;
disp(['Movement with ', num2str(N), ' values.']);
%disp(['Movement with ', num2str(k), ' invalid InvKin.']);
time = t(1,N)/1000;
disp(['Time from PIC: ', num2str(time), ' s']);

for j=1:3;
    for i=1:N
        posArray1(j,i) = (((posArray1(j,i)-VAL_ANGLE_MIN)*ANGLE_RANGE)/VAL_ANGLE_RANGE)+ANGLE_MIN;
        posArray2(j,i) = (((posArray2(j,i)-VAL_ANGLE_MIN)*ANGLE_RANGE)/VAL_ANGLE_RANGE)+ANGLE_MIN;
    end
end
posArray1 = posArray1(:,1:N);
posArray2 = posArray2(:,1:N);
t = t(1,1:N);

figure
hold on
plot(t',posArray1','.',t',posArray2','*')
xlabel('Tempo (ms)')
ylabel('Posição (º)')
legend('Servo 8','Servo 7','Servo 6','Servo 11','Servo 10','Servo 9')
axis([t(1,1) max(t) -100 100])
grid on
hold off

for i=1:N;
    P = DirKin_4dof(posArray1(:,i),1);
    XYZL(:,i) = P(:,3);
    P = DirKin_4dof(posArray2(:,i),-1);
    XYZR(:,i) = P(:,3);
end

figure
hold on
plot3(XYZiL(1),XYZiL(2),XYZiL(3),'*r')
plot3(XYZL(1,:),XYZL(2,:),XYZL(3,:),'.b')
plot3(XYZfL(1),XYZfL(2),XYZfL(3),'*r')
plot3(XYZiR(1),XYZiR(2),XYZiR(3),'*r')
plot3(XYZR(1,:),XYZR(2,:),XYZR(3,:),'.g')
plot3(XYZfR(1),XYZfR(2),XYZfR(3),'*r')
xlabel('pos X')
ylabel('pos Y')
zlabel('pos Z')
grid on
axis square
hold off

end