robot.m

来自「realize analysis and design for computer」· M 代码 · 共 53 行

function [sys, x0] = robmech(t,x,u,flag)
% S-function for robot
% 
% Computer Controlled Systems (3rd ed)
% Author: B. Wittenmark
% Last edit: 1996-12-08
% Copyright (c) 1996 by K. J. 舠tr鰉, B. Wittenmark and
% Department of Automatic Control, Lund Institute of
% Technology, Lund, Sweden
%
% State space matrices.
J1=10/9;
J2=10;
k=1;
d=0.1;
kI=1;
%om0=1; 
om0=sqrt(k*(J1+J2)/(J1*J2));
al=J1/(J1+J2);
be1=d/(J1*om0);
be2=d/(J2*om0);
ga=kI/(J1*om0);
de=1/(J1*om0);
%
A=om0*[  0     1    -1
         al-1 -be1   be1
         al    be2  -be2];
B=[0  0
   ga de
   0  0];
C=[ 0 0 om0
1 0 0
0 1 0
0 0 1];
D=[0 0];
% If no right hand arguments then return the number of states, outputs 
% and inputs.
if nargin==0,  sys=[3,0,4,2,0,0]; x0 = zeros(3,1); return, end

%
% Linear Systems Description

if abs(flag) == 1
        sys = A*x + B*u;        % Derivatives
elseif flag == 3
        sys =  C*x + D*u;       % Outputs
elseif abs(flag) == 0
        sys=[3,0,4,2,0,0]; x0 = zeros(3,1);
else
        sys = [];               % Real time update (ignored).
end