function inverse(block)
% Level-2 MATLAB file S-Function for times two demo.
% Copyright 1990-2009 The MathWorks, Inc.
% $Revision: 1.1.6.2 $
setup(block);
%endfunction
function setup(block)
%% Register number of input and output ports
block.NumInputPorts = 1;
block.NumOutputPorts = 1;
%% Setup functional port properties to dynamically
%% inherited.
block.SetPreCompInpPortInfoToDynamic;
block.SetPreCompOutPortInfoToDynamic;
% block.InputPort(1).DirectFeedthrough = true;
%% Set block sample time to inherited
block.SampleTimes = [-1 0];
%% Set the block simStateCompliance to default (i.e., same as a built-in block)
block.SimStateCompliance = 'DefaultSimState';
%% Run accelerator on TLC
% block.SetAccelRunOnTLC(true);
%% Register methods
block.RegBlockMethod('Outputs', @Output);
%endfunction
function Output(block)
%block.OutputPort(1).Data = 2*block.InputPort(1).Data;
La=195;
Lb=395;
R=120;
r=35;
X=block.InputPort(1).Data(1);
Y=block.InputPort(1).Data(2);
Z=block.InputPort(1).Data(3);
K=La*La-Lb*Lb+X*X+Y*Y+Z*Z+(R-r)*(R-r);
K1=((K-(R-r)*(sqrt(3)*X+Y))/La)-2*(R-r)+(sqrt(3)*X+Y);
K2=((K-(R-r)*(sqrt(3)*X-Y))/La)-2*(R-r)-(sqrt(3)*X-Y);
K3=((K+2*Y*(R-r))/(2*La))-(R-r+Y);
U1=4*Z;
U2=4*Z;
U3=2*Z ;
V1=((K-(R-r)*(sqrt(3)*X+Y))/La)+2*(R-r)-(sqrt(3)*X+Y);
V2=((K+(R-r)*(sqrt(3)*X-Y))/La)+2*(R-r)+(sqrt(3)*X+Y);
V3=((K+2*Y*(R-r))/(2*La))+(R-r+Y) ;
t1=((-1)*U1-sqrt(U1*U1-4*K1*V1))/(2*K1);
t2=((-1)*U2-sqrt(U2*U2-4*K2*V2))/(2*K2);
t3=((-1)*U3-sqrt(U3*U3-4*K3*V3))/(2*K3);
% Angle1_next=360*atan(t1)/pi;
% Angle2_next=360*atan(t2)/pi;
% Angle3_next=360*atan(t3)/pi;
Angle1_next=2*atan(t1);
Angle2_next=2*atan(t2);
Angle3_next=2*atan(t3);
block.OutputPort(1).Data(1) =Angle1_next ;
block.OutputPort(1).Data(2) =Angle2_next ;
block.OutputPort(1).Data(3) =Angle3_next ;
%endfunction
评论0