static void c1_emlrt_marshallIn(SFc1_newtons_cradleInstanceStruct *chartInstance,
const mxArray *c1_u)
{
real_T (*c1_p_out)[3];
real_T (*c1_v_out)[3];
real_T (*c1_p)[3];
real_T (*c1_v)[3];
c1_v = (real_T (*)[3])(ssGetContStates(chartInstance->S) + 3);
c1_p = (real_T (*)[3])(ssGetContStates(chartInstance->S) + 0);
c1_v_out = (real_T (*)[3])ssGetOutputPortSignal(chartInstance->S, 2);
c1_p_out = (real_T (*)[3])ssGetOutputPortSignal(chartInstance->S, 1);
c1_b_emlrt_marshallIn(chartInstance, sf_mex_dup(sf_mex_getcell(c1_u, 0)),
"p_out", *c1_p_out);
c1_b_emlrt_marshallIn(chartInstance, sf_mex_dup(sf_mex_getcell(c1_u, 1)),
"v_out", *c1_v_out);
c1_b_emlrt_marshallIn(chartInstance, sf_mex_dup(sf_mex_getcell(c1_u, 2)), "p",
*c1_p);
c1_b_emlrt_marshallIn(chartInstance, sf_mex_dup(sf_mex_getcell(c1_u, 3)), "v",
*c1_v);
chartInstance->c1_is_active_c1_newtons_cradle = c1_d_emlrt_marshallIn
(chartInstance, sf_mex_dup(sf_mex_getcell(c1_u, 4)),
"is_active_c1_newtons_cradle");
chartInstance->c1_is_c1_newtons_cradle = c1_d_emlrt_marshallIn(chartInstance,
sf_mex_dup(sf_mex_getcell(c1_u, 5)), "is_c1_newtons_cradle");
sf_mex_assign(&chartInstance->c1_setSimStateSideEffectsInfo,
c1_f_emlrt_marshallIn(chartInstance, sf_mex_dup(sf_mex_getcell
(c1_u, 6)), "setSimStateSideEffectsInfo"), TRUE);
sf_mex_destroy(&c1_u);
}
/* Function Definitions */
static void initialize_c1_newtons_cradle(SFc1_newtons_cradleInstanceStruct
*chartInstance)
{
int32_T c1_i0;
real_T (*c1_p)[3];
real_T (*c1_v)[3];
real_T (*c1_p_out)[3];
real_T (*c1_v_out)[3];
c1_v = (real_T (*)[3])(ssGetContStates(chartInstance->S) + 3);
c1_p = (real_T (*)[3])(ssGetContStates(chartInstance->S) + 0);
c1_v_out = (real_T (*)[3])ssGetOutputPortSignal(chartInstance->S, 2);
c1_p_out = (real_T (*)[3])ssGetOutputPortSignal(chartInstance->S, 1);
_sfTime_ = (real_T)ssGetT(chartInstance->S);
chartInstance->c1_doSetSimStateSideEffects = 0U;
chartInstance->c1_setSimStateSideEffectsInfo = NULL;
chartInstance->c1_is_active_c1_newtons_cradle = 0U;
chartInstance->c1_is_c1_newtons_cradle = c1_IN_NO_ACTIVE_CHILD;
for (c1_i0 = 0; c1_i0 < 3; c1_i0++) {
(*c1_p)[c1_i0] = 0.0;
(*c1_v)[c1_i0] = 0.0;
}
chartInstance->c1_g = 9.81;
chartInstance->c1_l = 1.0;
chartInstance->c1_pi = 3.141;
chartInstance->c1_k = 3;
if (!(cdrGetOutputPortReusable(chartInstance->S, 1) != 0)) {
for (c1_i0 = 0; c1_i0 < 3; c1_i0++) {
(*c1_p_out)[c1_i0] = 0.0;
}
}
if (!(cdrGetOutputPortReusable(chartInstance->S, 2) != 0)) {
for (c1_i0 = 0; c1_i0 < 3; c1_i0++) {
(*c1_v_out)[c1_i0] = 0.0;
}
}
chartInstance->c1_is_active_c1_newtons_cradle = 1U;
for (c1_i0 = 0; c1_i0 < 3; c1_i0++) {
(*c1_p)[c1_i0] = 0.0;
(*c1_v)[c1_i0] = 0.0;
}
(*c1_p)[0] = 1.5705;
(*c1_p)[1] = 0.392625;
(*c1_p)[2] = -0.78525;
chartInstance->c1_is_c1_newtons_cradle = c1_IN_FreeDynamics;
}
/* Function: mdlUpdate ======================================================
* Abstract:
* This function is called once for every major integration time step.
* Discrete states are typically updated here, but this function is useful
* for performing any tasks that should only take place once per
* integration step.
*/
static void mdlUpdate(SimStruct *S, int_T tid)
{
real_T t, *xC, *xD, *uWQ, *uES;
SS=S;
t = ssGetT(S);
xC = ssGetContStates(S);
xD = ssGetDiscStates(S);
uWQ = ssGetInputPortRealSignal(S,0);
// verwijzing naar exra inputs
if (B(S,"Setpoints")>0)
{
uES = &(uWQ[(int) (U(S,"Number")*B(S,"WaterIn"))]);
}
else
{
uES=NULL;
}
wsgetq_Update(t,xC,xD,uWQ,uES,
ssGetSFcnParam(S, 0),ssGetSFcnParam(S, 2),ssGetSFcnParam(S, 3));
}
/* Function: mdlDerivatives =================================================
* Abstract:
* xdot = Ax + Bu
*/
static void mdlDerivatives(SimStruct *S)
{
real_T *dx = ssGetdX(S);
real_T *x = ssGetContStates(S);
InputRealPtrsType uPtrs = ssGetInputPortRealSignalPtrs(S,0);
const real_T *apr = mxGetPr(A_PARAM(S));
const real_T *bpr = mxGetPr(B_PARAM(S));
int_T nStates = ssGetNumContStates(S);
int_T nInputs = ssGetInputPortWidth(S,0);
int_T i, j;
real_T accum;
/* Matrix Multiply: dx = Ax + Bu */
for (i = 0; i < nStates; i++) {
accum = 0.0;
/* Ax */
for (j = 0; j < nStates; j++) {
accum += apr[i + nStates*j] * x[j];
}
/* Bu */
for (j = 0; j < nInputs; j++) {
accum += bpr[i + nStates*j] * U(j);
}
dx[i] = accum;
}
}
/* Function: mdlOutputs =======================================================
* Abstract:
* y = Cx + Du
*/
static void mdlOutputs(SimStruct *S, int_T tid)
{
real_T *y = ssGetOutputPortRealSignal(S,0);
real_T *x = ssGetContStates(S);
InputRealPtrsType uPtrs = ssGetInputPortRealSignalPtrs(S,0);
const real_T *cpr = mxGetPr(C_PARAM(S));
const real_T *dpr = mxGetPr(D_PARAM(S));
int_T nStates = ssGetNumContStates(S);
int_T nInputs = ssGetInputPortWidth(S,0);
int_T nOutputs = ssGetOutputPortWidth(S,0);
int_T i, j;
real_T accum;
UNUSED_ARG(tid); /* not used in single tasking mode */
/* Matrix Multiply: y = Cx + Du */
for (i = 0; i < nOutputs; i++) {
accum = 0.0;
/* Cx */
for (j = 0; j < nStates; j++) {
accum += cpr[i + nOutputs*j] * x[j];
}
/* Du */
for (j = 0; j < nInputs; j++) {
accum += dpr[i + nOutputs*j] * U(j);
}
y[i] = accum;
}
}
/* Function: mdlDerivatives =================================================
* Abstract:
* In this function, you compute the S-function block's derivatives.
* The derivatives are placed in the derivative vector, ssGetdX(S).
*/
static void mdlDerivatives(SimStruct *S)
{
real_T t, *xC, *xD, *uWQ, *uES, *sys;
int i;
SS=S;
t = ssGetT(S);
xC = ssGetContStates(S);
xD = ssGetDiscStates(S);
uWQ = ssGetInputPortRealSignal(S,0);
sys = ssGetdX(S);
for (i=0; i<(int)B(S,"CStates"); i++)
{
sys[i]=0.0;
}
// verwijzing naar exra inputs
if (B(S,"Setpoints")>0)
{
uES = &(uWQ[(int) (U(S,"Number")*B(S,"WaterIn"))]);
}
else
{
uES=NULL;
}
wsgetq_Derivatives(sys,t,xC,xD,uWQ,uES,
ssGetSFcnParam(S, 0),ssGetSFcnParam(S, 2),ssGetSFcnParam(S, 3));
}
static void mdlDerivatives(SimStruct *S) {
const real_T *x = ssGetContStates(S);
real_T *dx = ssGetdX(S);
dx[0] = x[1];
dx[1] = 9.8 * sin(x[0]);
}
static void mdlOutputs ( SimStruct * S , int_T tid ) { real_T B_0_15_0 ;
real_T currentTime ; BlockIO_main * _rtB ; Parameters_main * _rtP ;
D_Work_main * _rtDW ; _rtDW = ( ( D_Work_main * ) ssGetRootDWork ( S ) ) ;
_rtP = ( ( Parameters_main * ) ssGetDefaultParam ( S ) ) ; _rtB = ( (
BlockIO_main * ) _ssGetBlockIO ( S ) ) ; ssCallAccelRunBlock ( S , 0 , 0 ,
SS_CALL_MDL_OUTPUTS ) ; if ( ssIsSampleHit ( S , 1 , 0 ) ) {
ssCallAccelRunBlock ( S , 0 , 1 , SS_CALL_MDL_OUTPUTS ) ; } _rtB -> B_0_2_0 =
_rtB -> B_0_0_0 [ 2 ] * _rtB -> B_0_0_0 [ 1 ] ; if ( ssIsSampleHit ( S , 1 ,
0 ) ) { ssCallAccelRunBlock ( S , 0 , 3 , SS_CALL_MDL_OUTPUTS ) ; } ( (
BlockIO_main * ) _ssGetBlockIO ( S ) ) -> B_0_4_0 = ssGetT ( S ) ; if (
ssIsSampleHit ( S , 1 , 0 ) ) { ssCallAccelRunBlock ( S , 0 , 5 ,
SS_CALL_MDL_OUTPUTS ) ; _rtB -> B_0_6_0 = _rtP -> P_0 ; } _rtB -> B_0_7_0 =
_rtB -> B_0_6_0 - _rtB -> B_0_0_0 [ 1 ] ; if ( ssIsSampleHit ( S , 1 , 0 ) )
{ ssCallAccelRunBlock ( S , 0 , 8 , SS_CALL_MDL_OUTPUTS ) ;
ssCallAccelRunBlock ( S , 0 , 9 , SS_CALL_MDL_OUTPUTS ) ; ssCallAccelRunBlock
( S , 0 , 10 , SS_CALL_MDL_OUTPUTS ) ; } currentTime = ssGetTaskTime ( S , 0
) ; if ( currentTime < _rtP -> P_1 ) { _rtB -> B_0_11_0 = _rtP -> P_2 ; }
else { _rtB -> B_0_11_0 = _rtP -> P_3 ; } ssCallAccelRunBlock ( S , 0 , 12 ,
SS_CALL_MDL_OUTPUTS ) ; if ( ssIsSampleHit ( S , 1 , 0 ) ) {
ssCallAccelRunBlock ( S , 0 , 13 , SS_CALL_MDL_OUTPUTS ) ; _rtB -> B_0_14_0 =
_rtDW -> Delay_DSTATE ; } B_0_15_0 = _rtP -> P_7 * ( ( ContinuousStates_main
* ) ssGetContStates ( S ) ) -> TransferFcn_CSTATE ; ssCallAccelRunBlock ( S ,
0 , 16 , SS_CALL_MDL_OUTPUTS ) ; _rtB -> B_0_17_0 = _rtB -> B_0_16_0 -
B_0_15_0 ; _rtB -> B_0_18_0 [ 0 ] = _rtB -> B_0_12_0 ; _rtB -> B_0_18_0 [ 1 ]
= B_0_15_0 ; if ( ssIsSampleHit ( S , 1 , 0 ) ) { ssCallAccelRunBlock ( S , 0
, 19 , SS_CALL_MDL_OUTPUTS ) ; ssCallAccelRunBlock ( S , 0 , 20 ,
SS_CALL_MDL_OUTPUTS ) ; } UNUSED_PARAMETER ( tid ) ; }
static void mdlInitializeConditions(SimStruct *S)
{
real_T *x0 = ssGetContStates(S);
int_T lp;
for (lp=0;lp<6;lp++) {
*x0++=0.0;
}
}
static void mdlDerivatives ( SimStruct * S ) { BlockIO_main * _rtB ;
Parameters_main * _rtP ; _rtP = ( ( Parameters_main * ) ssGetDefaultParam ( S
) ) ; _rtB = ( ( BlockIO_main * ) _ssGetBlockIO ( S ) ) ; ssCallAccelRunBlock
( S , 0 , 0 , SS_CALL_MDL_DERIVATIVES ) ; { ( ( StateDerivatives_main * )
ssGetdX ( S ) ) -> TransferFcn_CSTATE = ( _rtP -> P_5 ) * ( (
ContinuousStates_main * ) ssGetContStates ( S ) ) -> TransferFcn_CSTATE ; ( (
StateDerivatives_main * ) ssGetdX ( S ) ) -> TransferFcn_CSTATE += _rtP ->
P_6 * ( ( BlockIO_main * ) _ssGetBlockIO ( S ) ) -> B_0_16_0 ; } } static
/* Function: mdlOutputs =======================================================
*
*/
static void mdlOutputs(SimStruct *S, int_T tid)
{
const real_T *u0 = (const real_T*) ssGetInputPortSignal(S,0);
real_T *y0 = (real_T *)ssGetOutputPortRealSignal(S,0);
const real_T *xC = ssGetContStates(S);
builderC_Outputs_wrapper(u0, y0, xC);
}
/* Function: mdlInitializeConditions ========================================
* Abstract:
* Initialize the states
*/
static void mdlInitializeConditions(SimStruct *S)
{
real_T *xC = ssGetContStates(S);
xC[0] = 0;
xC[1] = 0;
}
/* Function: mdlDerivatives =================================================
* Abstract:
* In this function, you compute the S-function block's derivatives.
* The derivatives are placed in the derivative vector, ssGetdX(S).
*/
static void mdlDerivatives(SimStruct *S)
{
const real_T *u0 = (const real_T*) ssGetInputPortSignal(S,0);
real_T *dx = ssGetdX(S);
real_T *xC = ssGetContStates(S);
real_T *y0 = (real_T *) ssGetOutputPortRealSignal(S,0);
builderC_Derivatives_wrapper(u0, y0,dx, xC);
}
static void mdlInitializeConditions(SimStruct *S)
{
real_T *x0 = ssGetContStates(S);
x0[0] = 0.0; /* Ics */
x0[1] = 0.0; /* dIcs */
x0[2] = 0.0; /* pA */
x0[3] = 0.0; /* pB */
}
static void outputs_c1_newtons_cradle(SFc1_newtons_cradleInstanceStruct
*chartInstance)
{
int32_T c1_i2;
real_T (*c1_p_out)[3];
real_T (*c1_v_out)[3];
real_T (*c1_p)[3];
real_T (*c1_v)[3];
c1_v = (real_T (*)[3])(ssGetContStates(chartInstance->S) + 3);
c1_p = (real_T (*)[3])(ssGetContStates(chartInstance->S) + 0);
c1_v_out = (real_T (*)[3])ssGetOutputPortSignal(chartInstance->S, 2);
c1_p_out = (real_T (*)[3])ssGetOutputPortSignal(chartInstance->S, 1);
_sfTime_ = (real_T)ssGetT(chartInstance->S);
for (c1_i2 = 0; c1_i2 < 3; c1_i2++) {
(*c1_p_out)[c1_i2] = (*c1_p)[c1_i2];
(*c1_v_out)[c1_i2] = (*c1_v)[c1_i2];
}
}
static void mdlOutputs(SimStruct *S, int_T tid)
{
real_T *y = ssGetOutputPortRealSignal(S,0);
real_T *x = ssGetContStates(S);
int_T i;
for(i=0;i<6;i++)
{
y[i]=x[i];
}
}
static const mxArray *c1_emlrt_marshallOut(SFc1_newtons_cradleInstanceStruct
*chartInstance)
{
const mxArray *c1_y;
int32_T c1_i3;
real_T c1_dv0[3];
real_T (*c1_v)[3];
real_T (*c1_p)[3];
real_T (*c1_v_out)[3];
real_T (*c1_p_out)[3];
c1_v = (real_T (*)[3])(ssGetContStates(chartInstance->S) + 3);
c1_p = (real_T (*)[3])(ssGetContStates(chartInstance->S) + 0);
c1_v_out = (real_T (*)[3])ssGetOutputPortSignal(chartInstance->S, 2);
c1_p_out = (real_T (*)[3])ssGetOutputPortSignal(chartInstance->S, 1);
c1_y = NULL;
sf_mex_assign(&c1_y, sf_mex_createcellarray(6), FALSE);
for (c1_i3 = 0; c1_i3 < 3; c1_i3++) {
c1_dv0[c1_i3] = (*c1_p_out)[c1_i3];
}
sf_mex_setcell(c1_y, 0, c1_b_emlrt_marshallOut(chartInstance, c1_dv0));
for (c1_i3 = 0; c1_i3 < 3; c1_i3++) {
c1_dv0[c1_i3] = (*c1_v_out)[c1_i3];
}
sf_mex_setcell(c1_y, 1, c1_b_emlrt_marshallOut(chartInstance, c1_dv0));
for (c1_i3 = 0; c1_i3 < 3; c1_i3++) {
c1_dv0[c1_i3] = (*c1_p)[c1_i3];
}
sf_mex_setcell(c1_y, 2, c1_b_emlrt_marshallOut(chartInstance, c1_dv0));
for (c1_i3 = 0; c1_i3 < 3; c1_i3++) {
c1_dv0[c1_i3] = (*c1_v)[c1_i3];
}
sf_mex_setcell(c1_y, 3, c1_b_emlrt_marshallOut(chartInstance, c1_dv0));
sf_mex_setcell(c1_y, 4, c1_c_emlrt_marshallOut(chartInstance,
chartInstance->c1_is_active_c1_newtons_cradle));
sf_mex_setcell(c1_y, 5, c1_c_emlrt_marshallOut(chartInstance,
chartInstance->c1_is_c1_newtons_cradle));
return c1_y;
}
/* Function: mdlInitializeConditions ========================================
* Abstract:
* In this function, you should initialize the continuous and discrete
* states for your S-function block. The initial states are placed
* in the state vector, ssGetContStates(S) or ssGetRealDiscStates(S).
* You can also perform any other initialization activities that your
* S-function may require. Note, this routine will be called at the
* start of simulation and if it is present in an enabled subsystem
* configured to reset states, it will be call when the enabled subsystem
* restarts execution to reset the states.
*/
static void mdlInitializeConditions(SimStruct *S)
{
/*
* #undef MDL_INITIALIZE_CONDITIONS if you don't have any
* continuous states.
*/
real_T *x = ssGetContStates(S);
/* set the values of the states (x) to start with */
}
static void mdlDerivatives(SimStruct *S)
{
real_T *dx = ssGetdX(S);
real_T *x = ssGetContStates(S);
InputRealPtrsType uPtrs = ssGetInputPortRealSignalPtrs(S,0);
double *m=mxGetPr(ssGetSFcnParam(S,0));
real_T mm=m[0];
dx[0]=x[3]/mm;
dx[1]=x[4]/mm;
dx[2]=x[5]/mm;
dx[3]=U(0);
dx[4]=U(1);
dx[5]=U(2);
}
/* Function: mdlUpdate ======================================================
* Abstract:
* This function is called once for every major integration time step.
* Discrete states are typically updated here, but this function is useful
* for performing any tasks that should only take place once per
* integration step.
*/
static void mdlUpdate(SimStruct *S, int_T tid)
{
real_T t, *xC, *xD, *u;
int i;
t = ssGetT(S);
xC = ssGetContStates(S);
xD = ssGetDiscStates(S);
u = ssGetInputPortRealSignal(S,0);
block0_Update(t,xC,xD,u,
ssGetSFcnParam(S, 0),ssGetSFcnParam(S, 2),ssGetSFcnParam(S, 3));
}
/* Function: mdlOutputs =======================================================
* Abstract:
* In this function, you compute the outputs of your S-function
* block. Generally outputs are placed in the output vector, ssGetY(S).
*/
static void mdlOutputs(SimStruct *S, int_T tid)
{
real_T t, *xC, *xD, *u, *sys;
int i;
t = ssGetT(S);
xC = ssGetContStates(S);
xD = ssGetDiscStates(S);
u = ssGetInputPortRealSignal(S,0);
sys = ssGetOutputPortRealSignal(S,0);
block0_Outputs(sys,t,xC,xD,u,
ssGetSFcnParam(S, 0),ssGetSFcnParam(S, 2),ssGetSFcnParam(S, 3));
}
/* Function: mdlOutputs */
static void mdlOutputs(SimStruct *S, int_T tid)
{
double Aa = ( mxGetPr(ssGetSFcnParam(S,14)) )[0];
double Ab = ( mxGetPr(ssGetSFcnParam(S,15)) )[0];
double fv = ( mxGetPr(ssGetSFcnParam(S,18)) )[0];
real_T *y = ssGetOutputPortRealSignal(S,0);
real_T *x = ssGetContStates(S);
InputRealPtrsType uPtrs = ssGetInputPortRealSignalPtrs(S,0);
UNUSED_ARG(tid);
/* OUTPUT equation*/
y[0] = (Aa*x[2] - Ab*x[3]) - fv*U(2);
}
static void derivatives_c1_newtons_cradle(SFc1_newtons_cradleInstanceStruct
*chartInstance)
{
int32_T c1_i1;
real_T (*c1_p_dot)[3];
real_T (*c1_v_dot)[3];
real_T (*c1_v)[3];
real_T (*c1_p)[3];
c1_v_dot = (real_T (*)[3])(ssGetdX(chartInstance->S) + 3);
c1_v = (real_T (*)[3])(ssGetContStates(chartInstance->S) + 3);
c1_p_dot = (real_T (*)[3])(ssGetdX(chartInstance->S) + 0);
c1_p = (real_T (*)[3])(ssGetContStates(chartInstance->S) + 0);
for (c1_i1 = 0; c1_i1 < 3; c1_i1++) {
(*c1_p_dot)[c1_i1] = 0.0;
(*c1_v_dot)[c1_i1] = 0.0;
}
_sfTime_ = (real_T)ssGetT(chartInstance->S);
for (c1_i1 = 0; c1_i1 < 3; c1_i1++) {
(*c1_p_dot)[c1_i1] = (*c1_v)[c1_i1];
(*c1_v_dot)[c1_i1] = muDoubleScalarSin((*c1_p)[c1_i1]);
(*c1_v_dot)[c1_i1] *= -9.81;
}
}
/* Function: mdlOutputs =======================================================
* Abstract:
* In this function, you compute the outputs of your S-function
* block. Generally outputs are placed in the output vector, ssGetY(S).
*/
static void mdlOutputs(SimStruct *S, int_T tid)
{
real_T t, *xC, *xD, *uWQ, *uES, *sysWQ, *sysEM;
int i;
SS=S;
t = ssGetT(S);
xC = ssGetContStates(S);
xD = ssGetDiscStates(S);
uWQ = ssGetInputPortRealSignal(S,0);
sysWQ = ssGetOutputPortRealSignal(S,0);
for (i=0;i<U(S,"Number")*B(S,"WaterIn")*B(S,"WaterOut");i++)
{
sysWQ[i] = uWQ[i];
}
// verwijzing naar exra inputs
if (B(S,"Setpoints")>0)
{
uES = &(uWQ[ (int) (U(S,"Number")*B(S,"WaterIn"))]);
}
else
{
uES=NULL;
}
// verwijzing naar exra metingen
if (B(S,"Measurements")>0)
{
sysEM = &(sysWQ[(int) (U(S,"Number")*B(S,"WaterOut"))]);
for (i=0;i<B(S,"Measurements");i++)
{
sysEM[i] = 0.0;
}
}
else
{
sysEM=NULL;
}
wsgetq_Outputs(sysWQ,sysEM,t,xC,xD,uWQ,uES,
ssGetSFcnParam(S, 0),ssGetSFcnParam(S, 2),ssGetSFcnParam(S, 3));
}
/* Function: mdlDerivatives =================================================
* Abstract:
* In this function, you compute the S-function block's derivatives.
* The derivatives are placed in the derivative vector, ssGetdX(S).
*/
static void mdlDerivatives(SimStruct *S)
{
real_T t, *xC, *xD, *u, *sys;
int i;
t = ssGetT(S);
xC = ssGetContStates(S);
xD = ssGetDiscStates(S);
u = ssGetInputPortRealSignal(S,0);
sys = ssGetdX(S);
buffer_Derivatives(sys,t,xC,xD,u,
ssGetSFcnParam(S, 0),ssGetSFcnParam(S, 2),ssGetSFcnParam(S, 3));
}
/* Function: mdlDerivatives */
static void mdlDerivatives(SimStruct *S)
{
double wn = ( mxGetPr(ssGetSFcnParam(S,0)) )[0];
double E = ( mxGetPr(ssGetSFcnParam(S,1)) )[0];
double In = ( mxGetPr(ssGetSFcnParam(S,2)) )[0];
double Kva = ( mxGetPr(ssGetSFcnParam(S,3)) )[0];
double Kvb = ( mxGetPr(ssGetSFcnParam(S,4)) )[0];
double KvlkA = ( mxGetPr(ssGetSFcnParam(S,5)) )[0];
double KvlkB = ( mxGetPr(ssGetSFcnParam(S,6)) )[0];
double ps = ( mxGetPr(ssGetSFcnParam(S,7)) )[0];
double pt = ( mxGetPr(ssGetSFcnParam(S,8)) )[0];
double up_dz = ( mxGetPr(ssGetSFcnParam(S,9)) )[0];
double lw_dz = ( mxGetPr(ssGetSFcnParam(S,10)) )[0];
double beta = ( mxGetPr(ssGetSFcnParam(S,11)) )[0];
double Va0 = ( mxGetPr(ssGetSFcnParam(S,12)) )[0];
double Vb0 = ( mxGetPr(ssGetSFcnParam(S,13)) )[0];
double Aa = ( mxGetPr(ssGetSFcnParam(S,14)) )[0];
double Ab = ( mxGetPr(ssGetSFcnParam(S,15)) )[0];
double L = ( mxGetPr(ssGetSFcnParam(S,16)) )[0];
double leak = ( mxGetPr(ssGetSFcnParam(S,17)) )[0];
real_T *dx = ssGetdX(S);
real_T *x = ssGetContStates(S);
InputRealPtrsType uPtrs = ssGetInputPortRealSignalPtrs(S,0);
dx[0]= x[1];
dx[1]= -wn*wn*x[0] - 2*E*wn*x[1] + wn*wn*sat(U(0),-In,In);
if ((sat(U(0),-In,In) >= up_dz) && (sat(U(0),-In,In) <= In))
{
dx[2] = (beta/(Va0 + Aa*U(1))) * (((Kva *(x[0]/In) + KvlkA) * sgn(ps-x[2]) * sqrt(abso(ps-x[2])) - KvlkA * sgn(x[2]-pt) * sqrt(abso(x[2]-pt))) - Aa*U(2)- leak*(x[2]-x[3]));
dx[3] = (beta/(Vb0 + Ab*(L - U(1)))) * (Ab*U(2) - ((Kvb *(x[0]/In) + KvlkB) * sgn(x[3]-pt) * sqrt(abso(x[3]-pt)) - KvlkB * sgn(ps-x[3]) * sqrt(abso(ps-x[3]))) + leak*(x[2]-x[3]));
}
else if ((sat(U(0),-In,In) >= -In) && (sat(U(0),-In,In) <= lw_dz))
{
dx[2] = (beta/(Va0 + Aa*U(1))) * ((-(Kva *(abso(x[0])/In) + KvlkA) * sgn(x[2]-pt) * sqrt(abso(x[2]-pt)) + KvlkA * sgn(ps-x[2]) * sqrt(abso(ps-x[2]))) - Aa*U(2)- leak*(x[2]-x[3]));
dx[3] = (beta/(Vb0 + Ab*(L - U(1)))) * (Ab*U(2) - (-(Kvb *(abso(x[0])/In) + KvlkB) * sgn(ps-x[3]) * sqrt(abso(ps-x[3])) + KvlkB * sgn(x[3]-pt) * sqrt(abso(x[3]-pt))) + leak*(x[2]-x[3]));
}
else
{
dx[2] = 0;
dx[3] = 0;
}
}
static void mdlOutputs(SimStruct *S, int_T tid) {
real_T *x = ssGetContStates(S);
real_T *y = (real_T *)ssGetOutputPortSignal(S, 0);
if (ssIsMajorTimeStep(S)) {
// then check for zero-crossing events and apply them
if (x[0] >= rw_gamma + alpha) {
x[0] = rw_gamma - alpha;
x[1] = cos(2 * alpha) * x[1];
} else if (x[0] <= rw_gamma - alpha) {
x[0] = rw_gamma + alpha;
x[1] = cos(2 * alpha) * x[1];
}
}
y[0] = x[0];
y[1] = x[1];
}
static void mdlOutputs(SimStruct *S, int_T tid)
{
real_T *y = ssGetOutputPortRealSignal(S,0);
real_T *x = ssGetContStates(S);
InputRealPtrsType uPtrs = ssGetInputPortRealSignalPtrs(S,0);
// double *params0 = mxGetPr(ssGetSFcnParam(S,0));
int i;
double u[N_INPUTS];
double B_[NS*NI], *Ko_ = y;
/* copy inputs to u[] to ensure contiguity ... and easy access*/
for (i=0; i<N_INPUTS; i++)
u[i] = *uPtrs[i];
/* output new optimal gain */
optBK(B_, Ko_, x, u);
}
/* Function: mdlInitializeConditions ========================================
* Abstract:
* If the initial condition parameter (X0) is not an empty matrix,
* then use it to set up the initial conditions, otherwise,
* set the initial conditions to all 0.0
*/
static void mdlInitializeConditions(SimStruct *S)
{
real_T *x0 = ssGetContStates(S);
int_T i, nStates;
nStates = ssGetNumContStates(S);
if (mxGetM(X0_PARAM(S)) != 0) {
const real_T *pr = mxGetPr(X0_PARAM(S));
for (i = 0; i < nStates; i++) {
*x0++ = *pr++;
}
} else {
for (i = 0; i < nStates; i++) {
*x0++ = 0.0;
}
}
}
static void zeroCrossings_c1_newtons_cradle(SFc1_newtons_cradleInstanceStruct
*chartInstance)
{
boolean_T c1_stateChanged;
int32_T c1_i;
int32_T c1_exitg1;
real_T *c1_zcVar;
real_T (*c1_p)[3];
c1_p = (real_T (*)[3])(ssGetContStates(chartInstance->S) + 0);
c1_zcVar = (real_T *)(ssGetNonsampledZCs(chartInstance->S) + 0);
_sfTime_ = (real_T)ssGetT(chartInstance->S);
if (chartInstance->c1_lastMajorTime == _sfTime_) {
*c1_zcVar = -1.0;
} else {
c1_stateChanged = (boolean_T)0;
c1_i = 0;
do {
c1_exitg1 = 0;
if (c1_i < 2) {
if ((*c1_p)[c1_i] < (*c1_p)[c1_i + 1]) {
c1_i = 1;
c1_exitg1 = 1;
} else {
c1_i++;
_SF_MEX_LISTEN_FOR_CTRL_C(chartInstance->S);
}
} else {
c1_i = 0;
c1_exitg1 = 1;
}
} while (c1_exitg1 == 0);
if (c1_i != 0) {
c1_stateChanged = TRUE;
}
if (c1_stateChanged) {
*c1_zcVar = 1.0;
} else {
*c1_zcVar = -1.0;
}
}
}