%% Inverted pendulum control and reference tracking (servo)
% 
%  file:   ipend2.m
%  author: Peter Polcz <ppolcz@gmail.com> 
%  
%  Created on 2017.05.02. Tuesday, 22:39:05
%  Reviewed on 2017. November 22. [servo control]
%  Reviewed on 2017. November 29. [augmented, corrected, published]
%
%%
% Helper functions: <file ipend_simulate_Pi.m>, <file ipend_simulate_0.m>.
% 
% Helper scripts: <file ipend1_levezetesek.m>.
%  

try c = evalin('caller','p100ersist'); catch; c = []; end
persist = pcz_persist(mfilename('fullpath'), c); clear c; 
persist.backup();
%clear persist

global PUBLISH
PUBLISH = 1;

open_system ipend2_sim_servo
open_system ipend2_sim_servo_baby

%% Nonlinear model - parameters (A)
% No friction

syms t x v phi omega

% model parameters
M = 0.5;
m = 0.2;
l = 1;
g = 9.8;
b = 0;

q = 4*(M+m) - 3*m*cos(phi)^2;

f_sym = [
    v
    (4*m*l*sin(phi)*omega^2 - 1.5*m*g*sin(2*phi) -4*b*v) / q
    omega
    3*(-m*l*sin(2*phi)*omega^2 / 2 + (M+m)*g*sin(phi) + b*cos(phi)*v) / (l*q)
    ];

g_sym = [
    0
    4*l
    0
    -3*cos(phi)
    ] / (l*q);

f_ode = matlabFunction(f_sym, 'vars', {t , [ x ; v ; phi ; omega ]});
g_ode = matlabFunction(g_sym, 'vars', {t , [ x ; v ; phi ; omega ]});


%% Linearized model around the unstable equilibrium point

% model parameters
M = 0.5;
m = 0.2;
l = 1;
g = 9.8;
b = 0;

A = [
    0                     1                             0  0
    0      -(4*b)/(4*M + m)            -(3*g*m)/(4*M + m)  0
    0                     0                             0  1
    0   (3*b)/(l*(4*M + m))   (3*g*(M + m))/(l*(4*M + m))  0
    ];

B = [
    0
    4/(m+4*M)
    0
    -3/(m+4*M)/l
    ];

C = [
    1 0 0 0
    0 0 1 0
    ];

D = [ 0 ; 0 ];

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

K = place(A,B,-4:-1);
pcz_num2str(K)

K = lqr(A,B,eye(4),1);
pcz_num2str(K)

L = place(A',C',-4:0.1:-3.7)'
pcz_num2str(L)

%% Transfer function of the closed loop system

Ac = [ A-B*K B*K ; zeros(4,4) A-L*C ];
Bc = [ B ; zeros(4,1) ];
Cc = [ 1 0 0 0 -1 0 0 0 ];
Dc = 0;

cls = ss(Ac,Bc,Cc,Dc);
Ge = minreal(tf(cls))
pzmap(Ge)

Gi = pidtune(Ge,'I')

G_all = minreal(Ge*Gi / (1 + Ge*Gi));

%% Linear-Quadratic-Integral control: |lqi|

C = [
    1 0 0 0
    ];

D = [ 0 ];

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

K = lqr([A zeros(4,1) ; -C 0], [B ; 0], eye(5), 1)

K_lqi = lqi(sys, 1*eye(5),1);
K = K_lqi(:,1:4);
Ki = -K_lqi(end);

set_param('ipend2_sim_servo/K_lqi','Gain',pcz_num2str(-K_lqi))
set_param('ipend2_sim_servo_baby/K_x','Gain',pcz_num2str(K))
set_param('ipend2_sim_servo_baby/K_I','Gain',pcz_num2str(Ki))

%% Simulate simulink results
%% Simulink model (<sim ipend2_sim_servo_baby.slx>)
open_system ipend2_sim_servo_baby
print -dpng fig/ipend2_sim_servo_baby.png -sipend2_sim_servo_baby

%%%
% <img fig/ipend2_sim_servo_baby.png>

%% Simulate 

sim ipend2_sim_servo_baby

%% Visualize external control force

t = simx.Time;
x = simx.Data;
u = simu.Data;
r = simr.Data;
ipend_simulate_0(t,x,u,r)

%% Simulink model (<sim ipend2_sim_servo.slx>)
open_system ipend2_sim_servo
print -dpng fig/ipend2_sim_servo.png -sipend2_sim_servo

%%%
% <img fig/ipend2_sim_servo.png>

%% Simulate
sim ipend2_sim_servo

%% Visualize external control force

t = simx.Time;
x = simx.Data;
u = simu.Data;
r = simr.Data;
ipend_simulate_0(t,x,u,r)
