%%
clear all
close all
clc
%% Init.
Mc = 700; %Massa de 1/4 do veiculo (kg)
mp = 20; %Massa da roda e do sistema de amortecimento (kg)

 kc = 60000; %Rigidez da mola do amortecedor (N/m)
 kp = 600000; %Rigidez do pneu (N/m)
 c = logspace(1,7,100); %Declaracao do vetor com os valores c que iremos testar
 stpData = zeros(2, length(c)); %Inicializacao da matriz onde registraremos os criterios de desempenho para cada c
 %c = 1e5
 %%
for i =1:length(c) %para cada c
     c2 = c(i); %Coef. de amortecimento do amortecedor (Ns/m)

     %Forma matricial Mx'' + Cx'+ Kx = B1
     K = [kc -kc; -kc kc+kp]; 
     C = [c2 -c2; -c2 c2];
     M = diag([Mc mp]);
     B1 = [0;kp];

     %Modelo de estados
     A = [zeros(2) eye(2);-M\K -M\C];
     B = [ 0;0;M\B1];
     %Funcao de transferencia
     G= tf(ss(A,B, [0 1 0 0], 0));
     
     aux= stepinfo(0.01*G); %a funcao stepinfo retorna uma estrutura com os criterios de desempenho
     stpData(1,i) = aux.SettlingTime;
     stpData(2,i) = aux.Overshoot;
end
%%
[~, i_e] = min(stpData(2,:)); %o til ignora o primeiro elemento de retorno
c_e = c(i_e);
c_e = 5000
%Forma matricial Mx'' + Cx'+ Kx = B1
K = [kc -kc; -kc kc+kp]; 
C = [c_e -c_e; -c_e c_e];
M = diag([Mc mp]);
B1 = [0;kp];

%Modelo de estados
A = [zeros(2) eye(2);-M\K -M\C];
B = [ 0;0;M\B1];
C = [0 1 0 0];
D = [0];

%Funcao de transferencia
G= tf(ss(A,B, C, D));

%Simulacao 
T = 2; %limite da simulacao
dt =0.0001; % passo da simulacao
t = 0:dt:T;
v = 2; %m/s
w = 0.15*sin(pi*v*t.*(t<=1/v)); % o operador .* faz a multiplicacao valor por valor
y = lsim(G,w,t); %simulacao
dy = [diff(y); 0]/dt; %derivada numerica da posicao
ddy = [diff(dy) ;0]/dt; %derivada numerica da velocidade
%% Saidas graficas
figure  
subplot(2,1,1)
loglog(c, stpData(1,:))
ylabel('Tempo de estabilizacao (s)')
xlabel('Coeficiente de Amortecimento (Ns/m)')

subplot(2,1,2)
loglog(c, stpData(2,:))
ylabel('Overshoot (%)')
xlabel('Coeficiente de Amortecimento (Ns/m)')

figure
subplot(2,2,1)
plot(t,w)
ylabel('Perturbacao (m)')
xlabel('Tempo (s)')

subplot(2,2,2)
plot(t,y)
ylabel('Amplitude (m)')
xlabel('Tempo (s)')

subplot(2,2,3)
plot(t,dy)
ylabel('Velocidade (m/s)')
xlabel('Tempo (s)')

subplot(2,2,4)
plot(t,ddy)
ylabel('Aceleracao (m/s^2)')
xlabel('Tempo (s)')
