function ContMov_2Arms_v2(H)

globalVar;

XYZ = zeros(3);
disp(' ');
arm = input('Use Lefth Arm? y/n: ','s');
if isempty(arm)
    arm = 'y';
end
if (arm == 'y')
    disp(' ** Lefth Arm - XYZ **');
    XYZ(1,1) = input('Pos Xf [-234 234](mm): ');
    while (XYZ(1,1) ~= [-234:234])
        XYZ(1,1) = input('Pos Xf [-234 234](mm): ');
    end
    XYZ(1,2) = input('Pos Yf [140 374](mm): ');
    while (XYZ(1,2) ~= [140:374])
        XYZ(1,2) = input('Pos Yf [140 374](mm): ');
    end
    XYZ(1,3) = input('Pos Zf [-84 384] (mm): ');
    while (XYZ(1,3) ~= [-84:384])
        XYZ(1,3) = input('Pos Zf [-84 384](mm): ');
    end
    XYZ(1,:) = XYZ(1,:)+400;
    
    XYZ(3,1) = input('Time [0 5](s): ');
    while (XYZ(3,1) ~= [0:0.01:5])
        XYZ(3,1) = input('Time [0 5](s): ');
    end
    XYZ(3,1) = XYZ(3,1)*100;
end

disp(' ');
arm = input('Use Rigth Arm? y/n: ','s');
if isempty(arm)
    arm = 'n';
end
if (arm == 'y')
    disp(' ** Rigth Arm - XYZ **');
    XYZ(2,1) = input('Pos Xf [-234 234](mm): ');
    while (XYZ(2,1) ~= [-234:234])
        XYZ(2,1) = input('Pos Xf [-234 234](mm): ');
    end
    XYZ(2,2) = input('Pos Yf [-374 -140](mm): ');
    while (XYZ(2,2) ~= [-374:-140])
        XYZ(2,2) = input('Pos Yf [-374 -140](mm): ');
    end
    XYZ(2,3) = input('Pos Zf [-84 384](mm): ');
    while (XYZ(2,3) ~= [-84:384])
        XYZ(2,3) = input('Pos Zf [-84 384](mm): ');
    end
    XYZ(2,:) = XYZ(2,:)+400;
    
    XYZ(3,2) = input('Time [0 5](s): ');
    while (XYZ(3,2) ~= [0:0.01:5])
        XYZ(3,2) = input('Time [0 5](s): ');
    end
    XYZ(3,2) = XYZ(3,2)*100;
end

disp(' ');
ord = input('Tipe of Order (3 or 5): ');
if isempty(ord)
    ord = 3;
end
XYZ(3,3) = ord;

Tramas = 3;
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,17,param1,param2,param3,param4,param5,param6);
end

[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;
XYZfL = XYZ(1,:)-400;

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;
end

disp(' ');
disp(' ** RESULTS **');
a= [XYZ(3,1) XYZ(3,2)];
pause(max(a)/100);
[command,return1,return2,return3] = readServo(H);
if(command ~= 17)
    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
        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);

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

for i=1:N;
    P = DirKin_4dof(posArray(:,i),1);
    XYZL(:,i) = P(:,3);
end

% desenhar XYZiL, XYZfL e os XYZL
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),'.g')
xlabel('pos X')
ylabel('pos Y')
zlabel('pos Z')
grid on
axis square
hold off

end