%% ------------------------------------------------------------------------
%                     UNIVERSIDADE DE SÃO PAULO - (EESC - USP)
%                           Sistemas Lineares: Lista 01       
%                               Paulo Victor Galvão
% ------------------------------------------------------------------------
%% Lista 01: questão 03 - B
clc
clear all
close all

%% Parametros do sistema

m = 0.1;            %Massa do sistema
b = 0.015;          %Coeficiente de atrito
L = 0.2;            %Comprimento da haste
g = 9.81;           %Aceleração gravitacional
T = 1.5*pi;         %Tempo de simulação
u = 0;              %Entrada de controle

x0 = [pi/18 pi/6 pi/3; 0 0 0];
t0(:,1:5) = 0;
k = 1.5*pi;
cor = ['b', 'm', 'c'];

A = [0 1; -g/L -b/(m*L^2)];
B = [0; 1/(m*L^2)];
C = [1 0; 0 1];
D = [0; 0];

sys = ss(A,B,C,D);          %Armazena o espaço de esados na variável sys

%% Cálculo do sistema não-linear
for i = 1:3
    
  sistema = @(t,x) [x(2); (-b/(m*L^2))*x(2) - g/L*sin(x(1))];
  [t,x] = ode45(sistema, [0 T], [x0(:,i)]); %([solução], [intervalo], [Cond. iniciais])


  t1 = 0:0.01:T;                   %Tempo de simulação
  u = zeros(length(t1), 1);        %Vetor de entrado cocm mesmo tamnaho do vetor tempo
  y = lsim(sys, u, t1, x0(:,i));   %Função para cálculo do sistema linear
  
%Gráficos para as três condições iniciais dadas
figure(1)
title('Comparação entre o Sistema Não Linear e Linearizado')
handle = plot(t, x(:,1), cor(i));
hold on
plot(t1, y(:,1), 'k--');
grid
set(handle,'LineWidth',[1.25])
str = {'\theta_0 = \pi/18', '\theta_0 = \pi/18 Linearizado', '\theta_0 = \pi/6', '\theta_0 = \pi/6 Linearizado', '\theta_0 = \pi/3', '\theta_0 = \pi/3 Linearizado'};
legend(str)
xlabel('T');
ylabel('\theta');
end