void eye(real_T n, real_T I_data[289], int32_T I_size[2])
{
int32_T q;
int32_T i;
real_T d0;
I_size[0] = (int32_T)n;
I_size[1] = (int32_T)n;
q = (int32_T)n * (int32_T)n - 1;
for (i = 0; i <= q; i++) {
I_data[i] = 0.0;
}
d0 = rt_roundd_snf(n);
if (d0 < 2.147483648E+9) {
if (d0 >= -2.147483648E+9) {
q = (int32_T)d0;
} else {
q = MIN_int32_T;
}
} else if (d0 >= 2.147483648E+9) {
q = MAX_int32_T;
} else {
q = 0;
}
if (q > 0) {
for (i = 0; i + 1 <= q; i++) {
I_data[i + I_size[0] * i] = 1.0;
}
}
}
void eye(real_T n, emxArray_real_T *I)
{
int32_T q;
int32_T loop_ub;
real_T d0;
q = I->size[0] * I->size[1];
I->size[0] = (int32_T)n;
I->size[1] = (int32_T)n;
emxEnsureCapacity((emxArray__common *)I, q, (int32_T)sizeof(real_T));
loop_ub = (int32_T)n * (int32_T)n - 1;
for (q = 0; q <= loop_ub; q++) {
I->data[q] = 0.0;
}
d0 = rt_roundd_snf(n);
if (d0 < 2.147483648E+9) {
if (d0 >= -2.147483648E+9) {
q = (int32_T)d0;
} else {
q = MIN_int32_T;
}
} else if (d0 >= 2.147483648E+9) {
q = MAX_int32_T;
} else {
q = 0;
}
if (q > 0) {
for (loop_ub = 0; loop_ub + 1 <= q; loop_ub++) {
I->data[loop_ub + I->size[0] * loop_ub] = 1.0;
}
}
}
/* Model output function */
void motor_io_position_output(void)
{
real_T temp;
ZCEventType zcEvent;
real_T u1;
real_T u2;
if (rtmIsMajorTimeStep(motor_io_position_M)) {
/* set solver stop time */
if (!(motor_io_position_M->Timing.clockTick0+1)) {
rtsiSetSolverStopTime(&motor_io_position_M->solverInfo,
((motor_io_position_M->Timing.clockTickH0 + 1) *
motor_io_position_M->Timing.stepSize0 * 4294967296.0));
} else {
rtsiSetSolverStopTime(&motor_io_position_M->solverInfo,
((motor_io_position_M->Timing.clockTick0 + 1) *
motor_io_position_M->Timing.stepSize0 +
motor_io_position_M->Timing.clockTickH0 *
motor_io_position_M->Timing.stepSize0 * 4294967296.0));
}
} /* end MajorTimeStep */
/* Update absolute time of base rate at minor time step */
if (rtmIsMinorTimeStep(motor_io_position_M)) {
motor_io_position_M->Timing.t[0] = rtsiGetT(&motor_io_position_M->solverInfo);
}
if (rtmIsMajorTimeStep(motor_io_position_M)) {
/* S-Function (rti_commonblock): '<S13>/S-Function1' */
/* This comment workarounds a code generation problem */
/* Gain: '<S5>/fi1_scaling' */
motor_io_position_B.fi1_scaling = motor_io_position_P.fi1_scaling_Gain *
motor_io_position_B.SFunction1;
/* DiscreteTransferFcn: '<Root>/Gfbreal' */
temp = motor_io_position_B.fi1_scaling;
temp -= motor_io_position_P.Gfbreal_DenCoef[1] *
motor_io_position_DW.Gfbreal_states[0];
temp -= motor_io_position_P.Gfbreal_DenCoef[2] *
motor_io_position_DW.Gfbreal_states[1];
temp /= motor_io_position_P.Gfbreal_DenCoef[0];
motor_io_position_DW.Gfbreal_tmp = temp;
temp = motor_io_position_P.Gfbreal_NumCoef[0] *
motor_io_position_DW.Gfbreal_tmp;
temp += motor_io_position_P.Gfbreal_NumCoef[1] *
motor_io_position_DW.Gfbreal_states[0];
temp += motor_io_position_P.Gfbreal_NumCoef[2] *
motor_io_position_DW.Gfbreal_states[1];
motor_io_position_B.Gfbreal = temp;
}
/* SignalGenerator: '<Root>/SinGenerator' */
motor_io_position_B.SinGenerator = sin
(motor_io_position_P.SinGenerator_Frequency * motor_io_position_M->Timing.t
[0]) * motor_io_position_P.SinGenerator_Amplitude;
/* SignalGenerator: '<Root>/SquareGenerator' */
temp = motor_io_position_P.SquareGenerator_Frequency *
motor_io_position_M->Timing.t[0];
if (temp - floor(temp) >= 0.5) {
motor_io_position_B.SquareGenerator =
motor_io_position_P.SquareGenerator_Amplitude;
} else {
motor_io_position_B.SquareGenerator =
-motor_io_position_P.SquareGenerator_Amplitude;
}
/* End of SignalGenerator: '<Root>/SquareGenerator' */
/* Switch: '<Root>/Switch' incorporates:
* Constant: '<Root>/SignalSelector[0Square,1Sine]'
*/
if (motor_io_position_P.SignalSelector0Square1Sine_Valu != 0.0) {
motor_io_position_B.ref = motor_io_position_B.SinGenerator;
} else {
motor_io_position_B.ref = motor_io_position_B.SquareGenerator;
}
/* End of Switch: '<Root>/Switch' */
if (rtmIsMajorTimeStep(motor_io_position_M)) {
/* DiscreteTransferFcn: '<Root>/Gffreal' */
temp = motor_io_position_B.ref;
temp -= motor_io_position_P.Gffreal_DenCoef[1] *
motor_io_position_DW.Gffreal_states[0];
temp -= motor_io_position_P.Gffreal_DenCoef[2] *
motor_io_position_DW.Gffreal_states[1];
temp /= motor_io_position_P.Gffreal_DenCoef[0];
motor_io_position_DW.Gffreal_tmp = temp;
temp = motor_io_position_P.Gffreal_NumCoef[0] *
motor_io_position_DW.Gffreal_tmp;
temp += motor_io_position_P.Gffreal_NumCoef[1] *
motor_io_position_DW.Gffreal_states[0];
temp += motor_io_position_P.Gffreal_NumCoef[2] *
motor_io_position_DW.Gffreal_states[1];
motor_io_position_B.Gffreal = temp;
/* Sum: '<Root>/Sum' */
motor_io_position_B.Sum = motor_io_position_B.Gffreal -
motor_io_position_B.Gfbreal;
/* Gain: '<Root>/Gain' */
motor_io_position_B.Gain = motor_io_position_P.Gain_Gain *
motor_io_position_B.Sum;
/* Saturate: '<S2>/Saturation' */
temp = motor_io_position_B.Gain;
u1 = motor_io_position_P.Saturation_LowerSat;
u2 = motor_io_position_P.Saturation_UpperSat;
if (temp > u2) {
motor_io_position_B.Volt = u2;
} else if (temp < u1) {
motor_io_position_B.Volt = u1;
} else {
motor_io_position_B.Volt = temp;
}
/* End of Saturate: '<S2>/Saturation' */
/* Gain: '<S2>/pwm_skalning' */
motor_io_position_B.pwm_skalning = motor_io_position_P.pwm_skalning_Gain *
motor_io_position_B.Volt;
/* Sum: '<S2>/Sum' incorporates:
* Constant: '<S2>/pwm_offstet'
*/
motor_io_position_B.Sum_f = motor_io_position_B.pwm_skalning +
motor_io_position_P.pwm_offstet_Value;
/* S-Function (rti_commonblock): '<S9>/S-Function1' */
/* This comment workarounds a code generation problem */
/* dSPACE I/O Board DS1104 #1 Unit:PWM Group:PWM */
ds1104_slave_dsp_pwm_duty_write(0, rti_slv1104_fcn_index[6],
motor_io_position_B.Sum_f);
/* S-Function (rti_commonblock): '<S9>/S-Function2' */
/* This comment workarounds a code generation problem */
/* S-Function (rti_commonblock): '<S9>/S-Function3' */
/* This comment workarounds a code generation problem */
/* S-Function (rti_commonblock): '<S9>/S-Function4' */
/* This comment workarounds a code generation problem */
/* DataTypeConversion: '<S2>/Data Type Conversion' incorporates:
* Constant: '<S2>/Enable[1_Off, 0_On]'
*/
motor_io_position_B.DataTypeConversion =
(motor_io_position_P.Enable1_Off0_On_Value != 0.0);
/* S-Function (rti_commonblock): '<S8>/S-Function1' */
/* This comment workarounds a code generation problem */
/* dSPACE I/O Board DS1104 #1 Unit:BIT_IO Group:BIT_OUT */
if (motor_io_position_B.DataTypeConversion > 0) {
ds1104_bit_io_set(DS1104_DIO0);
} else {
ds1104_bit_io_clear(DS1104_DIO0);
}
/* DiscreteTransferFcn: '<Root>/Gff' */
temp = motor_io_position_B.ref;
temp -= motor_io_position_P.Gff_DenCoef[1] *
motor_io_position_DW.Gff_states[0];
temp -= motor_io_position_P.Gff_DenCoef[2] *
motor_io_position_DW.Gff_states[1];
temp /= motor_io_position_P.Gff_DenCoef[0];
motor_io_position_DW.Gff_tmp = temp;
temp = motor_io_position_P.Gff_NumCoef[0] * motor_io_position_DW.Gff_tmp;
temp += motor_io_position_P.Gff_NumCoef[1] *
motor_io_position_DW.Gff_states[0];
temp += motor_io_position_P.Gff_NumCoef[2] *
motor_io_position_DW.Gff_states[1];
motor_io_position_B.Gff = temp;
}
/* Integrator: '<S1>/Integrator1' */
motor_io_position_B.Integrator1 = motor_io_position_X.Integrator1_CSTATE;
/* Quantizer: '<S4>/Quantizer' */
temp = motor_io_position_B.Integrator1;
motor_io_position_B.Quantizer = rt_roundd_snf(temp / motor_io_position_P.quant)
* motor_io_position_P.quant;
if (rtmIsMajorTimeStep(motor_io_position_M)) {
/* ZeroOrderHold: '<S4>/Zero-Order Hold' */
motor_io_position_B.ZeroOrderHold = motor_io_position_B.Quantizer;
/* DiscreteTransferFcn: '<Root>/Gfb' */
temp = motor_io_position_B.ZeroOrderHold;
temp -= motor_io_position_P.Gfb_DenCoef[1] *
motor_io_position_DW.Gfb_states[0];
temp -= motor_io_position_P.Gfb_DenCoef[2] *
motor_io_position_DW.Gfb_states[1];
temp /= motor_io_position_P.Gfb_DenCoef[0];
motor_io_position_DW.Gfb_tmp = temp;
temp = motor_io_position_P.Gfb_NumCoef[0] * motor_io_position_DW.Gfb_tmp;
temp += motor_io_position_P.Gfb_NumCoef[1] *
motor_io_position_DW.Gfb_states[0];
temp += motor_io_position_P.Gfb_NumCoef[2] *
motor_io_position_DW.Gfb_states[1];
motor_io_position_B.Gfb = temp;
/* Sum: '<Root>/Sum1' */
motor_io_position_B.Sum1 = motor_io_position_B.Gff - motor_io_position_B.Gfb;
/* Saturate: '<Root>/Saturation' */
temp = motor_io_position_B.Sum1;
u1 = motor_io_position_P.Saturation_LowerSat_c;
u2 = motor_io_position_P.Saturation_UpperSat_p;
if (temp > u2) {
motor_io_position_B.Saturation = u2;
} else if (temp < u1) {
motor_io_position_B.Saturation = u1;
} else {
motor_io_position_B.Saturation = temp;
}
/* End of Saturate: '<Root>/Saturation' */
}
/* Integrator: '<S1>/Integrator' */
motor_io_position_B.Integrator = motor_io_position_X.Integrator_CSTATE;
/* Gain: '<S1>/Gain1' */
motor_io_position_B.Gain1 = motor_io_position_P.Gain1_Gain *
motor_io_position_B.Integrator;
/* Sum: '<S1>/Add' */
motor_io_position_B.Add = motor_io_position_B.Saturation -
motor_io_position_B.Gain1;
/* Gain: '<S1>/k//R ' */
motor_io_position_B.kR = motor_io_position_P.kR_Gain * motor_io_position_B.Add;
/* Saturate: '<S6>/Saturate to Fc' */
temp = motor_io_position_B.kR;
u1 = motor_io_position_P.SaturatetoFc_LowerSat;
u2 = motor_io_position_P.F_c;
if (temp > u2) {
motor_io_position_B.Stickslipregion = u2;
} else if (temp < u1) {
motor_io_position_B.Stickslipregion = u1;
} else {
motor_io_position_B.Stickslipregion = temp;
}
/* End of Saturate: '<S6>/Saturate to Fc' */
/* Abs: '<S6>/Abs' */
motor_io_position_B.Abs = fabs(motor_io_position_B.Integrator);
/* RelationalOperator: '<S7>/Compare' incorporates:
* Constant: '<S7>/Constant'
*/
motor_io_position_B.Compare = (motor_io_position_B.Abs <=
motor_io_position_P.Constant_Value);
/* Gain: '<S6>/Vicous friction' */
motor_io_position_B.Vicousfriction = motor_io_position_P.Vicousfriction_Gain *
motor_io_position_B.Integrator;
/* Signum: '<S6>/Sign' */
temp = motor_io_position_B.Integrator;
if (temp < 0.0) {
motor_io_position_B.Sign = -1.0;
} else if (temp > 0.0) {
motor_io_position_B.Sign = 1.0;
} else if (temp == 0.0) {
motor_io_position_B.Sign = 0.0;
} else {
motor_io_position_B.Sign = temp;
}
/* End of Signum: '<S6>/Sign' */
/* Product: '<S6>/Product' incorporates:
* Constant: '<S6>/F_c'
*/
motor_io_position_B.Product = motor_io_position_P.F_c *
motor_io_position_B.Sign;
/* Sum: '<S6>/Add' */
motor_io_position_B.Viscousregion = motor_io_position_B.Vicousfriction +
motor_io_position_B.Product;
/* Switch: '<S6>/Switch' */
if (motor_io_position_B.Compare) {
motor_io_position_B.Friction = motor_io_position_B.Stickslipregion;
} else {
motor_io_position_B.Friction = motor_io_position_B.Viscousregion;
}
/* End of Switch: '<S6>/Switch' */
/* Sum: '<S1>/Add1' */
motor_io_position_B.Add1 = motor_io_position_B.kR -
motor_io_position_B.Friction;
if (rtmIsMajorTimeStep(motor_io_position_M)) {
/* Gain: '<S1>/Gain2' incorporates:
* Constant: '<S1>/Load inertia'
*/
motor_io_position_B.Gain2 = motor_io_position_P.Gain2_Gain *
motor_io_position_P.J1;
/* Sum: '<S1>/Add2' incorporates:
* Constant: '<S1>/Motor inertia'
*/
motor_io_position_B.Add2 = motor_io_position_B.Gain2 +
motor_io_position_P.Motorinertia_Value;
}
/* Product: '<S1>/Inertias 1//J' */
motor_io_position_B.Inertias1J = 1.0 / motor_io_position_B.Add2 *
motor_io_position_B.Add1;
if (rtmIsMajorTimeStep(motor_io_position_M)) {
}
/* Switch: '<S6>/Switch1' incorporates:
* Constant: '<S6>/Constant1'
* Constant: '<S6>/F_c'
*/
if (motor_io_position_B.Integrator > motor_io_position_P.Switch1_Threshold) {
motor_io_position_B.Switch1 = motor_io_position_P.F_c;
} else {
motor_io_position_B.Switch1 = motor_io_position_P.Constant1_Value;
}
/* End of Switch: '<S6>/Switch1' */
if (rtmIsMajorTimeStep(motor_io_position_M)) {
/* S-Function (rti_commonblock): '<S13>/S-Function2' */
/* This comment workarounds a code generation problem */
/* Gain: '<S5>/w1_scaling' */
motor_io_position_B.w1_scaling = motor_io_position_P.w1_scaling_Gain *
motor_io_position_B.SFunction2;
/* Outputs for Triggered SubSystem: '<S5>/DS1104ENC_SET_POS_C1' incorporates:
* TriggerPort: '<S15>/Trigger'
*/
if (rtmIsMajorTimeStep(motor_io_position_M)) {
/* Constant: '<S5>/Reset enc' */
zcEvent = rt_ZCFcn(RISING_ZERO_CROSSING,
&motor_io_position_PrevZCX.DS1104ENC_SET_POS_C1_Trig_ZCE,
(motor_io_position_P.Resetenc_Value));
if (zcEvent != NO_ZCEVENT) {
/* S-Function (rti_commonblock): '<S15>/S-Function1' */
/* This comment workarounds a code generation problem */
/* dSPACE I/O Board DS1104 Unit:ENC_SET */
ds1104_inc_position_write(1, 0, DS1104_INC_LINE_SUBDIV_4);
}
}
/* End of Outputs for SubSystem: '<S5>/DS1104ENC_SET_POS_C1' */
/* S-Function (rti_commonblock): '<S14>/S-Function1' */
/* This comment workarounds a code generation problem */
/* S-Function (rti_commonblock): '<S14>/S-Function2' */
/* This comment workarounds a code generation problem */
}
}
// Model step function
void etasetget256Discretized2_step(void)
{
real32_T DistanceTraveledX;
real32_T DistanceTraveledY;
real32_T Niterations;
int32_T idx;
int32_T ixstart;
int32_T n;
int32_T c_ix;
boolean_T guard1;
boolean_T varargout_1;
int32_T DiscretizedPath_sizes[2];
int32_T tmp_sizes[2];
// Outputs for Atomic SubSystem: '<Root>/Subscribe'
// Start for MATLABSystem: '<S4>/SourceBlock' incorporates:
// MATLABSystem: '<S4>/SourceBlock'
varargout_1 = Sub_etasetget256Discretized2_404.get_latest_msg
(&etasetget256Discretized2_B.varargout_2);
// Outputs for Enabled SubSystem: '<S4>/Enabled Subsystem' incorporates:
// EnablePort: '<S9>/Enable'
// MATLABSystem: '<S4>/SourceBlock' incorporates:
// Inport: '<S9>/In1'
if (varargout_1) {
etasetget256Discretized2_B.In1 = etasetget256Discretized2_B.varargout_2;
}
// End of Outputs for SubSystem: '<S4>/Enabled Subsystem'
// End of Outputs for SubSystem: '<Root>/Subscribe'
// Outputs for Atomic SubSystem: '<Root>/Subscribe2'
// Start for MATLABSystem: '<S6>/SourceBlock' incorporates:
// MATLABSystem: '<S6>/SourceBlock'
varargout_1 = Sub_etasetget256Discretized2_370.get_latest_msg
(&etasetget256Discretized2_B.varargout_2_m);
// Outputs for Enabled SubSystem: '<S6>/Enabled Subsystem' incorporates:
// EnablePort: '<S11>/Enable'
// MATLABSystem: '<S6>/SourceBlock' incorporates:
// Inport: '<S11>/In1'
if (varargout_1) {
etasetget256Discretized2_B.In1_k = etasetget256Discretized2_B.varargout_2_m;
}
// End of Outputs for SubSystem: '<S6>/Enabled Subsystem'
// End of Outputs for SubSystem: '<Root>/Subscribe2'
// Outputs for Atomic SubSystem: '<Root>/Subscribe1'
// Start for MATLABSystem: '<S5>/SourceBlock' incorporates:
// MATLABSystem: '<S5>/SourceBlock'
varargout_1 = Sub_etasetget256Discretized2_405.get_latest_msg
(&etasetget256Discretized2_B.varargout_2_k);
// Outputs for Enabled SubSystem: '<S5>/Enabled Subsystem' incorporates:
// EnablePort: '<S10>/Enable'
// MATLABSystem: '<S5>/SourceBlock' incorporates:
// Inport: '<S10>/In1'
if (varargout_1) {
etasetget256Discretized2_B.In1_b = etasetget256Discretized2_B.varargout_2_k;
}
// End of Outputs for SubSystem: '<S5>/Enabled Subsystem'
// End of Outputs for SubSystem: '<Root>/Subscribe1'
// MATLAB Function: '<Root>/MATLAB Function1'
// MATLAB Function 'MATLAB Function1': '<S3>:1'
// '<S3>:1:3'
etasetget256Discretized2_B.y = etasetget256Discretized2_B.In1_b.Data;
if (etasetget256Discretized2_B.In1_b.Data == 0.0) {
// '<S3>:1:5'
// '<S3>:1:6'
etasetget256Discretized2_B.y = 0.25;
}
// End of MATLAB Function: '<Root>/MATLAB Function1'
// MATLAB Function: '<Root>/MATLAB Function' incorporates:
// SignalConversion: '<S2>/TmpSignal ConversionAt SFunction Inport2'
// MATLAB Function 'MATLAB Function': '<S2>:1'
// '<S2>:1:5'
// save('TESTlist2Eta')
// '<S2>:1:9'
for (ixstart = 0; ixstart < 128; ixstart++) {
etasetget256Discretized2_B.Values[ixstart] =
etasetget256Discretized2_B.In1.Data[ixstart];
etasetget256Discretized2_B.Values[ixstart + 128] =
etasetget256Discretized2_B.In1.Data[128 + ixstart];
}
// '<S2>:1:11'
for (ixstart = 0; ixstart < 256; ixstart++) {
etasetget256Discretized2_B.x[ixstart] =
(etasetget256Discretized2_B.Values[ixstart] == 1000.0F);
}
idx = 0;
ixstart = 1;
varargout_1 = false;
while ((!varargout_1) && (ixstart < 257)) {
guard1 = false;
if (etasetget256Discretized2_B.x[ixstart - 1]) {
idx++;
etasetget256Discretized2_B.ii_data[idx - 1] = (int16_T)ixstart;
if (idx >= 256) {
varargout_1 = true;
} else {
guard1 = true;
}
} else {
guard1 = true;
}
if (guard1) {
ixstart++;
}
}
if (1 > idx) {
idx = 0;
}
n = idx + 1;
for (ixstart = 0; ixstart < idx; ixstart++) {
etasetget256Discretized2_B.varargin_1_data[ixstart] =
etasetget256Discretized2_B.ii_data[ixstart];
}
etasetget256Discretized2_B.varargin_1_data[idx] = 128;
idx = etasetget256Discretized2_B.varargin_1_data[0];
if (n > 1) {
for (ixstart = 1; ixstart + 1 <= n; ixstart++) {
if (etasetget256Discretized2_B.varargin_1_data[ixstart] < idx) {
idx = etasetget256Discretized2_B.varargin_1_data[ixstart];
}
}
}
for (ixstart = 0; ixstart < 256; ixstart++) {
etasetget256Discretized2_B.x[ixstart] =
(etasetget256Discretized2_B.In1.Data[ixstart] == 0.0F);
}
if ((etasetget256Discretized2_B.In1.Data[0] == 0.0F) &&
(etasetget256Discretized2_sum(etasetget256Discretized2_B.x) == 256.0)) {
// '<S2>:1:13'
// '<S2>:1:14'
etasetget256Discretized2_B.counter = 0.0;
etasetget256Discretized2_B.y = 0.0;
} else if (idx == 2) {
// '<S2>:1:15'
// asl=1
// a=asl(2);
// '<S2>:1:18'
etasetget256Discretized2_B.counter = etasetget256Discretized2_B.Values[128];
// '<S2>:1:19'
etasetget256Discretized2_B.y = etasetget256Discretized2_B.Values[0];
} else {
// DiscretizedPath
// '<S2>:1:23'
etasetget256Discretized2_B.counter = 1.0;
// '<S2>:1:24'
memset(&etasetget256Discretized2_B.DiscretizedPath[0], 0, 10000U * sizeof
(real_T));
// '<S2>:1:25'
for (ixstart = 0; ixstart <= idx - 3; ixstart++) {
// '<S2>:1:25'
// '<S2>:1:26'
// '<S2>:1:27'
DistanceTraveledX = etasetget256Discretized2_B.Values[ixstart + 1] -
etasetget256Discretized2_B.Values[ixstart];
// '<S2>:1:28'
DistanceTraveledY = etasetget256Discretized2_B.Values[ixstart + 129] -
etasetget256Discretized2_B.Values[ixstart + 128];
// '<S2>:1:29'
etasetget256Discretized2_B.Values_c[0] =
etasetget256Discretized2_B.Values[ixstart + 1] -
etasetget256Discretized2_B.Values[ixstart];
etasetget256Discretized2_B.Values_c[1] =
etasetget256Discretized2_B.Values[ixstart + 129] -
etasetget256Discretized2_B.Values[ixstart + 128];
Niterations = (real32_T)floor((real_T)(etasetget256Discretized2_norm
(etasetget256Discretized2_B.Values_c) / 0.01F));
// '<S2>:1:30'
for (n = 0; n < (int32_T)Niterations; n++) {
// '<S2>:1:30'
// '<S2>:1:31'
etasetget256Discretized2_B.DiscretizedPath[(int32_T)
etasetget256Discretized2_B.counter - 1] = (1.0F + (real32_T)n) *
DistanceTraveledX / Niterations +
etasetget256Discretized2_B.Values[ixstart];
// '<S2>:1:32'
etasetget256Discretized2_B.DiscretizedPath[(int32_T)
etasetget256Discretized2_B.counter + 4999] = (1.0F + (real32_T)n) *
DistanceTraveledY / Niterations +
etasetget256Discretized2_B.Values[ixstart + 128];
// '<S2>:1:33'
etasetget256Discretized2_B.counter++;
// '<S2>:1:30'
}
// '<S2>:1:25'
}
// '<S2>:1:36'
idx = 0;
ixstart = 1;
varargout_1 = false;
while ((!varargout_1) && (ixstart < 10001)) {
guard1 = false;
if (etasetget256Discretized2_B.DiscretizedPath[ixstart - 1] == (rtInf)) {
idx++;
etasetget256Discretized2_B.c_ii_data[idx - 1] = (int16_T)ixstart;
if (idx >= 10000) {
varargout_1 = true;
} else {
guard1 = true;
}
} else {
guard1 = true;
}
if (guard1) {
ixstart++;
}
}
if (1 > idx) {
idx = 0;
}
n = idx + 1;
for (ixstart = 0; ixstart < idx; ixstart++) {
etasetget256Discretized2_B.varargin_1_data[ixstart] =
etasetget256Discretized2_B.c_ii_data[ixstart];
}
etasetget256Discretized2_B.varargin_1_data[idx] = 5000;
idx = etasetget256Discretized2_B.varargin_1_data[0];
if (n > 1) {
for (ixstart = 1; ixstart + 1 <= n; ixstart++) {
if (etasetget256Discretized2_B.varargin_1_data[ixstart] < idx) {
idx = etasetget256Discretized2_B.varargin_1_data[ixstart];
}
}
}
if (1 > idx - 1) {
idx = 0;
} else {
idx--;
}
// '<S2>:1:37'
// %a=asl
// '<S2>:1:40'
// Discretizing PathVector
// for i=1:64
// AvstandDummy=(eta(1)-Values(i,1))^2+(eta(2)-Values(i,2))^2;
// if AvstandDummy<AvstandSaved
// AvstandDummy=AvstandSaved;
// NearestPointPos=i;
// end
// end
etasetget256Discretized2_B.dv0[0] = etasetget256Discretized2_B.In1_k.Y;
etasetget256Discretized2_B.dv0[1] = etasetget256Discretized2_B.In1_k.X;
etasetget256Discretized2_repmat(etasetget256Discretized2_B.dv0, (real_T)idx,
etasetget256Discretized2_B.tmp_data, etasetget256Discretized2_B.tmp_sizes);
DiscretizedPath_sizes[0] = idx;
DiscretizedPath_sizes[1] = 2;
for (ixstart = 0; ixstart < idx; ixstart++) {
etasetget256Discretized2_B.DiscretizedPath_data[ixstart] =
etasetget256Discretized2_B.DiscretizedPath[ixstart] -
etasetget256Discretized2_B.tmp_data[ixstart];
}
for (ixstart = 0; ixstart < idx; ixstart++) {
etasetget256Discretized2_B.DiscretizedPath_data[ixstart + idx] =
etasetget256Discretized2_B.DiscretizedPath[ixstart + 5000] -
etasetget256Discretized2_B.tmp_data[ixstart +
etasetget256Discretized2_B.tmp_sizes[0]];
}
etasetget256Discretized2_abs(etasetget256Discretized2_B.DiscretizedPath_data,
DiscretizedPath_sizes, etasetget256Discretized2_B.tmp_data,
etasetget256Discretized2_B.tmp_sizes);
etasetget256Discretized2_power(etasetget256Discretized2_B.tmp_data,
etasetget256Discretized2_B.tmp_sizes,
etasetget256Discretized2_B.DiscretizedPath_data, DiscretizedPath_sizes);
tmp_sizes[0] = 2;
tmp_sizes[1] = DiscretizedPath_sizes[0];
idx = DiscretizedPath_sizes[0];
for (ixstart = 0; ixstart < idx; ixstart++) {
etasetget256Discretized2_B.tmp_data[ixstart << 1] =
etasetget256Discretized2_B.DiscretizedPath_data[ixstart];
etasetget256Discretized2_B.tmp_data[1 + (ixstart << 1)] =
etasetget256Discretized2_B.DiscretizedPath_data[ixstart +
DiscretizedPath_sizes[0]];
}
etasetget256Discretized2_sum_k(etasetget256Discretized2_B.tmp_data,
tmp_sizes, etasetget256Discretized2_B.b_varargin_1_data,
etasetget256Discretized2_B.tmp_sizes);
ixstart = 1;
n = etasetget256Discretized2_B.tmp_sizes[1];
etasetget256Discretized2_B.counter =
etasetget256Discretized2_B.b_varargin_1_data[0];
idx = 1;
if (etasetget256Discretized2_B.tmp_sizes[1] > 1) {
if (rtIsNaN(etasetget256Discretized2_B.b_varargin_1_data[0])) {
c_ix = 2;
varargout_1 = false;
while ((!varargout_1) && (c_ix <= n)) {
ixstart = c_ix;
if (!rtIsNaN(etasetget256Discretized2_B.b_varargin_1_data[c_ix - 1]))
{
etasetget256Discretized2_B.counter =
etasetget256Discretized2_B.b_varargin_1_data[c_ix - 1];
idx = c_ix;
varargout_1 = true;
} else {
c_ix++;
}
}
}
if (ixstart < etasetget256Discretized2_B.tmp_sizes[1]) {
while (ixstart + 1 <= n) {
if (etasetget256Discretized2_B.b_varargin_1_data[ixstart] <
etasetget256Discretized2_B.counter) {
etasetget256Discretized2_B.counter =
etasetget256Discretized2_B.b_varargin_1_data[ixstart];
idx = ixstart + 1;
}
ixstart++;
}
}
}
// '<S2>:1:62'
// '<S2>:1:64'
etasetget256Discretized2_B.counter = (real_T)idx - rt_roundd_snf
(etasetget256Discretized2_B.y / 0.01);
etasetget256Discretized2_B.y = 1.0;
if (etasetget256Discretized2_B.counter > 1.0) {
etasetget256Discretized2_B.y = etasetget256Discretized2_B.counter;
}
// '<S2>:1:65'
etasetget256Discretized2_B.counter =
etasetget256Discretized2_B.DiscretizedPath[(int32_T)
etasetget256Discretized2_B.y + 4999];
// '<S2>:1:66'
etasetget256Discretized2_B.y = etasetget256Discretized2_B.DiscretizedPath
[(int32_T)etasetget256Discretized2_B.y - 1];
// etaset = u;11
// HEADING:
// end
}
// End of MATLAB Function: '<Root>/MATLAB Function'
// BusAssignment: '<S1>/Bus Assignment1' incorporates:
// Constant: '<S1>/Constant'
// '<S2>:1:71'
etasetget256Discretized2_B.BusAssignment1.X =
etasetget256Discretized2_B.counter;
etasetget256Discretized2_B.BusAssignment1.Y = etasetget256Discretized2_B.y;
etasetget256Discretized2_B.BusAssignment1.Z =
etasetget256Discretized2_P.Constant_Value_bx;
// Outputs for Atomic SubSystem: '<S1>/Publish1'
// MATLABSystem: '<S8>/SinkBlock'
Pub_etasetget256Discretized2_400.publish
(&etasetget256Discretized2_B.BusAssignment1);
// End of Outputs for SubSystem: '<S1>/Publish1'
}
/* Function Definitions */
void meanfile(const char_T filename_data[200], const int32_T filename_size[2],
real_T IRgf[3], real_T UVgf[3], real_T IRavg[2], real_T UVavg[2])
{
int32_T i3;
int32_T posA;
char_T tmp_data[201];
char_T cv6[2];
static const char_T cv7[2] = { 'r', '\x00' };
FILE * f;
int32_T tempvar;
static char_T bufline[1048576];
int32_T nStep;
uint8_T char100;
int32_T posB;
int32_T nSam;
int32_T nFs;
int32_T k4step;
char_T cv8[51];
static const char_T cv9[51] = { '%', 'c', 'S', 'W', 'N', ' ', 'p', 'o', 's',
'A', '=', '%', 'd', ',', ' ', 'p', 'o', 's', 'B', '=', '%', 'd', ',', ' ',
'n', 'S', 't', 'e', 'p', 's', '=', '%', 'd', ',', ' ', 'n', 'S', 'a', 'm',
'=', '%', 'd', ',', ' ', 'F', 's', '=', '%', 'd', '\x0d', '\x0a' };
emxArray_real_T *dataIR;
emxArray_real_T *dataUV;
char_T cv10[3];
static const char_T cv11[3] = { '%', 'd', ' ' };
int32_T q0;
real_T dataIR_std;
uint32_T qY;
emxArray_real_T *dataIR_mean;
emxArray_real_T *dataUV_mean;
emxArray_real_T *b_dataIR;
emxArray_real_T *b_dataUV;
real_T dataUV_std;
emxArray_real_T *c_dataIR;
emxArray_real_T *c_dataUV;
real_T aIR;
real_T aUV;
real_T muUV;
real_T sigUV;
/* Calcuate stastistics of IR and UV signals */
/* */
/* version: .\work_gfit_rdfile\meanfile_v3.m */
/* Return values: IRgf=[muIR, sigIR, aIR] for mean, width and amplitude of */
/* Gaussian fitting over hist(IR) */
/* UVgf=[muUV, sigUV, aUV] for mean, width and amplitude of */
/* Gaussian fitting over hist(IR) */
/* IRavg=[meanIR, stdIR] for mean and std of IR by mean() and std() */
/* UVavg=[meanUV, stdUV] for mean and std of UV by mean() and std() */
/* */
/* The 'fprintf' function will not be compiled and instead passed */
/* to the MATLAB runtime. If we choose to generate code for this example, */
/* all calls to extrinsic functions are automatically eliminated. */
/* Put class and size constraints on function input. */
for (i3 = 0; i3 < 2; i3++) {
IRavg[i3] = 0.0;
UVavg[i3] = 0.0;
}
/* Define a new opaque variable 'f' which will be of type 'FILE *' */
/* in the generated C code initially with the value NULL. */
/* Call fopen(filename 'r'), but we need to convert the MATLAB */
/* string into a C type string (which is the same string with the */
/* NUL (\0) string terminator). */
/* Create a NUL terminated C string given a MATLAB string */
posA = filename_size[1] - 1;
for (i3 = 0; i3 <= posA; i3++) {
tmp_data[i3] = filename_data[filename_size[0] * i3];
}
tmp_data[filename_size[1]] = '\x00';
for (i3 = 0; i3 < 2; i3++) {
cv6[i3] = cv7[i3];
}
f = fopen(&tmp_data[0], cv6);
tempvar = (int)(f);
if (tempvar == 0) {
for (i3 = 0; i3 < 3; i3++) {
IRgf[i3] = -1.0;
UVgf[i3] = -1.0;
}
} else {
/* Call fseek(f, 0, SEEK_END) to set file position to the end of */
/* the file. */
fseek(f, 0, SEEK_END);
/* We need to initialize the variable 'filelen' to the proper type */
/* as custom C functions are not analyzed. */
/* Call ftell(f) which will return the length of the file in bytes */
/* (as current file position is at the end of the file). */
ftell(f);
/* Reset current file position */
fseek(f, 0, SEEK_SET);
/* Initialize a buffer */
/* buffer = zeros(1,bufSIZE,'uint8'); */
memset(&bufline[0], 0, sizeof(uint8_T) << 20);
/* NOTE: bufline will be define as uint8_T type in generated C code */
/* manually change its type to char_T in the generated c codes, */
/* like char_T bufline[65536]; */
/* strline=zeros(1,1024,'uint8'); */
nStep = 0;
/* Remaining is the number of bytes to read (from the file) */
/* Index is the current position to read into the buffer */
char100 = 0;
posA = 0;
posB = 0;
nSam = 0;
nFs = 0;
k4step = 0;
/* a temp variable to keep the value of current steps */
fgets(&bufline[0], 1024, f);
strlen(&bufline[0]);
for (i3 = 0; i3 < 51; i3++) {
cv8[i3] = cv9[i3];
}
tempvar = fscanf(f, cv8, &char100, &posA, &posB, &nStep, &nSam, &nFs);
if (tempvar != 6) {
fclose(f);
for (i3 = 0; i3 < 3; i3++) {
IRgf[i3] = -2.0;
UVgf[i3] = -2.0;
}
} else {
for (tempvar = 0; tempvar < 3; tempvar++) {
fgets(&bufline[0], 8192, f);
strlen(&bufline[0]);
}
b_emxInit_real_T(&dataIR, 2);
/* read data from 7-th line now */
/* k=nStep; */
i3 = dataIR->size[0] * dataIR->size[1];
dataIR->size[0] = nStep;
dataIR->size[1] = nSam;
emxEnsureCapacity((emxArray__common *)dataIR, i3, (int32_T)sizeof(real_T));
posA = nStep * nSam - 1;
for (i3 = 0; i3 <= posA; i3++) {
dataIR->data[i3] = 0.0;
}
b_emxInit_real_T(&dataUV, 2);
i3 = dataUV->size[0] * dataUV->size[1];
dataUV->size[0] = nStep;
dataUV->size[1] = nSam;
emxEnsureCapacity((emxArray__common *)dataUV, i3, (int32_T)sizeof(real_T));
posA = nStep * nSam - 1;
for (i3 = 0; i3 <= posA; i3++) {
dataUV->data[i3] = 0.0;
}
/* read data line-by-line */
if (nStep != 1) {
/* refir.txt, refuv.txt, wtrir.txt, wtruv.txt should contain */
/* ONE data line ONLY. */
for (i3 = 0; i3 < 3; i3++) {
IRgf[i3] = -3.0;
UVgf[i3] = -3.0;
}
} else {
/* fprintf('line %d, ',nline); */
tempvar = mul_s32_s32_s32_sat(nSam, 10);
posA = 7 + tempvar;
if ((tempvar > 0) && (posA <= 0)) {
posA = MAX_int32_T;
}
tempvar = posA + 2;
if ((posA > 0) && (tempvar <= 0)) {
tempvar = MAX_int32_T;
}
fgets(&bufline[0], tempvar, f);
strlen(&bufline[0]);
/* to debug if fgets ok */
/* % read motor step data */
for (i3 = 0; i3 < 3; i3++) {
cv10[i3] = cv11[i3];
}
sscanf(&bufline[0], cv10, &k4step);
/* fprintf(' nStep=%d \n', k4step); */
(double)(k4step);
/* % read IR UV data */
for (posB = 1; posB <= nSam; posB++) {
/* nVar=coder.ceval('sscanf',coder.ref(bufline(7+10*(i-1))), ['%d %d'],coder.ref(xir), coder.ref(xuv)); */
posA = posB - 1;
if ((posB < 0) && (posA >= 0)) {
posA = MIN_int32_T;
}
tempvar = mul_s32_s32_s32_sat(10, posA);
nFs = 7 + tempvar;
if ((tempvar > 0) && (nFs <= 0)) {
nFs = MAX_int32_T;
}
nStep = 0;
posA = 0;
/* fprintf('bufline=%s, ',bufline(kChar:kChar+9)); */
for (k4step = 0; k4step < 4; k4step++) {
/* x4Char(ii)=bufline(kChar+ii-1)-30; */
q0 = mul_s32_s32_s32_sat(nStep, 10);
dataIR_std = rt_roundd_snf((real_T)nFs + (1.0 + (real_T)k4step));
if (dataIR_std < 2.147483648E+9) {
i3 = (int32_T)dataIR_std;
} else {
i3 = MAX_int32_T;
}
tempvar = bufline[i3 - 2];
qY = (uint32_T)tempvar - 48U;
if (qY > (uint32_T)tempvar) {
qY = 0U;
}
tempvar = (int32_T)qY;
nStep = q0 + tempvar;
if ((q0 > 0) && ((tempvar > 0) && (nStep <= 0))) {
nStep = MAX_int32_T;
}
q0 = mul_s32_s32_s32_sat(posA, 10);
posA = nFs + 5;
if ((nFs > 0) && (posA <= 0)) {
posA = MAX_int32_T;
}
dataIR_std = rt_roundd_snf((real_T)posA + (1.0 + (real_T)k4step));
if (dataIR_std < 2.147483648E+9) {
i3 = (int32_T)dataIR_std;
} else {
i3 = MAX_int32_T;
}
tempvar = bufline[i3 - 2];
qY = (uint32_T)tempvar - 48U;
if (qY > (uint32_T)tempvar) {
qY = 0U;
}
tempvar = (int32_T)qY;
posA = q0 + tempvar;
if ((q0 > 0) && ((tempvar > 0) && (posA <= 0))) {
posA = MAX_int32_T;
}
}
/* if (i<5) */
/* fprintf ('xir=%04d xuv=%04d \r\n', xir, xuv); */
/* end; */
/* dataIR(k,i)=coder.ceval('(double)',xir); */
/* dataUV(k,i)=coder.ceval('(double)',xuv); */
dataIR->data[dataIR->size[0] * (posB - 1)] = (real_T)nStep;
dataUV->data[dataUV->size[0] * (posB - 1)] = (real_T)posA;
}
emxInit_real_T(&dataIR_mean, 1);
emxInit_real_T(&dataUV_mean, 1);
b_emxInit_real_T(&b_dataIR, 2);
/* Close file */
fclose(f);
/* % data processing */
/* calculating by mean() and std() */
mean(dataIR, dataIR_mean);
mean(dataUV, dataUV_mean);
i3 = dataIR->size[1];
tempvar = b_dataIR->size[0] * b_dataIR->size[1];
b_dataIR->size[0] = 1;
b_dataIR->size[1] = i3;
emxEnsureCapacity((emxArray__common *)b_dataIR, tempvar, (int32_T)sizeof
(real_T));
posA = i3 - 1;
for (i3 = 0; i3 <= posA; i3++) {
b_dataIR->data[b_dataIR->size[0] * i3] = dataIR->data[dataIR->size[0] *
i3];
}
b_emxInit_real_T(&b_dataUV, 2);
dataIR_std = b_std(b_dataIR);
i3 = dataUV->size[1];
tempvar = b_dataUV->size[0] * b_dataUV->size[1];
b_dataUV->size[0] = 1;
b_dataUV->size[1] = i3;
emxEnsureCapacity((emxArray__common *)b_dataUV, tempvar, (int32_T)sizeof
(real_T));
emxFree_real_T(&b_dataIR);
posA = i3 - 1;
for (i3 = 0; i3 <= posA; i3++) {
b_dataUV->data[b_dataUV->size[0] * i3] = dataUV->data[dataUV->size[0] *
i3];
}
dataUV_std = b_std(b_dataUV);
/* set return values of IRavg, UVavg */
tempvar = dataIR_mean->size[0];
emxFree_real_T(&b_dataUV);
i3 = 0;
while (i3 <= 0) {
posA = tempvar - 1;
for (i3 = 0; i3 <= posA; i3++) {
IRavg[i3] = dataIR_mean->data[i3];
}
i3 = 1;
}
emxFree_real_T(&dataIR_mean);
IRavg[1] = dataIR_std;
tempvar = dataUV_mean->size[0];
i3 = 0;
while (i3 <= 0) {
posA = tempvar - 1;
for (i3 = 0; i3 <= posA; i3++) {
UVavg[i3] = dataUV_mean->data[i3];
}
i3 = 1;
}
emxFree_real_T(&dataUV_mean);
b_emxInit_real_T(&c_dataIR, 2);
UVavg[1] = dataUV_std;
/* calculalting by Gaussian fitting over hist() */
i3 = dataIR->size[1];
tempvar = c_dataIR->size[0] * c_dataIR->size[1];
c_dataIR->size[0] = 1;
c_dataIR->size[1] = i3;
emxEnsureCapacity((emxArray__common *)c_dataIR, tempvar, (int32_T)sizeof
(real_T));
posA = i3 - 1;
for (i3 = 0; i3 <= posA; i3++) {
c_dataIR->data[c_dataIR->size[0] * i3] = dataIR->data[dataIR->size[0] *
i3];
}
b_emxInit_real_T(&c_dataUV, 2);
b_gfitPDF(c_dataIR, &dataIR_std, &dataUV_std, &aIR);
/* sigUV=0; muUV=0; aUV=0; */
i3 = dataUV->size[1];
tempvar = c_dataUV->size[0] * c_dataUV->size[1];
c_dataUV->size[0] = 1;
c_dataUV->size[1] = i3;
emxEnsureCapacity((emxArray__common *)c_dataUV, tempvar, (int32_T)sizeof
(real_T));
emxFree_real_T(&c_dataIR);
posA = i3 - 1;
for (i3 = 0; i3 <= posA; i3++) {
c_dataUV->data[c_dataUV->size[0] * i3] = dataUV->data[dataUV->size[0] *
i3];
}
b_gfitPDF(c_dataUV, &sigUV, &muUV, &aUV);
/* return the optimal step */
IRgf[0] = dataUV_std;
IRgf[1] = dataIR_std;
IRgf[2] = aIR;
UVgf[0] = muUV;
UVgf[1] = sigUV;
UVgf[2] = aUV;
/* % % plot mean data */
/* title(sprintf('IR/UV vs steps \n opt(UV)=%d, opt(IR)=%d',round(muUV), round(muIR)), 'fontsize', 14); */
/* save('instData', 'dataIR', 'dataUV','dataIR_mean','dataUV_mean'); */
emxFree_real_T(&c_dataUV);
}
emxFree_real_T(&dataUV);
emxFree_real_T(&dataIR);
}
}
}
