clear all
close all
clc

M=10;    %mass of the cart          [kg]
m=2;     %mass of the pendulum      [kg]
l=1;     %lenght                    [m]
ra=10;   %armature resistance       [ohm]
rr=.1;   %radius of the wheels      [m]
ke=2;    %constant                  [N*m/A]
kc=2;    %constant                  [V*s/rad]
g=9.81;
T = 0.1; %discretization period     [s]

%% (b) - Time based linearization

% Run simulink openloop
sim('pendulum_openloop_anim')

A = pendulum_openloop_anim_Time_Based_Linearization.a;
B = pendulum_openloop_anim_Time_Based_Linearization.b;
C = pendulum_openloop_anim_Time_Based_Linearization.c;
D = pendulum_openloop_anim_Time_Based_Linearization.d;

sysc = ss(A,B,C,D);

%% (c) - Discretization

sysd = c2d(sysc,T);
Ad = sysd.A;
Bd = sysd.B;
Cd = sysd.C;
Dd = sysd.D;

%% (d) - Pole placement with the discretized system

% Check that the discretized system is reachable
rank(ctrb(Ad,Bd))      % = 4

% Design controller
P = [exp(-2*T) exp(-2.5*T) exp(-3*T) exp(-4*T)];
K = place(Ad,Bd,P);    % = [-20.9806  -52.3014 -272.3911  -84.2567]

eig(Ad-Bd*K)           % = [0.6703; 0.7408; 0.8187; 0.7788] 
                        ... = [exp(-4*T) exp(-3*T) exp(-2*T) exp(-2.5*T)]
                            
%% (d) - Closed loop simulation with pole placement 

% Implement this control gain K into the Simulink model and simulate 
    ... the closed-loop system for different values of pulse disturbance
sim('pole_placement_pendulum_anim')

%% (e) - Pole placement - faster poles

% Design controller
P = [exp(-10*T) exp(-3*T) exp(-6.5*T) exp(-6*T)];
K = place(Ad,Bd,P);    % = [-216.3529 -197.7486 -815.2830 -257.9526]

eig(Ad-Bd*K)           % = [0.3679; 0.5220; 0.5488; 0.7408] 
                        ... = [exp(-10*T) exp(-6.5*T) exp(-6*T) exp(-3*T)]
                            
%% (d) - Closed loop simulation with pole placement - faster poles

% Implement this new control gain K into the Simulink model and simulate 
    ... the closed-loop system for different values of pulse disturbance
    ... again 
sim('pole_placement_pendulum_anim')

% It is intuitive that the second controller with faster poles (poles that 
    ... are closer to origin in complex domain) would yield better 
    ... performance. This is because smaller closed-loop poles mean that 
    ... the modes of the system will be decaying quickly. This in turn 
    ... means that the effect of disturbance on the system response dies  
    ... out quickly, which translates into better disturbance rejection.