% Calibração Inicial
     factorLon = 1.6812;
     factorLat = -1.6812;
    
    offset_1 = [15 30 10];
    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 do plano variável
% 
%     c = 1;      % comprimento %  
%     l = 0.3;    % largura     %% centimetros
%     [xc, yc, zc] = cylinder(0.2, 1000);
%     zc = [zc(1,:)-l; zc(2,:)*l];
%     color = [zeros(1,length(xc)); ones(1,length(xc))];
%     R = makehgtform('xrotate', pi/2);
% 
% % inicialização de variaveis e constantes
% 
%     defs = defineRob;
%     XYZRef = [-c/2 0 0];
%     perna = deg2rad([pos_init_slave1 pos_init_slave3(1)]);    % Criação de vector de posição inicial das juntas (tronco, anca, joelho, pé)   
% 
%     
% % Criação da zona de construcção dos plots
% 
%     [AZ, EL] = view(3);
%     f_sim = figure(1);
%     set(f_sim,  'Units', 'normalized',...
%                 'Position', [0.55 0.5 0.45 0.45],...
%                 'Name', 'Teste Inclinómetros',...
%                 'DoubleBuffer', 'on',...
%                 'Renderer', 'OpenGl');
%     
% % criação e animação do robo
% 
%     % robo virtual
%        hg = hgtransform;
%        hc = hgtransform;
%        b = patch(2*[0 -c -c 0], 2*[-l/2 -l/2 l/2 l/2], 2*[0 0 0 0] , 'g', 'Parent', get(hg, 'Parent'));
%        a = patch([0 -c -c 0], [-l/2 -l/2 l/2 l/2], [0 0 0 0] , 'c', 'Parent', hg);
%        Ai = CinDir4dof(perna, defs.L, XYZRef);
%        [P, C, F, E] = CoordJuntas(Ai, XYZRef, defs.L, defs.O);
%        DesenharRoboPZ(P, C, F, E, hg)
%        s = surface([], [], [], [], 'Parent', hc);
%        set(hc, 'Matrix', R);       
%        axis('auto'), axis('off'), axis('equal')
%        t = text('Position', [-c/2, 0, 1], 'Parent', hc);
       
      

% inicio


while(1)

    clc
    
    servo(1:3) = ler_pos(H,1,offset_1);
    servo(4:6) = ler_pos(H,3,offset_3);
    perna = deg2rad(round(servo(1:4)));

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

    teta = ler_inclinometro(H,3,factorLon);
    tronco_long = pi/2-((pi/2-perna(2))+perna(3)-perna(4));
    beta = teta - (tronco_long);

    teta = ler_inclinometro(H,1,factorLat);
    zeta = teta; % - (pi/2-((pi/2-perna(2))+perna(3)+perna(4)));
    
    fprintf('Tronco = %d\n', round(rad2deg(tronco_long)));
    fprintf('longitudinal = %d\nlateral = %d\n', round(rad2deg([beta; zeta])));

    pause(1)
%     drawnow;
end

% FIM