% **********************************************************
% * Universidade do Estado do Rio de Janeiro               *
% * Faculdade de Engenharia                                *
% * Programa de Pos Graduacao em Engenharia Eletronica     *
% * Disciplina: Sistemas Lineares                          *
% * Professor: Jose Paulo V. S. da Cunha                   *
% * Trabalho 1 - Controle de um Pendulo Invertido          *
% * Alunos: Elaine de M. Silva e Richard Henrique Antunes  *
% **********************************************************


% variaveis do modelo
jm=3.87e-7;                       % inercia armadura
mc=0.45;                          % massa do carrinho
kg=3.7;                           % relacao engrenagens 
g=9.8;                            % cte. acel. gravidade
mp=0.21;                          % massa pendulo
r=0.00635;                        % raio engrenagem do motor
Mtc=((jm*(kg^2))/(r^2))+ mc;      % massa total (massa carrinho + gerada pelo motor)
km=0.00767;                       % constante torque do motor
Ra=2.6;                           % resistencia armadura
l=0.61/2;                         % comprimento pendulo
I=(1/3)*mp*(l^2);                 % inercia pendulo




% variaveis definidas para melhorar visualizacao da equacao de estado
d = (Mtc+mp)*I+ Mtc*mp*(l^2);     % denominador comum 
n1= ((mp*l*g)*(Mtc+mp));          % relaciona acel. pendulo ao angulo pendulo
n2= -((mp^2)*(l^2)*g);            % relaciona acel. carrinho ao angulo pendulo
n3= (mp*l*(km^2)*kg);             % relaciona acel. pendulo a vel.  carrinho 
n4= - (I+mp*(l^2))*(km^2)*kg;     % relaciona acel. carrinho a vel. carrinho 
n5= -(mp*l*km*kg);
n6= ((km*kg)*(I+mp*l^2));


% matrizes da equacao de estado
A = [0     0  1  0; 
     0     0  0  1;
    (n1/d) 0  0  (n3/d*Ra*r^2);
    (n2/d) 0  0  n4/d*Ra*r^2];

B=[ 0;
    0;
    n5/(d*Ra*r);
    n6/(d*Ra*r)];

 
%  Bf=[ 0;
%       0;
%       (n5*(0.068*180/3.14)/d*Ra*r);
%       n6*(0.00107)/d*Ra*r]

C = [1 0 0 0;     % relaciona ang. pendulo a saida y1
     0 1 0 0];    % relaciona pos. carrinho a saida y2

D = [0
     0];



% Tentando fazer com que a saida y2 (pos. carro) siga uma entrada. Para
% calcular ganho de alimentacao direta p eh necessario saber se a funcao de
% transf. da planta possui zeros em s = 0. 

% Verificando se a funcao de transferencia do carro possui zeros em s = 0
[NUM1,DEN1] = ss2tf(A,B,C,D);
num1 = NUM1(1,:);
num2 = NUM1(2,:);
[zp,pp,kp] = tf2zp(num1,DEN1);
[zc,pc,kc] = tf2zp(num2,DEN1);
for cont1=1:size(zc)
    if (zc(cont1) ~= 0)
        cont2 = cont1;
    end
end
if cont2 == cont1
    disp('A funcao de transferencia do carro nao possui zeros em s = 0')
else
    disp('A funcao de transferencia do carro possui zeros em s = 0. Nao e possivel fazer rastreamento')
end


% Montando nova funcao de transferencia por realimentacao e alocacao de
% polos

P = [-2.5 + 1.65*j  -2.5 -  1.65*j -10  -9]';

% alocando novos polos
K = place(A,B,P);

Ac=[(A-B*K)];
Bc = [B];
Cc = [C];
Dc = [D];

[NUM2,DEN2] = ss2tf(Ac,Bc,Cc,Dc);
num3 = NUM2(1,:)
num4 = NUM2(2,:)
den2 = DEN2

% ganho p = /alfa5 / beta5

p = den2(1,5)/num4(1,5)
x0=[0;0;0;0];  % condicoes iniciais
sys = ss(Ac,Bc*p,Cc,Dc);
% vetores de tempo e degrau
T = 0:0.01:5;
U = 0.3*ones(size(T));

% Exibe autovalores do controlador 
disp('Autovalores do controlador:')
eig(Ac)

% Alocando polos do observador que sao 10 vezes mais rapidos que o polo
% mais lento do controlador 
Po=[-25 -26 -27 -28]
L = place(A',C',Po)'

Aco = [A-B*K        B*K;
    zeros(size(A))  (A-L*C)];
Bco = [ B*p;
    zeros(size(B))];
Cco = [Cc zeros(size(Cc))];
Dco = [0;0];

obs_ss = ss(Aco, Bco, Cco, Dco);


% Resposta ao degrau (30 cm)
[Y,T1,X] = lsim(obs_ss,U,T);
Y1 = [1 0]*Y';
Y2 = [0 1]*Y';

% Sinal de controle 
u1 = p*U -K*X';

subplot(3,1,1); plot(T,Y1)
grid
title('Resp. degrau 0.30m - ang. pend. OBS')
xlabel('t(s)')
ylabel('x1')
subplot(3,1,2); plot(T,Y2)
grid
title('Resp. degrau 0.30m - pos. carr. OBS')
xlabel('t(s)')
ylabel('x2')
subplot(3,1,3); plot(T,u1)
grid
title('Sinal controle')
xlabel('t(s)')
ylabel('u1')
