%% ME - 221 Dynamical systems, Spring 2025
% Solution to MATLAB PROBLEM SET 2
% Student Name:
% SCIPER:
% Submission date:

% Initiate the script
clc; clear; close all

%% 1DOF: Define the parameters
m =  ;     % kg
c2 = ;     % Nms
L =  ;     % m
g =  ;     % m/s^2

%% Q4a Define the linear 1DOF state-space
% Define the state space matrices for the linearized sdof system
A_sdof = ;
B_sdof = ;
C_sdof = ;
D_sdof = ;

Lsys_sdof = ss(,,,); % Create a state-space model from matrices

%% Q4b Simulate the SDOF system

IC = zeros(2,2);
IC(1,:) = ; % COMPLETE

% Initial conditions
tt = linspace(0,12,1e3).';  % define the time vector
opts = odeset('RelTol',1e-8,'AbsTol',1e-10);

% Simulate the four cases                     
[~,zNL_1] = ode45(@(t,z)  EX_4_NLode_1DOF_2025(), tt, IC(:,1), opts);
[~,zNL_2] = ode45(@(t,z)  EX_4_NLode_1DOF_2025(), tt, IC(:,2), opts);

zL_1 = initial(,,); % COMPLETE
zL_2 = initial(,,); % COMPLETE

z_1_diff = ; %COMPLETE
z_2_diff = ; %COMPLETE

figure(2);clf
    h = plot(tt,[zNL_1(:,1) zL_1(:,1) z_1_diff(:,1)]/pi*180);
    set(h(:),'LineWidth',1.5)
    set(h(2),'LineStyle','--')
    set(h(3),'LineStyle','--','color','r')
    xlabel('$t$(s)','interpreter','Latex')
    ylabel('$\theta$($^\circ$)','interpreter','Latex')
    hl = legend('Nonlinear','Linear','Error');
    set(hl,'interpreter','Latex')
    set(gca,'FontSize',8)
    set(gcf,'color','w','units','centimeters','outerposition',[20 15 12 8])
    exportgraphics(gcf,'Figure_4b_1.png','Resolution',300);
figure(3);clf
    h = plot(tt,[zNL_2(:,1) zL_2(:,1) z_2_diff(:,1)]/pi*180);
    set(h(:),'LineWidth',1.5)
    set(h(2),'LineStyle','--')
    set(h(3),'LineStyle','--','color','r')
    xlabel('$t$(s)','interpreter','Latex')
    ylabel('$\theta$($^\circ$)','interpreter','Latex')
    hl = legend('Nonlinear','Linear','Error');
    set(hl,'interpreter','Latex')
    set(gca,'FontSize',8)
    set(gcf,'color','w','units','centimeters','outerposition',[20 15 12 8])
    exportgraphics(gcf,'Figure_4b_2.png','Resolution',300);

%% 2DOF: Define the remaining parameters

M = ;      % kg
k = ;   % N/m
c1 = ;   % Nms

%% Q7a Plot the potential energy
y0 = -(m+M)*g/k; % Center point of y plot

yV = linspace(2*y0,0,5e2+1).';
thV = linspace(-1.5*pi,1.5*pi,2e2+1).';

[YY,THTH] = meshgrid(??,??); %COMPLETE
VSurf = ; % Calculate potential energy at meshgrid values

figure(1); clf
contourf(YY*1e3,THTH/pi*180,VSurf,20); hold all
hc = colorbar;
title(hc,'$V(y,\theta)$','interpreter','Latex')
plot(y0*1e3*[1 1 1].',[-180 0 180].','ro','MarkerFaceColor','r')
xlabel('$y$(mm)','interpreter','Latex')
ylabel('$\theta$($^{\circ}$)','interpreter','Latex')
set(gca,'FontSize',8)
set(gcf,'color','w','units','centimeters','outerposition',[20 15 12 12])
exportgraphics(gcf,'Figure_7.png','Resolution',300);

%% Q7b Calculate the eigenvalues of the Hessian matrix at the eq. points

% You can use the eig function.



%% Q8 Simulate the 2 DOF system - IC 
yb = ; % Value of y at stable equilibrium point

% Linearized state-space model
A = ;
B = ;
C = ;
D = ;

Lsys = ss(??,??,??,??); % Create a state-space model from matrices

IC = zeros(4,4);
IC(1,:) = ;
IC(2,:) = []/180*pi; 
ICL = IC-[]; %Center around stable equilibrium point

% Initial conditions
tt = linspace(0,12,1e3).';  % define the time vector
opts = odeset('RelTol',1e-8,'AbsTol',1e-10);

% Simulate the four cases           
[~,zNL_1] = ode45(@(t,z)  EX_4_NLode_2025(), tt, IC(:,1), opts);
[~,zNL_2] = ode45(@(t,z)  EX_4_NLode_2025(), tt, IC(:,2), opts);
[~,zNL_3] = ode45(@(t,z)  EX_4_NLode_2025(), tt, IC(:,3), opts);
[~,zNL_4] = ode45(@(t,z)  EX_4_NLode_2025(), tt, IC(:,4), opts);
zL_1 = initial(??,??,??);
zL_2 = initial(??,??,??);
zL_3 = initial(??,??,??);
zL_4 = initial(??,??,??);

% Plots for zNL are shifted to be centered at yb to match zL
figure(4);clf
subplot(2,1,1)
    h = plot(tt,1e3*[zNL_1(:,1)-yb zL_1(:,1) zNL_1(:,1)-yb-zL_1(:,1)]);
    set(h(:),'LineWidth',1.5)
    set(h(2),'LineStyle','--')
    set(h(3),'LineStyle','--','color','r')
    xlabel('$t$(s)','interpreter','Latex')
    ylabel('$\delta{y}$(mm)','interpreter','Latex')
subplot(2,1,2)
    h = plot(tt,[zNL_1(:,2) zL_1(:,2) zNL_1(:,2)-zL_1(:,2)]/pi*180);
    set(h(:),'LineWidth',1.5)
    set(h(2),'LineStyle','--')
    set(h(3),'LineStyle','--','color','r')
    xlabel('$t$(s)','interpreter','Latex')
    ylabel('$\theta$($^\circ$)','interpreter','Latex')
    hl = legend('Nonlinear','Linear','Error');
    set(hl,'interpreter','Latex')
    set(gca,'FontSize',8)
    set(gcf,'color','w','units','centimeters','outerposition',[20 15 12 8])
    exportgraphics(gcf,'Figure_8a.png','Resolution',300);

figure(5);clf
subplot(2,1,1)
    h = plot(tt,1e3*[zNL_2(:,1)-yb zL_2(:,1) zNL_2(:,1)-yb-zL_2(:,1)]);
    set(h(:),'LineWidth',1.5)
    set(h(2),'LineStyle','--')
    set(h(3),'LineStyle','--','color','r')
    xlabel('$t$(s)','interpreter','Latex')
    ylabel('$\delta{y}$(mm)','interpreter','Latex')
subplot(2,1,2)
    h = plot(tt,[zNL_2(:,2) zL_2(:,2) zNL_2(:,2)-zL_2(:,2)]/pi*180);
    set(h(:),'LineWidth',1.5)
    set(h(2),'LineStyle','--')
    set(h(3),'LineStyle','--','color','r')
    xlabel('$t$(s)','interpreter','Latex')
    ylabel('$\theta$($^\circ$)','interpreter','Latex')
    hl = legend('Nonlinear','Linear','Error');
    set(hl,'interpreter','Latex')
    set(gca,'FontSize',8)
    set(gcf,'color','w','units','centimeters','outerposition',[20 15 12 8])
    exportgraphics(gcf,'Figure_8b.png','Resolution',300);

figure(6);clf
subplot(2,1,1)
    h = plot(tt,1e3*[zNL_3(:,1)-yb zL_3(:,1) zNL_3(:,1)-yb-zL_3(:,1)]);
    set(h(:),'LineWidth',1.5)
    set(h(2),'LineStyle','--')
    set(h(3),'LineStyle','--','color','r')
    xlabel('$t$(s)','interpreter','Latex')
    ylabel('$\delta{y}$(mm)','interpreter','Latex')
subplot(2,1,2)
    h = plot(tt,[zNL_3(:,2) zL_3(:,2) zNL_3(:,2)-zL_3(:,2)]/pi*180);
    set(h(:),'LineWidth',1.5)
    set(h(2),'LineStyle','--')
    set(h(3),'LineStyle','--','color','r')
    xlabel('$t$(s)','interpreter','Latex')
    ylabel('$\theta$($^\circ$)','interpreter','Latex')
    hl = legend('Nonlinear','Linear','Error');
    set(hl,'interpreter','Latex')
    set(gca,'FontSize',8)
    set(gcf,'color','w','units','centimeters','outerposition',[20 15 12 8])
    exportgraphics(gcf,'Figure_8c.png','Resolution',300);

figure(7);clf
subplot(2,1,1)
    h = plot(tt,1e3*[zNL_4(:,1)-yb zL_4(:,1) zNL_4(:,1)-yb-zL_4(:,1)]);
    set(h(:),'LineWidth',1.5)
    set(h(2),'LineStyle','--')
    set(h(3),'LineStyle','--','color','r')
    xlabel('$t$(s)','interpreter','Latex')
    ylabel('$\delta{y}$(mm)','interpreter','Latex')
subplot(2,1,2)
    h = plot(tt,[zNL_4(:,2) zL_4(:,2) zNL_4(:,2)-zL_4(:,2)]/pi*180);
    set(h(:),'LineWidth',1.5)
    set(h(2),'LineStyle','--')
    set(h(3),'LineStyle','--','color','r')
    xlabel('$t$(s)','interpreter','Latex')
    ylabel('$\theta$($^\circ$)','interpreter','Latex')
    hl = legend('Nonlinear','Linear','Error');
    set(hl,'interpreter','Latex')
    set(gca,'FontSize',8)
    set(gcf,'color','w','units','centimeters','outerposition',[20 15 12 8])
    exportgraphics(gcf,'Figure_8d.png','Resolution',300);

%% Q9 Simulate the 2 DOF system - T = 0, F-harmonic

IC = []; %COMPLETE
ICL = []; %COMPLETE
tt = linspace(0,25,2e3).';  % define the time vector
A = [];          % N Force amplitude   
w = [];           % rad/s frequency    

%Forced response of linear system
U_1 = ;
U_2 = ;
U_3 = ;

zL_1 = lsim(Lsys,U_1,tt,ICL);
zL_2 = lsim(Lsys,U_2,tt,ICL);
zL_3 = lsim(Lsys,U_3,tt,ICL);

%Forced response of non-linear system
[~,zNL_1] = ode45(@(t,z)  EX_4_NLode_2025(), tt, IC, opts);
[~,zNL_2] = ode45(@(t,z)  EX_4_NLode_2025(), tt, IC, opts);
[~,zNL_3] = ode45(@(t,z)  EX_4_NLode_2025(), tt, IC, opts);

% Plots for zNL are shifted to be centered at yb to match zL
figure(8);clf
subplot(2,1,1)
    h = plot(tt,1e3*[zNL_1(:,1)-yb zL_1(:,1) zNL_1(:,1)-yb-zL_1(:,1)]);
    set(h(:),'LineWidth',1.5)
    set(h(2),'LineStyle','--')
    set(h(3),'LineStyle','--','color','r')
    xlabel('$t$(s)','interpreter','Latex')
    ylabel('$\delta{y}$(mm)','interpreter','Latex')
    set(gca,'FontSize',8)
subplot(2,1,2)
    h = plot(tt,[zNL_1(:,2) zL_1(:,2) zNL_1(:,2)-zL_1(:,2)]/pi*180);
    set(h(:),'LineWidth',1.5)
    set(h(2),'LineStyle','--')
    set(h(3),'LineStyle','--','color','r')
    xlabel('$t$(s)','interpreter','Latex')
    ylabel('$\theta$($^\circ$)','interpreter','Latex')
    hl = legend('Nonlinear','Linear','Error');
    set(hl,'interpreter','Latex')
    set(gca,'FontSize',8)
    set(gcf,'color','w','units','centimeters','outerposition',[20 15 12 8])
    exportgraphics(gcf,'Figure_9a.png','Resolution',300);

figure(9);clf
subplot(2,1,1)
    h = plot(tt,1e3*[zNL_2(:,1)-yb zL_2(:,1) zNL_2(:,1)-yb-zL_2(:,1)]);
    set(h(:),'LineWidth',1.5)
    set(h(2),'LineStyle','--')
    set(h(3),'LineStyle','--','color','r')
    xlabel('$t$(s)','interpreter','Latex')
    ylabel('$\delta{y}$(mm)','interpreter','Latex')
    set(gcf,'color','w','units','centimeters','outerposition',[20 15 12 8])
subplot(2,1,2)
    h = plot(tt,[zNL_2(:,2) zL_2(:,2) zNL_2(:,2)-zL_2(:,2)]/pi*180);
    set(h(:),'LineWidth',1.5)
    set(h(2),'LineStyle','--')
    set(h(3),'LineStyle','--','color','r')
    xlabel('$t$(s)','interpreter','Latex')
    ylabel('$\theta$($^\circ$)','interpreter','Latex')
    hl = legend('Nonlinear','Linear','Error');
    set(hl,'interpreter','Latex')
    set(gca,'FontSize',8)
    set(gcf,'color','w','units','centimeters','outerposition',[20 15 12 8])
    exportgraphics(gcf,'Figure_9b.png','Resolution',300);

figure(10);clf
subplot(2,1,1)
    h = plot(tt,1e3*[zNL_3(:,1)-yb zL_3(:,1) zNL_3(:,1)-yb-zL_3(:,1)]);
    set(h(:),'LineWidth',1.5)
    set(h(2),'LineStyle','--')
    set(h(3),'LineStyle','--','color','r')
    xlabel('$t$(s)','interpreter','Latex')
    ylabel('$\delta{y}$(mm)','interpreter','Latex')
    set(gca,'FontSize',8)
subplot(2,1,2)
    h = plot(tt,[zNL_3(:,2) zL_3(:,2) zNL_3(:,2)-zL_3(:,2)]/pi*180);
    set(h(:),'LineWidth',1.5)
    set(h(2),'LineStyle','--')
    set(h(3),'LineStyle','--','color','r')
    xlabel('$t$(s)','interpreter','Latex')
    ylabel('$\theta$($^\circ$)','interpreter','Latex')
    hl = legend('Nonlinear','Linear','Error');
    set(hl,'interpreter','Latex')
    set(gca,'FontSize',8)
    set(gcf,'color','w','units','centimeters','outerposition',[20 15 12 8])
    exportgraphics(gcf,'Figure_9c.png','Resolution',300);

%% Q10 Plot Simulate the system
IC = [];
tt = linspace(0,15,2e3).';  % define the time vector
AF = [];              % N Force amplitude
AT = [];              % N Force amplitude

%Linear system
zL = step(); % Calculate all step responses of the system.

zLF = squeeze(); % Response to step force as input.
zLT = squeeze(); % Response to step torque as input.

zL_F1 = ; % Response to first force input AF(1).
zL_F2 = ; % Response to second force input AF(2).
zL_T1 = ; % Response to first torque input AT(1).
zL_T2 = ; % Response to second torque input AT(2).

%Nonlinear system
[~,zNL_1] = ode45(@(t,z)  EX_4_NLode_2025(), tt, IC, opts);
[~,zNL_2] = ode45(@(t,z)  EX_4_NLode_2025(), tt, IC, opts);
[~,zNL_3] = ode45(@(t,z)  EX_4_NLode_2025(), tt, IC, opts);
[~,zNL_4] = ode45(@(t,z)  EX_4_NLode_2025(), tt, IC, opts);

% Plots for zNL are shifted to be centered at yb to match zL
figure(11);clf
subplot(2,1,1)
    h = plot(tt,1e3*[zNL_1(:,1)-yb zL_F1(:,1) zNL_1(:,1)-yb-zL_F1(:,1)]);
    set(h(:),'LineWidth',1.5)
    set(h(2),'LineStyle','--')
    set(h(3),'LineStyle','--','color','r')
    xlabel('$t$(s)','interpreter','Latex')
    ylabel('$\delta{y}$(mm)','interpreter','Latex')
    set(gca,'FontSize',8)
subplot(2,1,2)
    h = plot(tt,[zNL_1(:,2) zL_F1(:,2) zNL_1(:,2)-zL_F1(:,2)]/pi*180);
    set(h(:),'LineWidth',1.5)
    set(h(2),'LineStyle','--')
    set(h(3),'LineStyle','--','color','r')
    xlabel('$t$(s)','interpreter','Latex')
    ylabel('$\theta$($^\circ$)','interpreter','Latex')
    hl = legend('Nonlinear','Linear','Error');
    set(hl,'interpreter','Latex')
    set(gca,'FontSize',8)
    set(gcf,'color','w','units','centimeters','outerposition',[20 15 12 8])
    exportgraphics(gcf,'Figure_10a.png','Resolution',300);

figure(12);clf
subplot(2,1,1)
    h = plot(tt,1e3*[zNL_2(:,1)-yb zL_F2(:,1) zNL_2(:,1)-yb-zL_F2(:,1)]);
    set(h(:),'LineWidth',1.5)
    set(h(2),'LineStyle','--')
    set(h(3),'LineStyle','--','color','r')
    xlabel('$t$(s)','interpreter','Latex')
    ylabel('$\delta{y}$(mm)','interpreter','Latex')
    set(gca,'FontSize',8)
subplot(2,1,2)
    h = plot(tt,[zNL_2(:,2) zL_F2(:,2) zNL_2(:,2)-zL_F2(:,2)]/pi*180);
    set(h(:),'LineWidth',1.5)
    set(h(2),'LineStyle','--')
    set(h(3),'LineStyle','--','color','r')
    xlabel('$t$(s)','interpreter','Latex')
    ylabel('$\theta$($^\circ$)','interpreter','Latex')
    hl = legend('Nonlinear','Linear','Error');
    set(hl,'interpreter','Latex')
	set(gca,'FontSize',8)
    set(gcf,'color','w','units','centimeters','outerposition',[20 15 12 8])
    exportgraphics(gcf,'Figure_10b.png','Resolution',300);

figure(13);clf
subplot(2,1,1)
    h = plot(tt,1e3*[zNL_3(:,1)-yb zL_T1(:,1) zNL_3(:,1)-yb-zL_T1(:,1)]);
    set(h(:),'LineWidth',1.5)
    set(h(3),'LineStyle','--','color','r')
    xlabel('$t$(s)','interpreter','Latex')
    ylabel('$\delta{y}$(mm)','interpreter','Latex')
    set(gca,'FontSize',8)
subplot(2,1,2)
    h = plot(tt,[zNL_3(:,2) zL_T1(:,2) zNL_3(:,2)-zL_T1(:,2)]/pi*180);
    set(h(:),'LineWidth',1.5)
    set(h(3),'LineStyle','--','color','r')
    xlabel('$t$(s)','interpreter','Latex')
    ylabel('$\theta$($^\circ$)','interpreter','Latex')
    hl = legend('Nonlinear','Linear','Error');
    set(hl,'interpreter','Latex')
	set(gca,'FontSize',8)
    set(gcf,'color','w','units','centimeters','outerposition',[20 15 12 8])
    exportgraphics(gcf,'Figure_10c.png','Resolution',300);

figure(14);clf
subplot(2,1,1)
    h = plot(tt,1e3*[zNL_4(:,1)-yb zL_T2(:,1) zNL_4(:,1)-yb-zL_T2(:,1)]);
    set(h(:),'LineWidth',1.5)
    set(h(3),'LineStyle','--','color','r')
    xlabel('$t$(s)','interpreter','Latex')
    ylabel('$\delta{y}$(mm)','interpreter','Latex')
    set(gca,'FontSize',8)
subplot(2,1,2)
    h = plot(tt,[zNL_4(:,2) zL_T2(:,2) zNL_4(:,2)-zL_T2(:,2)]/pi*180);
    set(h(:),'LineWidth',1.5)
    set(h(3),'LineStyle','--','color','r')
    xlabel('$t$(s)','interpreter','Latex')
    ylabel('$\theta$($^\circ$)','interpreter','Latex')
    hl = legend('Nonlinear','Linear','Error');
    set(hl,'interpreter','Latex')
	set(gca,'FontSize',8)
    set(gcf,'color','w','units','centimeters','outerposition',[20 15 12 8])
    exportgraphics(gcf,'Figure_10d.png','Resolution',300);

