% Esta função permite redefinir o centro de pressão referencia de uma forma
% interactiva!

function copref_exe(H,scuid,Kone)

% Velocidade
speed=50;

% Ganhos de compensacão do controlador de equilibrio
Kp=Kone*[1 1 1];

% Centro de pressão referencia inicial
CoP=[0 0 10];

% Características do pé
footdata;

% Desactivação dos PWMs
[error,errorstr]=killuc(H,scuid);
if (error==1), return, end

% Definição de velocidade dos servos
applycontrol(H,scuid,3,[0 0 0]);
applyjoint(H,scuid,1,[speed speed speed]);

% Activação dos PWMs
[error,errorstr]=inituc(H,[scuid scuid+2]);
if (error==1), return, end

% Esperar que a activação dos PWMs seja feito
disp 'Activando PWMs...'
[stat,error,errorstr]=waitpwmon(H,scuid);
if (stat==0), return, end
disp 'PWMs activados!'

% Reset dos ganhos de compensação do controlador de equilíbrio
applycontrol(H,scuid,2,[0 0 0]);
% Activação do controlador de equilíbrio
applycontrol(H,scuid,3,[3 3 3]);
% Ajuste do centro de pressão referencia inicial
applyjoint(H,scuid,0,[0 0 0]);
% Ajuste do periodo de trajectoria
applyjoint(H,scuid,1,[0 0 0]);

disp 'Pressione ENTER para activar controlador...'
pause
% Ajuste dos ganhos de compensação do controlo de equilíbrio
applycontrol(H,scuid,2,Kp);
disp 'Done!'

close all
figure('doublebuffer','on')
while (1)
    
    % Ajuste do CoP referencia
    applyjoint(H,scuid,0,CoP);
    
    % Desenho do pé
    drawfoot(CoP/10);

    hold on
    exitbox=[footW      footL/2*0.8;
             footW*1.5  footL/2];
    plot([exitbox(1,1) exitbox(2,1) exitbox(2,1) exitbox(1,1) exitbox(1,1)],[exitbox(1,2) exitbox(1,2) exitbox(2,2) exitbox(2,2) exitbox(1,2)],'k')
    text(exitbox(1,1)*1.15,exitbox(2,2)*0.90,'EXIT');
    hold off
    
    % Introduzir Nova referencia
    [x,y]=ginput(1);
    
    if (x>=-footW/2 && x<=footW/2 && y>=-footL/2 && y<=footL/2)
        CoP(1:2)=[x y]*10
    elseif (x>=exitbox(1,1) && x<=exitbox(2,1) && y>=exitbox(1,2) && y<=exitbox(2,2))
        break
    else
        ans=str2num(char(inputdlg('Altura da anca:','Input',1,{num2str(CoP(3))})));
        if (~isempty(ans)), CoP(3)=ans; end
    end
end

% Reset dos ganhos de compensação
applycontrol(H,scuid,2,[0 0 0]);

% Ajuste do centro de pressão referencia inicial
applyjoint(H,scuid,0,[0 0 0]);

% Ajuste do periodo de trajectoria
applyjoint(H,scuid,1,[0 0 0]);

% Desactivação do controlador
applycontrol(H,scuid,3,[0 0 0]);

% Esperando reposição a posição inicial
pause(speed/50*2);

% Desactivação dos PWMs
[error,errorstr]=killuc(H,[scuid scuid+2]);
if (error==1), return, end

close all
