/* Model output function */
void SingleCylinderTest_output(void)
{
uint16_T rtb_FeedbackVoltage_0;
static const int8_T b[6] = { 1, 2, 3, 4, 3, 2 };
real_T rtb_Abs1;
real_T rtb_Product;
real_T rtb_Switch;
real_T rtb_Abs1_p;
uint64m_T tmp;
uint32_T tmp_0;
uint8_T rtb_Abs1_k;
/* Step: '<Root>/Step' */
if (((SingleCylinderTest_M->Timing.clockTick0) * 0.01) <
SingleCylinderTest_P.Step_Time) {
rtb_Product = SingleCylinderTest_P.Step_Y0;
} else {
rtb_Product = SingleCylinderTest_P.Step_YFinal;
}
/* End of Step: '<Root>/Step' */
/* Product: '<S4>/Product' incorporates:
* Abs: '<S6>/Abs'
* Constant: '<S6>/Constant'
* RelationalOperator: '<S6>/Relational Operator'
* Sum: '<S6>/Sum'
* UnitDelay: '<Root>/Unit Delay'
* UnitDelay: '<S4>/Unit Delay1'
*/
rtb_Product *= (real_T)(fabs(SingleCylinderTest_DW.UnitDelay_DSTATE -
SingleCylinderTest_DW.UnitDelay1_DSTATE) <
SingleCylinderTest_P.Constant_Value_m);
/* Chart: '<S5>/Chart' incorporates:
* UnitDelay: '<S5>/Unit Delay1'
*/
/* Gateway: High Level System Controller/System Model/State Machine/Chart */
/* During: High Level System Controller/System Model/State Machine/Chart */
if (SingleCylinderTest_DW.is_active_c3_SingleCylinderTest == 0U) {
/* Entry: High Level System Controller/System Model/State Machine/Chart */
SingleCylinderTest_DW.is_active_c3_SingleCylinderTest = 1U;
/* Entry Internal: High Level System Controller/System Model/State Machine/Chart */
/* Transition: '<S8>:23' */
SingleCylinderTest_DW.is_c3_SingleCylinderTest = SingleCylinderTest_IN_Idle;
/* Entry 'Idle': '<S8>:1' */
SingleCylinderTest_B.index_out_n = SingleCylinderTest_DW.UnitDelay1_DSTATE_k;
} else {
switch (SingleCylinderTest_DW.is_c3_SingleCylinderTest) {
case SingleCylinderTest_IN_Forward:
/* During 'Forward': '<S8>:2' */
if (rtb_Product != 1.0) {
/* Transition: '<S8>:7' */
SingleCylinderTest_DW.is_c3_SingleCylinderTest =
SingleCylinderTest_IN_Idle;
/* Entry 'Idle': '<S8>:1' */
SingleCylinderTest_B.index_out_n =
SingleCylinderTest_DW.UnitDelay1_DSTATE_k;
} else {
SingleCylinderTest_B.index_out_n =
SingleCylinderTest_DW.UnitDelay1_DSTATE_k + 1.0;
}
break;
case SingleCylinderTest_IN_Idle:
/* During 'Idle': '<S8>:1' */
if (rtb_Product == 1.0) {
/* Transition: '<S8>:4' */
SingleCylinderTest_DW.is_c3_SingleCylinderTest =
SingleCylinderTest_IN_Forward;
/* Entry 'Forward': '<S8>:2' */
SingleCylinderTest_B.index_out_n =
SingleCylinderTest_DW.UnitDelay1_DSTATE_k + 1.0;
} else {
if (rtb_Product == -1.0) {
/* Transition: '<S8>:5' */
SingleCylinderTest_DW.is_c3_SingleCylinderTest =
SingleCylinderTest_IN_Reverse;
/* Entry 'Reverse': '<S8>:3' */
SingleCylinderTest_B.index_out_n =
SingleCylinderTest_DW.UnitDelay1_DSTATE_k - 1.0;
}
}
break;
default:
/* During 'Reverse': '<S8>:3' */
if (rtb_Product != -1.0) {
/* Transition: '<S8>:6' */
SingleCylinderTest_DW.is_c3_SingleCylinderTest =
SingleCylinderTest_IN_Idle;
/* Entry 'Idle': '<S8>:1' */
SingleCylinderTest_B.index_out_n =
SingleCylinderTest_DW.UnitDelay1_DSTATE_k;
} else {
SingleCylinderTest_B.index_out_n =
SingleCylinderTest_DW.UnitDelay1_DSTATE_k - 1.0;
}
break;
}
}
/* End of Chart: '<S5>/Chart' */
/* MATLAB Function: '<S5>/MATLAB Function' */
/* MATLAB Function 'High Level System Controller/System Model/State Machine/MATLAB Function': '<S9>:1' */
/* '<S9>:1:3' */
SingleCylinderTest_B.index_out = SingleCylinderTest_B.index_out_n;
if (SingleCylinderTest_B.index_out_n > 6.0) {
/* '<S9>:1:4' */
/* '<S9>:1:5' */
SingleCylinderTest_B.index_out = 1.0;
}
if (SingleCylinderTest_B.index_out_n < 1.0) {
/* '<S9>:1:7' */
/* '<S9>:1:8' */
SingleCylinderTest_B.index_out = 6.0;
}
/* End of MATLAB Function: '<S5>/MATLAB Function' */
/* MATLAB Function: '<S7>/MATLAB Function' */
/* MATLAB Function 'High Level System Controller/System Model/State Machine/Calculate Foot Position/MATLAB Function': '<S10>:1' */
/* '<S10>:1:3' */
rtb_Product = SingleCylinderTest_B.index_out;
if (SingleCylinderTest_B.index_out < 1.0) {
/* '<S10>:1:4' */
/* '<S10>:1:5' */
rtb_Product = 1.0;
} else {
if (SingleCylinderTest_B.index_out > 6.0) {
/* '<S10>:1:6' */
/* '<S10>:1:7' */
rtb_Product = 6.0;
}
}
/* '<S10>:1:9' */
/* divide by 5 to normalize */
/* '<S10>:1:11' */
SingleCylinderTest_B.x = b[(int16_T)rtb_Product - 1];
/* End of MATLAB Function: '<S7>/MATLAB Function' */
/* S-Function (arduinoanaloginput_sfcn): '<S2>/Feedback Voltage' */
rtb_FeedbackVoltage_0 = MW_analogRead(SingleCylinderTest_P.FeedbackVoltage_p1);
/* Gain: '<Root>/P' */
rtb_Product = SingleCylinderTest_P.P_Gain * SingleCylinderTest_B.x;
/* Gain: '<S2>/P' incorporates:
* Gain: '<S2>/Normalize Feedback'
* S-Function (arduinoanaloginput_sfcn): '<S2>/Feedback Voltage'
* Sum: '<S2>/Sum1'
*/
rtb_Abs1 = (rtb_Product - (real_T)((uint32_T)
SingleCylinderTest_P.NormalizeFeedback_Gain_g * rtb_FeedbackVoltage_0) *
2.9802322387695313E-8) * SingleCylinderTest_P.P_Gain_b;
/* Switch: '<S13>/Switch' incorporates:
* Constant: '<S13>/Constant1'
* Constant: '<S13>/Constant2'
*/
if (rtb_Abs1 >= SingleCylinderTest_P.Switch_Threshold) {
rtb_Switch = SingleCylinderTest_P.Constant1_Value;
} else {
rtb_Switch = SingleCylinderTest_P.Constant2_Value;
}
/* End of Switch: '<S13>/Switch' */
/* Abs: '<S13>/Abs' incorporates:
* Product: '<S13>/0 if Negative'
*/
rtb_Abs1_p = fabs(rtb_Abs1 * rtb_Switch);
/* DataTypeConversion: '<S11>/Data Type Conversion' incorporates:
* MinMax: '<S2>/Extend Saturation'
*/
if (rtb_Abs1_p < 256.0) {
rtb_Abs1_k = (uint8_T)rtb_Abs1_p;
} else {
rtb_Abs1_k = MAX_uint8_T;
}
/* End of DataTypeConversion: '<S11>/Data Type Conversion' */
/* S-Function (arduinoanalogoutput_sfcn): '<S11>/PWM' */
MW_analogWrite(SingleCylinderTest_P.PWM_pinNumber, rtb_Abs1_k);
/* Product: '<S13>/0 If Positive' incorporates:
* Constant: '<S13>/Constant3'
* Sum: '<S13>/Sum'
*/
rtb_Abs1 *= rtb_Switch - SingleCylinderTest_P.Constant3_Value;
/* Abs: '<S13>/Abs1' */
rtb_Abs1 = fabs(rtb_Abs1);
/* DataTypeConversion: '<S12>/Data Type Conversion' incorporates:
* MinMax: '<S2>/Retract Saturation'
*/
if (rtb_Abs1 < 256.0) {
rtb_Abs1_k = (uint8_T)rtb_Abs1;
} else {
rtb_Abs1_k = MAX_uint8_T;
}
/* End of DataTypeConversion: '<S12>/Data Type Conversion' */
/* S-Function (arduinoanalogoutput_sfcn): '<S12>/PWM' */
MW_analogWrite(SingleCylinderTest_P.PWM_pinNumber_n, rtb_Abs1_k);
/* Sum: '<S2>/Sum2' */
SingleCylinderTest_B.Sum2 = rtb_Abs1_p + rtb_Abs1;
/* S-Function (arduinoanaloginput_sfcn): '<S3>/Feedback Voltage' */
rtb_FeedbackVoltage_0 = MW_analogRead
(SingleCylinderTest_P.FeedbackVoltage_p1_n);
/* Gain: '<S3>/Normalize Feedback' incorporates:
* Gain: '<S3>/Length Compensation'
* S-Function (arduinoanaloginput_sfcn): '<S3>/Feedback Voltage'
*/
tmp_0 = (uint32_T)SingleCylinderTest_P.LengthCompensation_Gain *
rtb_FeedbackVoltage_0;
uMultiWordMul(&SingleCylinderTest_P.NormalizeFeedback_Gain, 1, &tmp_0, 1,
&tmp.chunks[0U], 2);
/* Gain: '<S3>/P' incorporates:
* Constant: '<Root>/Constant'
* Sum: '<Root>/Sum'
* Sum: '<S3>/Sum1'
*/
rtb_Abs1_p = ((SingleCylinderTest_P.Constant_Value - rtb_Product) -
uMultiWord2Double(&tmp.chunks[0U], 2, 0) *
1.3877787807814457E-17) * SingleCylinderTest_P.P_Gain_h;
/* Switch: '<S16>/Switch' incorporates:
* Constant: '<S16>/Constant1'
* Constant: '<S16>/Constant2'
*/
if (rtb_Abs1_p >= SingleCylinderTest_P.Switch_Threshold_m) {
rtb_Switch = SingleCylinderTest_P.Constant1_Value_j;
} else {
rtb_Switch = SingleCylinderTest_P.Constant2_Value_d;
}
/* End of Switch: '<S16>/Switch' */
/* DataTypeConversion: '<S14>/Data Type Conversion' incorporates:
* Abs: '<S16>/Abs'
* Product: '<S16>/0 if Negative'
*/
rtb_Product = fabs(rtb_Abs1_p * rtb_Switch);
if (rtb_Product < 256.0) {
rtb_Abs1_k = (uint8_T)rtb_Product;
} else {
rtb_Abs1_k = MAX_uint8_T;
}
/* End of DataTypeConversion: '<S14>/Data Type Conversion' */
/* S-Function (arduinoanalogoutput_sfcn): '<S14>/PWM' */
MW_analogWrite(SingleCylinderTest_P.PWM_pinNumber_p, rtb_Abs1_k);
/* Product: '<S16>/0 If Positive' incorporates:
* Constant: '<S16>/Constant3'
* Sum: '<S16>/Sum'
*/
rtb_Abs1_p *= rtb_Switch - SingleCylinderTest_P.Constant3_Value_p;
/* Abs: '<S16>/Abs1' */
rtb_Abs1_p = fabs(rtb_Abs1_p);
/* DataTypeConversion: '<S15>/Data Type Conversion' incorporates:
* MinMax: '<S3>/Retract Saturation'
*/
if (rtb_Abs1_p < 256.0) {
rtb_Abs1_k = (uint8_T)rtb_Abs1_p;
} else {
rtb_Abs1_k = MAX_uint8_T;
}
/* End of DataTypeConversion: '<S15>/Data Type Conversion' */
/* S-Function (arduinoanalogoutput_sfcn): '<S15>/PWM' */
MW_analogWrite(SingleCylinderTest_P.PWM_pinNumber_g, rtb_Abs1_k);
}
/* Model output function */
void testArduino_send_serial_output(void)
{
uint16_T rtb_AnalogInput_0;
uint16_T rtb_AnalogInput1_0;
uint16_T rtb_AnalogInput2_0;
uint8_T tmp[3];
real_T u;
real_T v;
if (testArduino_send_serial_M->Timing.TaskCounters.TID[1] == 0) {
/* S-Function (arduinoanaloginput_sfcn): '<Root>/Analog Input' */
rtb_AnalogInput_0 = MW_analogRead(testArduino_send_serial_P.AnalogInput_p1);
/* S-Function (arduinoanaloginput_sfcn): '<Root>/Analog Input1' */
rtb_AnalogInput1_0 = MW_analogRead(testArduino_send_serial_P.AnalogInput1_p1);
/* S-Function (arduinoanaloginput_sfcn): '<Root>/Analog Input2' */
rtb_AnalogInput2_0 = MW_analogRead(testArduino_send_serial_P.AnalogInput2_p1);
/* DataTypeConversion: '<Root>/Data Type Conversion' incorporates:
* Constant: '<Root>/Constant'
* Constant: '<Root>/Constant1'
* Constant: '<Root>/Constant2'
* Product: '<Root>/Product'
* S-Function (arduinoanaloginput_sfcn): '<Root>/Analog Input'
* Sum: '<Root>/Subtract'
* Sum: '<Root>/Subtract1'
*/
u = testArduino_send_serial_P.Constant2_Value - ((real_T)rtb_AnalogInput_0 -
testArduino_send_serial_P.Constant_Value) *
testArduino_send_serial_P.Constant1_Value;
v = fabs(u);
if (v < 4.503599627370496E+15) {
if (v >= 0.5) {
u = floor(u + 0.5);
} else {
u *= 0.0;
}
}
if (rtIsNaN(u) || rtIsInf(u)) {
u = 0.0;
} else {
u = fmod(u, 256.0);
}
/* S-Function (arduinoserialwrite_sfcn): '<Root>/Serial Transmit' incorporates:
* DataTypeConversion: '<Root>/Data Type Conversion'
* Sum: '<Root>/Subtract'
*/
tmp[0] = (uint8_T)(u < 0.0 ? (int16_T)(uint8_T)-(int8_T)(uint8_T)-u :
(int16_T)(uint8_T)u);
/* DataTypeConversion: '<Root>/Data Type Conversion' incorporates:
* Constant: '<Root>/Constant'
* Constant: '<Root>/Constant1'
* Constant: '<Root>/Constant2'
* Product: '<Root>/Product'
* S-Function (arduinoanaloginput_sfcn): '<Root>/Analog Input1'
* Sum: '<Root>/Subtract'
* Sum: '<Root>/Subtract1'
*/
u = testArduino_send_serial_P.Constant2_Value - ((real_T)rtb_AnalogInput1_0
- testArduino_send_serial_P.Constant_Value) *
testArduino_send_serial_P.Constant1_Value;
v = fabs(u);
if (v < 4.503599627370496E+15) {
if (v >= 0.5) {
u = floor(u + 0.5);
} else {
u *= 0.0;
}
}
if (rtIsNaN(u) || rtIsInf(u)) {
u = 0.0;
} else {
u = fmod(u, 256.0);
}
/* S-Function (arduinoserialwrite_sfcn): '<Root>/Serial Transmit' incorporates:
* DataTypeConversion: '<Root>/Data Type Conversion'
* Sum: '<Root>/Subtract'
*/
tmp[1] = (uint8_T)(u < 0.0 ? (int16_T)(uint8_T)-(int8_T)(uint8_T)-u :
(int16_T)(uint8_T)u);
/* DataTypeConversion: '<Root>/Data Type Conversion' incorporates:
* Constant: '<Root>/Constant'
* Constant: '<Root>/Constant1'
* Constant: '<Root>/Constant2'
* Product: '<Root>/Product'
* S-Function (arduinoanaloginput_sfcn): '<Root>/Analog Input2'
* Sum: '<Root>/Subtract'
* Sum: '<Root>/Subtract1'
*/
u = testArduino_send_serial_P.Constant2_Value - ((real_T)rtb_AnalogInput2_0
- testArduino_send_serial_P.Constant_Value) *
testArduino_send_serial_P.Constant1_Value;
v = fabs(u);
if (v < 4.503599627370496E+15) {
if (v >= 0.5) {
u = floor(u + 0.5);
} else {
u *= 0.0;
}
}
if (rtIsNaN(u) || rtIsInf(u)) {
u = 0.0;
} else {
u = fmod(u, 256.0);
}
/* S-Function (arduinoserialwrite_sfcn): '<Root>/Serial Transmit' incorporates:
* DataTypeConversion: '<Root>/Data Type Conversion'
* Sum: '<Root>/Subtract'
*/
tmp[2] = (uint8_T)(u < 0.0 ? (int16_T)(uint8_T)-(int8_T)(uint8_T)-u :
(int16_T)(uint8_T)u);
Serial_write(testArduino_send_serial_P.SerialTransmit_portNumber, tmp, 3UL);
}
}
