%      Dynamometer.M
%   
%  Supervision of a dynamometer
%  
% J. Lunze
% 
% 31.01.2023
% Version v. 7.2.2023
%
echo off
clear
close all

%%   Parameters
%              Units: m, s, kg, N
r2=1.0;      % Radius of the roll in m
r1=0.310;      % Radius of the wheel, type R15 in m
J1=1.2;        % moment of inertia of a vehicle wheel in kg m^2 
J2=8;        % moment of intertia of the roll in kg m^2
m=1500/4;      % 1/4 of the mass of a vehicle in kg
g=9.81;        % gravity constant in m/s^2
Fz=m*g;        % 1/4 of the mass force in N
%
%   Typical values:
%  Fx: required braking force for a wheel: 1/4 mass force of the vehicle
%  m = 1500 kg
%  Fx = m/4*g/2   1840 N
%  MB = Fx*r1 = 570 Nm
%  MR = Fx*r2 = 1840 Nm
%  power of the system: Fx*v = 50 kW
%  velocity 100 km/h corresponds to omega1=100 km/h / r1 = 900 rad/s
%
%  Formel und Parameter aus Kiencke/Nielson S. 241 (aus AT: carbrake.m)
c1Asphalt=1.2801;   % trockener Asphalt
c2Asphalt=23.99;
c3Asphalt=0.52;
c1Nass=0.857;       % nasser Asphalt
c2Nass=33.822;
c3Nass=0.347;
c1Eis=0.1946;       % Eis
c2Eis=94.129;
c3Eis=0.0646;
% muAsphalt=c1Asphalt*(ones(size(lambda))-exp(-c2Asphalt*lambda))-c3Asphalt*lambda;
% muNass=c1Nass*(ones(size(lambda))-exp(-c2Nass*lambda))-c3Nass*lambda;
% muEis=c1Eis*(ones(size(lambda))-exp(-c2Eis*lambda))-c3Eis*lambda;
%  Linear approximation of mu/lambda curve
lambda=0.03;
mu=c1Asphalt*(1-exp(-c2Asphalt*lambda))-c3Asphalt*lambda;
klm=mu/lambda;                        % result: 21.3
%
%  Force of the blocking wheel on wet asphalt
lambda=1;
muBlock=c1Nass*(1-exp(-c2Nass*lambda))-c3Nass*lambda; % 0.51
%FxBlock=muBlock*Fz;
FxBlock=750;
%
%  Test signals
MB1=170;            % braking moment in Nm        350
MB2=330;            %                             750
MRmax=600;          % accelerating moment in Nm   100
%  0...t1:  acceleration by MR only
%  t1...t2: braking moment MB is active
%  t2...Tend: higher braking moment MB
Tend=6; 
dt=0.01;   %  time constant of omega_2 feedback loop: 0.005 s
Time=[0:dt:Tend];
t0=Tend/10;
t1=Tend/3;       % Tend/3
t2=2*Tend/3;     % 2*Tend/3
k0e=ceil(t0/dt);
k1e=ceil(t1/dt);
k2e=ceil(t2/dt);
k3e=length(Time);
MB=[zeros(1,k1e) MB1*ones(1,k2e-k1e) MB2*ones(1,k3e-k2e)];
MR=[zeros(1,k0e) MRmax*ones(1,k3e-k0e)];
%
omega10=0;
omega20=r1/r2*omega10;
%
%    Evaluation of the signals by the supervision system
dtm=0.1;             % sampling time of the measurement    0.2
Tm=ceil(dtm/dt);     % ratio of simulation step dt and measurement step dtm
Nm=floor(k3e/Tm);    % Number of measurement points during simulation time interval Tend
%    Threshold for the residual
rbar=450;




%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%  Simulation for stiff couplings between roll and wheel
%  ensuring that the angular velocities satisfy  r1*omega_1 = r2*omega_2
% 
%
%  The feedback loop consisting of the constraints c7-c6-c5-c4-c3
% is very fast and approximated here by a static relation to get 
% the force Fx and omega_2
% then constraints c4-c2-c1 are used to get omega_1
figure(1)
Omega1=[];
Omega2=[];
Omega1(1)=omega10;
Omega2(1)=omega20;
Fx=[];
Fx(1)=0;
for k1=1:k3e-1
      Fx(k1+1)=(r2/J2*MR(k1)+r1/J1*MB(k1))/(r1*r1/J1+r2*r2/J2);
      M1=r1*Fx(k1+1);
      M2=r2*Fx(k1+1);
      Omega2(k1+1)=Omega2(k1)+(-M2+MR(k1))/J2*dt; % integration
end
%
subplot(4,1,[1,2]);
plot(Time, MR, 'b--', Time, MB, 'b');
xleer;
%latexylabel({'$$M_{\rm B}, M_{\rm R}$$', 'in Nm'});
rotateY;
yticks([0 300 600]);
latextext(5,500,'$$M_{\rm R}$$');
latextext(5,230,'$$M_{\rm B}$$');
xticksempty([0 2 4 6 8]);
axis([0 Tend -40 1.1*MRmax]);
%
subplot(4,1,[3 4]);
plot(Time, 3.6*r2*Omega2, 'b--');
hold on
%
%  Determination of omega_1
%  after determining the corresponding slip
for k1=1:k3e
    mu=Fx(k1)/Fz;
    lambda=mu/klm;
    Omega1(k1)=(1-lambda)*r2*Omega2(k1)/r1;
end
subplot(4,1,[3 4]);
plot(Time, 3.6*r1*Omega1, 'b');
%latexylabel({'$$r_1 \omega_1$$', '$$r_2 \omega_2$$'}); % in km/h
rotateY;
latextext(3.8,100,'$$r_1 \omega_1$$');
latextext(5.1,120,'$$r_2 \omega_2$$');
latexxlabel('$$t$$ in s');
xticks([0 2 4 6 8]);
yticks([0 50 100 150 200]);
axis([0 Tend -10 200]);
%


%%   Evaluation of the measurements of the faultless test
%
figure(2)
%  Measured signals: MR, omega1, omega2
Omega1m=[];
Omega2m=[];
MRm=[];
Timem=[];
for k2=1:Nm
    Omega1m(k2)=Omega1(Tm*k2);
    Omega2m(k2)=Omega2(Tm*k2);
    MRm(k2)=MR(Tm*k2);
    Timem(k2)=Time(Tm*k2);
end
%
%  Fault detection and estimation of braking moment
R2=[];             % residual
R2(1)=0;
R2(2)=0;
MBhat=[];          % estimated braking moment
MBhat(1)=0;
MBhat(2)=0;
Omega1dot=[];
Omega1dot(1)=0;
Omega1dot(2)=0;
Omega2dot=[];
Omega2dot(1)=0;
Omega2dot(2)=0;
Fxc=0;
k2=0;
for k2=2:length(Timem)
    Omega2dot=(Omega2m(k2)-Omega2m(k2-1))/dtm;       % constraint d2
    Omega1dot=(Omega1m(k2)-Omega1m(k2-1))/dtm;       % constraint d1
    if abs(Omega2m(k2))>0.1
       lambda=(r2*Omega2m(k2)-r1*Omega1m(k2))/r2/Omega2m(k2); % c6
       lambda=max(0,lambda);
       mu=c1Asphalt*(1-exp(-c2Asphalt*lambda))-c3Asphalt*lambda; % c5
       Fxc=mu*Fz;                                          % c4
       R2(k2)=J2*Omega2dot-MRm(k2)+r2*Fxc;                 % c7
       M1=r1*Fxc;                                          % c2
       MBhat(k2)=-J1*Omega1dot+M1;                          % c1
    else
        R2(k2)=0;
        MBhat(k2)=0;
    end
end
%  smoothing of r2
for k2=3:length(R2)-1
    R2(k2)=(R2(k2-1)+R2(k2)+R2(k2-2))/3;
end
for k2=2:length(R2)-1
    R2(k2)=(R2(k2-1)+R2(k2))/2;
end
%  smoothing of MBhat
for k2=3:length(MBhat)-1
    MBhat(k2)=(MBhat(k2-1)+MBhat(k2))/2;
end
subplot(4,1,[1,2]);
%  Evaluation of the residual
fill([0 Tend Tend 0 0], [rbar, rbar 0 0 rbar],  [0.9,0.9,0.9]);
hold on
plot(Timem,R2,'b--');
subplot(4,1,[3,4]);
plot(Timem,MBhat,'b--');
MBhat1=MBhat;     % store to plot it again
hold on



%%   Test with flat tire
%
figure(3)
%  Assumption: wheel reacts as on wet asphalt
%  
Omega1=[];
Omega2=[];
Omega1(1)=omega10;
Omega2(1)=omega20;
Fxc=0;
for k1=1:k3e-1
      Fxc=(r2/J2*MR(k1)+r1/J1*MB(k1))/(r1*r1/J1+r2*r2/J2);
      if Fxc>FxBlock               % fault situation
          Fxc=FxBlock;
          Omega1(k1+1)=Omega1(k1)+(-MB(k1)+r1*FxBlock)/J1*dt;
          Omega1(k1+1)=max(Omega1(k1+1), 0);
          M2=r2*Fxc;                                  % c3
          Omega2(k1+1)=Omega2(k1)+(-M2+MR(k1))/J2*dt; % integration
      else
          M2=r2*Fxc;                                 % c3
          Omega2(k1+1)=Omega2(k1)+(-M2+MR(k1))/J2*dt; % integration
          mu=Fxc/Fz;                                 % c4
          lambda=mu/klm;                            % c5
          Omega1(k1+1)=(1-lambda)*r2*Omega2(k1+1)/r1;   % c6
      end
end
%
subplot(4,1,[1,2]);
% ax=gca;
% ax.PositionConstraint='outerposition';
plot(Time, 3.6*r1*Omega1, 'b', Time, 3.6*r2*Omega2, 'b--');
hold on;
latexxlabel('$$t$$ in s');
%latexylabel({'$$r_1 \omega_1, r_2 \omega_2$$', 'in km/h'});
rotateY;
latextext(4.5,57,'$$r_1 \omega_1$$');
latextext(5,130,'$$r_2 \omega_2$$');
xticks([0 2 4 6 8]);
yticks([0 50 100 150 200]);
axis([0 Tend -10 200]);

%%   Evaluation of the measurements with flat tire
%
figure(2)
%  Measured signals: MR, omega1, omega2
Omega1m=[];
Omega2m=[];
MRm=[];
Timem=[];
for k2=1:Nm
    Omega1m(k2)=Omega1(Tm*k2);
    Omega2m(k2)=Omega2(Tm*k2);
    MRm(k2)=MR(Tm*k2);
    Timem(k2)=Time(Tm*k2);
end
%
%  Fault detection
R2=[];
R2(1)=0;
R2(2)=0;
MBhat=[];
MBhat(1)=0;
MBhat(2)=0;
Omega1dot=[];
Omega1dot(1)=0;
Omega1dot(2)=0;
Omega2dot=[];
Omega2dot(1)=0;
Omega2dot(2)=0;
Fxc=0;
for k2=3:length(Timem)
    Omega2dot(k2)=(Omega2m(k2)-Omega2m(k2-1))/dtm;       % constraint d2
    Omega1dot(k2)=(Omega1m(k2)-Omega1m(k2-1))/dtm;       % constraint d1
    if Omega2m(k2)~=0
       lambda=(r2*Omega2m(k2)-r1*Omega1m(k2))/r2/Omega2m(k2);
       mu=c1Asphalt*(1-exp(-c2Asphalt*lambda))-c3Asphalt*lambda;
       Fxc=mu*Fz;
       R2(k2)=J2*Omega2dot(k2)-MRm(k2)+r2*Fxc;                 % c7
       M1=r1*Fxc;
       MBhat(k2)=-J1*Omega1dot(k2)+M1;
    else
       R2(k2)=0;
       MBhat(k2)=0;
    end
end
%  smoothing of r2
for k2=3:length(R2)-1
    R2(k2)=(R2(k2-1)+R2(k2)+R2(k2-2))/3;
end
for k2=2:length(R2)-1
    R2(k2)=(R2(k2-1)+R2(k2))/2;
end
% %  smoothing of MBhat
% for k2=3:length(MBhat)-1
%     MBhat(k2)=(MBhat(k2-1)+MBhat(k2)+MBhat(k2-2))/3;
% end
subplot(4,1,[1,2]);
plot(Timem,R2,'b');
hold on
latexylabel('$$r_2$$');
rotateY;
xleer;
yticks([0 1000 2000 3000 4000]);
%  determination of the event time: time at which R2>rbar
tevent=0;
k2=1;
while R2(k2)<rbar & k2<length(Timem)
      k2=k2+1;
end
if k2<length(Timem)
   tevent=k2*dtm;
   plot(tevent,R2(k2),'bo');
end
%
subplot(4,1,[3,4]);
MBhatmax=1500;
if tevent~=0
   fill([tevent Tend Tend tevent tevent],...
       [MBhatmax, MBhatmax 0 0 MBhatmax],  [0.9,0.9,0.9]);
   hold on
end
plot(Timem,MBhat,'b', Timem,MBhat1,'b--');
latexxlabel('$$t$$ in s');
latexylabel('$$\hat{M}_{\rm B}$$');  % in Nm
rotateY;
xticks([0 2 4 6 8]);
yticks([0 500 1000 1500]);
axis([0 Tend 0 MBhatmax]);
%
%  Mark the fault detection time in the preceding figure
figure(3);
subplot(4,1,[1,2]);
plot([tevent tevent],[-10 200],'b:');



%% Figures in *.eps
%
epsfigc('Dynamometer');



%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%
% %  Data for Simulink model
% c1=1.2801;   % trockener Asphalt
% c2=23.99;
% c3=0.52;
% %  Simulink model  DynamoDiag.slx
% %  Parameters: J1, J2, r1, r2, c1, c2, c3 (for adhesion-slip curve)
% %  Simulation parameters: Tend, dt
% %  Inputs: MRmax (constant), MB (time function)
% %  Outputs: M2, Omega2, Lambda, Mu, Omega1 (time functions)
% omega10=0.1;
% omega20=r1/r2*omega10;
% Omega1=[];
% Omega2=[];
% Lambda=[];
% Mu=[];
% M2=[];
% sim('DynamoDiag');
% 
% 
% %%
% 
% MRmax=0;
% omega10=3000;
% omega20=2900;
% Omega1=[];
% Omega2=[];
% Lambda=[];
% Mu=[];
% M2=[];
% sim('DynamoDiag');
% figure(5)
% plot(Time, r1*Omega1, 'b--', Time, r2*Omega2, 'r--');

