%==========================
%my_d_maglev.m
%   By Bjørnar Bøhagen 2003
%==========================
clear;
clc;

%************************
%Coil current loop design 
%************************
'Current loop design'
%------
%Modell
%------
L = 0.4125;
Rs = 1;
Rl = 10;
%Parameters
a33=(Rl+Rs)/(L);
b=1/L;
Acoil = [-a33 0 ; 1 0 ];
Bcoil = [b ; 0 ];

%-----------------------------
%LQR or pole controller design
%-----------------------------
lqrdesign = 0 %0==no
if(lqrdesign == 1) %current loop lqr design
    q = diag([100 100])
    r = 0.01
    Kcoil  = lqr(Acoil,Bcoil,q,r)
else %current loop pole placement design
    w01=3*(Rs+Rl)/L;
    zeta=1.5;
    s1=-zeta*w01+sqrt(zeta^2-1)*w01;
    s2=-zeta*w01-sqrt(zeta^2-1)*w01;
    evcoil=[s1,s2]; % desired poles
    Kcoil=place(Acoil,Bcoil,evcoil)
end
eigCoil=eig(Acoil-Bcoil*Kcoil)


%**********************
%Levitation loop design
%**********************
'Levitation loop design'
%------
%Modell
%------
%Constants
m = 0.068;
g = 9.81;
Ki = 125052*10^(-9);
d = 7.85*10^(-3);
%Linearization
xTypical = 7*10^(-3);
iTypical = sqrt((m*g)/Ki)*(xTypical+d);
a21 = 2*g/(xTypical+d);
a23 = sqrt((g*Ki)/(m))*((2)/(xTypical+d));
Aball = [0 1 0 ; a21 0 0 ; 1 0 0 ];
Bball = [0 ; -a23 ; 0];

%-----------------------------
%LQR or pole controller design
%-----------------------------
lqrdesign = 0 %0==no
if(lqrdesign == 1)
    q = diag([1 .01 10000]); % Var i fil fra quanser
    q = diag([100 .01 1]); % Var i fil fra quanser
    q = diag([1 0 1000]); % Var i fil fra quanser
    r=100000;
    Kball = lqr(a,b,q,r);
else
   w0=1/0.08;%20; % Eller genereltl 1.5/Ti der Ti blir ca. responstid for 2. ordens delen
   zeta=4;%1.1;
   T=0.15;%0.1;
   s1=-zeta*w0+sqrt(zeta^2-1)*w0;
   s2=-zeta*w0-sqrt(zeta^2-1)*w0;
   s3=-1/T; 
   p=[s1,s2,s3]; % Ønskede poler for regulert system
   Kball=place(Aball,Bball,p)
end;
%Begrenset derivatvirkning
fmax=200;
Td=1/(2*pi*fmax);
eigLevitation=eig(Aball-Bball*Kball)