void OmniRobot::brickPower() { int m1 = rt_SATURATE((int)pwm(2), -100, 100); int m2 = rt_SATURATE((int)pwm(3), -100, 100); int m3 = -rt_SATURATE((int)pwm(1), -100, 100); int m4 = -rt_SATURATE((int)pwm(0), -100, 100); brick.powerMotor("1")->setPower(m1); brick.powerMotor("2")->setPower(m2); brick.powerMotor("3")->setPower(m3); brick.powerMotor("4")->setPower(m4); }
/* Model output function */ void Crane_output(int_T tid) { /* local block i/o variables */ real_T rtb_Falcon_o1[3]; real_T rtb_Gain4[3]; boolean_T rtb_Falcon_o2[4]; /* Update absolute time of base rate at minor time step */ if (rtmIsMinorTimeStep(Crane_M)) { Crane_M->Timing.t[0] = rtsiGetT(&Crane_M->solverInfo); } if (rtmIsMajorTimeStep(Crane_M)) { /* set solver stop time */ rtsiSetSolverStopTime(&Crane_M->solverInfo, ((Crane_M->Timing.clockTick0+1)* Crane_M->Timing.stepSize0)); } /* end MajorTimeStep */ { real_T rtb_Product_idx; real_T rtb_Product_idx_0; real_T rtb_Product_idx_1; real_T rtb_Product1_idx; real_T rtb_Product1_idx_0; real_T rtb_Product1_idx_1; /* S-Function Block: <S10>/Block#1 */ { _rtMech_PWORK *mechWork = (_rtMech_PWORK *) Crane_DWork.Block1_PWORK; mechWork->genSimData.time = Crane_M->Timing.t[0]; mechWork->outSimData.majorTimestep = rtmIsMajorTimeStep(Crane_M); if (kinematicSfcnOutputMethod(mechWork->mechanism, &(mechWork->genSimData), &(mechWork->outSimData))) { { const ErrorRecord *err = getErrorMsg(); static char_T errorMsg[1024]; sprintf(errorMsg, err->errorMsg, err->blocks[0], err->blocks[1], err->blocks[2], err->blocks[3], err->blocks[4]); rtmSetErrorStatus(Crane_M, errorMsg); return; } } } /* Gain: '<S2>/gain_1' */ Crane_B.gain_1[0] = Crane_P.gain_1_Gain * Crane_B.Block1_o1[0]; Crane_B.gain_1[1] = Crane_P.gain_1_Gain * Crane_B.Block1_o1[1]; Crane_B.gain_1[2] = Crane_P.gain_1_Gain * Crane_B.Block1_o1[2]; if (rtmIsMajorTimeStep(Crane_M) && Crane_M->Timing.TaskCounters.TID[1] == 0) { /* Memory: '<Root>/Memory' */ Crane_B.Memory[0] = Crane_DWork.Memory_PreviousInput[0]; Crane_B.Memory[1] = Crane_DWork.Memory_PreviousInput[1]; Crane_B.Memory[2] = Crane_DWork.Memory_PreviousInput[2]; } /* Sum: '<S9>/Sum' */ Crane_B.Sum[0] = Crane_B.Memory[0] - Crane_B.gain_1[0]; Crane_B.Sum[1] = Crane_B.Memory[1] - Crane_B.gain_1[1]; Crane_B.Sum[2] = Crane_B.Memory[2] - Crane_B.gain_1[2]; /* Product: '<S9>/Product' incorporates: * Constant: '<Root>/Constant2' */ rtb_Product_idx = Crane_P.Constant2_Value * Crane_B.Sum[0]; rtb_Product_idx_0 = Crane_P.Constant2_Value * Crane_B.Sum[1]; rtb_Product_idx_1 = Crane_P.Constant2_Value * Crane_B.Sum[2]; /* Integrator: '<S9>/Integrator' */ rtb_Gain4[0] = Crane_X.Integrator_CSTATE[0]; rtb_Gain4[1] = Crane_X.Integrator_CSTATE[1]; rtb_Gain4[2] = Crane_X.Integrator_CSTATE[2]; /* Product: '<S9>/Product1' incorporates: * Constant: '<Root>/Constant5' */ rtb_Product1_idx = Crane_P.Constant5_Value * rtb_Gain4[0]; rtb_Product1_idx_0 = Crane_P.Constant5_Value * rtb_Gain4[1]; rtb_Product1_idx_1 = Crane_P.Constant5_Value * rtb_Gain4[2]; /* Derivative Block: '<S9>/Derivative' */ { real_T t = Crane_M->Timing.t[0]; real_T timeStampA = Crane_DWork.Derivative_RWORK.TimeStampA; real_T timeStampB = Crane_DWork.Derivative_RWORK.TimeStampB; if (timeStampA >= t && timeStampB >= t) { rtb_Gain4[0] = 0.0; rtb_Gain4[1] = 0.0; rtb_Gain4[2] = 0.0; } else { real_T deltaT; real_T *lastBank = &Crane_DWork.Derivative_RWORK.TimeStampA; if (timeStampA < timeStampB) { if (timeStampB < t) { lastBank += 4; } } else if (timeStampA >= t) { lastBank += 4; } deltaT = t - *lastBank++; rtb_Gain4[0] = (Crane_B.Sum[0] - *lastBank++) / deltaT; rtb_Gain4[1] = (Crane_B.Sum[1] - *lastBank++) / deltaT; rtb_Gain4[2] = (Crane_B.Sum[2] - *lastBank++) / deltaT; } } /* Sum: '<S9>/Sum1' incorporates: * Constant: '<Root>/Constant6' * Product: '<S9>/Product2' */ Crane_B.Sum1[0] = (rtb_Product_idx + rtb_Product1_idx) + Crane_P.Constant6_Value * rtb_Gain4[0]; Crane_B.Sum1[1] = (rtb_Product_idx_0 + rtb_Product1_idx_0) + Crane_P.Constant6_Value * rtb_Gain4[1]; Crane_B.Sum1[2] = (rtb_Product_idx_1 + rtb_Product1_idx_1) + Crane_P.Constant6_Value * rtb_Gain4[2]; /* Gain: '<Root>/Gain4' */ rtb_Gain4[0] = Crane_P.Gain4_Gain * Crane_B.Sum1[0]; rtb_Gain4[1] = Crane_P.Gain4_Gain * Crane_B.Sum1[1]; rtb_Gain4[2] = Crane_P.Gain4_Gain * Crane_B.Sum1[2]; /* Saturate: '<Root>/Saturation' */ Crane_B.Saturation[0] = rt_SATURATE(rtb_Gain4[2], Crane_P.Saturation_LowerSat, Crane_P.Saturation_UpperSat); Crane_B.Saturation[1] = rt_SATURATE(rtb_Gain4[0], Crane_P.Saturation_LowerSat, Crane_P.Saturation_UpperSat); Crane_B.Saturation[2] = rt_SATURATE(rtb_Gain4[1], Crane_P.Saturation_LowerSat, Crane_P.Saturation_UpperSat); if (rtmIsMajorTimeStep(Crane_M) && Crane_M->Timing.TaskCounters.TID[1] == 0) { /* S-Function Block: Crane/Falcon (falcon_block) */ { t_error result; double force_vector[3]; double position[3]; t_int read_buttons; force_vector[0] = Crane_B.Saturation[0]; force_vector[1] = Crane_B.Saturation[1]; force_vector[2] = Crane_B.Saturation[2]; /* read the position and buttons, and output the requested force to the falcon */ result = falcon_read_write(Crane_DWork.Falcon_Falcon, position, &read_buttons, force_vector); if (result < 0) { msg_get_error_messageA(NULL, result, _rt_error_message, sizeof (_rt_error_message)); rtmSetErrorStatus(Crane_M, _rt_error_message); return; } rtb_Falcon_o1[0] = position[0]; rtb_Falcon_o1[1] = position[1]; rtb_Falcon_o1[2] = position[2]; rtb_Falcon_o2[0] = ((read_buttons & 0x01) != 0); rtb_Falcon_o2[1] = ((read_buttons & 0x02) != 0); rtb_Falcon_o2[2] = ((read_buttons & 0x04) != 0); rtb_Falcon_o2[3] = ((read_buttons & 0x08) != 0); } /* Gain: '<Root>/Gain3' */ Crane_B.Gain3[0] = Crane_P.Gain3_Gain * rtb_Falcon_o1[0]; Crane_B.Gain3[1] = Crane_P.Gain3_Gain * rtb_Falcon_o1[1]; Crane_B.Gain3[2] = Crane_P.Gain3_Gain * rtb_Falcon_o1[2]; /* Constant: '<Root>/Constant' */ Crane_B.Constant = Crane_P.Constant_Value; /* Stop: '<Root>/Stop Simulation' */ if (rtb_Falcon_o2[0] || rtb_Falcon_o2[1] || rtb_Falcon_o2[2] || rtb_Falcon_o2[3]) { rtmSetStopRequested(Crane_M, 1); } /* Gain: '<Root>/Gain2' incorporates: * Constant: '<Root>/Constant1' */ Crane_B.Gain2 = Crane_P.Gain2_Gain * Crane_P.Constant1_Value; /* Gain: '<Root>/Gain1' incorporates: * Constant: '<Root>/Constant3' */ Crane_B.Gain1 = Crane_P.Gain1_Gain * Crane_P.Constant3_Value; } /* Gain: '<Root>/Gain' incorporates: * Sin: '<Root>/Sine Wave' */ Crane_B.Gain = (sin(Crane_P.SineWave_Freq * Crane_M->Timing.t[0] + Crane_P.SineWave_Phase) * Crane_P.SineWave_Amp + Crane_P.SineWave_Bias) * Crane_P.Gain_Gain; if (rtmIsMajorTimeStep(Crane_M) && Crane_M->Timing.TaskCounters.TID[1] == 0) { /* Gain: '<S10>/_gravity_conversion' incorporates: * Constant: '<S8>/SOURCE_BLOCK' */ Crane_B._gravity_conversion[0] = Crane_P._gravity_conversion_Gain * Crane_P.SOURCE_BLOCK_Value[0]; Crane_B._gravity_conversion[1] = Crane_P._gravity_conversion_Gain * Crane_P.SOURCE_BLOCK_Value[1]; Crane_B._gravity_conversion[2] = Crane_P._gravity_conversion_Gain * Crane_P.SOURCE_BLOCK_Value[2]; } /* S-Function Block: <S10>/Block#2 */ { _rtMech_PWORK *mechWork = (_rtMech_PWORK *) Crane_DWork.Block2_PWORK; mechWork->genSimData.time = Crane_M->Timing.t[0]; mechWork->outSimData.majorTimestep = rtmIsMajorTimeStep(Crane_M); if (dynamicSfcnOutputMethod(mechWork->mechanism, &(mechWork->genSimData), &(mechWork->outSimData))) { { const ErrorRecord *err = getErrorMsg(); static char_T errorMsg[1024]; sprintf(errorMsg, err->errorMsg, err->blocks[0], err->blocks[1], err->blocks[2], err->blocks[3], err->blocks[4]); rtmSetErrorStatus(Crane_M, errorMsg); return; } } } /* S-Function Block: <S10>/Block#3 */ { _rtMech_PWORK *mechWork = (_rtMech_PWORK *) Crane_DWork.Block3_PWORK; mechWork->genSimData.time = Crane_M->Timing.t[0]; mechWork->outSimData.majorTimestep = rtmIsMajorTimeStep(Crane_M); if (eventSfcnOutputMethod(mechWork->mechanism, &(mechWork->genSimData), &(mechWork->outSimData))) { { const ErrorRecord *err = getErrorMsg(); static char_T errorMsg[1024]; sprintf(errorMsg, err->errorMsg, err->blocks[0], err->blocks[1], err->blocks[2], err->blocks[3], err->blocks[4]); rtmSetErrorStatus(Crane_M, errorMsg); return; } } } } UNUSED_PARAMETER(tid); }
/* Model step function */ void balance_control(float args_cmd_forward, float args_cmd_turn, float args_gyro, float args_gyro_offset, float args_theta_m_l, float args_theta_m_r, float args_battery, signed char *ret_pwm_l, signed char *ret_pwm_r) { { float tmp_theta; float tmp_theta_lpf; float tmp_pwm_r_limiter; float tmp_psidot; float tmp_pwm_turn; float tmp_pwm_l_limiter; float tmp_thetadot_cmd_lpf; float tmp[4]; float tmp_theta_0[4]; long tmp_0; /* Sum: '<S8>/Sum' incorporates: * Constant: '<S3>/Constant6' * Constant: '<S8>/Constant' * Constant: '<S8>/Constant1' * Gain: '<S3>/Gain1' * Gain: '<S8>/Gain2' * Inport: '<Root>/cmd_forward' * Product: '<S3>/Divide' * Product: '<S8>/Product' * Sum: '<S8>/Sum1' * UnitDelay: '<S8>/Unit Delay' */ tmp_thetadot_cmd_lpf = (((args_cmd_forward / CMD_MAX) * K_THETADOT) * (1.0F - A_R)) + (A_R * ud_thetadot_cmd_lpf); /* Gain: '<S4>/Gain' incorporates: * Gain: '<S4>/deg2rad' * Gain: '<S4>/deg2rad1' * Inport: '<Root>/theta_m_l' * Inport: '<Root>/theta_m_r' * Sum: '<S4>/Sum1' * Sum: '<S4>/Sum4' * Sum: '<S4>/Sum6' * UnitDelay: '<S10>/Unit Delay' */ tmp_theta = (((DEG2RAD * args_theta_m_l) + ud_psi) + ((DEG2RAD * args_theta_m_r) + ud_psi)) * 0.5F; /* Sum: '<S11>/Sum' incorporates: * Constant: '<S11>/Constant' * Constant: '<S11>/Constant1' * Gain: '<S11>/Gain2' * Product: '<S11>/Product' * Sum: '<S11>/Sum1' * UnitDelay: '<S11>/Unit Delay' */ tmp_theta_lpf = ((1.0F - A_D) * tmp_theta) + (A_D * ud_theta_lpf); /* Gain: '<S4>/deg2rad2' incorporates: * Inport: '<Root>/gyro' * Inport: '<Root>/gyro_offset' * Sum: '<S4>/Sum2' */ tmp_psidot = (args_gyro - args_gyro_offset) * DEG2RAD; /* Gain: '<S2>/Gain' incorporates: * Constant: '<S3>/Constant2' * Constant: '<S3>/Constant3' * Constant: '<S6>/Constant' * Constant: '<S9>/Constant' * Gain: '<S1>/FeedbackGain' * Gain: '<S1>/IntegralGain' * Gain: '<S6>/Gain3' * Inport: '<Root>/battery' * Product: '<S2>/Product' * Product: '<S9>/Product' * Sum: '<S1>/Sum2' * Sum: '<S1>/sum_err' * Sum: '<S6>/Sum2' * Sum: '<S9>/Sum' * UnitDelay: '<S10>/Unit Delay' * UnitDelay: '<S11>/Unit Delay' * UnitDelay: '<S5>/Unit Delay' * UnitDelay: '<S7>/Unit Delay' */ tmp[0] = ud_theta_ref; tmp[1] = 0.0F; tmp[2] = tmp_thetadot_cmd_lpf; tmp[3] = 0.0F; tmp_theta_0[0] = tmp_theta; tmp_theta_0[1] = ud_psi; tmp_theta_0[2] = (tmp_theta_lpf - ud_theta_lpf) / EXEC_PERIOD; tmp_theta_0[3] = tmp_psidot; tmp_pwm_r_limiter = 0.0F; for (tmp_0 = 0; tmp_0 < 4; tmp_0++) { tmp_pwm_r_limiter += (tmp[tmp_0] - tmp_theta_0[tmp_0]) * K_F[(tmp_0)]; } tmp_pwm_r_limiter = (((K_I * ud_err_theta) + tmp_pwm_r_limiter) / ((BATTERY_GAIN * args_battery) - BATTERY_OFFSET)) * 100.0F; /* Gain: '<S3>/Gain2' incorporates: * Constant: '<S3>/Constant1' * Inport: '<Root>/cmd_turn' * Product: '<S3>/Divide1' */ tmp_pwm_turn = (args_cmd_turn / CMD_MAX) * K_PHIDOT; /* Sum: '<S2>/Sum' */ tmp_pwm_l_limiter = tmp_pwm_r_limiter + tmp_pwm_turn; /* Saturate: '<S2>/pwm_l_limiter' */ tmp_pwm_l_limiter = rt_SATURATE(tmp_pwm_l_limiter, -100.0F, 100.0F); /* Outport: '<Root>/pwm_l' incorporates: * DataTypeConversion: '<S1>/Data Type Conversion' */ (*ret_pwm_l) = (signed char)tmp_pwm_l_limiter; /* Sum: '<S2>/Sum1' */ tmp_pwm_r_limiter -= tmp_pwm_turn; /* Saturate: '<S2>/pwm_r_limiter' */ tmp_pwm_r_limiter = rt_SATURATE(tmp_pwm_r_limiter, -100.0F, 100.0F); /* Outport: '<Root>/pwm_r' incorporates: * DataTypeConversion: '<S1>/Data Type Conversion6' */ (*ret_pwm_r) = (signed char)tmp_pwm_r_limiter; /* Sum: '<S7>/Sum' incorporates: * Gain: '<S7>/Gain' * UnitDelay: '<S7>/Unit Delay' */ tmp_pwm_l_limiter = (EXEC_PERIOD * tmp_thetadot_cmd_lpf) + ud_theta_ref; /* Sum: '<S10>/Sum' incorporates: * Gain: '<S10>/Gain' * UnitDelay: '<S10>/Unit Delay' */ tmp_pwm_turn = (EXEC_PERIOD * tmp_psidot) + ud_psi; /* Sum: '<S5>/Sum' incorporates: * Gain: '<S5>/Gain' * Sum: '<S1>/Sum1' * UnitDelay: '<S5>/Unit Delay' * UnitDelay: '<S7>/Unit Delay' */ tmp_pwm_r_limiter = ((ud_theta_ref - tmp_theta) * EXEC_PERIOD) + ud_err_theta; /* user code (Update function Body) */ /* System '<Root>' */ /* 次回演算用状態量保存処理 */ /* Update for UnitDelay: '<S5>/Unit Delay' */ ud_err_theta = tmp_pwm_r_limiter; /* Update for UnitDelay: '<S7>/Unit Delay' */ ud_theta_ref = tmp_pwm_l_limiter; /* Update for UnitDelay: '<S8>/Unit Delay' */ ud_thetadot_cmd_lpf = tmp_thetadot_cmd_lpf; /* Update for UnitDelay: '<S10>/Unit Delay' */ ud_psi = tmp_pwm_turn; /* Update for UnitDelay: '<S11>/Unit Delay' */ ud_theta_lpf = tmp_theta_lpf; } }
/* Model output function */ void omni_interface_output(int_T tid) { /* local block i/o variables */ real_T rtb_HILRead_o3[2]; real_T rtb_Saturation[3]; real_T rtb_StreamRead1_o2[3]; t_stream_ptr rtb_StreamAnswer_o1; t_stream_ptr rtb_StreamWrite_o1; t_stream_ptr rtb_StreamAnswer1_o1; t_stream_ptr rtb_StreamRead1_o1; uint8_T rtb_StreamAnswer1_o2; boolean_T rtb_LogicalOperator2[2]; boolean_T rtb_StreamRead1_o4; boolean_T rtb_StreamRead1_o3; /* S-Function Block: omni_interface/HIL Read (hil_read_block) */ { t_error result = hil_read(omni_interface_DWork.HILInitialize_Card, omni_interface_P.HILRead_AnalogChannels, 3U, omni_interface_P.HILRead_EncoderChannels, 3U, omni_interface_P.HILRead_DigitalChannels, 2U, NULL, 0U, rtb_Saturation, &omni_interface_DWork.HILRead_EncoderBuffer[0], &omni_interface_DWork.HILRead_DigitalBuffer[0], NULL ); if (result < 0) { msg_get_error_messageA(NULL, result, _rt_error_message, sizeof (_rt_error_message)); rtmSetErrorStatus(omni_interface_M, _rt_error_message); } else { rtb_StreamRead1_o2[0] = omni_interface_DWork.HILRead_EncoderBuffer[0]; rtb_StreamRead1_o2[1] = omni_interface_DWork.HILRead_EncoderBuffer[1]; rtb_StreamRead1_o2[2] = omni_interface_DWork.HILRead_EncoderBuffer[2]; rtb_HILRead_o3[0] = omni_interface_DWork.HILRead_DigitalBuffer[0]; rtb_HILRead_o3[1] = omni_interface_DWork.HILRead_DigitalBuffer[1]; } } /* Gain: '<Root>/Gain2' incorporates: * Bias: '<Root>/Bias3' * Gain: '<Root>/Gain1' * Gain: '<Root>/Gear Ratio' */ omni_interface_B.Gain2[0] = (rtb_StreamRead1_o2[0] + omni_interface_P.Bias3_Bias[0]) * omni_interface_P.Gain1_Gain * omni_interface_P.GearRatio_Gain[0] * omni_interface_P.Gain2_Gain; omni_interface_B.Gain2[1] = (rtb_StreamRead1_o2[1] + omni_interface_P.Bias3_Bias[1]) * omni_interface_P.Gain1_Gain * omni_interface_P.GearRatio_Gain[1] * omni_interface_P.Gain2_Gain; omni_interface_B.Gain2[2] = (rtb_StreamRead1_o2[2] + omni_interface_P.Bias3_Bias[2]) * omni_interface_P.Gain1_Gain * omni_interface_P.GearRatio_Gain[2] * omni_interface_P.Gain2_Gain; /* Switch: '<S8>/Reset' incorporates: * Constant: '<S8>/Initial Condition' * MinMax: '<S2>/MinMax' * UnitDelay: '<S8>/FixPt Unit Delay1' */ if (0.0 != 0.0) { omni_interface_B.Reset = omni_interface_P.InitialCondition_Value; } else { omni_interface_B.Reset = rt_MIN(omni_interface_B.Gain2[0], omni_interface_DWork.FixPtUnitDelay1_DSTATE); } /* Switch: '<S9>/Reset' incorporates: * Constant: '<S9>/Initial Condition' * MinMax: '<S3>/MinMax' * UnitDelay: '<S9>/FixPt Unit Delay1' */ if (0.0 != 0.0) { omni_interface_B.Reset_c = omni_interface_P.InitialCondition_Value_d; } else { omni_interface_B.Reset_c = rt_MIN(omni_interface_B.Gain2[1], omni_interface_DWork.FixPtUnitDelay1_DSTATE_i); } /* Switch: '<S10>/Reset' incorporates: * Constant: '<S10>/Initial Condition' * MinMax: '<S4>/MinMax' * UnitDelay: '<S10>/FixPt Unit Delay1' */ if (0.0 != 0.0) { omni_interface_B.Reset_n = omni_interface_P.InitialCondition_Value_g; } else { omni_interface_B.Reset_n = rt_MIN(omni_interface_B.Gain2[2], omni_interface_DWork.FixPtUnitDelay1_DSTATE_a); } /* Switch: '<S11>/Reset' incorporates: * Constant: '<S11>/Initial Condition' * MinMax: '<S5>/MinMax' * UnitDelay: '<S11>/FixPt Unit Delay1' */ if (0.0 != 0.0) { omni_interface_B.Reset_c3 = omni_interface_P.InitialCondition_Value_gd; } else { omni_interface_B.Reset_c3 = rt_MAX(omni_interface_B.Gain2[0], omni_interface_DWork.FixPtUnitDelay1_DSTATE_e); } /* Switch: '<S12>/Reset' incorporates: * Constant: '<S12>/Initial Condition' * MinMax: '<S6>/MinMax' * UnitDelay: '<S12>/FixPt Unit Delay1' */ if (0.0 != 0.0) { omni_interface_B.Reset_l = omni_interface_P.InitialCondition_Value_m; } else { omni_interface_B.Reset_l = rt_MAX(omni_interface_B.Gain2[1], omni_interface_DWork.FixPtUnitDelay1_DSTATE_eh); } /* Switch: '<S13>/Reset' incorporates: * Constant: '<S13>/Initial Condition' * MinMax: '<S7>/MinMax' * UnitDelay: '<S13>/FixPt Unit Delay1' */ if (0.0 != 0.0) { omni_interface_B.Reset_i = omni_interface_P.InitialCondition_Value_f; } else { omni_interface_B.Reset_i = rt_MAX(omni_interface_B.Gain2[2], omni_interface_DWork.FixPtUnitDelay1_DSTATE_d); } /* S-Function Block: omni_interface/Stream Answer (stream_answer_block) */ { static const t_short endian_test = 0x0201; t_error result = 0; t_boolean close_listener = (FALSE != 0); t_boolean close_client = (FALSE != 0); rtb_StreamAnswer_o1 = NULL; switch (omni_interface_DWork.StreamAnswer_State) { case STREAM_ANSWER_STATE_NOT_LISTENING: { if (!close_listener) { result = stream_listen("tcpip://localhost:18000", true, &omni_interface_DWork.StreamAnswer_Listener); if (result == 0) { omni_interface_DWork.StreamAnswer_State = STREAM_ANSWER_STATE_NOT_CONNECTED; } } break; } case STREAM_ANSWER_STATE_NOT_CONNECTED: { if (!close_client) { result = stream_accept(omni_interface_DWork.StreamAnswer_Listener, omni_interface_P.StreamAnswer_SendBufferSize, omni_interface_P.StreamAnswer_ReceiveBufferSize, &omni_interface_DWork.StreamAnswer_Client); if (result == 0) { omni_interface_DWork.StreamAnswer_State = STREAM_ANSWER_STATE_CONNECTED; stream_set_swap_bytes(omni_interface_DWork.StreamAnswer_Client, *(t_byte *) &endian_test != omni_interface_P.StreamAnswer_Endian); rtb_StreamAnswer_o1 = &omni_interface_DWork.StreamAnswer_Client; } } break; } case STREAM_ANSWER_STATE_CONNECTED: { rtb_StreamAnswer_o1 = &omni_interface_DWork.StreamAnswer_Client; if (!close_client) { break; } /* Fall through deliberately */ } default: { t_error close_result = stream_close (omni_interface_DWork.StreamAnswer_Client); if (close_result == 0) { omni_interface_DWork.StreamAnswer_Client = NULL; omni_interface_DWork.StreamAnswer_State = STREAM_ANSWER_STATE_NOT_CONNECTED; rtb_StreamAnswer_o1 = NULL; if (close_listener) { close_result = stream_close (omni_interface_DWork.StreamAnswer_Listener); if (close_result == 0) { omni_interface_DWork.StreamAnswer_Listener = NULL; omni_interface_DWork.StreamAnswer_State = STREAM_ANSWER_STATE_NOT_LISTENING; } else if (result == 0) { result = close_result; } } } else if (result == 0) { result = close_result; } break; } } rtb_StreamAnswer1_o2 = omni_interface_DWork.StreamAnswer_State; omni_interface_B.StreamAnswer_o3 = (int32_T) result; } /* Logic: '<Root>/Logical Operator' incorporates: * Logic: '<Root>/Logical Operator1' * RelationalOperator: '<S1>/Compare' */ omni_interface_B.LogicalOperator = ((rtb_StreamAnswer1_o2 != 0U) && (!((omni_interface_B.StreamAnswer_o3 != 0) != 0U))); /* S-Function (stream_write_block): '<Root>/Stream Write' */ { t_error result; if (rtb_StreamAnswer_o1 != NULL) { result = stream_send_unit_array(*rtb_StreamAnswer_o1, omni_interface_B.Gain2, sizeof(real_T), 3); if (result == 1) { stream_flush(*rtb_StreamAnswer_o1); } if (result == -QERR_WOULD_BLOCK) { result = 0; } } rtb_StreamWrite_o1 = rtb_StreamAnswer_o1; } /* S-Function (stream_read_block): '<Root>/Stream Read' */ /* S-Function Block: omni_interface/Stream Read (stream_read_block) */ { t_error result; if (rtb_StreamWrite_o1 != NULL) { result = stream_receive_unit_array(*rtb_StreamWrite_o1, omni_interface_B.StreamRead_o2, sizeof(real_T), 3); omni_interface_B.StreamRead_o3 = (result > 0); if (result == -QERR_WOULD_BLOCK) { result = 0; } /* if (result <= 0 && result != -QERR_WOULD_BLOCK) { memset(omni_interface_B.StreamRead_o2, 0, 3 * sizeof(real_T)); } */ } else { memset(omni_interface_B.StreamRead_o2, 0, 3 * sizeof(real_T)); omni_interface_B.StreamRead_o3 = false; result = 0; } rtb_StreamRead1_o1 = rtb_StreamWrite_o1; } /* S-Function Block: omni_interface/Stream Answer1 (stream_answer_block) */ { static const t_short endian_test = 0x0201; t_error result = 0; t_boolean close_listener = (FALSE != 0); t_boolean close_client = (FALSE != 0); rtb_StreamAnswer1_o1 = NULL; switch (omni_interface_DWork.StreamAnswer1_State) { case STREAM_ANSWER_STATE_NOT_LISTENING: { if (!close_listener) { result = stream_listen("tcpip://localhost:18002", true, &omni_interface_DWork.StreamAnswer1_Listener); if (result == 0) { omni_interface_DWork.StreamAnswer1_State = STREAM_ANSWER_STATE_NOT_CONNECTED; } } break; } case STREAM_ANSWER_STATE_NOT_CONNECTED: { if (!close_client) { result = stream_accept(omni_interface_DWork.StreamAnswer1_Listener, omni_interface_P.StreamAnswer1_SendBufferSize, omni_interface_P.StreamAnswer1_ReceiveBufferSize, &omni_interface_DWork.StreamAnswer1_Client); if (result == 0) { omni_interface_DWork.StreamAnswer1_State = STREAM_ANSWER_STATE_CONNECTED; stream_set_swap_bytes(omni_interface_DWork.StreamAnswer1_Client, *(t_byte *) &endian_test != omni_interface_P.StreamAnswer1_Endian); rtb_StreamAnswer1_o1 = &omni_interface_DWork.StreamAnswer1_Client; } } break; } case STREAM_ANSWER_STATE_CONNECTED: { rtb_StreamAnswer1_o1 = &omni_interface_DWork.StreamAnswer1_Client; if (!close_client) { break; } /* Fall through deliberately */ } default: { t_error close_result = stream_close (omni_interface_DWork.StreamAnswer1_Client); if (close_result == 0) { omni_interface_DWork.StreamAnswer1_Client = NULL; omni_interface_DWork.StreamAnswer1_State = STREAM_ANSWER_STATE_NOT_CONNECTED; rtb_StreamAnswer1_o1 = NULL; if (close_listener) { close_result = stream_close (omni_interface_DWork.StreamAnswer1_Listener); if (close_result == 0) { omni_interface_DWork.StreamAnswer1_Listener = NULL; omni_interface_DWork.StreamAnswer1_State = STREAM_ANSWER_STATE_NOT_LISTENING; } else if (result == 0) { result = close_result; } } } else if (result == 0) { result = close_result; } break; } } omni_interface_B.StreamAnswer1_o3 = (int32_T) result; } /* Gain: '<Root>/Gain' */ rtb_StreamRead1_o2[0] = omni_interface_P.Gain_Gain[0] * omni_interface_B.StreamRead_o2[0]; rtb_StreamRead1_o2[1] = omni_interface_P.Gain_Gain[1] * omni_interface_B.StreamRead_o2[1]; rtb_StreamRead1_o2[2] = omni_interface_P.Gain_Gain[2] * omni_interface_B.StreamRead_o2[2]; /* Product: '<Root>/Product' */ rtb_Saturation[0] = (int32_T)omni_interface_B.LogicalOperator ? rtb_StreamRead1_o2[0] : 0.0; rtb_Saturation[1] = (int32_T)omni_interface_B.LogicalOperator ? rtb_StreamRead1_o2[1] : 0.0; rtb_Saturation[2] = (int32_T)omni_interface_B.LogicalOperator ? rtb_StreamRead1_o2[2] : 0.0; /* Saturate: '<Root>/Saturation' */ rtb_Saturation[0] = rt_SATURATE(rtb_Saturation[0], omni_interface_P.Saturation_LowerSat, omni_interface_P.Saturation_UpperSat); rtb_Saturation[1] = rt_SATURATE(rtb_Saturation[1], omni_interface_P.Saturation_LowerSat, omni_interface_P.Saturation_UpperSat); rtb_Saturation[2] = rt_SATURATE(rtb_Saturation[2], omni_interface_P.Saturation_LowerSat, omni_interface_P.Saturation_UpperSat); /* S-Function Block: omni_interface/HIL Write (hil_write_block) */ { t_error result; result = hil_write(omni_interface_DWork.HILInitialize_Card, NULL, 0U, omni_interface_P.HILWrite_PWMChannels, 3U, NULL, 0U, NULL, 0U, NULL, rtb_Saturation, NULL, NULL ); if (result < 0) { msg_get_error_messageA(NULL, result, _rt_error_message, sizeof (_rt_error_message)); rtmSetErrorStatus(omni_interface_M, _rt_error_message); } } /* S-Function (stream_read_block): '<Root>/Stream Read1' */ /* S-Function Block: omni_interface/Stream Read1 (stream_read_block) */ { t_error result; if (rtb_StreamAnswer1_o1 != NULL) { result = stream_receive_unit_array(*rtb_StreamAnswer1_o1, rtb_StreamRead1_o2, sizeof(real_T), 3); if (result == -QERR_WOULD_BLOCK) { result = 0; } /* if (result <= 0 && result != -QERR_WOULD_BLOCK) { memset(rtb_StreamRead1_o2, 0, 3 * sizeof(real_T)); } */ } else { memset(rtb_StreamRead1_o2, 0, 3 * sizeof(real_T)); result = 0; } rtb_StreamRead1_o1 = rtb_StreamAnswer1_o1; } /* Logic: '<Root>/Logical Operator2' */ rtb_LogicalOperator2[0] = ((rtb_HILRead_o3[0] != 0.0) || (0.0 != 0.0) || (rtb_StreamRead1_o2[1] != 0.0)); rtb_LogicalOperator2[1] = ((rtb_HILRead_o3[1] != 0.0) || (0.0 != 0.0) || (rtb_StreamRead1_o2[1] != 0.0)); /* Stop: '<Root>/Stop Simulation' */ if (rtb_LogicalOperator2[0] || rtb_LogicalOperator2[1]) { rtmSetStopRequested(omni_interface_M, 1); } UNUSED_PARAMETER(tid); }
/* Model output function */ void Maglev_PD_output(int_T tid) { /* local block i/o variables */ real_T rtb_Sinus; real_T rtb_Square; real_T rtb_DiscreteTransferFcn1; real_T rtb_Saturation; { real_T currentTime; /* S-Function Block: <S3>/Analog Input */ { ANALOGIOPARM parm; parm.mode = (RANGEMODE) Maglev_PD_P.AnalogInput_RangeMode; parm.rangeidx = Maglev_PD_P.AnalogInput_VoltRange; RTBIO_DriverIO(0, ANALOGINPUT, IOREAD, 1, &Maglev_PD_P.AnalogInput_Channels, &Maglev_PD_B.AnalogInput, &parm); } /* Step: '<Root>/Step' */ currentTime = Maglev_PD_M->Timing.t[0]; if (currentTime < Maglev_PD_P.Step_Time) { currentTime = Maglev_PD_P.Step_Y0; } else { currentTime = Maglev_PD_P.Step_YFinal; } /* SignalGenerator: '<Root>/Sinus' */ { real_T sin2PiFT = sin(6.2831853071795862E+000*Maglev_PD_P.Sinus_Frequency* Maglev_PD_M->Timing.t[0]); rtb_Sinus = Maglev_PD_P.Sinus_Amplitude*sin2PiFT; } /* SignalGenerator: '<Root>/Square' */ { real_T phase = Maglev_PD_P.Square_Frequency*Maglev_PD_M->Timing.t[0]; phase = phase-floor(phase); rtb_Square = ( phase >= 0.5 ) ? Maglev_PD_P.Square_Amplitude : -Maglev_PD_P.Square_Amplitude; } /* Switch: '<S5>/SwitchControl' incorporates: * Constant: '<Root>/Constant' * Constant: '<S5>/Constant' * Sum: '<Root>/Sum' */ if (Maglev_PD_P.Constant_Value_b > Maglev_PD_P.SwitchControl_Threshold) { Maglev_PD_B.SwitchControl = currentTime; } else { /* Switch: '<S6>/SwitchControl' incorporates: * Constant: '<S6>/Constant' */ if (Maglev_PD_P.Constant_Value_p > Maglev_PD_P.SwitchControl_Threshold_a) { currentTime = rtb_Sinus; } else { currentTime = rtb_Square; } Maglev_PD_B.SwitchControl = currentTime + Maglev_PD_P.Constant_Value_d; } /* Sum: '<Root>/Sum1' */ Maglev_PD_B.Sum1 = Maglev_PD_B.SwitchControl - Maglev_PD_B.AnalogInput; /* Gain: '<S7>/Gain' */ currentTime = Maglev_PD_P.Gain_Gain * Maglev_PD_B.Sum1; /* Gain: '<S7>/Gain3' */ Maglev_PD_B.Gain3 = Maglev_PD_P.Gain3_Gain * Maglev_PD_B.Sum1; /* DiscreteTransferFcn: '<S7>/Discrete Transfer Fcn1' */ rtb_DiscreteTransferFcn1 = Maglev_PD_P.DiscreteTransferFcn1_D* Maglev_PD_B.Gain3; rtb_DiscreteTransferFcn1 += Maglev_PD_P.DiscreteTransferFcn1_C* Maglev_PD_DWork.DiscreteTransferFcn1_DSTATE; /* Gain: '<S7>/Gain1' */ Maglev_PD_B.Gain1 = Maglev_PD_P.Gain1_Gain * Maglev_PD_B.Sum1; /* DiscreteTransferFcn: '<S7>/Discrete Transfer Fcn' */ rtb_Saturation = Maglev_PD_P.DiscreteTransferFcn_D*Maglev_PD_B.Gain1; rtb_Saturation += Maglev_PD_P.DiscreteTransferFcn_C* Maglev_PD_DWork.DiscreteTransferFcn_DSTATE; /* Gain: '<S7>/Gain2' incorporates: * Sum: '<S7>/Sum' */ Maglev_PD_B.Gain2 = ((currentTime + rtb_DiscreteTransferFcn1) + rtb_Saturation) * Maglev_PD_P.Gain2_Gain; /* Sum: '<S4>/Add' incorporates: * Constant: '<S4>/Constant' */ rtb_Saturation = Maglev_PD_P.Constant_Value + Maglev_PD_B.Gain2; /* Saturate: '<S4>/Saturation' */ rtb_Saturation = rt_SATURATE(rtb_Saturation, Maglev_PD_P.Saturation_LowerSat, Maglev_PD_P.Saturation_UpperSat); /* S-Function Block: <S4>/Analog Output */ { { ANALOGIOPARM parm; parm.mode = (RANGEMODE) Maglev_PD_P.AnalogOutput_RangeMode; parm.rangeidx = Maglev_PD_P.AnalogOutput_VoltRange; RTBIO_DriverIO(0, ANALOGOUTPUT, IOWRITE, 1, &Maglev_PD_P.AnalogOutput_Channels, &rtb_Saturation, &parm); } } /* Clock: '<Root>/Clock' */ Maglev_PD_B.Clock = Maglev_PD_M->Timing.t[0]; if (Maglev_PD_M->Timing.TaskCounters.TID[2] == 0) { } /* Sum: '<S1>/Sum1' incorporates: * Constant: '<S1>/Constant1' * Gain: '<S1>/Gain2' */ Maglev_PD_B.Sum1_f = Maglev_PD_P.Gain2_Gain_m * Maglev_PD_B.SwitchControl + Maglev_PD_P.Constant1_Value; /* Sum: '<S2>/Sum1' incorporates: * Constant: '<S2>/Constant1' * Gain: '<S2>/Gain2' */ Maglev_PD_B.Sum1_p = Maglev_PD_P.Gain2_Gain_n * Maglev_PD_B.AnalogInput + Maglev_PD_P.Constant1_Value_d; if (Maglev_PD_M->Timing.TaskCounters.TID[2] == 0) { } } UNUSED_PARAMETER(tid); }