animc2p.m

二阶倒立摆的simulink模型建模仿真。以及初始化代码

function [sys,x0,str,ts] = sfuntmpl(t,x,u,flag)
%SFUNTMPL General M-file S-function template
%   With M-file S-functions, you can define you own ordinary differential
%   equations (ODEs), discrete system equations, and/or just about
%   any type of algorithm to be used within a Simulink block diagram.
%
%   The general form of an M-File S-function syntax is:
%       [SYS,X0,STR,TS] = SFUNC(T,X,U,FLAG,P1,...,Pn)
%
%   What is returned by SFUNC at a given point in time, T, depends on the
%   value of the FLAG, the current state vector, X, and the current
%   input vector, U.
%
%   FLAG   RESULT             DESCRIPTION
%   -----  ------             --------------------------------------------
%   0      [SIZES,X0,STR,TS]  Initialization, return system sizes in SYS,
%                             initial state in X0, state ordering strings
%                             in STR, and sample times in TS.
%   1      DX                 Return continuous state derivatives in SYS.
%   2      DS                 Update discrete states SYS = X(n+1)
%   3      Y                  Return outputs in SYS.
%   4      TNEXT              Return next time hit for variable step sample
%                             time in SYS.
%   5                         Reserved for future (root finding).
%   9      []                 Termination, perform any cleanup SYS=[].
%
%
%   The state vectors, X and X0 consists of continuous states followed
%   by discrete states.
%
%   Optional parameters, P1,...,Pn can be provided to the S-function and
%   used during any FLAG operation.
%
%   When SFUNC is called with FLAG = 0, the following information
%   should be returned:
%
%      SYS(1) = Number of continuous states.
%      SYS(2) = Number of discrete states.
%      SYS(3) = Number of outputs.
%      SYS(4) = Number of inputs.
%               Any of the first four elements in SYS can be specified
%               as -1 indicating that they are dynamically sized. The
%               actual length for all other flags will be equal to the
%               length of the input, U.
%      SYS(5) = Reserved for root finding. Must be zero.
%      SYS(6) = Direct feedthrough flag (1=yes, 0=no). The s-function
%               has direct feedthrough if U is used during the FLAG=3
%               call. Setting this to 0 is akin to making a promise that
%               U will not be used during FLAG=3. If you break the promise
%               then unpredictable results will occur.
%      SYS(7) = Number of sample times. This is the number of rows in TS.
%
%
%      X0     = Initial state conditions or [] if no states.
%
%      STR    = State ordering strings which is generally specified as [].
%
%      TS     = An m-by-2 matrix containing the sample time
%               (period, offset) information. Where m = number of sample
%               times. The ordering of the sample times must be:
%
%               TS = [0      0,      : Continuous sample time.
%                     0      1,      : Continuous, but fixed in minor step
%                                      sample time.
%                     PERIOD OFFSET, : Discrete sample time where
%                                      PERIOD > 0 & OFFSET < PERIOD.
%                     -2     0];     : Variable step discrete sample time
%                                      where FLAG=4 is used to get time of
%                                      next hit.
%
%               There can be more than one sample time providing
%               they are ordered such that they are monotonically
%               increasing. Only the needed sample times should be
%               specified in TS. When specifying than one
%               sample time, you must check for sample hits explicitly by
%               seeing if
%                  abs(round((T-OFFSET)/PERIOD) - (T-OFFSET)/PERIOD)
%               is within a specified tolerance, generally 1e-8. This
%               tolerance is dependent upon your model's sampling times
%               and simulation time.
%
%               You can also specify that the sample time of the S-function
%               is inherited from the driving block. For functions which
%               change during minor steps, this is done by
%               specifying SYS(7) = 1 and TS = [-1 0]. For functions which
%               are held during minor steps, this is done by specifying
%               SYS(7) = 1 and TS = [-1 1].

%   Copyright 1990-2001 The MathWorks, Inc.
%   $Revision: 1.17 $

%
% The following outlines the general structure of an S-function.
%


global AnimC2pFigH AnimC2pFigTitle AnimC2pAxisH

switch flag,

  %%%%%%%%%%%%%%%%%%
  % Initialization %
  %%%%%%%%%%%%%%%%%%
  case 0,
    [sys,x0,str,ts]=mdlInitializeSizes;

  %%%%%%%%%%%%%%%
  % Derivatives %
  %%%%%%%%%%%%%%%
  case 1,
    sys=mdlDerivatives(t,x,u);

  %%%%%%%%%%
  % Update %
  %%%%%%%%%%
  case 2,
    sys=mdlUpdate(t,x,u);

  %%%%%%%%%%%
  % Outputs %
  %%%%%%%%%%%
  case 3,
    sys=mdlOutputs(t,x,u);

  %%%%%%%%%%%%%%%%%%%%%%%
  % GetTimeOfNextVarHit %
  %%%%%%%%%%%%%%%%%%%%%%%
  case 4,
    sys=mdlGetTimeOfNextVarHit(t,x,u);

  %%%%%%%%%%%%%
  % Terminate %
  %%%%%%%%%%%%%
  case 9,
    sys=mdlTerminate(t,x,u);

  %%%%%%%%%%%%%%%%%%%%
  % Unexpected flags %
  %%%%%%%%%%%%%%%%%%%%
  otherwise
    error(['Unhandled flag = ',num2str(flag)]);

end

% end sfuntmpl

%
%=============================================================================
% mdlInitializeSizes
% Return the sizes, initial conditions, and sample times for the S-function.
%=============================================================================
%
function [sys,x0,str,ts]=mdlInitializeSizes

%
% call simsizes for a sizes structure, fill it in and convert it to a
% sizes array.
%
% Note that in this example, the values are hard coded.  This is not a
% recommended practice as the characteristics of the block are typically
% defined by the S-function parameters.
%

global AnimC2pFigH AnimC2pFigTitle AnimC2pAxisH
global poleI_length
sizes = simsizes;

sizes.NumContStates  = 0;
sizes.NumDiscStates  = 0;
sizes.NumOutputs     = 0;
sizes.NumInputs      = 3;
sizes.DirFeedthrough = 1;
sizes.NumSampleTimes = 1;   % at least one sample time is needed

sys = simsizes(sizes);

%
% initialize the initial conditions
%
x0  = [];

%
% str is always an empty matrix
%
str = [];

%
% initialize the array of sample times
%
ts  = [0 0];
%
% to add your init code here
%
AnimC2pFigTitle='Two Inverted Pendum';
AnimC2pFigH = figure( ...
            'Name', AnimC2pFigTitle, ...
            'NumberTitle', 'off');
        figPos = get(AnimC2pFigH, 'position');
      
        % ====== proportion of UI frame and axes
        ui_area = 0.2;
        axis_area = 1-ui_area;
        % ====== animation area 
        axisPos = [0+20 figPos(4)*ui_area figPos(3) figPos(4)*axis_area];
        % weird thing: if you don't use normalized unit for
        % axes, patch for ground doesn't appear
        axisPos = axisPos./[figPos(3) figPos(4) figPos(3) figPos(4)];
        AnimC2pAxisH = ...
            axes('unit', 'normal', 'pos', axisPos, 'visible', 'on');
        
        
         % ###### animation objects ######
        % ====== cart
        cart_height = 0.2;
        cart_length = 0.4;
        cart = cart_length/2*[-1 1 1 -1 -1] + ...
            j*(cart_height/2*[-1 -1 1 1 -1]-cart_height/2 +.005);
        cartH = patch(real(cart), imag(cart), 'm');
        set(cartH, 'erase', 'xor');
        set(cartH, 'userdata', cart);
        % ====== axis settings
        pos_range = [-2 2];
        set(AnimC2pAxisH, 'clim', [1 64], ...
            'xlim', pos_range, ...
            'ylim', [-cart_height 1*1.2], ...
            'box', 'on');
        axis equal;
        set(AnimC2pAxisH, 'visible', 'on');
        % ====== pole I
        poleI_length = 0.2; % this must be the same as plant block
        poleI_radius = 0.02;
        poleI = poleI_radius*[-1 1 1 -1 -1] + ...
            j*(poleI_length/2*[-1 -1 1 1 -1]+poleI_length/2+.003);
        poleIH = patch(real(poleI), imag(poleI), 'y');
        set(poleIH, 'erase', 'xor', 'clipping', 'off');
        set(poleIH, 'userdata', poleI);
        % ====== poleII
        poleII_length = 0.7; % this must be the same as plant block
        poleII_radius = 0.02;
        poleII = poleII_radius*[-1 1 1 -1 -1] + ...
            j*(poleII_length/2*[-1 -1 1 1 -1]+poleII_length/2 + poleI_length + .03);%相对位置有问题
        poleIIH = patch(real(poleII), imag(poleII), 'y');
        set(poleIIH, 'erase', 'xor', 'clipping', 'off');
        set(poleIIH, 'userdata', poleII);
      
        % ====== force arrow
        force_x = [-1 0 nan -0.1 0 -0.1];
        force_y = [0 0 nan 0.1 0 -0.1];
        force = force_x + j*force_y;
        forceH = line(real(force), imag(force), ...
            'erase', 'xor', 'color', 'c', 'clip', 'off');
        set(forceH, 'userdata', force, ...
            'xdata', [0], 'ydata', [0]);
        % ====== reference triangle
        ref = cart_length/2*[-1 1 0 -1] + ...
            j*cart_height*([0 0 1 0] - 1.1);
        ref = ref - 2*j*cart_height;
        refH = line(real(ref), imag(ref));
        set(refH, 'color', 'g', 'linewidth', 2);
    %   refH = patch(real(ref), imag(ref), 'g');
        set(refH, 'erase', 'background');
        set(refH, 'userdata', ref);
        
        
         % ====== append the handles as third row of userdata
        tmp = [cartH poleIH poleIIH forceH refH -1 -1 -1 -1 -1 -1];
        set(AnimC2pFigH, 'userdata', [get(AnimC2pFigH, 'userdata'); tmp]);


% end mdlInitializeSizes

%
%=============================================================================
% mdlDerivatives
% Return the derivatives for the continuous states.
%=============================================================================
%
function sys=mdlDerivatives(t,x,u)

sys = [];

% end mdlDerivatives

%
%=============================================================================
% mdlUpdate
% Handle discrete state updates, sample time hits, and major time step
% requirements.
%=============================================================================
%
function sys=mdlUpdate(t,x,u)


global AnimC2pFigH AnimC2pFigTitle AnimC2pAxisH
global poleI_length
sys = [];

%
% to add your Update code here by lzm
%
    pos = u(1); theta1 = u(2); theta2 = u(3); %curr_force = u(3); curr_ref = u(4);
    tmp = get(AnimC2pFigH, 'userdata');
    objectH = tmp(1, :);
    
    % ====== update cart
    cartH = objectH(1);
    cart = get(cartH, 'userdata');
    new_cart = cart + pos; 
    set(cartH, 'xdata', real(new_cart), 'ydata', imag(new_cart));
    % ====== update pole I
    poleIH = objectH(2);
    poleI = get(poleIH, 'userdata');

        
    new_poleI = poleI*exp(-j*theta1) + pos;
    set(poleIH, 'xdata', real(new_poleI), 'ydata', imag(new_poleI));
    % ====== update pole II
    poleIIH = objectH(3);
    poleII = get(poleIIH, 'userdata');

  
    new_poleII = (real(poleII) + j*(imag(poleII) - poleI_length))*exp(-j*theta2);  %先回到原点再转动
    new_poleII = real(new_poleII) + pos + poleI_length*sin(theta1) + j*(imag(new_poleII) + poleI_length*cos(theta1));%根据杆一的位置作平移
    set(poleIIH, 'xdata', real(new_poleII), 'ydata', imag(new_poleII));
    
% end mdlUpdate

%
%=============================================================================
% mdlOutputs
% Return the block outputs.
%=============================================================================
%
function sys=mdlOutputs(t,x,u)

sys = [];

% end mdlOutputs

%
%=============================================================================
% mdlGetTimeOfNextVarHit
% Return the time of the next hit for this block.  Note that the result is
% absolute time.  Note that this function is only used when you specify a
% variable discrete-time sample time [-2 0] in the sample time array in
% mdlInitializeSizes.
%=============================================================================
%
function sys=mdlGetTimeOfNextVarHit(t,x,u)

sampleTime = 1;    %  Example, set the next hit to be one second later.
sys = t + sampleTime;

% end mdlGetTimeOfNextVarHit

%
%=============================================================================
% mdlTerminate
% Perform any end of simulation tasks.
%=============================================================================
%
function sys=mdlTerminate(t,x,u)

sys = [];

% end mdlTerminate