
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Motor de Corrente Continua
%%%%%%%%%%%%%
% Modelagem e resposta dinamica do motor CC
clear all 
close all 
clc
c=2;K=1/2;Jc=10;Jm=1;b=1.0;L=1;R=1;

JT = Jc+Jm*(c^2);

A = [-R/L -K*c/L; K*c/JT -b/JT];
B = [1/L 0]';
C = [0 1];

sys = ss(A,B,C,0);
[y,t] = step(100*sys);
plot(t,y)

%%%%%%%%%%%%%
% Projeto do regulador K para alocacao dos polos em malha fechada em posicoes
% especificas e de M para obter erro nulo para a entrada degrau

% Forma controlavel
clear all 
clc
A = [0 1;-0.1429 -1.0714];
B = [0;1];
C = [0.07143 0]; 

% Item a % Polos em MF em -0.25
K1 = acker(A,B,[-0.25,-0.25]);

S = tf(ss(A-B*K1,B,C,0));

S0 = 0.07143/0.0625;
m1 = 1/(S0*K1(1));

M1 = [m1;0];

F1 = S*K1*M1;

% Item b % Polos em MF em -0.5
K2 = acker(A,B,[-0.5,-0.5]);

S = tf(ss(A-B*K2,B,C,0));

S0 = 0.07143/0.25;
m2 = 1/(S0*K2(1));

M2 = [m2;0];

F2 = S*K2*M2;

% Item c % Polos em MF em -2
K3 = acker(A,B,[-2,-2]);

S = tf(ss(A-B*K3,B,C,0));

S0 = 0.07143/4;
m3 = 1/(S0*K3(1));

M3 = [m3;0];

F3 = S*K3*M3;
step(50*F1,50*F2,50*F3)

% Determinacao dos esforcos de controle correspondentes

Fu1 = K1*M1-tf(ss(A-B*K1,B*K1*M1,K1,0));
Fu2 = K2*M2-tf(ss(A-B*K2,B*K2*M2,K2,0));
Fu3 = K3*M3-tf(ss(A-B*K3,B*K3*M3,K3,0));

figure(2)
step(50*Fu1,50*Fu2,50*Fu3)

%% Para erro nulo para entrada rampa (somente item b)
s = tf('s');
tau = 6; % Já calculado através de dF(0)/ds = 0;
h2 = K2*M2*((tau*s+1)/(2*s+1));
F2N = tf(ss(A-B*K2,B,C,0))*h2;

t = 0:0.01:30;
y2Nd = step(50*F2N,t);
y2Nr = step(F2N/s,t);
y2d = step(50*F2,t);
y2r = step(F2/s,t);

figure(3)
plot(t,y2d,t,y2Nd)

figure(4)
plot(t,y2r,t,y2Nr)


%%%%%%%%%%%%% 
% Projeto do Regulador Linear Quadratico para o Motor CC considerando Q = I
% e rho = {2,10,50} e de M para erro nulo para entrada degrau unitario
clear all 
close all
clc
A = [0 1;-0.1429 -1.0714];
B = [0;1];
C = [0.07143 0]; 
D = 0;
Q = eye(2);

rho = 2;
[K1,P,E] = lqr(A,B,Q,rho);
S = tf(ss(A-B*K1,B,C,0));
S0 = 0.07143/0.7214;
M1 = (1/S0)*K1'/(K1*K1');
F1 = tf(ss(A-B*K1,B*K1*M1,C,0))

rho = 10;
[K2,P,E] = lqr(A,B,Q,rho);
S = tf(ss(A-B*K2,B,C,0));
S0 = 0.07143/0.347;
M2 = (1/S0)*K2'/(K2*K2');
F2 = tf(ss(A-B*K2,B*K2*M2,C,0))

rho = 50;
[K3,P,E] = lqr(A,B,Q,rho);
S = tf(ss(A-B*K3,B,C,0));
S0 = 0.07143/0.201;
M3 = (1/S0)*K3'/(K3*K3');
F3 = tf(ss(A-B*K3,B*K3*M3,C,0))
figure(1)
step(50*F1,50*F2,50*F3)

% Determinacao do esforco de controle correspondente

Fu1 = K1*M1-tf(ss(A-B*K1,B*K1*M1,K1,0));
Fu2 = K2*M2-tf(ss(A-B*K2,B*K2*M2,K2,0));
Fu3 = K3*M3-tf(ss(A-B*K3,B*K3*M3,K3,0));

figure(2)
step(50*Fu1,50*Fu2,50*Fu3)

%%%%%%%%%%%%% 
% Projeto do Regulador Linear Quadratico utilizando lugar das raizes
% simetrico. 
% Especificacoes: Menor tempo de estabilizacao e sem oscilacao
clear all 
close all
clc
A = [0 1;-0.1429 -1.0714];
B = [0;1];
C = [0.07143 0]; 
D = 0;

V = C; 
phi = tf(ss(A,B,V,0));
sisotool(phi'*phi)
rho = 0.0311;

s = tf('s');
P = (s+0.66)*(s+0.66);
K = [0.2927 0.2486] 

[K,P,E] = lqr(A,B,V'*V,0.0311);

%%%
V = [1 0];
phi = tf(ss(A,B,V,0));
sisotool(phi'*phi)
rho = 1/0.16538;

s = tf('s');
P = (s+0.66)*(s+0.66);
K = [0.2927 0.2486] 

[K,P,E] = lqr(A,B,V'*V,1/0.16538);
%%%%%%%%%%%%% 
% Projeto de observador considerando que a carga do motor esta engastada em
% um ponto fixo atraves de um eixo flexivel com constante elastica kappa
% Novo modelo
clear all 
close all
clc 
c=2;K=1/2;Jc=10;Jm=1;b=1.0;L=1;R=1;kappa=2;

JT = Jc+Jm*(c^2);

A = [-R/L 0 -K*c/L; 0 0 1; K*c/JT -kappa/JT -b/JT];
B = [1/L 0 0]';
C = [0 0 1];
D = 0;
x0 = [1;pi/3;0];

sysG = ss(A,B,C,D)

% Verificacao de observabilidade
Obv = obsv(A,C);
det(Obv)

% Projeto do observador levando em conta que -0.9 e o polo mais rapido do
% sistema. Logo, alocou-se os polos do observador em -3.6 (4x mais rapido que -0.9)
po = -2;
L1 = acker(A',C',[po po po]);
L1 = L1';

% Definindo um sistema aumentado com xa = [x;u;xo];
Aa=[A B zeros(3,3); zeros(1,7); L1*C B A-L1*C];
Ba=[x0; pi/3; zeros(3,1)];
Ca=[eye(3) zeros(3,4); zeros(3,4) eye(3)];
Da=zeros(6,1);

Ga=ss(Aa,Ba,Ca,Da);
Tsim=0:0.1:50;
ya=impulse(Ga,Tsim);

figure(1)
plot(Tsim,(180/pi)*ya(:,2),Tsim,(180/pi)*ya(:,5),'--r')

% Projeto do observador, considerando que os sinais de controle e de saida
% sao afetados por ruidos brancos com as seguintes caracteristicas
U = 1e-1*B;
mu = (1e-2)^2;
[Lric,R,E] = lqr(A',C',U*U',mu);
Lric = Lric';

% Programa Simulink - Observador 

Lsim = Lric;
sim('proj_observador',[0 50])

figure(4)
plot(theta(:,1),theta(:,2),'--r',theta(:,1),theta(:,3),'-b')
axis([0 50 -20 60])

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Pendulo invertido
%%%%%%%%%%%%%
close all 
clear all 
clc
M = 10;
m = 2;
ell = 1;
g  =9.8;
A = [0 1 0 0;0 0 (m/M)*g 0; 0 0 0 1; 0 0 (M+m)*g/(M*ell) 0];
B = [0;1/M;0;1/(M*ell)];
C = [0 0 1 0];
K = -[10 50 -300 -100]; % F = -K*x
x0 = [0;0;(pi/4)-(pi/2);0];
sys = ss(A-B*K,x0,[0 0 1 0],0);
t = 0:0.01:15;
[theta,t] = impulse(sys,t);

sys = ss(A-B*K,x0,[1 0 0 0],0);
[xp,t] = impulse(sys,t);

% subplot(1,2,1)
% plot(t,(theta+pi/2)*180/pi)
% subplot(1,2,2)
% plot(t,xp)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Projeto de servomecanismo, saida medida: y = x1-x3;
C = [1 0 -1 0];
D = 0;
x0=[0 0 -pi/4 0]';

Q = [1 0 0 0;0 0 0 0;0 0 1 0;0 0 0 0]
rho  = 1e-2;
[K,P,E] = lqr(A,B,Q,rho); 

% Para o observador considerou-se
U = 1e-1*eye(4);
mu = 1e-4;
[Lric,R,E] = lqr(A',C',U*U',mu);
Lric = Lric';

% % Determinacao das funcoes de transferencia
% Cu = tf(ss(A-Lric*C,B-Lric*D,K,0));
% Cy = tf(ss(A-Lric*C,Lric,K,0)); 
% G = tf(ss(A,B,C,0));
% 
% [Nu,Du] = tfdata(Cu,'v');
% [Ny,Dy] = tfdata(Cy,'v');
% [Ng,Dg] = tfdata(G,'v');
% %%%

Aa=[A-B*K B*K; zeros(size(A)) A-Lric*C];
Ha=[x0; x0];
Ca=[[1 0 0 0] zeros(1,4); [0 0 1 0] zeros(1,4)];
Da=[0 0]';

GGa=ss(Aa,Ha,Ca,Da)

GGk=ss(A-B*K,x0,[1 0 0 0; 0 0 1 0],0);

Tsim=0:0.05:15;
ya=impulse(GGa,Tsim);
yk=impulse(GGk,Tsim);
%% 
figure(1)
plot(Tsim,ya(:,1),'--r',Tsim,yk(:,1),'-b')

figure(2)
plot(Tsim,(180/pi)*(ya(:,2)+pi/2),'--r',Tsim,(180/pi)*(yk(:,2)+pi/2),'-b')
%% 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%% SISTEMAS A TEMPO DISCRETO
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Motor de Corrente Continua
%%%%%%%%%%%%%
clear all 
close all 
clc
c=2;K=1/2;Jc=10;Jm=1;b=1.0;L=1;R=1;

JT = Jc+Jm*(c^2);

A = [-R/L -K*c/L; K*c/JT -b/JT];
B = [1/L 0]';
C = [0 1];
D = 0;
G = ss(A,B,C,0);

%%% Discretizacao
n = size(A,1);
T = 1;
Aa = [A B;zeros(1,n+1)];
Ada = expm(Aa*T); 

Ad = Ada(1:n,1:n);
Bd = Ada(1:n,n+1:end);
Gd = tf(ss(Ad,Bd,C,D,T))

%%%%%%%%%%%%%
% Projeto do regulador K para alocacao dos polos em malha fechada em posicoes
% especificas e de M para obter erro nulo para a entrada degrau

% Forma Canonica Controlavel
Ad = [0 1;-0.3425 1.256];
Bd = [0; 1];
Cd = [0.01785 0.02547];
Dd = 0;

% Item a % Polos em MF em -0.25 (tempo continuo)
K1 = acker(Ad,Bd,[exp(-0.25*T),exp(-0.25*T)]);

S = tf(ss(Ad-Bd*K1,Bd,Cd,0,T));

S1 = 0.8932;
m1 = 1/(S1*K1(1));

M1 = [m1;0];

F1 = S*K1*M1;
step(50*F1)

% Item c % Polos em MF em -2 (tempo continuo)
K2 = acker(Ad,Bd,[exp(-2*T),exp(-2*T)]);

S = tf(ss(Ad-Bd*K2,Bd,Cd,0,T));

S2 = 0.0579;
m1 = 1/(S2*K2(1));

M2 = [m1;0];

F2 = S*K2*M2;
figure(1)
step(50*F1,50*F2)

% Determinacao do esforco de controle correspondente

Fu1 = K1*M1-tf(ss(Ad-Bd*K1,Bd*K1*M1,K1,0,T));
Fu2 = K2*M2-tf(ss(Ad-Bd*K2,Bd*K2*M2,K2,0,T));
figure(2)
step(50*Fu1,50*Fu2)

%%%%%%%%%%%%% 
% Projeto do Regulador Linear Quadratico para o Motor CC considerando Q = I
% e rho = {2,50} e de M para erro nulo para entrada degrau unitario
clear all 
clc
Ad = [0 1;-0.3425 1.256];
Bd = [0; 1];
Cd = [0.01785 0.02547];
Dd = 0;
T = 1;
Q = eye(2);

%%%
rho = 2;
sys = ss(Ad,Bd,Cd,Dd,T);
[K1,S,E] = lqr(sys,Q,rho);

S = tf(ss(Ad-Bd*K1,Bd,Cd,0,T));
S1 = 0.0730;
m1 = 1/(S1*K1(1));

M1 = [m1;0];

F1 = S*K1*M1;

%%%%
rho = 50;
[K2,S,E] = dlqr(Ad,Bd,Q,rho);
S = tf(ss(Ad-Bd*K2,Bd,Cd,0,T));

S2 = 0.2192;
m1 = 1/(S2*K2(1));

M2 = [m1;0];

F2 = S*K2*M2;
figure(1)
step(50*F1,50*F2)

% Determinacao do esforco de controle correspondente
Fu1 = K1*M1-tf(ss(Ad-Bd*K1,Bd*K1*M1,K1,0,T));
Fu2 = K2*M2-tf(ss(Ad-Bd*K2,Bd*K2*M2,K2,0,T));
figure(2)
step(50*Fu1,50*Fu2)

%%%%%%%%%%%%% 
% Projeto do Regulador Linear Quadratico utilizando lugar das raizes
% simetrico. 
% Especificacoes: Estavel e sem oscilacao
clear all 
clc
Ad = [0 1;-0.3425 1.256];
Bd = [0; 1];
Cd = [0.01785 0.02547];
Dd = 0;
T = 1;

V = [0 1];
phi = tf(ss(Ad,Bd,V,Dd,T));

sisotool(phi*phi');

rho = 1/0.065488;
z = tf('z',1);
Pz = (z-0.5113)*(z-0.5113);
K1t = [0.2614-0.3425 -1.023+1.256]

[Kl,S,E] = dlqr(Ad,Bd,V'*V,rho);

%%%%%%%
V = Cd;
phi = tf(ss(Ad,Bd,Cd,Dd,T));

%sisotool(phi*phi');

rho = 1/31.459;
z = tf('z',1);
Pz = (z-0.521)*(z-0.521);
K2t = [0.2714-0.3425 -1.042+1.256]
[K2,S,E] = dlqr(Ad,Bd,V'*V,rho);

%%%%%%%%%%%%% 
% Projeto de observador considerando que a carga do motor esta engastada em
% um ponto fixo atraves de um eixo flexivel com constante elastica kappa
% Novo modelo a tempo continuo com variaveis de estado x = [i;theta;dtheta]
clear all 
close all
clc 
c=2;K=1/2;Jc=10;Jm=1;b=1.0;L=1;R=1;kappa=2;

JT = Jc+Jm*(c^2);

A = [-R/L 0 -K*c/L; 0 0 1; K*c/JT -kappa/JT -b/JT];
B = [1/L 0 0]';
C = [0 0 1];
D = 0;
x0 = [1;pi/3;0];

sysG = ss(A,B,C,D)

%%% Discretizacao
n = size(A,1);
T = 1;
Aa = [A B;zeros(1,n+1)];
Ada = expm(Aa); 

Ad = Ada(1:n,1:n);
Bd = Ada(1:n,n+1:end);
Gd = tf(ss(Ad,Bd,C,D,T))


% Verificacao de observabilidade
Obv = obsv(Ad,C);
det(Obv)

% Projeto do observador levando em conta que -0.9 e o polo mais rapido do
% sistema. Logo, alocou-se os polos do observador em -2 (tempo continuo)
po = exp(-2*T);
L1 = acker(Ad',C',[po po po]);
L1 = L1';

% Projeto do observador, considerando que os sinais de controle e de saida
% sao afetados por ruidos brancos com as seguintes caracteristicas
U = 1e-1*B;
mu = (1e-2)^2;
[Lric,R,E] = dlqr(Ad',C',U*U',mu);
Lric = Lric';

% Programa Simulink - Observador 

Lsim = Lric;
sim('proj_observador_discreto',[0 50])

figure(2)
stairs(theta(:,1),theta(:,2),'--r')
hold on 
stairs(theta(:,1),theta(:,3),'-b')
axis([0 50 -20 60])


%%%%%%%%%%%%%%%%%%%%%%


% Massa Mola 
k = 0.002;
m = 2;
b = 0.01;
A = [0 1;-k/m -b/m];
B = [0;1];
C = [1 0];

sys = ss(A,B,C,0);
impulse(sys)

% Pendulo
clear all 
close all 
clc
g = 9.8;
ell = 1;
b = 1;
m = 0.5;

A = [0 1;-sqrt(2)*g/(2*ell) -b/m];
B = [0 1]';
C = [1 0];
sys = ss(A,B,C,0);
impulse((180/pi)*sys)

