% sfbdesign.m
% Macro to make state feedback design of robot mechanism
%
% Computer Controlled Systems (3rd ed)
% Author: B. Wittenmark
% Last edit: 1996-12-08
% Copyright (c) 1996 by K. J. Åström, B. Wittenmark and
% Department of Automatic Control, Lund Institute of
% Technology, Lund, Sweden


%The continuous time model
J1=10/9;
J2=10;
k=1;
d=0.1;
kI=1;
om0=1; 
al=J1/(J1+J2);
be1=d/(J1*om0);
be2=d/(J2*om0);
ga=kI/(J1*om0);
de=1/(J1*om0);
A=om0*[0 1 -1
al-1 -be1 be1
al be2 -be2];
B=[0 0; ga de; 0 0];
C=[0 0 om0];
D=[0 0];

%Sampling of the system
h=0.5;   %Sampling period
[phi,gamma]=c2d(A,B,h);

%Definition of continuous time closed loop characteristic polynomial
omega_m=0.5;
zeta_m=0.7;
alpha_1=2;
den=conv([1 2*zeta_m*omega_m omega_m*omega_m],[1 alpha_1*omega_m]);

%Conversion to sampled data characteristic polynomial
[As,Bs,Cs,Ds]=tf2ss([0 0 0 1],den);
[phis,gas]=c2d(As,Bs,h);
P=eig(phis);

%Poleplacement
L=acker(phi,gamma(:,1),P);

%Determination of l_c
l_c=1/(C*inv(eye(3)-phi+gamma(:,1)*L)*gamma(:,1));
