% Linearized 4th order model and analysis of eigenvalues
% Equations based on Schwab et al 2004
% kja 040611

aminit;

%Parameters of a bicycle model
%kja 040613
%Basic data is given by 26 parameters
%Acceleration of gravity [m/s^2]
g=9.81;
%Wheel base [m]
b=1.00;	
%Trail [m]
c=0.08;
%c=-0.5 %A large negative trail seems to help for rear-wheel steering!
%Wheel radii
Rrw=0.35;
Rfw=0.35;
%Head angle [radians]
lambda=pi*70/180;	
%Rear frame mass [kg], center of mass [m], and inertia tensor [kgm^2]
mrf=12;xrf=0.439;zrf=0.579;
Jxxrf=0.475656;Jxzrf=0.273996;Jyyrf=1.033092;Jzzrf=0.527436;
mrf=87;xrf=0.491586;zrf=1.028138;
Jxxrf=3.283666;Jxzrf=0.602765;Jyyrf=3.8795952;Jzzrf=0.565929;
%Front frame mass [kg], center of mass [m], and inertia tensor [kgm^2]
mff=2;xff=0.866;zff=0.676;
Jxxff=0.08;Jxzff=-0.02;Jyyff=0.07;Jzzff=0.02;
%Rear wheel mass [kg], center of mass [m], and inertia tensor [kgm^2]
mrw=1.5;Jxxrw=0.07;Jyyrw=0.14;
%Front wheel mass [kg], center of mass [m], and inertia tensor [kgm^2]
%mfw=3;Rfw=0.35;Jxxfw=0.14;Jyyfw=0.28;
mfw=1.5;Jxxfw=0.07;Jyyfw=0.14;
%Auxiliary variables
xrw=0;zrw=Rrw;xfw=b;zfw=Rfw;
Jzzrw=Jxxrw;Jzzfw=Jxxfw;
mt=mrf+mrw+mff+mfw;
xt=(mrf*xrf+mrw*xrw+mff*xff+mfw*xfw)/mt;
zt=(mrf*zrf+mrw*zrw+mff*zff+mfw*zfw)/mt;
Jxxt=Jxxrf+mrf*zrf^2+Jxxrw+mrw*zrw^2+Jxxff+mff*zff^2+Jxxfw+mfw*zfw^2;
Jxzt=Jxzrf+mrf*xrf*zrf+mrw*xrw*zrw+Jxzff+mff*xff*zff+mfw*xfw*zfw;
Jzzt=Jzzrf+mrf*xrf^2+Jzzrw+mrw*xrw^2+Jzzff+mff*xff^2+Jzzfw+mfw*xfw^2;
mf=mff+mfw;
xf=(mff*xff+mfw*xfw)/mf;zf=(mff*zff+mfw*zfw)/mf;
Jxxf=Jxxff+mff*(zff-zf)^2+Jxxfw+mfw*(zfw-zf)^2;
Jxzf=Jxzff+mff*(xff-xf)*(zff-zf)+mfw*(xfw-xf)*(zfw-zf);
Jzzf=Jzzff+mff*(xff-xf)^2+Jzzfw+mfw*(xfw-xf)^2;
d=(xf-b-c)*sin(lambda)+zf*cos(lambda);
Fll=mf*d^2+Jxxf*cos(lambda)^2+2*Jxzf*sin(lambda)*cos(lambda)+Jzzf*sin(lambda)^2;
Flx=mf*d*zf+Jxxf*cos(lambda)+Jxzf*sin(lambda);
Flz=mf*d*xf+Jxzf*cos(lambda)+Jzzf*sin(lambda);
gamma=c*sin(lambda)/b;
Sr=Jyyrw/Rrw;Sf=Jyyfw/Rfw;St=Sr+Sf;Su=mf*d+gamma*mt*xt;

%Matrices for linearized fourth order model
kp=10;kd=10;kpd=5;
kp=0;kd=0;kpd=0;
M=[Jxxt -Flx-gamma*Jxzt;-Flx-gamma*Jxzt Fll+2*gamma*Flz+gamma^2*Jzzt]
K0=[-mt*g*zt g*Su;g*Su+kp  -g*Su*cos(lambda)+kpd]
K1=[0 -(St+mt*zt)*sin(lambda)/b;0 (Su+Sf*cos(lambda))*sin(lambda)/b]
c12=gamma*St+Sf*sin(lambda)+Jxzt*sin(lambda)/b+gamma*mt*zt;
c22=Flz*sin(lambda)/b+gamma*(Su+Jzzt*sin(lambda)/b);
C0=[0 -c12;(gamma*St+Sf*sin(lambda))+kd c22] 
one=diag([1 1]);null=zeros(2,2);

%Plot eigenvalues as function of velocity
figure(1); clf; subplot(221); hold on

plot([-15 15], [0 0], 'k-', 'LineWidth', AM_thin_linewidth);
plot([0 0], [-15 15], 'k-', 'LineWidth', AM_thin_linewidth);

A=[one null;null M]; B=[null one;-K0 -C0*0];
D=eig(B,A)
pl=plot( real(D(1)), imag(D(1)), 'ro', real(D(2)), imag(D(2)), 'ro', ...
    real(D(3)), imag(D(3)), 'ro', real(D(4)), imag(D(4)), 'ro')
set(pl,'Linewidth', AM_data_linewidth);
set(pl,'MarkerSize', AM_data_markersize);

% Generate the set of velocities for the root locus plot
for V=[0.1:0.01:0.6 0.6:0.001:2 2:0.01:15]
  C=C0*V;
  K=K0+K1*V^2;
  A=[one null;null M];B=[null one;-K -C];
  D=eig(B,A);
  rplot(D(1));rplot(D(2));rplot(D(3));rplot(D(4));
 end

amaxis([-15 15 -10 10]);
axis square; box on;
xlabel('re');ylabel('im');

%
% Now add some labels on important points
%

% Label the points where the eigenvalues cross
plot([0 0], [5.75, -5.75], '.', 'MarkerSize', AM_data_markersize*3);
amtext(1.5, 6, '{\itV} = 6.1'); 
amtext(1.5, -6, '{\itV} = 6.1');

% Arrows to indicate the direction of the locus lines
amtext(-13, -1.8, '{\leftarrow}{\itV}');
amtext(-4.5, -1.8, '{\itV}{\rightarrow}');

% Plot some arrows just before we transition
V = 4; C=C0*V; K=K0+K1*V^2;
A=[one null;null M];B=[null one;-K -C];
D1=eig(B,A);

V = 4.01; C=C0*V; K=K0+K1*V^2;
A=[one null;null M];B=[null one;-K -C];
D2=eig(B,A);

% set up the color
color = 'b';
if (AMPRINT_FLAG==2) color = 'k'; end;

arrow([real(D1(1)), imag(D1(1))], [real(D2(1)), imag(D2(1))], ...
  AM_normal_arrowsize, 'FaceColor', color, 'EdgeColor', color);

arrow([real(D1(1)), -imag(D1(1))], [real(D2(1)), -imag(D2(1))], ...
  AM_normal_arrowsize, 'FaceColor', color, 'EdgeColor', color);

amprint('bike-rlocus.eps');
