%% Inverted pendulum on cart

close all
clear all

% This is the skeleton script for the second exercise session in the PhD
% class Control System Synthesis.

% The objective is to stabilize the pendulum in an upward position by
% controlling the force applied to the cart, while meeting these
% requirements:
% Settling time for x and theta of less than 5 seconds
% Pendulum angle theta never more than 20 degrees (0.35 radians) from the vertical

%  Model parameters
M=0.5; % mass of the cart (kg)
m=0.2; % mass of the pendulum (kg)
b=0.1; % coefficient of friction for the cart (N/m/sec)
l=0.3; % length to pendulum center of mass (l)
J=0.006; % mass moment inertia of the pendulum (kg.m^2$)
g = 9.81;

%% System

% Expression of the linearized system in a state space form

A = ;
B = ;
C = ;
D = ;

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

% Controllability
% ctrb(A,B) gives the controllability matrix of the system
% is the system controllable? what does that imply?

%% Pole placement

% poles of the uncontrolled system
eig(A)

% come value for p (to be changed afterwards)
p_cl=[-0.1; -0.2; -0.3; -0.4];

K=place(A,B,p_cl);
eig(A-B*K)

% controlled system
Pc = ss(A-B*K,zeros(4,1),C-D*K,zeros(2,1));

% simulation 
t = 0:0.01:8; % time vector
x0 = [0; 0; 0; 0.5]; % initial state (taking the pendulum a bit away from the equilibrium)
y = lsim(Pc,zeros(size(t)),t,x0);

figure
subplot(211), plot(t,y(:,1))
ylabel('Cart position')
xlabel('Time (s)')
subplot(212), plot(t,y(:,2))
ylabel('Angle from the equilibrium')
xlabel('Time (s)')

%% LQR
Q = diag([1; 1; 10; 10]);
R = 1;
Klqr = lqr(A,B,Q,R)

eig(A-B*Klqr)

% controlled system
Plqr = ss(A-B*Klqr,zeros(4,1),C-D*Klqr,zeros(2,1));

% simulation 
t = 0:0.01:8; % time vector
x0 = [0; 0; 0; 0.5]; % initial state (taking the pendulum a bit away from the equilibrium)
ylqr = lsim(Plqr,zeros(size(t)),t,x0);

figure
subplot(211), plot(t,y(:,1),t,ylqr(:,1))
ylabel('Cart position')
xlabel('Time (s)')
legend('Pole placement','LQR')
subplot(212), plot(t,y(:,2),t,ylqr(:,1))
ylabel('Angle from the equilibrium')
xlabel('Time (s)')


