function ContMov_v4(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,24,0,0,0,0,0,0);
end

[command,return1,return2,return3] = readServo(H);
q(1) = return1*256 + return2;
temp_ret = return3;
[command,return1,return2,return3] = readServo(H);
q(2) = temp_ret*256 + return1;
q(3) = return2*256 + return3;
q = (((q-VAL_ANGLE_MIN)*ANGLE_RANGE)/VAL_ANGLE_RANGE)+ANGLE_MIN;
P = DirKin3dof(q);
XYZi = P';
XYZf(1,1) = 60;
XYZf(1,2) = 0;
XYZf(1,3) = 325;

disp(' ');
disp(' ** RESULTS **');
[command,return1,return2,return3] = readServo(H);
if(command ~= 24)
    disp('OPERATION FAILED: Testing Inverse Kinetic');
end
isVal = return1;
switch (isVal)
    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

if (isVal == 1)
    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);
        Qvel(1,i) = (return1*256) + return2;
        temp_ret = return3;
        [command,return1,return2,return3] = readServo(H);
        Qvel(2,i) = (temp_ret*256) + return1;
        Qvel(3,i) = (return2*256) + return3;
    end

    [command,return1,return2,return3] = readServo(H);
    j = return1;
    N = i-1;
    disp(' ');
    disp(['Movement with ', num2str(N), ' values.']);
    disp(['Movement with ', num2str(j), ' 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;
            
            % 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);
    Qvel = Qvel(:,1:N);
    
    for i=1:N;
    	XYZ(:,i) = DirKin3dof(posArray(:,i)');
    end
    
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
[command,return1,return2,return3] = readServo(H);
q(1) = return1*256 + return2;
temp_ret = return3;
[command,return1,return2,return3] = readServo(H);
q(2) = temp_ret*256 + return1;
q(3) = return2*256 + return3;
q = (((q-VAL_ANGLE_MIN)*ANGLE_RANGE)/VAL_ANGLE_RANGE)+ANGLE_MIN;
P = DirKin3dof(q);
XYZi2 = P';
XYZf2(1,1) = 60;
XYZf2(1,2) = 0;
XYZf2(1,3) = 325;

disp(' ');
disp(' ** RESULTS **');
[command,return1,return2,return3] = readServo(H);
if(command ~= 24)
    disp('OPERATION FAILED: Testing Inverse Kinetic');
end
isVal = return1;
switch (isVal)
    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

if (isVal == 1)
    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);
        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;

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

    [command,return1,return2,return3] = readServo(H);
    j = return1;
    N = i-1;
    disp(' ');
    disp(['Movement with ', num2str(N), ' values.']);
    disp(['Movement with ', num2str(j), ' invalid InvKin.']);
    time = t2(1,N)/1000;
    disp(['Time from PIC: ', num2str(time), ' s']);

    for j=1:3;
        for i=1:N
            posArray2(j,i) = (((posArray2(j,i)-VAL_ANGLE_MIN)*ANGLE_RANGE)/VAL_ANGLE_RANGE)+ANGLE_MIN;
            
            % TENTAR diminuir o erro
            if (posArray2(j,i)==-150)
                if (i-1 ~= 0)
                    posArray2(j,i) = posArray2(j,i-1);
                end
            end
        end
    end
    posArray2 = posArray2(:,1:N);
    t2 = t2(1,1:N);
    Qvel2 = Qvel2(:,1:N);
    
    for i=1:N;
    	XYZ2(:,i) = DirKin3dof(posArray2(:,i)');
    end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

    figure
    hold on
    plot3(XYZi(1),XYZi(2),XYZi(3),'*r')
    plot3(XYZ(1,:),XYZ(2,:),XYZ(3,:),'.b')
    plot3(XYZf(1),XYZf(2),XYZf(3),'*r')
    %plot3(XYZi2(1),XYZi2(2),XYZi2(3),'.r')     % já representados
    plot3(XYZ2(1,:),XYZ2(2,:),XYZ2(3,:),'.g')
    %plot3(XYZf2(1),XYZf2(2),XYZf2(3),'.g')
    xlabel('pos X')
    ylabel('pos Y')
    zlabel('pos Z')
    grid on
    axis square
    hold off

    figure
	subplot(1,2,1)
    hold on
    plot(t',posArray','*',t2',posArray2','.')
    xlabel('Tempo (ms)')
    ylabel('Posição (º)')
	legend('Pos Cor Servo1-Ankle1','Pos Cor Servo2-Ankle2','Pos Cor Servo3-Knee','Pos Nor Servo1-Ankle1','Pos Nor Servo2-Ankle2','Pos Nor Servo3-Knee')
    axis([0 max(t) -100 100])
    grid on
	hold off

	subplot(1,2,2)
    hold on
    plot(t',Qvel','*',t2',Qvel2','.')
    xlabel('Tempo (ms)')
    ylabel('Qvel (%)')
	legend('Qvel Cor Servo1-Ankle1','Qvel Cor Servo2-Ankle2','Qvel Cor Servo3-Knee','Qvel Nor Servo1-Ankle1','Qvel Nor Servo2-Ankle2','Qvel Nor Servo3-Knee')
    axis([0 max(t) 0 30])
    grid on
%    legend('Real Servo1 - Ankle1','Real Servo2 - Ankle2','Real Servo3 - Knee','Imag Servo1 - Ankle1','Imag Servo2 - Ankle2','Imag Servo3 - Knee')
    hold off

end

end