function test_incl(H)


close all
% clear all
clc
    
% Calibração Inicial
     factorLon = 1.6878;
     factorLat = 1.3759;
    
    offset_1 = [15 30 50];
    offset_3 = [0 0 0];
    if(~(exist('factorLat') & exist('factorLon')))
        [factorLat, factorLon] = calibra(H,3)
    end
    pos_init_slave1 = ler_pos(H,1,offset_1);
    pos_init_slave3 = ler_pos(H,3,offset_3);
    
    
% definições dos planos de superficie

    size(1) = 60;   % comprimento
    size(2) = size(1)/2;   % largura
    
    d = size(2)/2;
    [xc, yc, zc] = cylinder(d, 1000);
    zc = [zc(1,:)-d; zc(2,:)*d];
    color = [zeros(1,length(xc)); ones(1,length(xc))];
    
    Rx = makehgtform('xrotate', pi/2);
    Ry = makehgtform('yrotate', -pi/2, 'zrotate', pi);
    
    
% inicialização de variaveis e constantes

    defs = defineRob;
    XYZRef = [0 0 0];
    perna = deg2rad([0 0 0 0]);
    
% Criação da zona de construcção dos plots

    [AZ, EL] = view(3);
    set(gcf,'Units', 'normalized',...
            'Position', [0.5271    0.5156    0.4729    0.3989],...
            'Name', 'Teste Inclinómetros',...
            'DoubleBuffer', 'on',...
            'Renderer', 'OpenGl');

                
% criação e animação do robo

    % robo virtual
        Ai = CinDir4dof(perna, defs.L, XYZRef);
        [P, C, F, E] = CoordJuntas(Ai, XYZRef, defs.L, defs.O);

        planoLonH = hgtransform;                         % estrutura do plano longitudinal, suporta o plano transversal
        planoTraH = hgtransform('Parent', planoLonH);    % estrutura do plano transversal, suporta a perna
        cilinLonH = hgtransform('Parent', gca);          % estrutura do cilindro representativo da inclinação longitudinal
        cilinTraH = hgtransform('Parent', planoLonH);    % estrutura do cilindro representativo da inclinação transversal

        S(:,:,1) = sup(2*size, [-size(1) -size(2) 0]);  % plano principal
        S(:,:,2) = sup(size, [-size(1)/2 -size(2)/2 0]);    % plano longitudinal
        S(:,:,3) = sup(size, [-size(1)/2 -size(2)/2 0]);    % plano transversal

        [Sh, Hh, Eh, Jh, Fh, hr] = DesenharRoboPZ(S, P, C, F, E);
        
        set(Sh(2), 'Parent', planoLonH);
        set(Sh(3), 'Parent', planoTraH);
        set(hr, 'Parent', planoTraH);

        sx = surface([], [], [], [], 'Parent', cilinLonH);
        sy = surface([], [], [], [], 'Parent', cilinTraH);

        set(cilinLonH, 'Matrix', Rx);
        set(cilinTraH, 'Matrix', Ry);

        axis('auto'), axis('off'), axis('equal')

        t1 = text(-2*size(1), 0, 70, '');
        t2 = text(-2*size(1), 0, 50, '');

        text1 = {'Inclinação do tronco:',   'Longitudinal: ',...
                                            'Transversal: '};
        text2 = {'Inclinação do plano:',    'Longitudinal: ',...
                                            'Transversal: '};

        set(t1, 'String', text1);
        set(t2, 'String', text2);
        
        servo(1:3) = ler_pos(H,1,offset_1);
        servo(4:6) = ler_pos(H,3,offset_3);
        perna = deg2rad(servo(1:4));
    
        c = size(1)/2;
        l = size(2)/2;
        app1=offset_1;
        app1(3) = app1(3)-70;
        app3=offset_3;
        
    while(1)

        servo(1:3) = ler_pos(H,1,offset_1);
        servo(4:6) = ler_pos(H,3,offset_3);
        perna = deg2rad(servo(1:4));
        Ai = CinDir4dof(perna, defs.L, XYZRef);
        [P, C, F, E] = CoordJuntas(Ai, XYZRef, defs.L, defs.O);
        ActualizaRobo(Hh, Eh, Jh, Fh, hr, P, C, F, E);
        
            
        
        tronco_long = -perna(2)+perna(3)+perna(4);
        beta1 = ler_inclinometro(H,3,factorLon);
        beta = beta1 - (tronco_long);
        zeta1 = ler_inclinometro(H,1,factorLat);
        zeta = zeta1 + perna(1);
        
        text1 = {'Inclinação do tronco:',   ['Longitudinal: ', num2str(rad2deg(tronco_long))],...
                                            ['Transversal: ', num2str(rad2deg(perna(1)))]};
        text2 = {'Inclinação do plano:',    ['Longitudinal: ', num2str(rad2deg(beta))],...
                                            ['Transversal: ', num2str(rad2deg(zeta))]};
        
        set(t1, 'String', text1);
        set(t2, 'String', text2);
        
        trans_z = c*sin(abs(beta));
        trans_x = sign(beta)*(c-c*cos(beta));
        gama_x(1) = cos(pi-abs(beta))*d;
        j = ((xc(1,:)<=gama_x(1)) & (yc(1,:)>=0));
        switch sign(beta)
            case 1,
                zrot = makehgtform('yrotate', 0, 'translate', 2*l, 0, 0);
            case -1,
                zrot = makehgtform('yrotate', pi, 'translate', 2*l, 0, 0);
            otherwise
                zrot = makehgtform('yrotate', 0, 'translate', 2*l, 0, 0);
        end

        x = xc(:,j); y = yc(:,j); z = zc(:,j); col = color(:,j);
        if(length(x)>3)
            set(sx, 'xdata', x, 'ydata', y, 'zData', z, 'cData', col)
        else
            set(sx, 'xdata', [], 'ydata', [], 'zData', [], 'cData', [])
        end
    
        Rbeta = makehgtform('translate', trans_x, 0, trans_z, 'yrotate', beta);
        set(planoLonH, 'Matrix', Rbeta)
        set(cilinLonH, 'Matrix', Rx*zrot)

        
        trans_z = l*sin(abs(zeta));
        trans_y = sign(zeta)*(l-l*cos(zeta));
        gama_x(1) = cos(pi-abs(zeta))*d;
        j = ((xc(1,:)<=gama_x(1)) & (yc(1,:)>=0));
    
        switch sign(zeta)
            case 1,
                zrot = makehgtform('xrotate', 0, 'zrotate', pi/2, 'translate', l, 0, 0);
            case -1,
                zrot = makehgtform('xrotate', pi, 'zrotate', pi/2, 'translate', l, 0, 0);
        end

        x = xc(:,j); y = yc(:,j); z = zc(:,j); col = color(:,j);
        
        if(length(x)>3)
            set(sy, 'xdata', x, 'ydata', y, 'zData', z, 'cData', col)
        else
            set(sy, 'xdata', [], 'ydata', [], 'zData', [], 'cData', [])
        end
        
        Rzeta = makehgtform('translate', 0, -trans_y, trans_z, 'xrotate', zeta);
        set(planoTraH, 'Matrix', Rzeta)
        set(cilinTraH, 'Matrix', Ry*zrot)

    
        [teta1, teta2] = jacontrol(beta1, zeta1)
        teta1 = rad2deg(teta1) - app1(1);
        teta2 = rad2deg(teta2) - app3(1);
        app1 = [teta1/2 0 0] + app1;
        app3 = [teta2/2 0 0] + app3;
            
        applyjoint(H,1,0,app1);
        applyjoint(H,3,0,app3);
 
        pause(.5);
        if(get(gcf, 'currentCharacter') == 's')
            break
        end

    end
    
    
%%-Funções auxiliares--------------------------------------------------

function S = sup( size,XYZ )

    dx = size(1)+XYZ(1);
    dy = size(2)+XYZ(2);

    S(:,:) = [  XYZ(1)    dx      dx      XYZ(1)
                XYZ(2)    XYZ(2)  dy      dy      
                0         0       0       0     ];
                  
return