An example of importing Embedded Coder code into the mbed IDE. Currently doesn't connect IO to PWM, ADC, and Encoder, instead provides random inputs and measures execution time.
Dependencies: mbed-dsp mbed Nucleo_pmsmfoc
rtwdemo_pmsmfoc.cpp@2:bbc155b0b886, 2014-11-25 (annotated)
- Committer:
- paulcox
- Date:
- Tue Nov 25 07:52:02 2014 +0000
- Revision:
- 2:bbc155b0b886
- Parent:
- 0:70d27fec6d71
adding doc?
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
paulcox | 0:70d27fec6d71 | 1 | /* |
paulcox | 0:70d27fec6d71 | 2 | * File: rtwdemo_pmsmfoc.c |
paulcox | 0:70d27fec6d71 | 3 | * |
paulcox | 0:70d27fec6d71 | 4 | * Code generated for Simulink model 'rtwdemo_pmsmfoc'. |
paulcox | 0:70d27fec6d71 | 5 | * |
paulcox | 0:70d27fec6d71 | 6 | * Model version : 1.2949 |
paulcox | 0:70d27fec6d71 | 7 | * Simulink Coder version : 8.7 (R2014b) 11-Aug-2014 |
paulcox | 0:70d27fec6d71 | 8 | * C/C++ source code generated on : Sat Oct 11 00:06:05 2014 |
paulcox | 0:70d27fec6d71 | 9 | * |
paulcox | 0:70d27fec6d71 | 10 | * Target selection: ert.tlc |
paulcox | 0:70d27fec6d71 | 11 | * Embedded hardware selection: Generic->32-bit Embedded Processor |
paulcox | 0:70d27fec6d71 | 12 | * Code generation objective: Execution efficiency |
paulcox | 0:70d27fec6d71 | 13 | * Validation result: Not run |
paulcox | 0:70d27fec6d71 | 14 | */ |
paulcox | 0:70d27fec6d71 | 15 | |
paulcox | 0:70d27fec6d71 | 16 | #include "rtwdemo_pmsmfoc.h" |
paulcox | 2:bbc155b0b886 | 17 | #include "arm_math.h" |
paulcox | 0:70d27fec6d71 | 18 | |
paulcox | 0:70d27fec6d71 | 19 | /* Named constants for Chart: '<S1>/Controller_Mode_Scheduler' */ |
paulcox | 0:70d27fec6d71 | 20 | #define IN_Motor_Control ((uint8_T)1U) |
paulcox | 0:70d27fec6d71 | 21 | #define IN_Motor_On ((uint8_T)1U) |
paulcox | 0:70d27fec6d71 | 22 | #define IN_NO_ACTIVE_CHILD ((uint8_T)0U) |
paulcox | 0:70d27fec6d71 | 23 | #define IN_Position_Control ((uint8_T)1U) |
paulcox | 0:70d27fec6d71 | 24 | #define IN_Ramp_To_Stop ((uint8_T)2U) |
paulcox | 0:70d27fec6d71 | 25 | #define IN_Stand_By ((uint8_T)2U) |
paulcox | 0:70d27fec6d71 | 26 | #define IN_Startup_Open_Loop_Control ((uint8_T)2U) |
paulcox | 0:70d27fec6d71 | 27 | #define IN_Torque_Control ((uint8_T)3U) |
paulcox | 0:70d27fec6d71 | 28 | #define IN_Velocity_Control ((uint8_T)4U) |
paulcox | 0:70d27fec6d71 | 29 | |
paulcox | 0:70d27fec6d71 | 30 | /* Named constants for Chart: '<S49>/Wait_For_Valid_Position' */ |
paulcox | 0:70d27fec6d71 | 31 | #define IN_Position_Valid ((uint8_T)1U) |
paulcox | 0:70d27fec6d71 | 32 | #define IN_Wait_For_Valid_Position ((uint8_T)2U) |
paulcox | 0:70d27fec6d71 | 33 | #define IN_Wait_For_Valid_Velocity ((uint8_T)3U) |
paulcox | 0:70d27fec6d71 | 34 | #define WaitForValidVelocityTicks (124) |
paulcox | 0:70d27fec6d71 | 35 | |
paulcox | 0:70d27fec6d71 | 36 | /* Exported block signals */ |
paulcox | 0:70d27fec6d71 | 37 | real32_T phase_currents[2]; /* '<S48>/Product' */ |
paulcox | 0:70d27fec6d71 | 38 | real32_T rotor_position; /* '<S49>/Add' */ |
paulcox | 0:70d27fec6d71 | 39 | real32_T velocity_measured; /* '<S55>/Scale_Output' */ |
paulcox | 0:70d27fec6d71 | 40 | real32_T d_current_error; /* '<S12>/Sum2' */ |
paulcox | 0:70d27fec6d71 | 41 | real32_T q_current_command; /* '<S5>/Lo_to_Hi_Rate_Transition1' */ |
paulcox | 0:70d27fec6d71 | 42 | real32_T q_current_measured; /* '<S22>/Add' */ |
paulcox | 0:70d27fec6d71 | 43 | real32_T q_current_error; /* '<S12>/Sum' */ |
paulcox | 0:70d27fec6d71 | 44 | real32_T phase_voltages[3]; /* '<S27>/Select_Sector' */ |
paulcox | 0:70d27fec6d71 | 45 | real32_T velocity_error; /* '<S11>/Sum2' */ |
paulcox | 0:70d27fec6d71 | 46 | EnumControllerMode controller_mode; /* '<S1>/Controller_Mode_Scheduler' */ |
paulcox | 0:70d27fec6d71 | 47 | |
paulcox | 0:70d27fec6d71 | 48 | /* Exported block parameters */ |
paulcox | 0:70d27fec6d71 | 49 | CTRLPARAMS_STRUCT ctrlParams = { |
paulcox | 0:70d27fec6d71 | 50 | 10.0F, |
paulcox | 0:70d27fec6d71 | 51 | 10000.0F, |
paulcox | 0:70d27fec6d71 | 52 | 0.005F, |
paulcox | 0:70d27fec6d71 | 53 | 0.015F, |
paulcox | 0:70d27fec6d71 | 54 | 0.1F, |
paulcox | 0:70d27fec6d71 | 55 | 0.6F, |
paulcox | 0:70d27fec6d71 | 56 | 1.0F, |
paulcox | 0:70d27fec6d71 | 57 | 0.2F, |
paulcox | 0:70d27fec6d71 | 58 | 20.0F, |
paulcox | 0:70d27fec6d71 | 59 | 2252.25F, |
paulcox | 0:70d27fec6d71 | 60 | 0.00488400506F, |
paulcox | 0:70d27fec6d71 | 61 | -0.0F, |
paulcox | 0:70d27fec6d71 | 62 | 4.0F |
paulcox | 0:70d27fec6d71 | 63 | } ; /* Variable: ctrlParams |
paulcox | 0:70d27fec6d71 | 64 | * Referenced by: |
paulcox | 0:70d27fec6d71 | 65 | * '<S1>/Controller_Mode_Scheduler' |
paulcox | 0:70d27fec6d71 | 66 | * '<S48>/ADC_Driver_Units_To_Amps' |
paulcox | 0:70d27fec6d71 | 67 | * '<S48>/ADC_Zero_Offset' |
paulcox | 0:70d27fec6d71 | 68 | * '<S49>/Offset_Between_Encoder_Zero_And_Mechanical_Zero' |
paulcox | 0:70d27fec6d71 | 69 | * '<S14>/number_of_pole_pairs' |
paulcox | 0:70d27fec6d71 | 70 | * '<S42>/Integral Gain' |
paulcox | 0:70d27fec6d71 | 71 | * '<S42>/Proportional Gain' |
paulcox | 0:70d27fec6d71 | 72 | * '<S45>/Integral Gain' |
paulcox | 0:70d27fec6d71 | 73 | * '<S45>/Proportional Gain' |
paulcox | 0:70d27fec6d71 | 74 | * '<S63>/Startup_Acceleration_Constant' |
paulcox | 0:70d27fec6d71 | 75 | * '<S17>/Integral Gain' |
paulcox | 0:70d27fec6d71 | 76 | * '<S17>/Proportional Gain' |
paulcox | 0:70d27fec6d71 | 77 | * '<S18>/Integral Gain' |
paulcox | 0:70d27fec6d71 | 78 | * '<S18>/Proportional Gain' |
paulcox | 0:70d27fec6d71 | 79 | */ |
paulcox | 0:70d27fec6d71 | 80 | |
paulcox | 0:70d27fec6d71 | 81 | /* Constant parameters (auto storage) */ |
paulcox | 0:70d27fec6d71 | 82 | const ConstParam ConstP = { |
paulcox | 0:70d27fec6d71 | 83 | /* Computed Parameter: Lookup_Table_table |
paulcox | 0:70d27fec6d71 | 84 | * Referenced by: '<S28>/Lookup_Table' |
paulcox | 0:70d27fec6d71 | 85 | */ |
paulcox | 0:70d27fec6d71 | 86 | { 2U, 2U, 6U, 1U, 4U, 3U, 5U } |
paulcox | 0:70d27fec6d71 | 87 | }; |
paulcox | 0:70d27fec6d71 | 88 | |
paulcox | 0:70d27fec6d71 | 89 | /* Block signals and states (auto storage) */ |
paulcox | 0:70d27fec6d71 | 90 | D_Work DWork; |
paulcox | 0:70d27fec6d71 | 91 | |
paulcox | 0:70d27fec6d71 | 92 | /* Real-time model */ |
paulcox | 0:70d27fec6d71 | 93 | RT_MODEL M_; |
paulcox | 0:70d27fec6d71 | 94 | RT_MODEL *const M = &M_; |
paulcox | 0:70d27fec6d71 | 95 | extern real32_T rt_roundf(real32_T u); |
paulcox | 0:70d27fec6d71 | 96 | extern real32_T rt_modf(real32_T u0, real32_T u1); |
paulcox | 0:70d27fec6d71 | 97 | static void rate_scheduler(void); |
paulcox | 0:70d27fec6d71 | 98 | |
paulcox | 0:70d27fec6d71 | 99 | /* |
paulcox | 0:70d27fec6d71 | 100 | * This function updates active task flag for each subrate. |
paulcox | 0:70d27fec6d71 | 101 | * The function is called at model base rate, hence the |
paulcox | 0:70d27fec6d71 | 102 | * generated code self-manages all its subrates. |
paulcox | 0:70d27fec6d71 | 103 | */ |
paulcox | 0:70d27fec6d71 | 104 | static void rate_scheduler(void) |
paulcox | 0:70d27fec6d71 | 105 | { |
paulcox | 0:70d27fec6d71 | 106 | /* Compute which subrates run during the next base time step. Subrates |
paulcox | 0:70d27fec6d71 | 107 | * are an integer multiple of the base rate counter. Therefore, the subtask |
paulcox | 0:70d27fec6d71 | 108 | * counter is reset when it reaches its limit (zero means run). |
paulcox | 0:70d27fec6d71 | 109 | */ |
paulcox | 0:70d27fec6d71 | 110 | (M->Timing.TaskCounters.TID[1])++; |
paulcox | 0:70d27fec6d71 | 111 | if ((M->Timing.TaskCounters.TID[1]) > 124) {/* Sample time: [0.005s, 0.0s] */ |
paulcox | 0:70d27fec6d71 | 112 | M->Timing.TaskCounters.TID[1] = 0; |
paulcox | 0:70d27fec6d71 | 113 | } |
paulcox | 0:70d27fec6d71 | 114 | } |
paulcox | 0:70d27fec6d71 | 115 | |
paulcox | 0:70d27fec6d71 | 116 | real32_T rt_roundf(real32_T u) |
paulcox | 0:70d27fec6d71 | 117 | { |
paulcox | 0:70d27fec6d71 | 118 | real32_T y; |
paulcox | 0:70d27fec6d71 | 119 | if (((real32_T)fabs(u)) < 8.388608E+6F) { |
paulcox | 0:70d27fec6d71 | 120 | if (u >= 0.5F) { |
paulcox | 0:70d27fec6d71 | 121 | y = (real32_T)floor(u + 0.5F); |
paulcox | 0:70d27fec6d71 | 122 | } else if (u > -0.5F) { |
paulcox | 0:70d27fec6d71 | 123 | y = 0.0F; |
paulcox | 0:70d27fec6d71 | 124 | } else { |
paulcox | 0:70d27fec6d71 | 125 | y = (real32_T)ceil(u - 0.5F); |
paulcox | 0:70d27fec6d71 | 126 | } |
paulcox | 0:70d27fec6d71 | 127 | } else { |
paulcox | 0:70d27fec6d71 | 128 | y = u; |
paulcox | 0:70d27fec6d71 | 129 | } |
paulcox | 0:70d27fec6d71 | 130 | |
paulcox | 0:70d27fec6d71 | 131 | return y; |
paulcox | 0:70d27fec6d71 | 132 | } |
paulcox | 0:70d27fec6d71 | 133 | |
paulcox | 0:70d27fec6d71 | 134 | real32_T rt_modf(real32_T u0, real32_T u1) |
paulcox | 0:70d27fec6d71 | 135 | { |
paulcox | 0:70d27fec6d71 | 136 | real32_T y; |
paulcox | 0:70d27fec6d71 | 137 | real32_T tmp; |
paulcox | 0:70d27fec6d71 | 138 | if (u1 == 0.0F) { |
paulcox | 0:70d27fec6d71 | 139 | y = u0; |
paulcox | 0:70d27fec6d71 | 140 | } else { |
paulcox | 0:70d27fec6d71 | 141 | tmp = u0 / u1; |
paulcox | 0:70d27fec6d71 | 142 | if (u1 <= ((real32_T)floor(u1))) { |
paulcox | 0:70d27fec6d71 | 143 | y = u0 - (((real32_T)floor(tmp)) * u1); |
paulcox | 0:70d27fec6d71 | 144 | } else if (((real32_T)fabs(tmp - rt_roundf(tmp))) <= (FLT_EPSILON * |
paulcox | 0:70d27fec6d71 | 145 | ((real32_T)fabs(tmp)))) { |
paulcox | 0:70d27fec6d71 | 146 | y = 0.0F; |
paulcox | 0:70d27fec6d71 | 147 | } else { |
paulcox | 0:70d27fec6d71 | 148 | y = (tmp - ((real32_T)floor(tmp))) * u1; |
paulcox | 0:70d27fec6d71 | 149 | } |
paulcox | 0:70d27fec6d71 | 150 | } |
paulcox | 0:70d27fec6d71 | 151 | |
paulcox | 0:70d27fec6d71 | 152 | return y; |
paulcox | 0:70d27fec6d71 | 153 | } |
paulcox | 0:70d27fec6d71 | 154 | |
paulcox | 0:70d27fec6d71 | 155 | /* Model step function */ |
paulcox | 0:70d27fec6d71 | 156 | extern "C" EnumErrorType Controller(uint16_T motor_on, EnumCommandType command_type, |
paulcox | 0:70d27fec6d71 | 157 | real32_T current_request, SENSORS_STRUCT *sensors, uint16_T pwm_compare[3]) |
paulcox | 0:70d27fec6d71 | 158 | { |
paulcox | 0:70d27fec6d71 | 159 | real32_T sin_coefficient; |
paulcox | 0:70d27fec6d71 | 160 | int32_T Wrap_To_Pi; |
paulcox | 0:70d27fec6d71 | 161 | real32_T electrical_angle; |
paulcox | 0:70d27fec6d71 | 162 | real32_T cos_coefficient; |
paulcox | 0:70d27fec6d71 | 163 | real32_T SignDeltaU_b; |
paulcox | 0:70d27fec6d71 | 164 | int16_T Enum_To_Int; |
paulcox | 0:70d27fec6d71 | 165 | uint8_T FixPtRelationalOperator; |
paulcox | 0:70d27fec6d71 | 166 | real32_T SignDeltaU; |
paulcox | 0:70d27fec6d71 | 167 | real32_T Gain1; |
paulcox | 0:70d27fec6d71 | 168 | int8_T rtPrevAction; |
paulcox | 0:70d27fec6d71 | 169 | real32_T alpha_voltage; |
paulcox | 0:70d27fec6d71 | 170 | real32_T SignPreIntegrator_f; |
paulcox | 0:70d27fec6d71 | 171 | real32_T IntegralGain_j; |
paulcox | 0:70d27fec6d71 | 172 | boolean_T RelationalOperator; |
paulcox | 0:70d27fec6d71 | 173 | boolean_T NotEqual_b; |
paulcox | 0:70d27fec6d71 | 174 | real32_T Switch_fr; |
paulcox | 0:70d27fec6d71 | 175 | real32_T Sectors_2_and_5_idx_1; |
paulcox | 0:70d27fec6d71 | 176 | real32_T Sectors_2_and_5_idx_2; |
paulcox | 0:70d27fec6d71 | 177 | int16_T u0; |
paulcox | 0:70d27fec6d71 | 178 | |
paulcox | 0:70d27fec6d71 | 179 | /* specified return value */ |
paulcox | 0:70d27fec6d71 | 180 | EnumErrorType error; |
paulcox | 0:70d27fec6d71 | 181 | |
paulcox | 0:70d27fec6d71 | 182 | /* Product: '<S48>/Product' incorporates: |
paulcox | 0:70d27fec6d71 | 183 | * Constant: '<S48>/ADC_Driver_Units_To_Amps' |
paulcox | 0:70d27fec6d71 | 184 | * Constant: '<S48>/ADC_Zero_Offset' |
paulcox | 0:70d27fec6d71 | 185 | * Inport: '<Root>/sensors' |
paulcox | 0:70d27fec6d71 | 186 | * Sum: '<S48>/Add' |
paulcox | 0:70d27fec6d71 | 187 | */ |
paulcox | 0:70d27fec6d71 | 188 | phase_currents[0] = (((real32_T)sensors->adc_phase_currents[0]) - |
paulcox | 0:70d27fec6d71 | 189 | ctrlParams.AdcZeroOffsetDriverUnits) * |
paulcox | 0:70d27fec6d71 | 190 | ctrlParams.AdcDriverUnitsToAmps; |
paulcox | 0:70d27fec6d71 | 191 | phase_currents[1] = (((real32_T)sensors->adc_phase_currents[1]) - |
paulcox | 0:70d27fec6d71 | 192 | ctrlParams.AdcZeroOffsetDriverUnits) * |
paulcox | 0:70d27fec6d71 | 193 | ctrlParams.AdcDriverUnitsToAmps; |
paulcox | 0:70d27fec6d71 | 194 | |
paulcox | 0:70d27fec6d71 | 195 | /* Chart: '<S49>/Wait_For_Valid_Position' incorporates: |
paulcox | 0:70d27fec6d71 | 196 | * Inport: '<Root>/sensors' |
paulcox | 0:70d27fec6d71 | 197 | */ |
paulcox | 0:70d27fec6d71 | 198 | /* Gateway: Motor_Control/Sensors_To_Engineering_Units/Encoder_To_Position/Wait_For_Valid_Position */ |
paulcox | 0:70d27fec6d71 | 199 | if (DWork.temporalCounter_i1 < 127U) { |
paulcox | 0:70d27fec6d71 | 200 | DWork.temporalCounter_i1++; |
paulcox | 0:70d27fec6d71 | 201 | } |
paulcox | 0:70d27fec6d71 | 202 | |
paulcox | 0:70d27fec6d71 | 203 | /* During: Motor_Control/Sensors_To_Engineering_Units/Encoder_To_Position/Wait_For_Valid_Position */ |
paulcox | 0:70d27fec6d71 | 204 | if (DWork.is_active_c2_rtwdemo_pmsmfoc == 0U) { |
paulcox | 0:70d27fec6d71 | 205 | /* Entry: Motor_Control/Sensors_To_Engineering_Units/Encoder_To_Position/Wait_For_Valid_Position */ |
paulcox | 0:70d27fec6d71 | 206 | DWork.is_active_c2_rtwdemo_pmsmfoc = 1U; |
paulcox | 0:70d27fec6d71 | 207 | |
paulcox | 0:70d27fec6d71 | 208 | /* Entry Internal: Motor_Control/Sensors_To_Engineering_Units/Encoder_To_Position/Wait_For_Valid_Position */ |
paulcox | 0:70d27fec6d71 | 209 | /* Transition: '<S52>:113' */ |
paulcox | 0:70d27fec6d71 | 210 | if (sensors->encoder_valid == 0) { |
paulcox | 0:70d27fec6d71 | 211 | /* Transition: '<S52>:114' */ |
paulcox | 0:70d27fec6d71 | 212 | /* Transition: '<S52>:115' */ |
paulcox | 0:70d27fec6d71 | 213 | DWork.is_c2_rtwdemo_pmsmfoc = IN_Wait_For_Valid_Position; |
paulcox | 0:70d27fec6d71 | 214 | } else { |
paulcox | 0:70d27fec6d71 | 215 | /* Transition: '<S52>:120' */ |
paulcox | 0:70d27fec6d71 | 216 | /* Transition: '<S52>:119' */ |
paulcox | 0:70d27fec6d71 | 217 | DWork.is_c2_rtwdemo_pmsmfoc = IN_Position_Valid; |
paulcox | 0:70d27fec6d71 | 218 | DWork.Position_Valid = 1U; |
paulcox | 0:70d27fec6d71 | 219 | } |
paulcox | 0:70d27fec6d71 | 220 | } else { |
paulcox | 0:70d27fec6d71 | 221 | switch (DWork.is_c2_rtwdemo_pmsmfoc) { |
paulcox | 0:70d27fec6d71 | 222 | case IN_Position_Valid: |
paulcox | 0:70d27fec6d71 | 223 | /* During 'Position_Valid': '<S52>:25' */ |
paulcox | 0:70d27fec6d71 | 224 | break; |
paulcox | 0:70d27fec6d71 | 225 | |
paulcox | 0:70d27fec6d71 | 226 | case IN_Wait_For_Valid_Position: |
paulcox | 0:70d27fec6d71 | 227 | /* During 'Wait_For_Valid_Position': '<S52>:99' */ |
paulcox | 0:70d27fec6d71 | 228 | if (sensors->encoder_valid != 0) { |
paulcox | 0:70d27fec6d71 | 229 | /* Transition: '<S52>:100' */ |
paulcox | 0:70d27fec6d71 | 230 | DWork.is_c2_rtwdemo_pmsmfoc = IN_Wait_For_Valid_Velocity; |
paulcox | 0:70d27fec6d71 | 231 | DWork.temporalCounter_i1 = 0U; |
paulcox | 0:70d27fec6d71 | 232 | } |
paulcox | 0:70d27fec6d71 | 233 | break; |
paulcox | 0:70d27fec6d71 | 234 | |
paulcox | 0:70d27fec6d71 | 235 | default: |
paulcox | 0:70d27fec6d71 | 236 | /* During 'Wait_For_Valid_Velocity': '<S52>:101' */ |
paulcox | 0:70d27fec6d71 | 237 | if (DWork.temporalCounter_i1 >= WaitForValidVelocityTicks) { |
paulcox | 0:70d27fec6d71 | 238 | /* Transition: '<S52>:102' */ |
paulcox | 0:70d27fec6d71 | 239 | DWork.is_c2_rtwdemo_pmsmfoc = IN_Position_Valid; |
paulcox | 0:70d27fec6d71 | 240 | DWork.Position_Valid = 1U; |
paulcox | 0:70d27fec6d71 | 241 | } |
paulcox | 0:70d27fec6d71 | 242 | break; |
paulcox | 0:70d27fec6d71 | 243 | } |
paulcox | 0:70d27fec6d71 | 244 | } |
paulcox | 0:70d27fec6d71 | 245 | |
paulcox | 0:70d27fec6d71 | 246 | /* End of Chart: '<S49>/Wait_For_Valid_Position' */ |
paulcox | 0:70d27fec6d71 | 247 | |
paulcox | 0:70d27fec6d71 | 248 | /* Sum: '<S49>/Add' incorporates: |
paulcox | 0:70d27fec6d71 | 249 | * Constant: '<S49>/Offset_Between_Encoder_Zero_And_Mechanical_Zero' |
paulcox | 0:70d27fec6d71 | 250 | * Gain: '<S49>/radians_per_counts' |
paulcox | 0:70d27fec6d71 | 251 | * Inport: '<Root>/sensors' |
paulcox | 0:70d27fec6d71 | 252 | */ |
paulcox | 0:70d27fec6d71 | 253 | rotor_position = (0.000785398181F * ((real32_T)sensors->encoder_counter)) + |
paulcox | 0:70d27fec6d71 | 254 | ctrlParams.EncoderToMechanicalZeroOffsetRads; |
paulcox | 0:70d27fec6d71 | 255 | if (M->Timing.TaskCounters.TID[1] == 0) { |
paulcox | 0:70d27fec6d71 | 256 | /* Gain: '<S55>/Wrap_To_Pi' incorporates: |
paulcox | 0:70d27fec6d71 | 257 | * DataTypeConversion: '<S55>/Convert_to_Uint32' |
paulcox | 0:70d27fec6d71 | 258 | * Gain: '<S55>/Scale_Input' |
paulcox | 0:70d27fec6d71 | 259 | */ |
paulcox | 0:70d27fec6d71 | 260 | Wrap_To_Pi = (((int32_T)(1.70891312E+8F * rotor_position)) << 2); |
paulcox | 0:70d27fec6d71 | 261 | |
paulcox | 0:70d27fec6d71 | 262 | /* Gain: '<S55>/Scale_Output' incorporates: |
paulcox | 0:70d27fec6d71 | 263 | * DataTypeConversion: '<S55>/Difference_to_Single' |
paulcox | 0:70d27fec6d71 | 264 | * Delay: '<S55>/Position_Delay' |
paulcox | 0:70d27fec6d71 | 265 | * Sum: '<S55>/Difference_Wrap' |
paulcox | 0:70d27fec6d71 | 266 | */ |
paulcox | 0:70d27fec6d71 | 267 | velocity_measured = ((real32_T)(Wrap_To_Pi - DWork.Position_Delay_DSTATE)) * |
paulcox | 0:70d27fec6d71 | 268 | 2.92583621E-7F; |
paulcox | 0:70d27fec6d71 | 269 | |
paulcox | 0:70d27fec6d71 | 270 | /* Chart: '<S1>/Controller_Mode_Scheduler' incorporates: |
paulcox | 0:70d27fec6d71 | 271 | * Inport: '<Root>/command_type' |
paulcox | 0:70d27fec6d71 | 272 | * Inport: '<Root>/command_value' |
paulcox | 0:70d27fec6d71 | 273 | * Inport: '<Root>/motor_on' |
paulcox | 0:70d27fec6d71 | 274 | */ |
paulcox | 0:70d27fec6d71 | 275 | /* Gateway: Mode_Scheduler/Controller_Mode_Scheduler */ |
paulcox | 0:70d27fec6d71 | 276 | /* During: Mode_Scheduler/Controller_Mode_Scheduler */ |
paulcox | 0:70d27fec6d71 | 277 | if (DWork.is_c1_rtwdemo_pmsmfoc == IN_Motor_On) { |
paulcox | 0:70d27fec6d71 | 278 | /* During 'Motor_On': '<S4>:338' */ |
paulcox | 0:70d27fec6d71 | 279 | if (DWork.error_l != 0) { |
paulcox | 0:70d27fec6d71 | 280 | /* Transition: '<S4>:339' */ |
paulcox | 0:70d27fec6d71 | 281 | /* Transition: '<S4>:353' */ |
paulcox | 0:70d27fec6d71 | 282 | /* Exit Internal 'Motor_On': '<S4>:338' */ |
paulcox | 0:70d27fec6d71 | 283 | /* Exit Internal 'Motor_Control': '<S4>:344' */ |
paulcox | 0:70d27fec6d71 | 284 | DWork.is_Motor_Control = IN_NO_ACTIVE_CHILD; |
paulcox | 0:70d27fec6d71 | 285 | DWork.is_Motor_On = IN_NO_ACTIVE_CHILD; |
paulcox | 0:70d27fec6d71 | 286 | DWork.is_c1_rtwdemo_pmsmfoc = IN_Stand_By; |
paulcox | 0:70d27fec6d71 | 287 | |
paulcox | 0:70d27fec6d71 | 288 | /* Entry 'Stand_By': '<S4>:154' */ |
paulcox | 0:70d27fec6d71 | 289 | controller_mode = StandBy; |
paulcox | 0:70d27fec6d71 | 290 | } else if (DWork.is_Motor_On == IN_Motor_Control) { |
paulcox | 0:70d27fec6d71 | 291 | /* During 'Motor_Control': '<S4>:344' */ |
paulcox | 0:70d27fec6d71 | 292 | if (!(motor_on != 0)) { |
paulcox | 0:70d27fec6d71 | 293 | /* Transition: '<S4>:282' */ |
paulcox | 0:70d27fec6d71 | 294 | /* Exit Internal 'Motor_Control': '<S4>:344' */ |
paulcox | 0:70d27fec6d71 | 295 | DWork.is_Motor_Control = IN_NO_ACTIVE_CHILD; |
paulcox | 0:70d27fec6d71 | 296 | DWork.is_Motor_On = IN_Ramp_To_Stop; |
paulcox | 0:70d27fec6d71 | 297 | |
paulcox | 0:70d27fec6d71 | 298 | /* Entry 'Ramp_To_Stop': '<S4>:270' */ |
paulcox | 0:70d27fec6d71 | 299 | controller_mode = VelocityControl; |
paulcox | 0:70d27fec6d71 | 300 | DWork.velocity_command = 0.0F; |
paulcox | 0:70d27fec6d71 | 301 | DWork.torque_command = 0.0F; |
paulcox | 0:70d27fec6d71 | 302 | } else { |
paulcox | 0:70d27fec6d71 | 303 | switch (DWork.is_Motor_Control) { |
paulcox | 0:70d27fec6d71 | 304 | case IN_Position_Control: |
paulcox | 0:70d27fec6d71 | 305 | /* During 'Position_Control': '<S4>:226' */ |
paulcox | 0:70d27fec6d71 | 306 | DWork.position_command = current_request; |
paulcox | 0:70d27fec6d71 | 307 | break; |
paulcox | 0:70d27fec6d71 | 308 | |
paulcox | 0:70d27fec6d71 | 309 | case IN_Startup_Open_Loop_Control: |
paulcox | 0:70d27fec6d71 | 310 | /* During 'Startup_Open_Loop_Control': '<S4>:103' */ |
paulcox | 0:70d27fec6d71 | 311 | if (DWork.Position_Valid != 0) { |
paulcox | 0:70d27fec6d71 | 312 | /* Transition: '<S4>:157' */ |
paulcox | 0:70d27fec6d71 | 313 | /* Transition: '<S4>:233' */ |
paulcox | 0:70d27fec6d71 | 314 | switch (command_type) { |
paulcox | 0:70d27fec6d71 | 315 | case Velocity: |
paulcox | 0:70d27fec6d71 | 316 | /* Transition: '<S4>:162' */ |
paulcox | 0:70d27fec6d71 | 317 | DWork.is_Motor_Control = IN_Velocity_Control; |
paulcox | 0:70d27fec6d71 | 318 | |
paulcox | 0:70d27fec6d71 | 319 | /* Entry 'Velocity_Control': '<S4>:108' */ |
paulcox | 0:70d27fec6d71 | 320 | controller_mode = VelocityControl; |
paulcox | 0:70d27fec6d71 | 321 | DWork.velocity_command = current_request; |
paulcox | 0:70d27fec6d71 | 322 | break; |
paulcox | 0:70d27fec6d71 | 323 | |
paulcox | 0:70d27fec6d71 | 324 | case Position: |
paulcox | 0:70d27fec6d71 | 325 | /* Transition: '<S4>:235' */ |
paulcox | 0:70d27fec6d71 | 326 | /* Transition: '<S4>:234' */ |
paulcox | 0:70d27fec6d71 | 327 | DWork.is_Motor_Control = IN_Position_Control; |
paulcox | 0:70d27fec6d71 | 328 | |
paulcox | 0:70d27fec6d71 | 329 | /* Entry 'Position_Control': '<S4>:226' */ |
paulcox | 0:70d27fec6d71 | 330 | controller_mode = PositionControl; |
paulcox | 0:70d27fec6d71 | 331 | DWork.position_command = current_request; |
paulcox | 0:70d27fec6d71 | 332 | break; |
paulcox | 0:70d27fec6d71 | 333 | |
paulcox | 0:70d27fec6d71 | 334 | default: |
paulcox | 0:70d27fec6d71 | 335 | /* Transition: '<S4>:237' */ |
paulcox | 0:70d27fec6d71 | 336 | /* Transition: '<S4>:158' */ |
paulcox | 0:70d27fec6d71 | 337 | /* [command_type==Torque] */ |
paulcox | 0:70d27fec6d71 | 338 | DWork.is_Motor_Control = IN_Torque_Control; |
paulcox | 0:70d27fec6d71 | 339 | |
paulcox | 0:70d27fec6d71 | 340 | /* Entry 'Torque_Control': '<S4>:220' */ |
paulcox | 0:70d27fec6d71 | 341 | controller_mode = TorqueControl; |
paulcox | 0:70d27fec6d71 | 342 | DWork.torque_command = current_request; |
paulcox | 0:70d27fec6d71 | 343 | break; |
paulcox | 0:70d27fec6d71 | 344 | } |
paulcox | 0:70d27fec6d71 | 345 | } |
paulcox | 0:70d27fec6d71 | 346 | break; |
paulcox | 0:70d27fec6d71 | 347 | |
paulcox | 0:70d27fec6d71 | 348 | case IN_Torque_Control: |
paulcox | 0:70d27fec6d71 | 349 | /* During 'Torque_Control': '<S4>:220' */ |
paulcox | 0:70d27fec6d71 | 350 | DWork.torque_command = current_request; |
paulcox | 0:70d27fec6d71 | 351 | break; |
paulcox | 0:70d27fec6d71 | 352 | |
paulcox | 0:70d27fec6d71 | 353 | default: |
paulcox | 0:70d27fec6d71 | 354 | /* During 'Velocity_Control': '<S4>:108' */ |
paulcox | 0:70d27fec6d71 | 355 | DWork.velocity_command = current_request; |
paulcox | 0:70d27fec6d71 | 356 | break; |
paulcox | 0:70d27fec6d71 | 357 | } |
paulcox | 0:70d27fec6d71 | 358 | } |
paulcox | 0:70d27fec6d71 | 359 | } else { |
paulcox | 0:70d27fec6d71 | 360 | /* During 'Ramp_To_Stop': '<S4>:270' */ |
paulcox | 0:70d27fec6d71 | 361 | if (((real32_T)fabs(velocity_measured)) < ctrlParams.RampToStopVelocity) |
paulcox | 0:70d27fec6d71 | 362 | { |
paulcox | 0:70d27fec6d71 | 363 | /* Transition: '<S4>:169' */ |
paulcox | 0:70d27fec6d71 | 364 | /* Transition: '<S4>:355' */ |
paulcox | 0:70d27fec6d71 | 365 | DWork.is_Motor_On = IN_NO_ACTIVE_CHILD; |
paulcox | 0:70d27fec6d71 | 366 | DWork.is_c1_rtwdemo_pmsmfoc = IN_Stand_By; |
paulcox | 0:70d27fec6d71 | 367 | |
paulcox | 0:70d27fec6d71 | 368 | /* Entry 'Stand_By': '<S4>:154' */ |
paulcox | 0:70d27fec6d71 | 369 | controller_mode = StandBy; |
paulcox | 0:70d27fec6d71 | 370 | } |
paulcox | 0:70d27fec6d71 | 371 | } |
paulcox | 0:70d27fec6d71 | 372 | } else { |
paulcox | 0:70d27fec6d71 | 373 | /* During 'Stand_By': '<S4>:154' */ |
paulcox | 0:70d27fec6d71 | 374 | if ((motor_on != 0) && (!(DWork.error_l != 0))) { |
paulcox | 0:70d27fec6d71 | 375 | /* Transition: '<S4>:164' */ |
paulcox | 0:70d27fec6d71 | 376 | if (!(DWork.Position_Valid != 0)) { |
paulcox | 0:70d27fec6d71 | 377 | /* Transition: '<S4>:133' */ |
paulcox | 0:70d27fec6d71 | 378 | DWork.is_c1_rtwdemo_pmsmfoc = IN_Motor_On; |
paulcox | 0:70d27fec6d71 | 379 | DWork.is_Motor_On = IN_Motor_Control; |
paulcox | 0:70d27fec6d71 | 380 | DWork.is_Motor_Control = IN_Startup_Open_Loop_Control; |
paulcox | 0:70d27fec6d71 | 381 | |
paulcox | 0:70d27fec6d71 | 382 | /* Entry 'Startup_Open_Loop_Control': '<S4>:103' */ |
paulcox | 0:70d27fec6d71 | 383 | controller_mode = Startup; |
paulcox | 0:70d27fec6d71 | 384 | DWork.torque_command = ctrlParams.StartupCurrent; |
paulcox | 0:70d27fec6d71 | 385 | } else { |
paulcox | 0:70d27fec6d71 | 386 | /* Transition: '<S4>:137' */ |
paulcox | 0:70d27fec6d71 | 387 | /* Transition: '<S4>:233' */ |
paulcox | 0:70d27fec6d71 | 388 | switch (command_type) { |
paulcox | 0:70d27fec6d71 | 389 | case Velocity: |
paulcox | 0:70d27fec6d71 | 390 | /* Transition: '<S4>:162' */ |
paulcox | 0:70d27fec6d71 | 391 | DWork.is_c1_rtwdemo_pmsmfoc = IN_Motor_On; |
paulcox | 0:70d27fec6d71 | 392 | DWork.is_Motor_On = IN_Motor_Control; |
paulcox | 0:70d27fec6d71 | 393 | DWork.is_Motor_Control = IN_Velocity_Control; |
paulcox | 0:70d27fec6d71 | 394 | |
paulcox | 0:70d27fec6d71 | 395 | /* Entry 'Velocity_Control': '<S4>:108' */ |
paulcox | 0:70d27fec6d71 | 396 | controller_mode = VelocityControl; |
paulcox | 0:70d27fec6d71 | 397 | DWork.velocity_command = current_request; |
paulcox | 0:70d27fec6d71 | 398 | break; |
paulcox | 0:70d27fec6d71 | 399 | |
paulcox | 0:70d27fec6d71 | 400 | case Position: |
paulcox | 0:70d27fec6d71 | 401 | /* Transition: '<S4>:235' */ |
paulcox | 0:70d27fec6d71 | 402 | /* Transition: '<S4>:234' */ |
paulcox | 0:70d27fec6d71 | 403 | DWork.is_c1_rtwdemo_pmsmfoc = IN_Motor_On; |
paulcox | 0:70d27fec6d71 | 404 | DWork.is_Motor_On = IN_Motor_Control; |
paulcox | 0:70d27fec6d71 | 405 | DWork.is_Motor_Control = IN_Position_Control; |
paulcox | 0:70d27fec6d71 | 406 | |
paulcox | 0:70d27fec6d71 | 407 | /* Entry 'Position_Control': '<S4>:226' */ |
paulcox | 0:70d27fec6d71 | 408 | controller_mode = PositionControl; |
paulcox | 0:70d27fec6d71 | 409 | DWork.position_command = current_request; |
paulcox | 0:70d27fec6d71 | 410 | break; |
paulcox | 0:70d27fec6d71 | 411 | |
paulcox | 0:70d27fec6d71 | 412 | default: |
paulcox | 0:70d27fec6d71 | 413 | /* Transition: '<S4>:237' */ |
paulcox | 0:70d27fec6d71 | 414 | /* Transition: '<S4>:158' */ |
paulcox | 0:70d27fec6d71 | 415 | /* [command_type==Torque] */ |
paulcox | 0:70d27fec6d71 | 416 | DWork.is_c1_rtwdemo_pmsmfoc = IN_Motor_On; |
paulcox | 0:70d27fec6d71 | 417 | DWork.is_Motor_On = IN_Motor_Control; |
paulcox | 0:70d27fec6d71 | 418 | DWork.is_Motor_Control = IN_Torque_Control; |
paulcox | 0:70d27fec6d71 | 419 | |
paulcox | 0:70d27fec6d71 | 420 | /* Entry 'Torque_Control': '<S4>:220' */ |
paulcox | 0:70d27fec6d71 | 421 | controller_mode = TorqueControl; |
paulcox | 0:70d27fec6d71 | 422 | DWork.torque_command = current_request; |
paulcox | 0:70d27fec6d71 | 423 | break; |
paulcox | 0:70d27fec6d71 | 424 | } |
paulcox | 0:70d27fec6d71 | 425 | } |
paulcox | 0:70d27fec6d71 | 426 | } |
paulcox | 0:70d27fec6d71 | 427 | } |
paulcox | 0:70d27fec6d71 | 428 | |
paulcox | 0:70d27fec6d71 | 429 | /* End of Chart: '<S1>/Controller_Mode_Scheduler' */ |
paulcox | 0:70d27fec6d71 | 430 | |
paulcox | 0:70d27fec6d71 | 431 | /* RelationalOperator: '<S51>/Relational Operator' incorporates: |
paulcox | 0:70d27fec6d71 | 432 | * Constant: '<S62>/Constant' |
paulcox | 0:70d27fec6d71 | 433 | */ |
paulcox | 0:70d27fec6d71 | 434 | DWork.RelationalOperator_a = (controller_mode == Startup); |
paulcox | 0:70d27fec6d71 | 435 | } |
paulcox | 0:70d27fec6d71 | 436 | |
paulcox | 0:70d27fec6d71 | 437 | /* Outputs for Enabled SubSystem: '<S51>/Open Loop Position' incorporates: |
paulcox | 0:70d27fec6d71 | 438 | * EnablePort: '<S63>/Enable' |
paulcox | 0:70d27fec6d71 | 439 | */ |
paulcox | 0:70d27fec6d71 | 440 | if (DWork.RelationalOperator_a) { |
paulcox | 0:70d27fec6d71 | 441 | if (!DWork.OpenLoopPosition_MODE) { |
paulcox | 0:70d27fec6d71 | 442 | /* InitializeConditions for DiscreteIntegrator: '<S63>/Integrate_To_Position' */ |
paulcox | 0:70d27fec6d71 | 443 | DWork.Integrate_To_Position_DSTATE = 0.0F; |
paulcox | 0:70d27fec6d71 | 444 | |
paulcox | 0:70d27fec6d71 | 445 | /* InitializeConditions for DiscreteIntegrator: '<S63>/Integrate_To_Velocity' */ |
paulcox | 0:70d27fec6d71 | 446 | DWork.Integrate_To_Velocity_DSTATE = 0.0F; |
paulcox | 0:70d27fec6d71 | 447 | DWork.OpenLoopPosition_MODE = true; |
paulcox | 0:70d27fec6d71 | 448 | } |
paulcox | 0:70d27fec6d71 | 449 | |
paulcox | 0:70d27fec6d71 | 450 | /* DiscreteIntegrator: '<S63>/Integrate_To_Position' */ |
paulcox | 0:70d27fec6d71 | 451 | DWork.position = DWork.Integrate_To_Position_DSTATE; |
paulcox | 0:70d27fec6d71 | 452 | |
paulcox | 0:70d27fec6d71 | 453 | /* Update for DiscreteIntegrator: '<S63>/Integrate_To_Position' incorporates: |
paulcox | 0:70d27fec6d71 | 454 | * DiscreteIntegrator: '<S63>/Integrate_To_Velocity' |
paulcox | 0:70d27fec6d71 | 455 | */ |
paulcox | 0:70d27fec6d71 | 456 | DWork.Integrate_To_Position_DSTATE += 4.0E-5F * |
paulcox | 0:70d27fec6d71 | 457 | DWork.Integrate_To_Velocity_DSTATE; |
paulcox | 0:70d27fec6d71 | 458 | |
paulcox | 0:70d27fec6d71 | 459 | /* Update for DiscreteIntegrator: '<S63>/Integrate_To_Velocity' incorporates: |
paulcox | 0:70d27fec6d71 | 460 | * Constant: '<S63>/Startup_Acceleration_Constant' |
paulcox | 0:70d27fec6d71 | 461 | */ |
paulcox | 0:70d27fec6d71 | 462 | DWork.Integrate_To_Velocity_DSTATE += 4.0E-5F * |
paulcox | 0:70d27fec6d71 | 463 | ctrlParams.StartupAcceleration; |
paulcox | 0:70d27fec6d71 | 464 | } else { |
paulcox | 0:70d27fec6d71 | 465 | if (DWork.OpenLoopPosition_MODE) { |
paulcox | 0:70d27fec6d71 | 466 | DWork.OpenLoopPosition_MODE = false; |
paulcox | 0:70d27fec6d71 | 467 | } |
paulcox | 0:70d27fec6d71 | 468 | } |
paulcox | 0:70d27fec6d71 | 469 | |
paulcox | 0:70d27fec6d71 | 470 | /* End of Outputs for SubSystem: '<S51>/Open Loop Position' */ |
paulcox | 0:70d27fec6d71 | 471 | |
paulcox | 0:70d27fec6d71 | 472 | /* Switch: '<S51>/Switch' */ |
paulcox | 0:70d27fec6d71 | 473 | if (DWork.RelationalOperator_a) { |
paulcox | 0:70d27fec6d71 | 474 | Switch_fr = DWork.position; |
paulcox | 0:70d27fec6d71 | 475 | } else { |
paulcox | 0:70d27fec6d71 | 476 | Switch_fr = rotor_position; |
paulcox | 0:70d27fec6d71 | 477 | } |
paulcox | 0:70d27fec6d71 | 478 | |
paulcox | 0:70d27fec6d71 | 479 | /* End of Switch: '<S51>/Switch' */ |
paulcox | 0:70d27fec6d71 | 480 | |
paulcox | 0:70d27fec6d71 | 481 | /* Gain: '<S14>/number_of_pole_pairs' */ |
paulcox | 0:70d27fec6d71 | 482 | electrical_angle = ctrlParams.PmsmPolePairs * Switch_fr; |
paulcox | 0:70d27fec6d71 | 483 | |
paulcox | 0:70d27fec6d71 | 484 | /* Trigonometry: '<S14>/sine_cosine' */ |
paulcox | 2:bbc155b0b886 | 485 | //sin_coefficient = (real32_T)sin(electrical_angle); |
paulcox | 2:bbc155b0b886 | 486 | //cos_coefficient = (real32_T)cos(electrical_angle); |
paulcox | 2:bbc155b0b886 | 487 | sin_coefficient = arm_sin_f32(electrical_angle); |
paulcox | 2:bbc155b0b886 | 488 | cos_coefficient = arm_cos_f32(electrical_angle); |
paulcox | 2:bbc155b0b886 | 489 | |
paulcox | 0:70d27fec6d71 | 490 | /* Gain: '<S21>/Beta_Gain' incorporates: |
paulcox | 0:70d27fec6d71 | 491 | * Gain: '<S21>/B_Gain' |
paulcox | 0:70d27fec6d71 | 492 | * Sum: '<S21>/Add' |
paulcox | 0:70d27fec6d71 | 493 | */ |
paulcox | 0:70d27fec6d71 | 494 | SignDeltaU_b = ((2.0F * phase_currents[1]) + phase_currents[0]) * 0.577350259F; |
paulcox | 0:70d27fec6d71 | 495 | |
paulcox | 0:70d27fec6d71 | 496 | /* Sum: '<S12>/Sum2' incorporates: |
paulcox | 0:70d27fec6d71 | 497 | * Constant: '<S12>/d_current_command (A)' |
paulcox | 0:70d27fec6d71 | 498 | * Product: '<S22>/Product2' |
paulcox | 0:70d27fec6d71 | 499 | * Product: '<S22>/Product3' |
paulcox | 0:70d27fec6d71 | 500 | * Sum: '<S22>/Add1' |
paulcox | 0:70d27fec6d71 | 501 | */ |
paulcox | 0:70d27fec6d71 | 502 | d_current_error = 0.0F - ((phase_currents[0] * cos_coefficient) + |
paulcox | 0:70d27fec6d71 | 503 | (SignDeltaU_b * sin_coefficient)); |
paulcox | 0:70d27fec6d71 | 504 | |
paulcox | 0:70d27fec6d71 | 505 | /* DataTypeConversion: '<S8>/Enum_To_Int' */ |
paulcox | 0:70d27fec6d71 | 506 | Enum_To_Int = (int16_T)controller_mode; |
paulcox | 0:70d27fec6d71 | 507 | |
paulcox | 0:70d27fec6d71 | 508 | /* RelationalOperator: '<S13>/FixPt Relational Operator' incorporates: |
paulcox | 0:70d27fec6d71 | 509 | * UnitDelay: '<S13>/Delay Input1' |
paulcox | 0:70d27fec6d71 | 510 | */ |
paulcox | 0:70d27fec6d71 | 511 | FixPtRelationalOperator = (uint8_T)(Enum_To_Int != DWork.DelayInput1_DSTATE); |
paulcox | 0:70d27fec6d71 | 512 | |
paulcox | 0:70d27fec6d71 | 513 | /* DiscreteIntegrator: '<S17>/Integrator' */ |
paulcox | 0:70d27fec6d71 | 514 | if ((FixPtRelationalOperator != 0) || (DWork.Integrator_PrevResetState != 0)) |
paulcox | 0:70d27fec6d71 | 515 | { |
paulcox | 0:70d27fec6d71 | 516 | DWork.Integrator_DSTATE = 0.0F; |
paulcox | 0:70d27fec6d71 | 517 | } |
paulcox | 0:70d27fec6d71 | 518 | |
paulcox | 0:70d27fec6d71 | 519 | /* Sum: '<S17>/Sum' incorporates: |
paulcox | 0:70d27fec6d71 | 520 | * DiscreteIntegrator: '<S17>/Integrator' |
paulcox | 0:70d27fec6d71 | 521 | * Gain: '<S17>/Proportional Gain' |
paulcox | 0:70d27fec6d71 | 522 | */ |
paulcox | 0:70d27fec6d71 | 523 | SignDeltaU = (ctrlParams.Current_P * d_current_error) + |
paulcox | 0:70d27fec6d71 | 524 | DWork.Integrator_DSTATE; |
paulcox | 0:70d27fec6d71 | 525 | |
paulcox | 0:70d27fec6d71 | 526 | /* Saturate: '<S17>/Saturate' */ |
paulcox | 0:70d27fec6d71 | 527 | if (SignDeltaU > 12.0F) { |
paulcox | 0:70d27fec6d71 | 528 | Gain1 = 12.0F; |
paulcox | 0:70d27fec6d71 | 529 | } else if (SignDeltaU < -12.0F) { |
paulcox | 0:70d27fec6d71 | 530 | Gain1 = -12.0F; |
paulcox | 0:70d27fec6d71 | 531 | } else { |
paulcox | 0:70d27fec6d71 | 532 | Gain1 = SignDeltaU; |
paulcox | 0:70d27fec6d71 | 533 | } |
paulcox | 0:70d27fec6d71 | 534 | |
paulcox | 0:70d27fec6d71 | 535 | /* End of Saturate: '<S17>/Saturate' */ |
paulcox | 0:70d27fec6d71 | 536 | if (M->Timing.TaskCounters.TID[1] == 0) { |
paulcox | 0:70d27fec6d71 | 537 | /* SwitchCase: '<S5>/Switch Case' incorporates: |
paulcox | 0:70d27fec6d71 | 538 | * Inport: '<S10>/torque_command' |
paulcox | 0:70d27fec6d71 | 539 | */ |
paulcox | 0:70d27fec6d71 | 540 | rtPrevAction = DWork.SwitchCase_ActiveSubsystem; |
paulcox | 0:70d27fec6d71 | 541 | switch (controller_mode) { |
paulcox | 0:70d27fec6d71 | 542 | case VelocityControl: |
paulcox | 0:70d27fec6d71 | 543 | DWork.SwitchCase_ActiveSubsystem = 0; |
paulcox | 0:70d27fec6d71 | 544 | break; |
paulcox | 0:70d27fec6d71 | 545 | |
paulcox | 0:70d27fec6d71 | 546 | case PositionControl: |
paulcox | 0:70d27fec6d71 | 547 | DWork.SwitchCase_ActiveSubsystem = 1; |
paulcox | 0:70d27fec6d71 | 548 | break; |
paulcox | 0:70d27fec6d71 | 549 | |
paulcox | 0:70d27fec6d71 | 550 | default: |
paulcox | 0:70d27fec6d71 | 551 | DWork.SwitchCase_ActiveSubsystem = 2; |
paulcox | 0:70d27fec6d71 | 552 | break; |
paulcox | 0:70d27fec6d71 | 553 | } |
paulcox | 0:70d27fec6d71 | 554 | |
paulcox | 0:70d27fec6d71 | 555 | switch (DWork.SwitchCase_ActiveSubsystem) { |
paulcox | 0:70d27fec6d71 | 556 | case 0: |
paulcox | 0:70d27fec6d71 | 557 | if (DWork.SwitchCase_ActiveSubsystem != rtPrevAction) { |
paulcox | 0:70d27fec6d71 | 558 | /* InitializeConditions for IfAction SubSystem: '<S5>/Velocity_Control' incorporates: |
paulcox | 0:70d27fec6d71 | 559 | * InitializeConditions for ActionPort: '<S11>/Action Port' |
paulcox | 0:70d27fec6d71 | 560 | */ |
paulcox | 0:70d27fec6d71 | 561 | /* InitializeConditions for SwitchCase: '<S5>/Switch Case' incorporates: |
paulcox | 0:70d27fec6d71 | 562 | * InitializeConditions for DiscreteIntegrator: '<S45>/Integrator' |
paulcox | 0:70d27fec6d71 | 563 | */ |
paulcox | 0:70d27fec6d71 | 564 | DWork.Integrator_DSTATE_f = 0.0F; |
paulcox | 0:70d27fec6d71 | 565 | |
paulcox | 0:70d27fec6d71 | 566 | /* End of InitializeConditions for SubSystem: '<S5>/Velocity_Control' */ |
paulcox | 0:70d27fec6d71 | 567 | } |
paulcox | 0:70d27fec6d71 | 568 | |
paulcox | 0:70d27fec6d71 | 569 | /* Outputs for IfAction SubSystem: '<S5>/Velocity_Control' incorporates: |
paulcox | 0:70d27fec6d71 | 570 | * ActionPort: '<S11>/Action Port' |
paulcox | 0:70d27fec6d71 | 571 | */ |
paulcox | 0:70d27fec6d71 | 572 | /* Sum: '<S11>/Sum2' */ |
paulcox | 0:70d27fec6d71 | 573 | velocity_error = DWork.velocity_command - velocity_measured; |
paulcox | 0:70d27fec6d71 | 574 | |
paulcox | 0:70d27fec6d71 | 575 | /* Sum: '<S45>/Sum' incorporates: |
paulcox | 0:70d27fec6d71 | 576 | * DiscreteIntegrator: '<S45>/Integrator' |
paulcox | 0:70d27fec6d71 | 577 | * Gain: '<S45>/Proportional Gain' |
paulcox | 0:70d27fec6d71 | 578 | */ |
paulcox | 0:70d27fec6d71 | 579 | electrical_angle = (ctrlParams.Velocity_P * velocity_error) + |
paulcox | 0:70d27fec6d71 | 580 | DWork.Integrator_DSTATE_f; |
paulcox | 0:70d27fec6d71 | 581 | |
paulcox | 0:70d27fec6d71 | 582 | /* Saturate: '<S45>/Saturate' */ |
paulcox | 0:70d27fec6d71 | 583 | if (electrical_angle > 2.0F) { |
paulcox | 0:70d27fec6d71 | 584 | /* SignalConversion: '<S11>/Isolate_For_Merge' */ |
paulcox | 0:70d27fec6d71 | 585 | DWork.Merge = 2.0F; |
paulcox | 0:70d27fec6d71 | 586 | } else if (electrical_angle < -2.0F) { |
paulcox | 0:70d27fec6d71 | 587 | /* SignalConversion: '<S11>/Isolate_For_Merge' */ |
paulcox | 0:70d27fec6d71 | 588 | DWork.Merge = -2.0F; |
paulcox | 0:70d27fec6d71 | 589 | } else { |
paulcox | 0:70d27fec6d71 | 590 | /* SignalConversion: '<S11>/Isolate_For_Merge' */ |
paulcox | 0:70d27fec6d71 | 591 | DWork.Merge = electrical_angle; |
paulcox | 0:70d27fec6d71 | 592 | } |
paulcox | 0:70d27fec6d71 | 593 | |
paulcox | 0:70d27fec6d71 | 594 | /* End of Saturate: '<S45>/Saturate' */ |
paulcox | 0:70d27fec6d71 | 595 | |
paulcox | 0:70d27fec6d71 | 596 | /* DeadZone: '<S46>/DeadZone' */ |
paulcox | 0:70d27fec6d71 | 597 | if (electrical_angle > 2.0F) { |
paulcox | 0:70d27fec6d71 | 598 | electrical_angle -= 2.0F; |
paulcox | 0:70d27fec6d71 | 599 | } else if (electrical_angle >= -2.0F) { |
paulcox | 0:70d27fec6d71 | 600 | electrical_angle = 0.0F; |
paulcox | 0:70d27fec6d71 | 601 | } else { |
paulcox | 0:70d27fec6d71 | 602 | electrical_angle -= -2.0F; |
paulcox | 0:70d27fec6d71 | 603 | } |
paulcox | 0:70d27fec6d71 | 604 | |
paulcox | 0:70d27fec6d71 | 605 | /* End of DeadZone: '<S46>/DeadZone' */ |
paulcox | 0:70d27fec6d71 | 606 | |
paulcox | 0:70d27fec6d71 | 607 | /* RelationalOperator: '<S46>/NotEqual' */ |
paulcox | 0:70d27fec6d71 | 608 | NotEqual_b = (0.0F != electrical_angle); |
paulcox | 0:70d27fec6d71 | 609 | |
paulcox | 0:70d27fec6d71 | 610 | /* Signum: '<S46>/SignDeltaU' */ |
paulcox | 0:70d27fec6d71 | 611 | if (electrical_angle < 0.0F) { |
paulcox | 0:70d27fec6d71 | 612 | electrical_angle = -1.0F; |
paulcox | 0:70d27fec6d71 | 613 | } else { |
paulcox | 0:70d27fec6d71 | 614 | if (electrical_angle > 0.0F) { |
paulcox | 0:70d27fec6d71 | 615 | electrical_angle = 1.0F; |
paulcox | 0:70d27fec6d71 | 616 | } |
paulcox | 0:70d27fec6d71 | 617 | } |
paulcox | 0:70d27fec6d71 | 618 | |
paulcox | 0:70d27fec6d71 | 619 | /* End of Signum: '<S46>/SignDeltaU' */ |
paulcox | 0:70d27fec6d71 | 620 | |
paulcox | 0:70d27fec6d71 | 621 | /* Gain: '<S45>/Integral Gain' */ |
paulcox | 0:70d27fec6d71 | 622 | Switch_fr = ctrlParams.Velocity_I * velocity_error; |
paulcox | 0:70d27fec6d71 | 623 | |
paulcox | 0:70d27fec6d71 | 624 | /* DataTypeConversion: '<S46>/DataTypeConv1' */ |
paulcox | 0:70d27fec6d71 | 625 | if (electrical_angle < 128.0F) { |
paulcox | 0:70d27fec6d71 | 626 | rtPrevAction = (int8_T)electrical_angle; |
paulcox | 0:70d27fec6d71 | 627 | } else { |
paulcox | 0:70d27fec6d71 | 628 | rtPrevAction = MAX_int8_T; |
paulcox | 0:70d27fec6d71 | 629 | } |
paulcox | 0:70d27fec6d71 | 630 | |
paulcox | 0:70d27fec6d71 | 631 | /* End of DataTypeConversion: '<S46>/DataTypeConv1' */ |
paulcox | 0:70d27fec6d71 | 632 | |
paulcox | 0:70d27fec6d71 | 633 | /* Signum: '<S46>/SignPreIntegrator' */ |
paulcox | 0:70d27fec6d71 | 634 | if (Switch_fr < 0.0F) { |
paulcox | 0:70d27fec6d71 | 635 | electrical_angle = -1.0F; |
paulcox | 0:70d27fec6d71 | 636 | } else if (Switch_fr > 0.0F) { |
paulcox | 0:70d27fec6d71 | 637 | electrical_angle = 1.0F; |
paulcox | 0:70d27fec6d71 | 638 | } else { |
paulcox | 0:70d27fec6d71 | 639 | electrical_angle = Switch_fr; |
paulcox | 0:70d27fec6d71 | 640 | } |
paulcox | 0:70d27fec6d71 | 641 | |
paulcox | 0:70d27fec6d71 | 642 | /* Switch: '<S45>/Switch' incorporates: |
paulcox | 0:70d27fec6d71 | 643 | * Constant: '<S45>/Constant' |
paulcox | 0:70d27fec6d71 | 644 | * DataTypeConversion: '<S46>/DataTypeConv2' |
paulcox | 0:70d27fec6d71 | 645 | * Logic: '<S46>/AND' |
paulcox | 0:70d27fec6d71 | 646 | * RelationalOperator: '<S46>/Equal' |
paulcox | 0:70d27fec6d71 | 647 | * Signum: '<S46>/SignPreIntegrator' |
paulcox | 0:70d27fec6d71 | 648 | */ |
paulcox | 0:70d27fec6d71 | 649 | if (NotEqual_b && (rtPrevAction == ((int8_T)electrical_angle))) { |
paulcox | 0:70d27fec6d71 | 650 | Switch_fr = 0.0F; |
paulcox | 0:70d27fec6d71 | 651 | } |
paulcox | 0:70d27fec6d71 | 652 | |
paulcox | 0:70d27fec6d71 | 653 | /* End of Switch: '<S45>/Switch' */ |
paulcox | 0:70d27fec6d71 | 654 | |
paulcox | 0:70d27fec6d71 | 655 | /* Update for DiscreteIntegrator: '<S45>/Integrator' */ |
paulcox | 0:70d27fec6d71 | 656 | DWork.Integrator_DSTATE_f += 0.005F * Switch_fr; |
paulcox | 0:70d27fec6d71 | 657 | |
paulcox | 0:70d27fec6d71 | 658 | /* End of Outputs for SubSystem: '<S5>/Velocity_Control' */ |
paulcox | 0:70d27fec6d71 | 659 | break; |
paulcox | 0:70d27fec6d71 | 660 | |
paulcox | 0:70d27fec6d71 | 661 | case 1: |
paulcox | 0:70d27fec6d71 | 662 | if (DWork.SwitchCase_ActiveSubsystem != rtPrevAction) { |
paulcox | 0:70d27fec6d71 | 663 | /* InitializeConditions for IfAction SubSystem: '<S5>/Position_Control' incorporates: |
paulcox | 0:70d27fec6d71 | 664 | * InitializeConditions for ActionPort: '<S9>/Action Port' |
paulcox | 0:70d27fec6d71 | 665 | */ |
paulcox | 0:70d27fec6d71 | 666 | /* InitializeConditions for SwitchCase: '<S5>/Switch Case' incorporates: |
paulcox | 0:70d27fec6d71 | 667 | * InitializeConditions for DiscreteIntegrator: '<S42>/Integrator' |
paulcox | 0:70d27fec6d71 | 668 | */ |
paulcox | 0:70d27fec6d71 | 669 | DWork.Integrator_DSTATE_lc = 0.0F; |
paulcox | 0:70d27fec6d71 | 670 | |
paulcox | 0:70d27fec6d71 | 671 | /* End of InitializeConditions for SubSystem: '<S5>/Position_Control' */ |
paulcox | 0:70d27fec6d71 | 672 | } |
paulcox | 0:70d27fec6d71 | 673 | |
paulcox | 0:70d27fec6d71 | 674 | /* Outputs for IfAction SubSystem: '<S5>/Position_Control' incorporates: |
paulcox | 0:70d27fec6d71 | 675 | * ActionPort: '<S9>/Action Port' |
paulcox | 0:70d27fec6d71 | 676 | */ |
paulcox | 0:70d27fec6d71 | 677 | /* Sum: '<S9>/Sum2' */ |
paulcox | 0:70d27fec6d71 | 678 | Switch_fr = DWork.position_command - Switch_fr; |
paulcox | 0:70d27fec6d71 | 679 | |
paulcox | 0:70d27fec6d71 | 680 | /* Switch: '<S43>/Select_Angle' incorporates: |
paulcox | 0:70d27fec6d71 | 681 | * Constant: '<S43>/Neg_Pi_Constant' |
paulcox | 0:70d27fec6d71 | 682 | * Constant: '<S43>/Pi_Constant_1' |
paulcox | 0:70d27fec6d71 | 683 | * Constant: '<S43>/Pi_Constant_2' |
paulcox | 0:70d27fec6d71 | 684 | * Constant: '<S43>/Pi_Constant_3' |
paulcox | 0:70d27fec6d71 | 685 | * Constant: '<S43>/Two_Pi_Constant' |
paulcox | 0:70d27fec6d71 | 686 | * Logic: '<S43>/OR' |
paulcox | 0:70d27fec6d71 | 687 | * Math: '<S43>/Modulus' |
paulcox | 0:70d27fec6d71 | 688 | * RelationalOperator: '<S43>/Greater_Than' |
paulcox | 0:70d27fec6d71 | 689 | * RelationalOperator: '<S43>/Less_Than' |
paulcox | 0:70d27fec6d71 | 690 | * Sum: '<S43>/Add' |
paulcox | 0:70d27fec6d71 | 691 | * Sum: '<S43>/Subtract' |
paulcox | 0:70d27fec6d71 | 692 | */ |
paulcox | 0:70d27fec6d71 | 693 | if ((Switch_fr < -1.57079637F) || (Switch_fr >= 1.57079637F)) { |
paulcox | 0:70d27fec6d71 | 694 | Switch_fr = rt_modf(Switch_fr + 1.57079637F, 3.14159274F) - 1.57079637F; |
paulcox | 0:70d27fec6d71 | 695 | } |
paulcox | 0:70d27fec6d71 | 696 | |
paulcox | 0:70d27fec6d71 | 697 | /* End of Switch: '<S43>/Select_Angle' */ |
paulcox | 0:70d27fec6d71 | 698 | |
paulcox | 0:70d27fec6d71 | 699 | /* Sum: '<S42>/Sum' incorporates: |
paulcox | 0:70d27fec6d71 | 700 | * DiscreteIntegrator: '<S42>/Integrator' |
paulcox | 0:70d27fec6d71 | 701 | * Gain: '<S42>/Proportional Gain' |
paulcox | 0:70d27fec6d71 | 702 | */ |
paulcox | 0:70d27fec6d71 | 703 | electrical_angle = (ctrlParams.Position_P * Switch_fr) + |
paulcox | 0:70d27fec6d71 | 704 | DWork.Integrator_DSTATE_lc; |
paulcox | 0:70d27fec6d71 | 705 | |
paulcox | 0:70d27fec6d71 | 706 | /* Saturate: '<S42>/Saturate' */ |
paulcox | 0:70d27fec6d71 | 707 | if (electrical_angle > 2.0F) { |
paulcox | 0:70d27fec6d71 | 708 | /* SignalConversion: '<S9>/Isolate_For_Merge' */ |
paulcox | 0:70d27fec6d71 | 709 | DWork.Merge = 2.0F; |
paulcox | 0:70d27fec6d71 | 710 | } else if (electrical_angle < -2.0F) { |
paulcox | 0:70d27fec6d71 | 711 | /* SignalConversion: '<S9>/Isolate_For_Merge' */ |
paulcox | 0:70d27fec6d71 | 712 | DWork.Merge = -2.0F; |
paulcox | 0:70d27fec6d71 | 713 | } else { |
paulcox | 0:70d27fec6d71 | 714 | /* SignalConversion: '<S9>/Isolate_For_Merge' */ |
paulcox | 0:70d27fec6d71 | 715 | DWork.Merge = electrical_angle; |
paulcox | 0:70d27fec6d71 | 716 | } |
paulcox | 0:70d27fec6d71 | 717 | |
paulcox | 0:70d27fec6d71 | 718 | /* End of Saturate: '<S42>/Saturate' */ |
paulcox | 0:70d27fec6d71 | 719 | |
paulcox | 0:70d27fec6d71 | 720 | /* DeadZone: '<S44>/DeadZone' */ |
paulcox | 0:70d27fec6d71 | 721 | if (electrical_angle > 2.0F) { |
paulcox | 0:70d27fec6d71 | 722 | electrical_angle -= 2.0F; |
paulcox | 0:70d27fec6d71 | 723 | } else if (electrical_angle >= -2.0F) { |
paulcox | 0:70d27fec6d71 | 724 | electrical_angle = 0.0F; |
paulcox | 0:70d27fec6d71 | 725 | } else { |
paulcox | 0:70d27fec6d71 | 726 | electrical_angle -= -2.0F; |
paulcox | 0:70d27fec6d71 | 727 | } |
paulcox | 0:70d27fec6d71 | 728 | |
paulcox | 0:70d27fec6d71 | 729 | /* End of DeadZone: '<S44>/DeadZone' */ |
paulcox | 0:70d27fec6d71 | 730 | |
paulcox | 0:70d27fec6d71 | 731 | /* RelationalOperator: '<S44>/NotEqual' */ |
paulcox | 0:70d27fec6d71 | 732 | NotEqual_b = (0.0F != electrical_angle); |
paulcox | 0:70d27fec6d71 | 733 | |
paulcox | 0:70d27fec6d71 | 734 | /* Signum: '<S44>/SignDeltaU' */ |
paulcox | 0:70d27fec6d71 | 735 | if (electrical_angle < 0.0F) { |
paulcox | 0:70d27fec6d71 | 736 | electrical_angle = -1.0F; |
paulcox | 0:70d27fec6d71 | 737 | } else { |
paulcox | 0:70d27fec6d71 | 738 | if (electrical_angle > 0.0F) { |
paulcox | 0:70d27fec6d71 | 739 | electrical_angle = 1.0F; |
paulcox | 0:70d27fec6d71 | 740 | } |
paulcox | 0:70d27fec6d71 | 741 | } |
paulcox | 0:70d27fec6d71 | 742 | |
paulcox | 0:70d27fec6d71 | 743 | /* End of Signum: '<S44>/SignDeltaU' */ |
paulcox | 0:70d27fec6d71 | 744 | |
paulcox | 0:70d27fec6d71 | 745 | /* Gain: '<S42>/Integral Gain' */ |
paulcox | 0:70d27fec6d71 | 746 | Switch_fr *= ctrlParams.Position_I; |
paulcox | 0:70d27fec6d71 | 747 | |
paulcox | 0:70d27fec6d71 | 748 | /* DataTypeConversion: '<S44>/DataTypeConv1' */ |
paulcox | 0:70d27fec6d71 | 749 | if (electrical_angle < 128.0F) { |
paulcox | 0:70d27fec6d71 | 750 | rtPrevAction = (int8_T)electrical_angle; |
paulcox | 0:70d27fec6d71 | 751 | } else { |
paulcox | 0:70d27fec6d71 | 752 | rtPrevAction = MAX_int8_T; |
paulcox | 0:70d27fec6d71 | 753 | } |
paulcox | 0:70d27fec6d71 | 754 | |
paulcox | 0:70d27fec6d71 | 755 | /* End of DataTypeConversion: '<S44>/DataTypeConv1' */ |
paulcox | 0:70d27fec6d71 | 756 | |
paulcox | 0:70d27fec6d71 | 757 | /* Signum: '<S44>/SignPreIntegrator' */ |
paulcox | 0:70d27fec6d71 | 758 | if (Switch_fr < 0.0F) { |
paulcox | 0:70d27fec6d71 | 759 | electrical_angle = -1.0F; |
paulcox | 0:70d27fec6d71 | 760 | } else if (Switch_fr > 0.0F) { |
paulcox | 0:70d27fec6d71 | 761 | electrical_angle = 1.0F; |
paulcox | 0:70d27fec6d71 | 762 | } else { |
paulcox | 0:70d27fec6d71 | 763 | electrical_angle = Switch_fr; |
paulcox | 0:70d27fec6d71 | 764 | } |
paulcox | 0:70d27fec6d71 | 765 | |
paulcox | 0:70d27fec6d71 | 766 | /* Switch: '<S42>/Switch' incorporates: |
paulcox | 0:70d27fec6d71 | 767 | * Constant: '<S42>/Constant' |
paulcox | 0:70d27fec6d71 | 768 | * DataTypeConversion: '<S44>/DataTypeConv2' |
paulcox | 0:70d27fec6d71 | 769 | * Logic: '<S44>/AND' |
paulcox | 0:70d27fec6d71 | 770 | * RelationalOperator: '<S44>/Equal' |
paulcox | 0:70d27fec6d71 | 771 | * Signum: '<S44>/SignPreIntegrator' |
paulcox | 0:70d27fec6d71 | 772 | */ |
paulcox | 0:70d27fec6d71 | 773 | if (NotEqual_b && (rtPrevAction == ((int8_T)electrical_angle))) { |
paulcox | 0:70d27fec6d71 | 774 | Switch_fr = 0.0F; |
paulcox | 0:70d27fec6d71 | 775 | } |
paulcox | 0:70d27fec6d71 | 776 | |
paulcox | 0:70d27fec6d71 | 777 | /* End of Switch: '<S42>/Switch' */ |
paulcox | 0:70d27fec6d71 | 778 | |
paulcox | 0:70d27fec6d71 | 779 | /* Update for DiscreteIntegrator: '<S42>/Integrator' */ |
paulcox | 0:70d27fec6d71 | 780 | DWork.Integrator_DSTATE_lc += 0.005F * Switch_fr; |
paulcox | 0:70d27fec6d71 | 781 | |
paulcox | 0:70d27fec6d71 | 782 | /* End of Outputs for SubSystem: '<S5>/Position_Control' */ |
paulcox | 0:70d27fec6d71 | 783 | break; |
paulcox | 0:70d27fec6d71 | 784 | |
paulcox | 0:70d27fec6d71 | 785 | case 2: |
paulcox | 0:70d27fec6d71 | 786 | /* Outputs for IfAction SubSystem: '<S5>/Torque_Control' incorporates: |
paulcox | 0:70d27fec6d71 | 787 | * ActionPort: '<S10>/Action Port' |
paulcox | 0:70d27fec6d71 | 788 | */ |
paulcox | 0:70d27fec6d71 | 789 | DWork.Merge = DWork.torque_command; |
paulcox | 0:70d27fec6d71 | 790 | |
paulcox | 0:70d27fec6d71 | 791 | /* End of Outputs for SubSystem: '<S5>/Torque_Control' */ |
paulcox | 0:70d27fec6d71 | 792 | break; |
paulcox | 0:70d27fec6d71 | 793 | } |
paulcox | 0:70d27fec6d71 | 794 | |
paulcox | 0:70d27fec6d71 | 795 | /* End of SwitchCase: '<S5>/Switch Case' */ |
paulcox | 0:70d27fec6d71 | 796 | } |
paulcox | 0:70d27fec6d71 | 797 | |
paulcox | 0:70d27fec6d71 | 798 | /* RateTransition: '<S5>/Lo_to_Hi_Rate_Transition1' */ |
paulcox | 0:70d27fec6d71 | 799 | q_current_command = DWork.Merge; |
paulcox | 0:70d27fec6d71 | 800 | |
paulcox | 0:70d27fec6d71 | 801 | /* Sum: '<S22>/Add' incorporates: |
paulcox | 0:70d27fec6d71 | 802 | * Product: '<S22>/Product' |
paulcox | 0:70d27fec6d71 | 803 | * Product: '<S22>/Product1' |
paulcox | 0:70d27fec6d71 | 804 | */ |
paulcox | 0:70d27fec6d71 | 805 | q_current_measured = (SignDeltaU_b * cos_coefficient) - (phase_currents[0] * |
paulcox | 0:70d27fec6d71 | 806 | sin_coefficient); |
paulcox | 0:70d27fec6d71 | 807 | |
paulcox | 0:70d27fec6d71 | 808 | /* Sum: '<S12>/Sum' */ |
paulcox | 0:70d27fec6d71 | 809 | q_current_error = q_current_command - q_current_measured; |
paulcox | 0:70d27fec6d71 | 810 | |
paulcox | 0:70d27fec6d71 | 811 | /* DiscreteIntegrator: '<S18>/Integrator' */ |
paulcox | 0:70d27fec6d71 | 812 | if ((FixPtRelationalOperator != 0) || (DWork.Integrator_PrevResetState_c != 0)) |
paulcox | 0:70d27fec6d71 | 813 | { |
paulcox | 0:70d27fec6d71 | 814 | DWork.Integrator_DSTATE_l = 0.0F; |
paulcox | 0:70d27fec6d71 | 815 | } |
paulcox | 0:70d27fec6d71 | 816 | |
paulcox | 0:70d27fec6d71 | 817 | /* Sum: '<S18>/Sum' incorporates: |
paulcox | 0:70d27fec6d71 | 818 | * DiscreteIntegrator: '<S18>/Integrator' |
paulcox | 0:70d27fec6d71 | 819 | * Gain: '<S18>/Proportional Gain' |
paulcox | 0:70d27fec6d71 | 820 | */ |
paulcox | 0:70d27fec6d71 | 821 | SignDeltaU_b = (ctrlParams.Current_P * q_current_error) + |
paulcox | 0:70d27fec6d71 | 822 | DWork.Integrator_DSTATE_l; |
paulcox | 0:70d27fec6d71 | 823 | |
paulcox | 0:70d27fec6d71 | 824 | /* Saturate: '<S18>/Saturate' */ |
paulcox | 0:70d27fec6d71 | 825 | if (SignDeltaU_b > 12.0F) { |
paulcox | 0:70d27fec6d71 | 826 | alpha_voltage = 12.0F; |
paulcox | 0:70d27fec6d71 | 827 | } else if (SignDeltaU_b < -12.0F) { |
paulcox | 0:70d27fec6d71 | 828 | alpha_voltage = -12.0F; |
paulcox | 0:70d27fec6d71 | 829 | } else { |
paulcox | 0:70d27fec6d71 | 830 | alpha_voltage = SignDeltaU_b; |
paulcox | 0:70d27fec6d71 | 831 | } |
paulcox | 0:70d27fec6d71 | 832 | |
paulcox | 0:70d27fec6d71 | 833 | /* End of Saturate: '<S18>/Saturate' */ |
paulcox | 0:70d27fec6d71 | 834 | |
paulcox | 0:70d27fec6d71 | 835 | /* Sum: '<S24>/Add' incorporates: |
paulcox | 0:70d27fec6d71 | 836 | * Product: '<S24>/Product' |
paulcox | 0:70d27fec6d71 | 837 | * Product: '<S24>/Product1' |
paulcox | 0:70d27fec6d71 | 838 | */ |
paulcox | 0:70d27fec6d71 | 839 | SignPreIntegrator_f = (Gain1 * sin_coefficient) + (alpha_voltage * |
paulcox | 0:70d27fec6d71 | 840 | cos_coefficient); |
paulcox | 0:70d27fec6d71 | 841 | |
paulcox | 0:70d27fec6d71 | 842 | /* Gain: '<S29>/Gain' */ |
paulcox | 0:70d27fec6d71 | 843 | IntegralGain_j = 0.5F * SignPreIntegrator_f; |
paulcox | 0:70d27fec6d71 | 844 | |
paulcox | 0:70d27fec6d71 | 845 | /* Sum: '<S24>/Add1' incorporates: |
paulcox | 0:70d27fec6d71 | 846 | * Product: '<S24>/Product2' |
paulcox | 0:70d27fec6d71 | 847 | * Product: '<S24>/Product3' |
paulcox | 0:70d27fec6d71 | 848 | */ |
paulcox | 0:70d27fec6d71 | 849 | alpha_voltage = (Gain1 * cos_coefficient) - (alpha_voltage * sin_coefficient); |
paulcox | 0:70d27fec6d71 | 850 | |
paulcox | 0:70d27fec6d71 | 851 | /* Gain: '<S29>/Gain1' */ |
paulcox | 0:70d27fec6d71 | 852 | Gain1 = 0.866025388F * alpha_voltage; |
paulcox | 0:70d27fec6d71 | 853 | |
paulcox | 0:70d27fec6d71 | 854 | /* Gain: '<S30>/Space_Vector_Gain' incorporates: |
paulcox | 0:70d27fec6d71 | 855 | * Gain: '<S30>/Alpha_Gain' |
paulcox | 0:70d27fec6d71 | 856 | * Sum: '<S30>/Add' |
paulcox | 0:70d27fec6d71 | 857 | */ |
paulcox | 0:70d27fec6d71 | 858 | electrical_angle = (((1.73205078F * alpha_voltage) + 33.941124F) + |
paulcox | 0:70d27fec6d71 | 859 | SignPreIntegrator_f) * 0.353553385F; |
paulcox | 0:70d27fec6d71 | 860 | |
paulcox | 0:70d27fec6d71 | 861 | /* Gain: '<S33>/Va_Gain' incorporates: |
paulcox | 0:70d27fec6d71 | 862 | * Gain: '<S33>/Alpha_Gain' |
paulcox | 0:70d27fec6d71 | 863 | * Gain: '<S33>/Beta_Gain' |
paulcox | 0:70d27fec6d71 | 864 | * Sum: '<S33>/Add' |
paulcox | 0:70d27fec6d71 | 865 | */ |
paulcox | 0:70d27fec6d71 | 866 | Switch_fr = ((33.941124F - (1.73205078F * alpha_voltage)) + (3.0F * |
paulcox | 0:70d27fec6d71 | 867 | SignPreIntegrator_f)) * 0.353553385F; |
paulcox | 0:70d27fec6d71 | 868 | |
paulcox | 0:70d27fec6d71 | 869 | /* Gain: '<S36>/Space_Vector_Gain' incorporates: |
paulcox | 0:70d27fec6d71 | 870 | * Gain: '<S36>/Alpha_Gain' |
paulcox | 0:70d27fec6d71 | 871 | * Sum: '<S36>/Add' |
paulcox | 0:70d27fec6d71 | 872 | */ |
paulcox | 0:70d27fec6d71 | 873 | cos_coefficient = ((33.941124F - (1.73205078F * alpha_voltage)) - |
paulcox | 0:70d27fec6d71 | 874 | SignPreIntegrator_f) * 0.353553385F; |
paulcox | 0:70d27fec6d71 | 875 | |
paulcox | 0:70d27fec6d71 | 876 | /* Gain: '<S31>/Space_Vector_Gain' incorporates: |
paulcox | 0:70d27fec6d71 | 877 | * Constant: '<S27>/Bus_Voltage' |
paulcox | 0:70d27fec6d71 | 878 | * Gain: '<S31>/Alpha_Gain' |
paulcox | 0:70d27fec6d71 | 879 | * Sum: '<S31>/Add' |
paulcox | 0:70d27fec6d71 | 880 | */ |
paulcox | 0:70d27fec6d71 | 881 | sin_coefficient = ((2.44948983F * alpha_voltage) + 24.0F) * 0.5F; |
paulcox | 0:70d27fec6d71 | 882 | |
paulcox | 0:70d27fec6d71 | 883 | /* Gain: '<S34>/Va_Gain' incorporates: |
paulcox | 0:70d27fec6d71 | 884 | * Constant: '<S27>/Bus_Voltage' |
paulcox | 0:70d27fec6d71 | 885 | * Gain: '<S34>/Beta_Gain' |
paulcox | 0:70d27fec6d71 | 886 | * Sum: '<S34>/Add' |
paulcox | 0:70d27fec6d71 | 887 | */ |
paulcox | 0:70d27fec6d71 | 888 | Sectors_2_and_5_idx_1 = ((1.41421354F * SignPreIntegrator_f) + 24.0F) * 0.5F; |
paulcox | 0:70d27fec6d71 | 889 | |
paulcox | 0:70d27fec6d71 | 890 | /* Gain: '<S37>/Space_Vector_Gain' incorporates: |
paulcox | 0:70d27fec6d71 | 891 | * Constant: '<S27>/Bus_Voltage' |
paulcox | 0:70d27fec6d71 | 892 | * Gain: '<S37>/Beta_Gain' |
paulcox | 0:70d27fec6d71 | 893 | * Sum: '<S37>/Add' |
paulcox | 0:70d27fec6d71 | 894 | */ |
paulcox | 0:70d27fec6d71 | 895 | Sectors_2_and_5_idx_2 = (24.0F - (1.41421354F * SignPreIntegrator_f)) * 0.5F; |
paulcox | 0:70d27fec6d71 | 896 | |
paulcox | 0:70d27fec6d71 | 897 | /* Gain: '<S32>/Space_Vector_Gain' incorporates: |
paulcox | 0:70d27fec6d71 | 898 | * Gain: '<S32>/Alpha_Gain' |
paulcox | 0:70d27fec6d71 | 899 | * Sum: '<S32>/Add' |
paulcox | 0:70d27fec6d71 | 900 | */ |
paulcox | 0:70d27fec6d71 | 901 | phase_voltages[0] = (((1.73205078F * alpha_voltage) + 33.941124F) - |
paulcox | 0:70d27fec6d71 | 902 | SignPreIntegrator_f) * 0.353553385F; |
paulcox | 0:70d27fec6d71 | 903 | |
paulcox | 0:70d27fec6d71 | 904 | /* Gain: '<S35>/Va_Gain' incorporates: |
paulcox | 0:70d27fec6d71 | 905 | * Gain: '<S35>/Alpha_Gain' |
paulcox | 0:70d27fec6d71 | 906 | * Sum: '<S35>/Add' |
paulcox | 0:70d27fec6d71 | 907 | */ |
paulcox | 0:70d27fec6d71 | 908 | phase_voltages[1] = ((33.941124F - (1.73205078F * alpha_voltage)) + |
paulcox | 0:70d27fec6d71 | 909 | SignPreIntegrator_f) * 0.353553385F; |
paulcox | 0:70d27fec6d71 | 910 | |
paulcox | 0:70d27fec6d71 | 911 | /* Gain: '<S38>/Space_Vector_Gain' incorporates: |
paulcox | 0:70d27fec6d71 | 912 | * Gain: '<S38>/Alpha_Gain' |
paulcox | 0:70d27fec6d71 | 913 | * Gain: '<S38>/Beta_Gain' |
paulcox | 0:70d27fec6d71 | 914 | * Sum: '<S38>/Add' |
paulcox | 0:70d27fec6d71 | 915 | */ |
paulcox | 0:70d27fec6d71 | 916 | phase_voltages[2] = ((33.941124F - (1.73205078F * alpha_voltage)) - (3.0F * |
paulcox | 0:70d27fec6d71 | 917 | SignPreIntegrator_f)) * 0.353553385F; |
paulcox | 0:70d27fec6d71 | 918 | |
paulcox | 0:70d27fec6d71 | 919 | /* LookupNDDirect: '<S28>/Lookup_Table' incorporates: |
paulcox | 0:70d27fec6d71 | 920 | * Constant: '<S39>/Constant' |
paulcox | 0:70d27fec6d71 | 921 | * Constant: '<S40>/Constant' |
paulcox | 0:70d27fec6d71 | 922 | * Constant: '<S41>/Constant' |
paulcox | 0:70d27fec6d71 | 923 | * Gain: '<S28>/Sector_Gain_VB' |
paulcox | 0:70d27fec6d71 | 924 | * Gain: '<S28>/Sector_Gain_VC' |
paulcox | 0:70d27fec6d71 | 925 | * RelationalOperator: '<S39>/Compare' |
paulcox | 0:70d27fec6d71 | 926 | * RelationalOperator: '<S40>/Compare' |
paulcox | 0:70d27fec6d71 | 927 | * RelationalOperator: '<S41>/Compare' |
paulcox | 0:70d27fec6d71 | 928 | * Sum: '<S28>/Calculate_Phase_Advanced_Sector' |
paulcox | 0:70d27fec6d71 | 929 | * Sum: '<S29>/Add' |
paulcox | 0:70d27fec6d71 | 930 | * Sum: '<S29>/Add1' |
paulcox | 0:70d27fec6d71 | 931 | * |
paulcox | 0:70d27fec6d71 | 932 | * About '<S28>/Lookup_Table': |
paulcox | 0:70d27fec6d71 | 933 | * 1-dimensional Direct Look-Up returning a Scalar |
paulcox | 0:70d27fec6d71 | 934 | */ |
paulcox | 0:70d27fec6d71 | 935 | u0 = (int16_T)(((((Gain1 - IntegralGain_j) > 0.0F) << 1) + |
paulcox | 0:70d27fec6d71 | 936 | (SignPreIntegrator_f > 0.0F)) + ((((0.0F - IntegralGain_j) - |
paulcox | 0:70d27fec6d71 | 937 | Gain1) > 0.0F) << 2)); |
paulcox | 0:70d27fec6d71 | 938 | if (u0 > 6) { |
paulcox | 0:70d27fec6d71 | 939 | u0 = 6; |
paulcox | 0:70d27fec6d71 | 940 | } |
paulcox | 0:70d27fec6d71 | 941 | |
paulcox | 0:70d27fec6d71 | 942 | /* MultiPortSwitch: '<S27>/Select_Sector' incorporates: |
paulcox | 0:70d27fec6d71 | 943 | * LookupNDDirect: '<S28>/Lookup_Table' |
paulcox | 0:70d27fec6d71 | 944 | * |
paulcox | 0:70d27fec6d71 | 945 | * About '<S28>/Lookup_Table': |
paulcox | 0:70d27fec6d71 | 946 | * 1-dimensional Direct Look-Up returning a Scalar |
paulcox | 0:70d27fec6d71 | 947 | */ |
paulcox | 0:70d27fec6d71 | 948 | switch (ConstP.Lookup_Table_table[u0]) { |
paulcox | 0:70d27fec6d71 | 949 | case 1: |
paulcox | 0:70d27fec6d71 | 950 | phase_voltages[0] = electrical_angle; |
paulcox | 0:70d27fec6d71 | 951 | phase_voltages[1] = Switch_fr; |
paulcox | 0:70d27fec6d71 | 952 | phase_voltages[2] = cos_coefficient; |
paulcox | 0:70d27fec6d71 | 953 | break; |
paulcox | 0:70d27fec6d71 | 954 | |
paulcox | 0:70d27fec6d71 | 955 | case 2: |
paulcox | 0:70d27fec6d71 | 956 | phase_voltages[0] = sin_coefficient; |
paulcox | 0:70d27fec6d71 | 957 | phase_voltages[1] = Sectors_2_and_5_idx_1; |
paulcox | 0:70d27fec6d71 | 958 | phase_voltages[2] = Sectors_2_and_5_idx_2; |
paulcox | 0:70d27fec6d71 | 959 | break; |
paulcox | 0:70d27fec6d71 | 960 | |
paulcox | 0:70d27fec6d71 | 961 | case 3: |
paulcox | 0:70d27fec6d71 | 962 | break; |
paulcox | 0:70d27fec6d71 | 963 | |
paulcox | 0:70d27fec6d71 | 964 | case 4: |
paulcox | 0:70d27fec6d71 | 965 | phase_voltages[0] = electrical_angle; |
paulcox | 0:70d27fec6d71 | 966 | phase_voltages[1] = Switch_fr; |
paulcox | 0:70d27fec6d71 | 967 | phase_voltages[2] = cos_coefficient; |
paulcox | 0:70d27fec6d71 | 968 | break; |
paulcox | 0:70d27fec6d71 | 969 | |
paulcox | 0:70d27fec6d71 | 970 | case 5: |
paulcox | 0:70d27fec6d71 | 971 | phase_voltages[0] = sin_coefficient; |
paulcox | 0:70d27fec6d71 | 972 | phase_voltages[1] = Sectors_2_and_5_idx_1; |
paulcox | 0:70d27fec6d71 | 973 | phase_voltages[2] = Sectors_2_and_5_idx_2; |
paulcox | 0:70d27fec6d71 | 974 | break; |
paulcox | 0:70d27fec6d71 | 975 | } |
paulcox | 0:70d27fec6d71 | 976 | |
paulcox | 0:70d27fec6d71 | 977 | /* End of MultiPortSwitch: '<S27>/Select_Sector' */ |
paulcox | 0:70d27fec6d71 | 978 | |
paulcox | 0:70d27fec6d71 | 979 | /* Switch: '<S6>/Switch' */ |
paulcox | 0:70d27fec6d71 | 980 | if (DWork.Lo_to_Hi_Rate_Transition3_Buffe) { |
paulcox | 0:70d27fec6d71 | 981 | /* Outport: '<Root>/pwm_compare' */ |
paulcox | 0:70d27fec6d71 | 982 | pwm_compare[0] = 1500U; |
paulcox | 0:70d27fec6d71 | 983 | pwm_compare[1] = 1500U; |
paulcox | 0:70d27fec6d71 | 984 | pwm_compare[2] = 1500U; |
paulcox | 0:70d27fec6d71 | 985 | } else { |
paulcox | 0:70d27fec6d71 | 986 | /* Gain: '<S6>/Voltage to PWM Compare Units' */ |
paulcox | 0:70d27fec6d71 | 987 | electrical_angle = 125.0F * phase_voltages[0]; |
paulcox | 0:70d27fec6d71 | 988 | |
paulcox | 0:70d27fec6d71 | 989 | /* Saturate: '<S6>/Saturation' */ |
paulcox | 0:70d27fec6d71 | 990 | if (electrical_angle > 2999.0F) { |
paulcox | 0:70d27fec6d71 | 991 | /* Outport: '<Root>/pwm_compare' incorporates: |
paulcox | 0:70d27fec6d71 | 992 | * DataTypeConversion: '<S6>/Quantize' |
paulcox | 0:70d27fec6d71 | 993 | */ |
paulcox | 0:70d27fec6d71 | 994 | pwm_compare[0] = 2999U; |
paulcox | 0:70d27fec6d71 | 995 | } else if (electrical_angle < 0.0F) { |
paulcox | 0:70d27fec6d71 | 996 | /* Outport: '<Root>/pwm_compare' incorporates: |
paulcox | 0:70d27fec6d71 | 997 | * DataTypeConversion: '<S6>/Quantize' |
paulcox | 0:70d27fec6d71 | 998 | */ |
paulcox | 0:70d27fec6d71 | 999 | pwm_compare[0] = 0U; |
paulcox | 0:70d27fec6d71 | 1000 | } else { |
paulcox | 0:70d27fec6d71 | 1001 | /* Outport: '<Root>/pwm_compare' incorporates: |
paulcox | 0:70d27fec6d71 | 1002 | * DataTypeConversion: '<S6>/Quantize' |
paulcox | 0:70d27fec6d71 | 1003 | */ |
paulcox | 0:70d27fec6d71 | 1004 | pwm_compare[0] = (uint16_T)electrical_angle; |
paulcox | 0:70d27fec6d71 | 1005 | } |
paulcox | 0:70d27fec6d71 | 1006 | |
paulcox | 0:70d27fec6d71 | 1007 | /* Gain: '<S6>/Voltage to PWM Compare Units' */ |
paulcox | 0:70d27fec6d71 | 1008 | electrical_angle = 125.0F * phase_voltages[1]; |
paulcox | 0:70d27fec6d71 | 1009 | |
paulcox | 0:70d27fec6d71 | 1010 | /* Saturate: '<S6>/Saturation' */ |
paulcox | 0:70d27fec6d71 | 1011 | if (electrical_angle > 2999.0F) { |
paulcox | 0:70d27fec6d71 | 1012 | /* Outport: '<Root>/pwm_compare' incorporates: |
paulcox | 0:70d27fec6d71 | 1013 | * DataTypeConversion: '<S6>/Quantize' |
paulcox | 0:70d27fec6d71 | 1014 | */ |
paulcox | 0:70d27fec6d71 | 1015 | pwm_compare[1] = 2999U; |
paulcox | 0:70d27fec6d71 | 1016 | } else if (electrical_angle < 0.0F) { |
paulcox | 0:70d27fec6d71 | 1017 | /* Outport: '<Root>/pwm_compare' incorporates: |
paulcox | 0:70d27fec6d71 | 1018 | * DataTypeConversion: '<S6>/Quantize' |
paulcox | 0:70d27fec6d71 | 1019 | */ |
paulcox | 0:70d27fec6d71 | 1020 | pwm_compare[1] = 0U; |
paulcox | 0:70d27fec6d71 | 1021 | } else { |
paulcox | 0:70d27fec6d71 | 1022 | /* Outport: '<Root>/pwm_compare' incorporates: |
paulcox | 0:70d27fec6d71 | 1023 | * DataTypeConversion: '<S6>/Quantize' |
paulcox | 0:70d27fec6d71 | 1024 | */ |
paulcox | 0:70d27fec6d71 | 1025 | pwm_compare[1] = (uint16_T)electrical_angle; |
paulcox | 0:70d27fec6d71 | 1026 | } |
paulcox | 0:70d27fec6d71 | 1027 | |
paulcox | 0:70d27fec6d71 | 1028 | /* Gain: '<S6>/Voltage to PWM Compare Units' */ |
paulcox | 0:70d27fec6d71 | 1029 | electrical_angle = 125.0F * phase_voltages[2]; |
paulcox | 0:70d27fec6d71 | 1030 | |
paulcox | 0:70d27fec6d71 | 1031 | /* Saturate: '<S6>/Saturation' */ |
paulcox | 0:70d27fec6d71 | 1032 | if (electrical_angle > 2999.0F) { |
paulcox | 0:70d27fec6d71 | 1033 | /* Outport: '<Root>/pwm_compare' incorporates: |
paulcox | 0:70d27fec6d71 | 1034 | * DataTypeConversion: '<S6>/Quantize' |
paulcox | 0:70d27fec6d71 | 1035 | */ |
paulcox | 0:70d27fec6d71 | 1036 | pwm_compare[2] = 2999U; |
paulcox | 0:70d27fec6d71 | 1037 | } else if (electrical_angle < 0.0F) { |
paulcox | 0:70d27fec6d71 | 1038 | /* Outport: '<Root>/pwm_compare' incorporates: |
paulcox | 0:70d27fec6d71 | 1039 | * DataTypeConversion: '<S6>/Quantize' |
paulcox | 0:70d27fec6d71 | 1040 | */ |
paulcox | 0:70d27fec6d71 | 1041 | pwm_compare[2] = 0U; |
paulcox | 0:70d27fec6d71 | 1042 | } else { |
paulcox | 0:70d27fec6d71 | 1043 | /* Outport: '<Root>/pwm_compare' incorporates: |
paulcox | 0:70d27fec6d71 | 1044 | * DataTypeConversion: '<S6>/Quantize' |
paulcox | 0:70d27fec6d71 | 1045 | */ |
paulcox | 0:70d27fec6d71 | 1046 | pwm_compare[2] = (uint16_T)electrical_angle; |
paulcox | 0:70d27fec6d71 | 1047 | } |
paulcox | 0:70d27fec6d71 | 1048 | } |
paulcox | 0:70d27fec6d71 | 1049 | |
paulcox | 0:70d27fec6d71 | 1050 | /* End of Switch: '<S6>/Switch' */ |
paulcox | 0:70d27fec6d71 | 1051 | |
paulcox | 0:70d27fec6d71 | 1052 | /* DeadZone: '<S19>/DeadZone' */ |
paulcox | 0:70d27fec6d71 | 1053 | if (SignDeltaU > 12.0F) { |
paulcox | 0:70d27fec6d71 | 1054 | SignDeltaU -= 12.0F; |
paulcox | 0:70d27fec6d71 | 1055 | } else if (SignDeltaU >= -12.0F) { |
paulcox | 0:70d27fec6d71 | 1056 | SignDeltaU = 0.0F; |
paulcox | 0:70d27fec6d71 | 1057 | } else { |
paulcox | 0:70d27fec6d71 | 1058 | SignDeltaU -= -12.0F; |
paulcox | 0:70d27fec6d71 | 1059 | } |
paulcox | 0:70d27fec6d71 | 1060 | |
paulcox | 0:70d27fec6d71 | 1061 | /* End of DeadZone: '<S19>/DeadZone' */ |
paulcox | 0:70d27fec6d71 | 1062 | |
paulcox | 0:70d27fec6d71 | 1063 | /* RelationalOperator: '<S19>/NotEqual' */ |
paulcox | 0:70d27fec6d71 | 1064 | NotEqual_b = (0.0F != SignDeltaU); |
paulcox | 0:70d27fec6d71 | 1065 | |
paulcox | 0:70d27fec6d71 | 1066 | /* Signum: '<S19>/SignDeltaU' */ |
paulcox | 0:70d27fec6d71 | 1067 | if (SignDeltaU < 0.0F) { |
paulcox | 0:70d27fec6d71 | 1068 | SignDeltaU = -1.0F; |
paulcox | 0:70d27fec6d71 | 1069 | } else { |
paulcox | 0:70d27fec6d71 | 1070 | if (SignDeltaU > 0.0F) { |
paulcox | 0:70d27fec6d71 | 1071 | SignDeltaU = 1.0F; |
paulcox | 0:70d27fec6d71 | 1072 | } |
paulcox | 0:70d27fec6d71 | 1073 | } |
paulcox | 0:70d27fec6d71 | 1074 | |
paulcox | 0:70d27fec6d71 | 1075 | /* End of Signum: '<S19>/SignDeltaU' */ |
paulcox | 0:70d27fec6d71 | 1076 | |
paulcox | 0:70d27fec6d71 | 1077 | /* Gain: '<S17>/Integral Gain' */ |
paulcox | 0:70d27fec6d71 | 1078 | IntegralGain_j = ctrlParams.Current_I * d_current_error; |
paulcox | 0:70d27fec6d71 | 1079 | |
paulcox | 0:70d27fec6d71 | 1080 | /* DataTypeConversion: '<S19>/DataTypeConv1' */ |
paulcox | 0:70d27fec6d71 | 1081 | if (SignDeltaU < 128.0F) { |
paulcox | 0:70d27fec6d71 | 1082 | rtPrevAction = (int8_T)SignDeltaU; |
paulcox | 0:70d27fec6d71 | 1083 | } else { |
paulcox | 0:70d27fec6d71 | 1084 | rtPrevAction = MAX_int8_T; |
paulcox | 0:70d27fec6d71 | 1085 | } |
paulcox | 0:70d27fec6d71 | 1086 | |
paulcox | 0:70d27fec6d71 | 1087 | /* End of DataTypeConversion: '<S19>/DataTypeConv1' */ |
paulcox | 0:70d27fec6d71 | 1088 | |
paulcox | 0:70d27fec6d71 | 1089 | /* Signum: '<S19>/SignPreIntegrator' */ |
paulcox | 0:70d27fec6d71 | 1090 | if (IntegralGain_j < 0.0F) { |
paulcox | 0:70d27fec6d71 | 1091 | electrical_angle = -1.0F; |
paulcox | 0:70d27fec6d71 | 1092 | } else if (IntegralGain_j > 0.0F) { |
paulcox | 0:70d27fec6d71 | 1093 | electrical_angle = 1.0F; |
paulcox | 0:70d27fec6d71 | 1094 | } else { |
paulcox | 0:70d27fec6d71 | 1095 | electrical_angle = IntegralGain_j; |
paulcox | 0:70d27fec6d71 | 1096 | } |
paulcox | 0:70d27fec6d71 | 1097 | |
paulcox | 0:70d27fec6d71 | 1098 | /* Switch: '<S17>/Switch' incorporates: |
paulcox | 0:70d27fec6d71 | 1099 | * Constant: '<S17>/Constant' |
paulcox | 0:70d27fec6d71 | 1100 | * DataTypeConversion: '<S19>/DataTypeConv2' |
paulcox | 0:70d27fec6d71 | 1101 | * Logic: '<S19>/AND' |
paulcox | 0:70d27fec6d71 | 1102 | * RelationalOperator: '<S19>/Equal' |
paulcox | 0:70d27fec6d71 | 1103 | * Signum: '<S19>/SignPreIntegrator' |
paulcox | 0:70d27fec6d71 | 1104 | */ |
paulcox | 0:70d27fec6d71 | 1105 | if (NotEqual_b && (rtPrevAction == ((int8_T)electrical_angle))) { |
paulcox | 0:70d27fec6d71 | 1106 | electrical_angle = 0.0F; |
paulcox | 0:70d27fec6d71 | 1107 | } else { |
paulcox | 0:70d27fec6d71 | 1108 | electrical_angle = IntegralGain_j; |
paulcox | 0:70d27fec6d71 | 1109 | } |
paulcox | 0:70d27fec6d71 | 1110 | |
paulcox | 0:70d27fec6d71 | 1111 | /* End of Switch: '<S17>/Switch' */ |
paulcox | 0:70d27fec6d71 | 1112 | |
paulcox | 0:70d27fec6d71 | 1113 | /* DeadZone: '<S20>/DeadZone' */ |
paulcox | 0:70d27fec6d71 | 1114 | if (SignDeltaU_b > 12.0F) { |
paulcox | 0:70d27fec6d71 | 1115 | SignDeltaU_b -= 12.0F; |
paulcox | 0:70d27fec6d71 | 1116 | } else if (SignDeltaU_b >= -12.0F) { |
paulcox | 0:70d27fec6d71 | 1117 | SignDeltaU_b = 0.0F; |
paulcox | 0:70d27fec6d71 | 1118 | } else { |
paulcox | 0:70d27fec6d71 | 1119 | SignDeltaU_b -= -12.0F; |
paulcox | 0:70d27fec6d71 | 1120 | } |
paulcox | 0:70d27fec6d71 | 1121 | |
paulcox | 0:70d27fec6d71 | 1122 | /* End of DeadZone: '<S20>/DeadZone' */ |
paulcox | 0:70d27fec6d71 | 1123 | |
paulcox | 0:70d27fec6d71 | 1124 | /* RelationalOperator: '<S20>/NotEqual' */ |
paulcox | 0:70d27fec6d71 | 1125 | NotEqual_b = (0.0F != SignDeltaU_b); |
paulcox | 0:70d27fec6d71 | 1126 | |
paulcox | 0:70d27fec6d71 | 1127 | /* Signum: '<S20>/SignDeltaU' */ |
paulcox | 0:70d27fec6d71 | 1128 | if (SignDeltaU_b < 0.0F) { |
paulcox | 0:70d27fec6d71 | 1129 | SignDeltaU_b = -1.0F; |
paulcox | 0:70d27fec6d71 | 1130 | } else { |
paulcox | 0:70d27fec6d71 | 1131 | if (SignDeltaU_b > 0.0F) { |
paulcox | 0:70d27fec6d71 | 1132 | SignDeltaU_b = 1.0F; |
paulcox | 0:70d27fec6d71 | 1133 | } |
paulcox | 0:70d27fec6d71 | 1134 | } |
paulcox | 0:70d27fec6d71 | 1135 | |
paulcox | 0:70d27fec6d71 | 1136 | /* End of Signum: '<S20>/SignDeltaU' */ |
paulcox | 0:70d27fec6d71 | 1137 | |
paulcox | 0:70d27fec6d71 | 1138 | /* Gain: '<S18>/Integral Gain' */ |
paulcox | 0:70d27fec6d71 | 1139 | IntegralGain_j = ctrlParams.Current_I * q_current_error; |
paulcox | 0:70d27fec6d71 | 1140 | if (M->Timing.TaskCounters.TID[1] == 0) { |
paulcox | 0:70d27fec6d71 | 1141 | /* RelationalOperator: '<S6>/Relational Operator' incorporates: |
paulcox | 0:70d27fec6d71 | 1142 | * Constant: '<S47>/Constant' |
paulcox | 0:70d27fec6d71 | 1143 | */ |
paulcox | 0:70d27fec6d71 | 1144 | RelationalOperator = (controller_mode == StandBy); |
paulcox | 0:70d27fec6d71 | 1145 | |
paulcox | 0:70d27fec6d71 | 1146 | /* Outputs for Enabled SubSystem: '<S58>/Generate_Error' incorporates: |
paulcox | 0:70d27fec6d71 | 1147 | * EnablePort: '<S60>/Enable' |
paulcox | 0:70d27fec6d71 | 1148 | */ |
paulcox | 0:70d27fec6d71 | 1149 | /* Logic: '<S58>/AND' incorporates: |
paulcox | 0:70d27fec6d71 | 1150 | * Abs: '<S58>/Velocity_Abs' |
paulcox | 0:70d27fec6d71 | 1151 | * Constant: '<S58>/Max_Valid_Velocity_Change' |
paulcox | 0:70d27fec6d71 | 1152 | * Constant: '<S59>/Constant' |
paulcox | 0:70d27fec6d71 | 1153 | * Delay: '<S58>/Velocity_Delay' |
paulcox | 0:70d27fec6d71 | 1154 | * RelationalOperator: '<S58>/Excessive_Velocity_Change' |
paulcox | 0:70d27fec6d71 | 1155 | * RelationalOperator: '<S58>/Relational_Operator' |
paulcox | 0:70d27fec6d71 | 1156 | * Sum: '<S58>/Velocity_Difference' |
paulcox | 0:70d27fec6d71 | 1157 | */ |
paulcox | 0:70d27fec6d71 | 1158 | if ((controller_mode == VelocityControl) && (((real32_T)fabs |
paulcox | 0:70d27fec6d71 | 1159 | (velocity_measured - DWork.Velocity_Delay_DSTATE)) >= 628.318542F)) { |
paulcox | 0:70d27fec6d71 | 1160 | /* DataStoreWrite: '<S60>/Data_Store_Write' incorporates: |
paulcox | 0:70d27fec6d71 | 1161 | * Constant: '<S61>/Constant' |
paulcox | 0:70d27fec6d71 | 1162 | */ |
paulcox | 0:70d27fec6d71 | 1163 | DWork.error_l = MeasuredVelocityError; |
paulcox | 0:70d27fec6d71 | 1164 | } |
paulcox | 0:70d27fec6d71 | 1165 | |
paulcox | 0:70d27fec6d71 | 1166 | /* End of Logic: '<S58>/AND' */ |
paulcox | 0:70d27fec6d71 | 1167 | /* End of Outputs for SubSystem: '<S58>/Generate_Error' */ |
paulcox | 0:70d27fec6d71 | 1168 | } |
paulcox | 0:70d27fec6d71 | 1169 | |
paulcox | 0:70d27fec6d71 | 1170 | /* Outport: '<Root>/error' incorporates: |
paulcox | 0:70d27fec6d71 | 1171 | * DataStoreRead: '<Root>/Data Store Read' |
paulcox | 0:70d27fec6d71 | 1172 | */ |
paulcox | 0:70d27fec6d71 | 1173 | error = DWork.error_l; |
paulcox | 0:70d27fec6d71 | 1174 | |
paulcox | 0:70d27fec6d71 | 1175 | /* Update for RateTransition: '<S6>/Lo_to_Hi_Rate_Transition3' */ |
paulcox | 0:70d27fec6d71 | 1176 | if (M->Timing.TaskCounters.TID[1] == 0) { |
paulcox | 0:70d27fec6d71 | 1177 | DWork.Lo_to_Hi_Rate_Transition3_Buffe = RelationalOperator; |
paulcox | 0:70d27fec6d71 | 1178 | |
paulcox | 0:70d27fec6d71 | 1179 | /* Update for Delay: '<S55>/Position_Delay' */ |
paulcox | 0:70d27fec6d71 | 1180 | DWork.Position_Delay_DSTATE = Wrap_To_Pi; |
paulcox | 0:70d27fec6d71 | 1181 | } |
paulcox | 0:70d27fec6d71 | 1182 | |
paulcox | 0:70d27fec6d71 | 1183 | /* End of Update for RateTransition: '<S6>/Lo_to_Hi_Rate_Transition3' */ |
paulcox | 0:70d27fec6d71 | 1184 | |
paulcox | 0:70d27fec6d71 | 1185 | /* Update for UnitDelay: '<S13>/Delay Input1' */ |
paulcox | 0:70d27fec6d71 | 1186 | DWork.DelayInput1_DSTATE = Enum_To_Int; |
paulcox | 0:70d27fec6d71 | 1187 | |
paulcox | 0:70d27fec6d71 | 1188 | /* Update for DiscreteIntegrator: '<S17>/Integrator' */ |
paulcox | 0:70d27fec6d71 | 1189 | if (FixPtRelationalOperator == 0) { |
paulcox | 0:70d27fec6d71 | 1190 | DWork.Integrator_DSTATE += 4.0E-5F * electrical_angle; |
paulcox | 0:70d27fec6d71 | 1191 | } |
paulcox | 0:70d27fec6d71 | 1192 | |
paulcox | 0:70d27fec6d71 | 1193 | if (FixPtRelationalOperator > 0) { |
paulcox | 0:70d27fec6d71 | 1194 | DWork.Integrator_PrevResetState = 1; |
paulcox | 0:70d27fec6d71 | 1195 | } else { |
paulcox | 0:70d27fec6d71 | 1196 | DWork.Integrator_PrevResetState = 0; |
paulcox | 0:70d27fec6d71 | 1197 | } |
paulcox | 0:70d27fec6d71 | 1198 | |
paulcox | 0:70d27fec6d71 | 1199 | /* End of Update for DiscreteIntegrator: '<S17>/Integrator' */ |
paulcox | 0:70d27fec6d71 | 1200 | |
paulcox | 0:70d27fec6d71 | 1201 | /* Update for DiscreteIntegrator: '<S18>/Integrator' */ |
paulcox | 0:70d27fec6d71 | 1202 | if (FixPtRelationalOperator == 0) { |
paulcox | 0:70d27fec6d71 | 1203 | /* DataTypeConversion: '<S20>/DataTypeConv1' */ |
paulcox | 0:70d27fec6d71 | 1204 | if (SignDeltaU_b < 128.0F) { |
paulcox | 0:70d27fec6d71 | 1205 | rtPrevAction = (int8_T)SignDeltaU_b; |
paulcox | 0:70d27fec6d71 | 1206 | } else { |
paulcox | 0:70d27fec6d71 | 1207 | rtPrevAction = MAX_int8_T; |
paulcox | 0:70d27fec6d71 | 1208 | } |
paulcox | 0:70d27fec6d71 | 1209 | |
paulcox | 0:70d27fec6d71 | 1210 | /* End of DataTypeConversion: '<S20>/DataTypeConv1' */ |
paulcox | 0:70d27fec6d71 | 1211 | |
paulcox | 0:70d27fec6d71 | 1212 | /* Signum: '<S20>/SignPreIntegrator' */ |
paulcox | 0:70d27fec6d71 | 1213 | if (IntegralGain_j < 0.0F) { |
paulcox | 0:70d27fec6d71 | 1214 | electrical_angle = -1.0F; |
paulcox | 0:70d27fec6d71 | 1215 | } else if (IntegralGain_j > 0.0F) { |
paulcox | 0:70d27fec6d71 | 1216 | electrical_angle = 1.0F; |
paulcox | 0:70d27fec6d71 | 1217 | } else { |
paulcox | 0:70d27fec6d71 | 1218 | electrical_angle = IntegralGain_j; |
paulcox | 0:70d27fec6d71 | 1219 | } |
paulcox | 0:70d27fec6d71 | 1220 | |
paulcox | 0:70d27fec6d71 | 1221 | /* Switch: '<S18>/Switch' incorporates: |
paulcox | 0:70d27fec6d71 | 1222 | * Constant: '<S18>/Constant' |
paulcox | 0:70d27fec6d71 | 1223 | * DataTypeConversion: '<S20>/DataTypeConv2' |
paulcox | 0:70d27fec6d71 | 1224 | * Logic: '<S20>/AND' |
paulcox | 0:70d27fec6d71 | 1225 | * RelationalOperator: '<S20>/Equal' |
paulcox | 0:70d27fec6d71 | 1226 | * Signum: '<S20>/SignPreIntegrator' |
paulcox | 0:70d27fec6d71 | 1227 | */ |
paulcox | 0:70d27fec6d71 | 1228 | if (NotEqual_b && (rtPrevAction == ((int8_T)electrical_angle))) { |
paulcox | 0:70d27fec6d71 | 1229 | IntegralGain_j = 0.0F; |
paulcox | 0:70d27fec6d71 | 1230 | } |
paulcox | 0:70d27fec6d71 | 1231 | |
paulcox | 0:70d27fec6d71 | 1232 | /* End of Switch: '<S18>/Switch' */ |
paulcox | 0:70d27fec6d71 | 1233 | DWork.Integrator_DSTATE_l += 4.0E-5F * IntegralGain_j; |
paulcox | 0:70d27fec6d71 | 1234 | } |
paulcox | 0:70d27fec6d71 | 1235 | |
paulcox | 0:70d27fec6d71 | 1236 | if (FixPtRelationalOperator > 0) { |
paulcox | 0:70d27fec6d71 | 1237 | DWork.Integrator_PrevResetState_c = 1; |
paulcox | 0:70d27fec6d71 | 1238 | } else { |
paulcox | 0:70d27fec6d71 | 1239 | DWork.Integrator_PrevResetState_c = 0; |
paulcox | 0:70d27fec6d71 | 1240 | } |
paulcox | 0:70d27fec6d71 | 1241 | |
paulcox | 0:70d27fec6d71 | 1242 | /* End of Update for DiscreteIntegrator: '<S18>/Integrator' */ |
paulcox | 0:70d27fec6d71 | 1243 | if (M->Timing.TaskCounters.TID[1] == 0) { |
paulcox | 0:70d27fec6d71 | 1244 | /* Update for Delay: '<S58>/Velocity_Delay' */ |
paulcox | 0:70d27fec6d71 | 1245 | DWork.Velocity_Delay_DSTATE = velocity_measured; |
paulcox | 0:70d27fec6d71 | 1246 | } |
paulcox | 0:70d27fec6d71 | 1247 | |
paulcox | 0:70d27fec6d71 | 1248 | rate_scheduler(); |
paulcox | 0:70d27fec6d71 | 1249 | return error; |
paulcox | 0:70d27fec6d71 | 1250 | } |
paulcox | 0:70d27fec6d71 | 1251 | |
paulcox | 0:70d27fec6d71 | 1252 | /* Model initialize function */ |
paulcox | 0:70d27fec6d71 | 1253 | extern "C" void Controller_Init(void) |
paulcox | 0:70d27fec6d71 | 1254 | { |
paulcox | 0:70d27fec6d71 | 1255 | /* Registration code */ |
paulcox | 0:70d27fec6d71 | 1256 | |
paulcox | 0:70d27fec6d71 | 1257 | /* initialize real-time model */ |
paulcox | 0:70d27fec6d71 | 1258 | (void) memset((void *)M, 0, |
paulcox | 0:70d27fec6d71 | 1259 | sizeof(RT_MODEL)); |
paulcox | 0:70d27fec6d71 | 1260 | |
paulcox | 0:70d27fec6d71 | 1261 | /* block I/O */ |
paulcox | 0:70d27fec6d71 | 1262 | |
paulcox | 0:70d27fec6d71 | 1263 | /* exported global signals */ |
paulcox | 0:70d27fec6d71 | 1264 | phase_currents[0] = 0.0F; |
paulcox | 0:70d27fec6d71 | 1265 | phase_currents[1] = 0.0F; |
paulcox | 0:70d27fec6d71 | 1266 | rotor_position = 0.0F; |
paulcox | 0:70d27fec6d71 | 1267 | velocity_measured = 0.0F; |
paulcox | 0:70d27fec6d71 | 1268 | d_current_error = 0.0F; |
paulcox | 0:70d27fec6d71 | 1269 | q_current_command = 0.0F; |
paulcox | 0:70d27fec6d71 | 1270 | q_current_measured = 0.0F; |
paulcox | 0:70d27fec6d71 | 1271 | q_current_error = 0.0F; |
paulcox | 0:70d27fec6d71 | 1272 | phase_voltages[0] = 0.0F; |
paulcox | 0:70d27fec6d71 | 1273 | phase_voltages[1] = 0.0F; |
paulcox | 0:70d27fec6d71 | 1274 | phase_voltages[2] = 0.0F; |
paulcox | 0:70d27fec6d71 | 1275 | velocity_error = 0.0F; |
paulcox | 0:70d27fec6d71 | 1276 | controller_mode = StandBy; |
paulcox | 0:70d27fec6d71 | 1277 | |
paulcox | 0:70d27fec6d71 | 1278 | /* states (dwork) */ |
paulcox | 0:70d27fec6d71 | 1279 | (void) memset((void *)&DWork, 0, |
paulcox | 0:70d27fec6d71 | 1280 | sizeof(D_Work)); |
paulcox | 0:70d27fec6d71 | 1281 | |
paulcox | 0:70d27fec6d71 | 1282 | /* InitializeConditions for Enabled SubSystem: '<S51>/Open Loop Position' */ |
paulcox | 0:70d27fec6d71 | 1283 | /* InitializeConditions for DiscreteIntegrator: '<S63>/Integrate_To_Position' */ |
paulcox | 0:70d27fec6d71 | 1284 | DWork.Integrate_To_Position_DSTATE = 0.0F; |
paulcox | 0:70d27fec6d71 | 1285 | |
paulcox | 0:70d27fec6d71 | 1286 | /* InitializeConditions for DiscreteIntegrator: '<S63>/Integrate_To_Velocity' */ |
paulcox | 0:70d27fec6d71 | 1287 | DWork.Integrate_To_Velocity_DSTATE = 0.0F; |
paulcox | 0:70d27fec6d71 | 1288 | |
paulcox | 0:70d27fec6d71 | 1289 | /* End of InitializeConditions for SubSystem: '<S51>/Open Loop Position' */ |
paulcox | 0:70d27fec6d71 | 1290 | |
paulcox | 0:70d27fec6d71 | 1291 | /* Start for SwitchCase: '<S5>/Switch Case' */ |
paulcox | 0:70d27fec6d71 | 1292 | DWork.SwitchCase_ActiveSubsystem = -1; |
paulcox | 0:70d27fec6d71 | 1293 | |
paulcox | 0:70d27fec6d71 | 1294 | /* InitializeConditions for IfAction SubSystem: '<S5>/Velocity_Control' */ |
paulcox | 0:70d27fec6d71 | 1295 | /* InitializeConditions for DiscreteIntegrator: '<S45>/Integrator' */ |
paulcox | 0:70d27fec6d71 | 1296 | DWork.Integrator_DSTATE_f = 0.0F; |
paulcox | 0:70d27fec6d71 | 1297 | |
paulcox | 0:70d27fec6d71 | 1298 | /* End of InitializeConditions for SubSystem: '<S5>/Velocity_Control' */ |
paulcox | 0:70d27fec6d71 | 1299 | |
paulcox | 0:70d27fec6d71 | 1300 | /* InitializeConditions for IfAction SubSystem: '<S5>/Position_Control' */ |
paulcox | 0:70d27fec6d71 | 1301 | /* InitializeConditions for DiscreteIntegrator: '<S42>/Integrator' */ |
paulcox | 0:70d27fec6d71 | 1302 | DWork.Integrator_DSTATE_lc = 0.0F; |
paulcox | 0:70d27fec6d71 | 1303 | |
paulcox | 0:70d27fec6d71 | 1304 | /* End of InitializeConditions for SubSystem: '<S5>/Position_Control' */ |
paulcox | 0:70d27fec6d71 | 1305 | |
paulcox | 0:70d27fec6d71 | 1306 | /* InitializeConditions for Chart: '<S49>/Wait_For_Valid_Position' */ |
paulcox | 0:70d27fec6d71 | 1307 | DWork.temporalCounter_i1 = 0U; |
paulcox | 0:70d27fec6d71 | 1308 | DWork.is_active_c2_rtwdemo_pmsmfoc = 0U; |
paulcox | 0:70d27fec6d71 | 1309 | DWork.is_c2_rtwdemo_pmsmfoc = IN_NO_ACTIVE_CHILD; |
paulcox | 0:70d27fec6d71 | 1310 | DWork.Position_Valid = 0U; |
paulcox | 0:70d27fec6d71 | 1311 | |
paulcox | 0:70d27fec6d71 | 1312 | /* InitializeConditions for Chart: '<S1>/Controller_Mode_Scheduler' */ |
paulcox | 0:70d27fec6d71 | 1313 | DWork.is_Motor_On = IN_NO_ACTIVE_CHILD; |
paulcox | 0:70d27fec6d71 | 1314 | DWork.is_Motor_Control = IN_NO_ACTIVE_CHILD; |
paulcox | 0:70d27fec6d71 | 1315 | DWork.velocity_command = 0.0F; |
paulcox | 0:70d27fec6d71 | 1316 | DWork.position_command = 0.0F; |
paulcox | 0:70d27fec6d71 | 1317 | DWork.torque_command = 0.0F; |
paulcox | 0:70d27fec6d71 | 1318 | |
paulcox | 0:70d27fec6d71 | 1319 | /* Entry: Mode_Scheduler/Controller_Mode_Scheduler */ |
paulcox | 0:70d27fec6d71 | 1320 | /* Entry Internal: Mode_Scheduler/Controller_Mode_Scheduler */ |
paulcox | 0:70d27fec6d71 | 1321 | /* Transition: '<S4>:9' */ |
paulcox | 0:70d27fec6d71 | 1322 | DWork.is_c1_rtwdemo_pmsmfoc = IN_Stand_By; |
paulcox | 0:70d27fec6d71 | 1323 | |
paulcox | 0:70d27fec6d71 | 1324 | /* Entry 'Stand_By': '<S4>:154' */ |
paulcox | 0:70d27fec6d71 | 1325 | controller_mode = StandBy; |
paulcox | 0:70d27fec6d71 | 1326 | |
paulcox | 0:70d27fec6d71 | 1327 | /* InitializeConditions for DiscreteIntegrator: '<S17>/Integrator' */ |
paulcox | 0:70d27fec6d71 | 1328 | DWork.Integrator_PrevResetState = 0; |
paulcox | 0:70d27fec6d71 | 1329 | |
paulcox | 0:70d27fec6d71 | 1330 | /* InitializeConditions for DiscreteIntegrator: '<S18>/Integrator' */ |
paulcox | 0:70d27fec6d71 | 1331 | DWork.Integrator_PrevResetState_c = 0; |
paulcox | 0:70d27fec6d71 | 1332 | } |
paulcox | 0:70d27fec6d71 | 1333 | |
paulcox | 0:70d27fec6d71 | 1334 | /* |
paulcox | 0:70d27fec6d71 | 1335 | * File trailer for generated code. |
paulcox | 0:70d27fec6d71 | 1336 | * |
paulcox | 0:70d27fec6d71 | 1337 | * [EOF] |
paulcox | 0:70d27fec6d71 | 1338 | */ |