%
% SEM 536 - Sistemas de Controle I
% Escola de Engenharia de São Carlos - USP
%
% Adriano Siqueira - 2020
%

clear all
close all

Kt = 0.00767;
Km = 0.00767;
Rm = 2.6;
Kg = 70;
Jm = 4.6e-7;
Jeq = 2.13e-3;
Beq = 0.004;
etag = 0.9;
etam = 0.69;

num = [(etag*etam*Kt*Kg)/(Jeq*Rm)];
den = [1  (Beq*Rm+etag*etam*Km*Kt*Kg^2)/(Jeq*Rm) 0];

G = tf(num,den);

T = tf(2500,[1 60 2500]);

figure
[y,t] = step(T,2);
plot(t,y)
hold on

%Controle - Malha Aberta

Cma = tf([2500 85500 0],[60.2 3612 150500]);
Tma = Cma*G;
[y1,t] = step(Tma,2);
u = [ zeros(length(t)/2,1); ones(length(t)/2,1)];
[y2,t]=lsim(G,u,t);
y = y1+y2;
plot(t,y)


%Controle - Malha Fechada Baseada no Modelo

Cmf = tf([2500 85500 0],[60.2 3612 0]);
Tmf = feedback(Cmf*G,1);
[y1,t] = step(Tmf,2);
u = [ zeros(length(t)/2,1); ones(length(t)/2,1)];
Tmfd = feedback(G,Cmf);
[y2,t]=lsim(Tmfd,u,t);
y = y1+y2;
plot(t,y)

%Controle - Malha Fechada - Proporcional

Cp = 41.53;
Tp = feedback(Cp*G,1);
[y1,t] = step(Tp,2);
u = [ zeros(length(t)/2,1); ones(length(t)/2,1)];
Tpd = feedback(G,Cp);
[y2,t]=lsim(Tpd,u,t);
y = y1+y2;
plot(t,y)

%Controle - Malha Fechada - Proporcional-Derivativo

Cpd = tf([0.428 41.53],[1]);
Tpd = feedback(Cpd*G,1);
[y1,t] = step(Tpd,2);
u = [ zeros(length(t)/2,1); ones(length(t)/2,1)];
Tpdd = feedback(G,Cpd);
[y2,t]=lsim(Tpdd,u,t);
y = y1+y2;
plot(t,y)

%Controle - Malha Fechada - Proporcional-Integral-Derivativo

Cpid = tf([0.428 41.53 100],[1 0]);
Tpid = feedback(Cpid*G,1);
[y1,t] = step(Tpid,2);
u = [ zeros(length(t)/2,1); ones(length(t)/2,1)];
Tpidd = feedback(G,Cpid);
[y2,t]=lsim(Tpidd,u,t);
y = y1+y2;
plot(t,y)

ylabel('posição (rad)')
xlabel('tempo (s)')