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