% g = genbeam (n, g0)
%
% n denotes the number of flexible modes
% g0 denotes the gravity acceleration
%
% Generates a model with 2*(n+1) states of a flexible beam
%
% input:   motor torque at hinge
% outputs: 1 position of tip of beam
%          2 angle at hinge
%          3 acceleration of the tip
%
% Author: Anders Helmersson, ISY, Linköpings universitet
% 2013-06-16

function g = genbeam (n, g0)
if nargin < 2, g0 = 0; end
if nargin < 1, n = 2; end
n = max (0, n);

f{1} = @(x) sqrt(3)*x;
fd{1} = @(x) sqrt(3);
m(1) = fd{1}(0);  % df/dx at x=0
v(1) = f{1}(1);  % f at x=1

for i = 1:n
  k(i+1) = fzero(@(x) tan(x)-tanh(x), (i+0.25)*pi);
  f{i+1} = @(x) sin(k(i+1)*x)/sin(k(i+1)) + sinh(k(i+1)*x)/sinh(k(i+1));
  fd{i+1} = @(x) k(i+1)*(cos(k(i+1)*x)/sin(k(i+1)) + cosh(k(i+1)*x)/sinh(k(i+1)));
  m(i+1) = fd{i+1}(0);
  v(i+1) = f{i+1}(1);
end

for i=1:n+1, for j=1:n+1,
    G(i,j) = quad (@(x) (1-x).*fd{i}(x).*fd{j}(x), 0, 1);
end, end
om = k.^2;
K = diag (om.^2);
M = eye (size(K));
D = 0.01 * diag (om);

E = [0*M M; M D];
g = ss (E\blkdiag(M,g0*G-K), E\[0*m(:);m(:)], [0*v v;0*m m], [0;0]); 

% add acceration of tip as third output
g = ss (g.a, g.b, [g.c; g.c(1,:)*g.a^2], [g.d; g.c(1,:)*g.a*g.b], ...
        'outputname', {'tip pos', 'hinge angle', 'tip acc'}, ...
        'inputname', 'hinge moment');
