% main_inv_dpendel.m
% Regulering av to inverterte pendler som staar paa en vogn.

l1=0.7;                                           % lengde av pendelarm 1.
l2=0.4;                                           % lengde av pendelarm 2.
m11=0.044;                                        % masse av pendestang 1 (uten last).
m22=0.025;                                        % masse av pendelstang 2 (uten last). 
m1l=0.2;                                          % masse av last for pendel 1.
m2l=0.2;                                          % masse av last for pendel 2.
g=9.81;                                           % tyngdens akselerasjon.
% Beregning av modell-parametre.
m1=m11+m1l;                                       % total masse pendel 1.
m2=m22+m2l;                                       % total masse pendel 2.
lg1=l1*(m11+2*m1l)/(2*(m11+m1l));                 % avstand til tyngdepunktet.
lg2=l2*(m22+2*m2l)/(2*(m22+m2l));
Jt1=m11*(l1^2-3*l1*lg1+3*lg1^2)/3+m1l*(l1-lg1)^2; % treghetsmomentet for stang 1.
Jt2=m22*(l2^2-3*l2*lg2+3*lg2^2)/3+m2l*(l2-lg2)^2;

% Dette gir nominelle par.: lg1=0.637; lg2=0.378; Jt1=6.2147e-3; Jt2=1.2222e-3;

% Lineaer modell for to inverterte pendler
a21=(m1*lg1*g)/(Jt1+m1*lg1^2);
b21=(m1*lg1)/(Jt1+m1*lg1^2);                      % Merk: unodig regnearbeid.

a43=(m2*lg2*g)/(Jt2+m2*lg2^2);
b41=(m2*lg2)/(Jt2+m2*lg2^2);

A=zeros(4,4); A(2,1)=a21; A(4,3)=a43; A(1,2)=1; A(3,4)=1;
B=zeros(4,1); B(2,1)=-b21; B(4,1)=-b41;

D=[1,0,0,0;0,0,1,0]; % maaling av vinkelutslag.

% LQ-optimal regulator.
Q=eye(4)*10; P=1;
[G,R]=lqr(A,B,Q,P); G=-G;

q10=pi*8/180;   % starter med et vinkelutslag paa q10 grader for pendelarm 1.
q20=-pi*5/180;  % starter med et vinkelutslag paa q20 grader for pendelarm 2.
x0=[q10;0;q20;0]; % initial-tilstandsvektor.

% Simulering av lukket system (basert paa lineaer modell).
t=0:0.05:5;
x=lsim(A+B*G,zeros(4,1),eye(4),zeros(4,1),t,t,x0); % Simulering av lukket system.

figure(1)
subplot(411), plot(t,x(:,1)), ylabel('q_1')
title('To inverterte pendler paa vogn med LQ-regulator (lineaer modell)')
subplot(412), plot(t,x(:,2)), ylabel('dot(q_1)')
subplot(413), plot(t,x(:,3)), ylabel('q_2')
subplot(414), plot(t,x(:,4)), ylabel('dot(q_2)')
xlabel('Tid [sec]')

% simulering av lukket system (basert paa ulineaer modell)
[t2,x2]=ode15s('fx_inv_pendel',[0 5],x0);

figure(2)
subplot(411), plot(t2,x2(:,1)), ylabel('q_1')
title('To inverterte pendler paa vogn med LQ-regulator (ulineaer modell)')
subplot(412), plot(t2,x2(:,2)), ylabel('dot(q_1)')
subplot(413), plot(t2,x2(:,3)), ylabel('q_2')
subplot(414), plot(t2,x2(:,4)), ylabel('dot(q_2)')
xlabel('Tid [sec]')
