%        Raddyn.m
%
%   Untersuchung der Dynamik von Fahrrädern mit
%   Vorder- und Hinterradlenkung
%
%
% J. Lunze
% 26.10.2000
% für 3. Auflage: 2.4.2001
% für 4. Auflage: 19.9.2003
% für 5. Auflage: 11.6.2005
% für 6. Auflage: 4.12.2006
% für 7. Auflage: 28.4.2008
% für 8. Auflage: 2.2.2010
% für 9. Auflage: 16.2.2012
% für 10. Auflage: 27.1.2014
% für 12. Auflage: 23.03.2020
%
%   
close all
clear
%             Parameter des Fahrrades
a=0.6;          % m  x-Koordinate des Schwerpunktes
l=1;                % m  z-Koordinate des Schwerpunktes
b=1.2;          % m  Radabstand
m=75;               % kg Masse von Fahrrad und Fahrer
g=9.81;         % kg m/s^2  Erdbeschleunigung

Jp=m*l*l;       % kg m^2  Trägheitsmoment

theta0=...
   4/180*pi;    % Grad  Anfangsneigung
v0=2;               % m/s  Geschwindigkeit
xi0=1;          % m  Startposition in xi-Richtung
eta0=1;         % m  Startposition in eta-Richtung
phi0=pi/4;      % Bogenmaß Anfangswinkel

kP=4;               %   Reglerverstärkung

Ts=0.05;            % Sekunde  Abtastzeit

                    % Modellparameter:
k=(m*a*l*v0)/(b*Jp);
s0=-v0/a;
s1=sqrt(m*g*l/Jp);
s2=-s1;

                    % Zustandsraummodell bei Vorderradlenkung
A=[0 1; m*g*l/Jp 0];
B=[0; 1];
C=[m*l*v0*v0/(b*Jp)  m*a*l*v0/(b*Jp)];    % bei Vorderradlenkung
CH=[m*l*v0*v0/(b*Jp)  -m*a*l*v0/(b*Jp)];  % bei Hinterradlenkung
D=0;
RadVZR=ss(A, B, C, D);      % Rad mit Vorderradlenkung
RadHZR=ss(A, B, CH, D);     % Rad mit Hinterradlenkung
RadVKreis=ss(A-B*kP*C, B, C, D);
RadHKreis=ss(A-B*kP*CH, B, CH, D);

%%   Wurzelortskurve für Vorderradlenkung
%
figure(1)
subplot(4,4,[1 2 5 6])
RadV=tf(k*[1 -s0], [1 -s1-s2 s1*s2]);
[P, Z]=pzmap(RadV);
plot(real(P), imag(P), 'bx', real(Z), imag(Z), 'bo');
hold on;
Kvek=[0:0.005:15];
R=rlocus(RadV, Kvek);
plot([10 -10],[0,0],'k:');
plot(real(R(1,:)), imag(R(1,:)), 'b', real(R(2,:)), imag(R(2,:)), 'b');
axis('square');
axis([-6 4 -5 5]);
xticks([-4 0 4]);
yticks([-4 0 4]);
hold on
plot([0 0],[10 -10],'k:');
latextitle('Wurzelortskurve bei Vorderradlenkung');
latexxlabel('Re');
latexylabel('Im');
rotateY;
hold off

                % kritische Reglerverstärkung
kkrit=b*g/(v0*v0)

%%   Balancieren mit Vorderradlenkung
%
figure(2)
subplot(6,1,[1,2])
[Theta, T]=initial(RadVKreis, [0; theta0]);
plot(T, Theta*180/pi, 'b');
latextitle(['Balancierbewegung f\"ur $$k_P=$$',num2str(kP)])
%xlabel('Zeit in Sekunden');
xleer;
latexylabel('$$\theta$$  in Grad');
rotateY;
axis([0, 5, 0, 4]);
yticks([0 2 4]);
%
%
Adis=expm((A-B*kP*C)*Ts);
kend=200;
Theta=[];
Theta(1,:)=[0 theta0];
Beta(1)=-kP*theta0;
Phi(1)=phi0;
Alpha(1)=0;
V(1)=v0;
Xi=xi0;
Eta=eta0;
for k=1:kend
   Theta(k+1,:)=(Adis*Theta(k,:)')';
   Beta(k+1)=-kP*Theta(k+1,2);
   Phi(k+1)=Phi(k)+v0/b*tan(Beta(k+1))*Ts;
   Alpha(k+1)=atan(a/b*tan(Beta(k+1)));
   V(k+1)=v0/cos(Alpha(k+1));
   Xi(k+1)=Xi(k)+V(k+1)*Ts*cos(Alpha(k+1)+Phi(k+1));
   Eta(k+1)=Eta(k)+V(k+1)*Ts*sin(Alpha(k+1)+Phi(k+1));
end
Zeit=0:Ts:(kend)*Ts;
subplot(6,1,[3,4])
plot(Zeit, Beta*180/pi, 'b', Zeit, Alpha*180/pi, 'b--');
%latextitle('Lenkwinkel bei Vorderradlenkung');
latexxlabel('$$t$$ in s');
latexylabel('$$\beta, \alpha$$ in Grad');
rotateY;
yticks([-10 0]);
axis([0 5 -15 5]);
xticks([0 1 2 3 4 5]);


%%   Wurzelortskurve für Hinterradlenkung
%
%   
figure(1)
subplot(4,4,[3 4 7 8])
RadH=tf(k*[1 s0], [1 -s1-s2 s1*s2]);
[P, Z]=pzmap(RadH);
plot(real(P), imag(P), 'bx', real(Z), imag(Z), 'bo');
hold on;
Kvek=[0:0.005:15];
R=rlocus(RadH, Kvek);
plot([10 -10],[0,0],'k:');
plot(real(R(1,:)), imag(R(1,:)), 'b', real(R(2,:)), imag(R(2,:)), 'b');
axis('square');
axis([-6 4 -5 5]);
xticks([-4 0 4]);
yticks([-4 0 4]);
hold on
plot([0 0],[10 -10],'k:');
latextitle('Wurzelortskurve bei Hinterradlenkung');
latexxlabel('Re');
%latexylabel('Im');
%rotateY;
yleer;
hold off
 


%%  Kraft Fz für Vorderrad- und Hinterradlenkung
%   bei rampenförmiger Veränderung von Beta
%
figure(3)
Ts=0.02;
Zeit=0:Ts:4;
FzV=0;
FzH=0;
Beta=0;
for k=1:2/Ts
   Beta(k+1)=4/180*pi*k*Ts;
   cosBeta=cos(Beta(k+1));
   tanBeta=tan(Beta(k+1));
   FzV(k+1)=m*v0*v0/b*tanBeta+ m*a*v0/b/cosBeta/cosBeta*4/180*pi/2;
   FzH(k+1)=m*v0*v0/b*tanBeta- m*a*v0/b/cosBeta/cosBeta*4/180*pi/2;
end
for k=2/Ts+1:4/Ts
   Beta(k+1)=Beta(k);
   FzV(k+1)=m*v0*v0/b*tanBeta;
   FzH(k+1)=m*v0*v0/b*tanBeta;
end
subplot(6,1,[1,2])
plot(Zeit, Beta, 'b');
latextitle('Balancierkraft');
latexylabel('$$\beta$$ in rad');
rotateY;
axis([0 4 0 0.15]);
yticks([0 0.15]);
xleer;
%
subplot(6,1,[3,4])
plot(Zeit, FzV, 'b--', Zeit, FzH, 'b');
%latextitle('Balancierkraft');
latexylabel('$$F_{\rm z}$$  in N');
rotateY;
latexxlabel('$$t$$ in s');
axis([0 4 -5 40]);
xticks([0 1 2 3 4 5]);
yticks([0 20 40]);


%%  Kraft Fz für Vorderrad- und Hinterradlenkung
%   bei sinusförmiger Veränderung von Beta
%
%   
figure(4)
Ts=0.02;
Zeit=0:Ts:4;
Omega=2;
FzV=0;
FzH=0;
Beta=0;
for k=1:4/Ts
   Beta(k+1)=0.1*sin(Omega*k*Ts);
   BetaPunkt=0.1*cos(Omega*k*Ts);
   cosBeta=cos(Beta(k+1));
   tanBeta=tan(Beta(k+1));
   FzV(k+1)=m*v0*v0/b*tanBeta+ m*a*v0/b/cosBeta/cosBeta*BetaPunkt;
   FzH(k+1)=m*v0*v0/b*tanBeta- m*a*v0/b/cosBeta/cosBeta*BetaPunkt;
end
subplot(6,1,[1,2])
plot(Zeit, Beta, 'b');
latextitle('Balancierkraft');
latexylabel('$$\beta$$ in rad');
rotateY;
axis([0 4 -0.12 0.12]);
yticks([-0.1 0 0.1]);
xleer;
%
subplot(6,1,[3,4])
plot(Zeit, FzV, 'b--', Zeit, FzH, 'b');
%latextitle('Balancierkraft');
latexylabel('$$F_{\rm z}$$  in N');
rotateY;
latexxlabel('$$t$$ in s');
axis([0 4 -30 30]);
xticks([0 1 2 3 4 5]);
yticks([0 20 40]);


%%  Nichtminimalphasenverhalten des geregelten Fahrrades
%
figure(5)
%  Modell des geregelten Fahrrades
%  P-Regler  u(t) = - kP(y(t) - w(t))   -> bleibende Regelabweichung
%  Ausgang = Regelabweichung e(t) = w(t)-y(t)
RadVRegelabw=ss(A-B*kP*C, B*kP, -C, 1);
disp('Pole und Nullstellen');
[P, N]=pzmap(RadVRegelabw)
[E, T]=step(RadVRegelabw);
subplot(2,1,1)
plot(T, E, 'b',[0 5],[0 0],'k:');
hold on
plot([-0.3 0 0],[0 0 E(1,1)], 'b');
latexxlabel('$$t$$ in s');
latexylabel('$$\theta-\theta_{\rm Soll}$$  in Grad');
rotateY;
axis([-0.3 4 -3 3]);
xticks([0 1 2 3 4 5]);
yticks([-2 0 2]);
latextitle('Regelabweichung bei sprungf\"ormiger Ver\"anderung des Sollwertes');
hold off;
 

%%   ... jetzt mit PI-Regler
%
% mit PI-Regler
figure(6)
subplot(2,1,1)
kI=1;
Ag=[A-B*kP*C  -B*kI; C 0];
Bg=[B*kP; -1];
Cg=[-C 0];   % Ausgang ist die Regelabweichung
Dg=1;
RadVRegelabwI=ss(Ag, Bg, Cg, Dg);
[E, T]=step(RadVRegelabwI);
plot(T, E, 'b');
hold on
plot([-0.3 0 0],[0 0 E(1,1)],'b',[-1 6],[0 0],'k:');
latexxlabel('$$t$$ in s');
latexylabel('$$\theta_{\rm Soll}-\theta$$  in Grad');
rotateY;
axis([-0.3 6 -2 1.3]);
xticks([0 1 2 3 4 5 6]);
yticks([-1 0 1]);
latextitle('Regelabweichung bei sprungf\"ormiger Ver\"anderung des Sollwertes');
hold off;


%%     Figures as .EPS- or .PS-files
%
Epsbildc15('raddyn');
