%   Script CARBRKLIN.M
%
%  Lineares Fahrzeugmodell
%  1. Transformation in E/A-Normalform
%  2. Vorsteuerungsentwurf
%  3. Simulation des Regelkreises mit linearem Modell
%
% J. Lunze
% für 10. Auflage 27.1.2014    
% für 12. Auflage: 5.8.2019
%
%
clear
close all
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% Parameter:
%
%   Formel und Parameter aus Kiencke/Nielson S. 241
c1Nass=0.857;       % nasser Asphalt
c2Nass=33.822;
c3Nass=0.347;
c1Eis=0.1946;       % Eis
c2Eis=94.129;
c3Eis=0.0646;
c1Asphalt=1.2801;   % trockener Asphalt 
c2Asphalt=23.99;
c3Asphalt=0.52;
%  hier verwendete Koeffizienten:
c1=c1Asphalt;
c2=c2Asphalt;
c3=c3Asphalt;
%   Schlupfkennlinie linearisiert bei lambda=0.01
kml=c1*c2*exp(-c2*0.01)-c3;
%
%
%    Fahrzeugdaten (Viertelmodell)   
g=9.81;         % m/s^2
m=250;          % kg  (Masse von 1/4 eines Fahrzeugs)
r=0.27;         % m  (Radius eines Rades)
ri=0.17;        % m  innerer Radius des Reifens
ra=r;           % m  äußerer Radius des Reifens
mr=25;          % kg  Reifenmasse
J=m*(ri*ri+ra*ra)/2;      % Massenträgheitsmoment eines Rades 
%                    (berechnet für einen Hohlzylinder, [Dubbel, S. B32])
%    Anfangszustand
v0=120/3.6;     % m/s   Anfangsgeschwindigkeit 120 km/h
x0=0;           % m     Anfangsposition
omega0=v0/r;    % rad/s Anfangswinkelgeschwindigkeit des Rades
we=60/3.6;      % m/s   Endgeschwindigkeit 60 km/h
Te=6;           % Endzeit in Sekunden, kann bis 2.2s verkleinert werden
% Bremskraft u=Fb/m   in m/s^2  
%
dt=0.01;        % Schrittweite für Speicherung der Daten
T=0:dt:Te;
vinf=0.01;      % Abbruch der Berechnung von v
omegainf=0;     % Abbruch der Berechnung von omega
Tbeg=-1;        % grafische Darstellung beginnt bei Tbeg
Tend=10;        % Endzeit der grafischen Darstellung

%-------------------------------------------------------------------------
%% Linearisiertes Modell
%    Eingang: Beschleunigung in m/s^2
%    Ausgang: Geschwindigkeit in m/s
%   Arbeitspunkt
vquer=120/3.6;     % m/s   Geschwindigkeit 120 km/h
omegaquer=vquer/r;
uquer=0;
A=[-g*kml/vquer  g*kml*r/vquer; m*r*g*kml/J/vquer -m*r^2*g*kml/J/vquer];
B=[0; -m*r/J];
C=[1 0];
D=0;
Fahrzeug=ss(A, B, C, D);
%
%
%     Transformation in E/A-Normalform (r=n=2)
TE=inv([C; C*A]);
AE=inv(TE)*A*TE;
BE=inv(TE)*B;
CE=C*TE;
DE=D;
%
%------------------------------------------------------------------------
%%   Trajektorienplanung   aus CARBRKFORWCTR.m
figure(1)
%  Umsteuerung des Fahrzeugs von Geschwindigkeit v0=120 km/h in ve=60 km/h.
%  Die gesuchte Funktion ist die Solltrajektorie w(t).
%  Beschreibung der Trajektorie durch ein Polynom 3. Ordnung mit aw0=v0
%  Berechnung der Polynomkoeffizienten aw=[aw3; aw2; aw1] als Lösung der
%  Gleichung M aw= N, die die folgenden Forderungen wiedergibt:
%   w(0)=v0; wPunkt(0)=0, w(Te)=ve, wPunkt(Te)=0
aw0=v0;
aw1=0;
M=[Te^3 Te^2; 3*Te^2 2*Te];
N=[we-v0; 0];
aVektor=inv(M)*N;
aw3=aVektor(1,1);
aw2=aVektor(2,1);
WPoly=[aw3, aw2, aw1, aw0];   % Vektor der Polynomkoeffizienten von w(t)
W=polyval(WPoly, T);
WPunktPoly=[3*aw3, 2*aw2, aw1];
WPunkt=polyval(WPunktPoly, T);
WZweipunktPoly=[6*aw3, 2*aw2];
WZweipunkt=polyval(WZweipunktPoly, T);
subplot(6,1,[1,2]);
plot(T, W, 'b')
hold on
plot(0, v0, 'bo');
plot(Te, we, 'bo');
plot([Tbeg 0 0],[v0 v0 W(1,1)], 'b', [Te, Tend], [we, we], 'b');
axis([Tbeg, Tend 0 40]);
xleer;
yticks([0, 30]);
latexylabel('$$w$$ in $$\frac{\rm m}{\rm s}$$');
rotateY;
hold off
%
subplot(6,1,[3,4]);
plot(T, WPunkt, 'b')
hold on
plot([Tbeg 0 0],[0 0 WPunkt(1,1)], 'b', [Te, Tend], [0 0], 'b');
latexylabel('$$\frac{\mathrm{d}w}{\mathrm{d}t}$$ in $$\frac{\rm m}{\rm s^2}$$');
yticks([-3, 0]);
axis([Tbeg, Tend 1.1*min(WPunkt) 1]);
xleer;
rotateY;
hold off
%
subplot(6,1,[5,6]);
plot(T, WZweipunkt, 'b');
hold on
LengthWZweipunkt=length(WZweipunkt);
plot([Tbeg 0 0],[0 0 WZweipunkt(1,1)], 'b', [Te Te Tend], [WZweipunkt(LengthWZweipunkt),0,0], 'b');
latexylabel('$$\frac{\mathrm{d}^2w}{\mathrm{d}t^2}$$ in $$\frac{\rm m}{\rm s^3}$$');
yticks([-3, 0, 3]);
axis([Tbeg, Tend 1.2*min(WZweipunkt) 1.2*max(WZweipunkt)]);
rotateY;
latexxlabel('$$t$$ in s');
xticks([0 2 4 6 8 10]);
hold off
%
%-------------------------------------------------------------------------
%%  Vorsteuerung u=-1/bq(e1 e2 -1)(w wdot wddot)
figure(2)
bq=BE(2,1);
eIT=[AE(2,1) AE(2,2)];
U=-1/bq*(eIT(1,1)*W + eIT(1,2)*WPunkt - WZweipunkt);
kmax=length(U);
subplot(3,1,1)
plot(T,U, 'b')
hold on
plot([Tbeg 0 0],[0 0 U(1)], 'b', [Te, Te, Tend], [U(kmax) 0 0], 'b');
latexylabel('$$u$$ in $$\frac{\rm m}{\rm s^2}$$');
rotateY;
yticks([0, 5, 10]);
xleer;
axis([Tbeg Tend -0.3 1.1*max(U)]);
latextitle(' Abbremsen auf Asphalt');
hold off
%
%-------------------------------------------------------------------------
%% Verhalten des linearen Fahrzeugmodells bei dieser Vorsteuerung
figure(3)
%    Eintragen der oben berechneten Stellgröße
subplot(6,1,[1,2])
plot(T,U, 'b')
hold on
plot([Tbeg 0 0],[0 0 U(1)], 'b', [Te, Te, Tend], [U(kmax) 0 0], 'b');
latexylabel('$$u$$ in $$\frac{\rm m}{\rm s^2}$$');
rotateY;
yticks([0, 5, 10]);
xleer;
axis([Tbeg Tend -0.3 1.1*max(U)]);
latextitle(' Abbremsen auf Asphalt');
hold off
%
x0=[v0; omega0];
[Y,T,X]=lsim(Fahrzeug, U, T, x0);
%
subplot(6,1,[3,4]);
plot(T,Y,'b')
hold on
plot([Tbeg 0 0],[v0 v0 W(1,1)], 'b', [Te, Tend], [we, we], 'b');
latexylabel('$$v$$ in $$\frac{\rm m}{\rm s}$$');
rotateY;
yticks([15, 30]);
xticks([0 2 4 6 8 10]);
latexxlabel('$$t$$ in s');
axis([Tbeg Tend 0 40]);
hold off


%%    Bilder auf .EPS- und .PS-Dateien
%
Epsbildc15('carbrklin');
