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); } } }