level-2 matlab s-function error

Question

0 个投票

Hi,

please, I'd like to know what's wrong with the following code. When I run it, I get the following error message: "Derivative of state '1' in block 'model_4WID/Level-2 MATLAB S-Function3' at time 0.0 is not finite. The simulation will be stopped. There may be a singularity in the solution. If not, try reducing the step size (either by reducing the fixed step size or by tightening the error tolerances)"

I really need your help.

Thank you.

function dynamic(block)
 
setup(block);
  
function setup(block)
  % number of ports.
  block.NumInputPorts  = 1;
  block.NumOutputPorts = 1;
  
  % port properties.
  block.SetPreCompInpPortInfoToDynamic;
  block.SetPreCompOutPortInfoToDynamic;
  %input port properties.
  block.InputPort(1).DatatypeID  = 0;  % double
  block.InputPort(1).Complexity  = 'Real';
  block.InputPort(1).Dimensions  = 20;
  block.InputPort(1).DirectFeedthrough = false;
  
  %output port properties.
  block.OutputPort(1).DatatypeID  = 0; % double
  block.OutputPort(1).Complexity  = 'Real';
  block.OutputPort(1).Dimensions  = 18;
  
  % continuous states.
  block.NumContStates = 5;
   block.SampleTimes = [0 0];
  
  % Specify the block simStateCompliance. The allowed values are:
   %    'DefaultSimState', < Same SimState as a built-in block
  block.RegBlockMethod('InitializeConditions', @InitializeConditions);
 
 % block.RegBlockMethod('Start', @Start);
  block.RegBlockMethod('Outputs', @Outputs);
  block.RegBlockMethod('Derivatives', @Derivatives);
%endfunction
 
function InitializeConditions(block)
block.ContStates.Data(1) = 20;
block.ContStates.Data(2)= 0;
block.ContStates.Data(3)= 0;
block.ContStates.Data(4)= 0;
block.ContStates.Data(5)= 0;
%endfunction
function Outputs(block)
de=zeros(1,5); u=zeros(1,20);
for i=1:5
     de(i)=block.ContStates.Data(i); 
end
     
for i=1:22
    u(i)=block.InputPort(1).data(i); 
end
       
                                              %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
                                            %%%%%%%  non-linear dugoff tyre model %%%%%%%
                                             %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 
 
                                             
   Cz=0.001; %vertical deflection rate of the tyre  
   Cs=50000;
   epsilon=0.015;
   I=2.1;
   Iz=1627;
   R=0.35;
   Ca=30000;
   m=1298.9;
   lf=1; 
   lr=1.454;
   br=1.436;
   bf=br;
   mu=0.9;
   g=9.81;
   cons=m/(lr+lf);
   h=0.5;
   r=de(3); phi=atan2(de(2),de(1));
   C=[0 m*r 0 0 0;-m*r 0 0 0 0;0 0 0 0 0;0 0 0 cos(phi) -sin(phi);0 0 0 sin(phi) cos(phi)];
   M=[m 0 0 0 0;0 m 0 0 0;0 0 Iz 0 0;0 0 0 1 0;0 0 0 0 1];
   
   
   u1=(de(2)+lf*de(3))/(de(1)+0.5*bf*de(3));
   u2=(de(2)+lf*de(3))/(de(1)-0.5*bf*de(3));
   u3=(de(2)-lr*de(3))/(de(1)+0.5*br*de(3));    %% les ui représentent les rapports permettant de calculer les angles de dérives de chaque roue%%
   u4=(de(2)-lr*de(3))/(de(1)-0.5*br*de(3));
   U=[u1 u2 u3 u4];
   
   
                          %------vertical load/charge verticale:----------
                                   
       dde=(-de'*C)/M; %dérivée de de!
       
       Fz1= cons*(0.5*g*lr-0.5*dde(1)*h-lr*h*dde(2)/bf);
       Fz2= cons*(0.5*g*lr-0.5*dde(1)*h+lr*h*dde(2)/bf);
       Fz3= cons*(0.5*g*lr+0.5*dde(1)*h-lr*h*dde(2)/bf); 
       Fz4= cons*(0.5*g*lr+0.5*dde(1)*h+lr*h*dde(2)/bf);                        
       Fz=[Fz1 Fz2 Fz3 Fz4]';
  
            %%% velocity component in the wheel plane : is the longitunal velocity
       v1=(de(1)+0.5*bf*de(3))*cos(u(1))+(de(2)+lf*de(3))*sin(u(1));
       v2=(de(1)-0.5*bf*de(3))*cos(u(2))+(de(2)+lf*de(3))*sin(u(2));
       v3=(de(1)+0.5*br*de(3))*cos(u(3))+(de(2)-lr*de(3))*sin(u(3));
       v4=(de(1)-0.5*br*de(3))*cos(u(4))+(de(2)-lr*de(3))*sin(u(4));
       V=[v1 v2 v3 v4]';
       
   lamda=zeros(4,1);f=length(lamda);
   omega=zeros(4,1);
   alph=zeros(4,1);
   Re=zeros(4,1);
   S=zeros(4,1);
   dz=zeros(4,1);
   Fs=zeros(4,1);
   Ft=zeros(4,1);
  for i=1:4
      dz(i)=-Cz*Fz(i)+ 0.33*R;
      Re(i)=R-dz(i)./3;
      omega(i)=(-R*u(i+16)+u(i+4))/I;
      S(i)=1+omega(i)*Re(i)./V(i);
      alph(i)=atan(U(i))-u(i);
      lamda(i)= mu*Fz(i)*(1-S(i))*(1-epsilon*V(i)*sqrt((S(i))^2 + (tan(alph(i)))^2))/(2*sqrt((Cs^2)*(S(i))^2 + (Ca^2)*(tan(alph(i))^2)));
      if lamda(i)<1
         f(i)=lamda(i)*(2-lamda(i));
         Fs(i)=Ca*f(i)*tan(alph(i))/(1-S(i));
         Ft(i)=Cs*f(i)*S(i)/(1-S(i));
      elseif lamda(i)>1
          f(i)=1;
          Fs(i)=Ca*f(i)*tan(alph(i))/(1-S(i));
          Ft(i)=Cs*f(i)*S(i)/(1-S(i));
      end
  end
   
       
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%      
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Sorties %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%      
block.OutputPort(1).Data(1)=de(1);
block.OutputPort(1).Data(2)=de(2);
block.OutputPort(1).Data(3)= phi;
block.OutputPort(1).Data(4)=de(4);
block.OutputPort(1).Data(5)=de(5);
block.OutputPort(1).Data(6)=Ft(1);
block.OutputPort(1).Data(7)=Ft(2);
block.OutputPort(1).Data(8)=Ft(3);
block.OutputPort(1).Data(9)=Ft(4);
block.OutputPort(1).Data(10)=Fs(1);
block.OutputPort(1).Data(11)=Fs(2);
block.OutputPort(1).Data(12)=Fs(3);
block.OutputPort(1).Data(13)=Fs(4);
block.OutputPort(1).Data(14)= r;
block.OutputPort(1).Data(15)=Fz(1);
block.OutputPort(1).Data(16)=Fz(2);
block.OutputPort(1).Data(17)=Fz(3);
block.OutputPort(1).Data(18)=Fz(4);
   
%endfunction
function Derivatives(block)
m=1298.9; Iz=1627;lf=1; lr=1.454; br=1.436; bf=br;
de=zeros(1,5); u=zeros(1,22);
for i=1:5
     de(i)=block.ContStates.Data(i);   
end
     
for i=1:20
    u(i)=block.InputPort(1).Data(i); 
end
r=de(3); phi=atan2(de(2),de(1));
M=[m 0 0 0 0;0 m 0 0 0;0 0 Iz 0 0;0 0 0 1 0;0 0 0 0 1]; %matrice d'inertie
C=[0 m*r 0 0 0;-m*r 0 0 0 0;0 0 0 0 0;0 0 0 cos(phi) -sin(phi);0 0 0 sin(phi) cos(phi)];
F=[u(9)*cos(u(1))-u(13)*sin(u(1))+u(10)*cos(u(2))-u(14)*sin(u(2))+u(11)*cos(u(3))-u(15)*sin(u(3))+u(12)*cos(u(4))-u(16)*sin(u(4));u(13)*cos(u(1))+u(9)*sin(u(1))+u(14)*cos(u(2))+u(10)*sin(u(2))+u(15)*cos(u(3))+u(11)*sin(u(3))+u(16)*cos(u(4))+u(12)*sin(u(4));u(9)*(lf*sin(u(1))+0.5*bf*cos(u(1)))+u(10)*(lr*sin(u(2))-0.5*bf*cos(u(2)))+u(11)*(-lr*sin(u(3))+0.5*br*cos(u(3)))+u(12)*(-lr*sin(u(4))-0.5*br*cos(u(4)))+u(13)*(lf*sin(u(1))-0.5*bf*cos(u(1)))+u(14)*(lf*sin(u(2))+0.5*bf*cos(u(2)))+u(15)*(-lr*sin(u(3))-0.5*br*cos(u(3)))+u(16)*(-lr*sin(u(4))+0.5*br*cos(u(4)));0;0];
 X=(F'-C*de)/M;
 
block.Derivatives.Data(1) = X(1);
block.Derivatives.Data(2) = X(2);
block.Derivatives.Data(3) = X(3);
block.Derivatives.Data(4) = X(4);
block.Derivatives.Data(5) = X(5);
%endfunction
  