%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% controller.m
% This code is used to design a controller for the inverted pendulum 
% system created in 2004-2005 for an Master’s of Engineering Design Project
%
% The control design applies modern control theory using LQR and Kalman
% Filter design techniques.  Only the LQR design was used in the actual
% implementation of the controller.  Note to run these scripts it is
% necessary to have the control toolbox which using the lqr.m and lqe.m
% files for finding the optimal LQR control and Kalman Filter.
%
% The parameters were obtained by measurement.  
%
% Copyright 2005 by John Stang
close all;
clear all;
clc;

%%%%%%%%%%%%%%
% Parameters %
%%%%%%%%%%%%%%
alpha = [-0.6493, 0.2196, -0.39];
Mchain = 1.008;
MsprocketB = 0.0827;
MsprocketA = 0;
Mcart = .324;
Mpend = .1925;
M = Mchain+Mcart+Mpend;
Tmotor = 1.35581795;
DsprocketA = 0.0759968;
Fchain =  2*Tmotor/DsprocketA;
L = .617;
w = .0183;
d = .0065;
dm = Mpend/(L);
I = dm/3*(.6^3-(-.017)^3); 
g = 9.8;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Constants for Simplicity %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%
K1 = I+Mpend*L*L/4- Mpend*Mpend*L*L/4;
K2 = (I+Mpend*L*L/4)*M*2/L/Mpend+Mpend*L/2;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Create the State Space Model %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
A = [0 1 0 0;
    Mpend*g*L/2/K1 0 0 0;
    0 0 0 1;
    Mpend*g*L/2 0 0 0;];
Bu = [0 Mpend*L/2/M/K1 0 (I+Mpend*L*L/4)/K2*2/L/Mpend]';
Bd = [1 0 0 0;0 0 1 0]';
Cz = [1 0 0 0; 0 0 1 0];
Du = [0; 0];
Dd = [0 0;0 0];

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% get the poles and zeros the system %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
figure 
pzmap(ss(A,Bu,Cz,Du));
eig(A)
tzero(ss(A,Bu,Cz,Du))

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Define weighting matrices of LQR design %
% and maximum desired values              %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
degree = 4.5;
w = degree*pi/180;
x = .68;
Rzz = [1/w^2 0; 0   1/x^2];  % Performance weights
Q = Cz'*Rzz*Cz;
rho = 3;
R = 1/rho^2;    % Control Effort Weights
%%% LQR control %%%
F= lqr(A,Bu,Q,R);


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Define Noise Intensities for Sensors %
% (experimentally obtained)            %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
Sigv1 =0.009804*pi/180;
Sigv2 =4.454010e-008;
Sigw = 0.009804*pi/180;
%%% Kalman Filter %%%
H = lqe(A,Bd,Cz,Sigw,[Sigv1 0; 0 Sigv2]); 

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Poles and Zeros of Closed Loop     %
% System using just the LQR solution %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
figure 
pzmap(ss(A-Bu*F,Bd,Cz,Dd));
eig(A-Bu*F)
tzero(ss(A-Bu*F,Bd,Cz,Dd))

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Simulated with an arbitrary         %
% disturbance applied to the pendulum %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
figure
dt = .01;
tfinal = 5;
t = dt:dt:tfinal;
d = sqrt(Sigw)*randn(tfinal/dt,1)/sqrt(dt);
v1 = sqrt(Sigv1)*randn(length(t),1)/sqrt(dt);
v2 = sqrt(Sigv2)*randn(length(t),1)/sqrt(dt);
d = [v1,v2];
[y,t,x] = lsim(ss(A-Bu*F,Bd,Cz,D),d,t);
y = y';
x = x';
subplot(3,1,1)
plot(t,y(1,:)*180/pi)
ylabel('angle in degrees')
subplot(3,1,2)
plot(t,y(2,:))
ylabel('displacement')
subplot(3,1,3)
plot(t,(-F*x-alpha(2)*x(4,:)-alpha(3)*sign(x(4,:)))/alpha(1),'r')
ylabel('control effort')
xlabel('time')
