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
- Committer:
- paulcox
- Date:
- 2014-10-11
- Revision:
- 0:70d27fec6d71
- Child:
- 2:bbc155b0b886
File content as of revision 0:70d27fec6d71:
/* * File: rtwdemo_pmsmfoc.c * * Code generated for Simulink model 'rtwdemo_pmsmfoc'. * * Model version : 1.2949 * Simulink Coder version : 8.7 (R2014b) 11-Aug-2014 * C/C++ source code generated on : Sat Oct 11 00:06:05 2014 * * Target selection: ert.tlc * Embedded hardware selection: Generic->32-bit Embedded Processor * Code generation objective: Execution efficiency * Validation result: Not run */ #include "rtwdemo_pmsmfoc.h" /* Named constants for Chart: '<S1>/Controller_Mode_Scheduler' */ #define IN_Motor_Control ((uint8_T)1U) #define IN_Motor_On ((uint8_T)1U) #define IN_NO_ACTIVE_CHILD ((uint8_T)0U) #define IN_Position_Control ((uint8_T)1U) #define IN_Ramp_To_Stop ((uint8_T)2U) #define IN_Stand_By ((uint8_T)2U) #define IN_Startup_Open_Loop_Control ((uint8_T)2U) #define IN_Torque_Control ((uint8_T)3U) #define IN_Velocity_Control ((uint8_T)4U) /* Named constants for Chart: '<S49>/Wait_For_Valid_Position' */ #define IN_Position_Valid ((uint8_T)1U) #define IN_Wait_For_Valid_Position ((uint8_T)2U) #define IN_Wait_For_Valid_Velocity ((uint8_T)3U) #define WaitForValidVelocityTicks (124) /* Exported block signals */ real32_T phase_currents[2]; /* '<S48>/Product' */ real32_T rotor_position; /* '<S49>/Add' */ real32_T velocity_measured; /* '<S55>/Scale_Output' */ real32_T d_current_error; /* '<S12>/Sum2' */ real32_T q_current_command; /* '<S5>/Lo_to_Hi_Rate_Transition1' */ real32_T q_current_measured; /* '<S22>/Add' */ real32_T q_current_error; /* '<S12>/Sum' */ real32_T phase_voltages[3]; /* '<S27>/Select_Sector' */ real32_T velocity_error; /* '<S11>/Sum2' */ EnumControllerMode controller_mode; /* '<S1>/Controller_Mode_Scheduler' */ /* Exported block parameters */ CTRLPARAMS_STRUCT ctrlParams = { 10.0F, 10000.0F, 0.005F, 0.015F, 0.1F, 0.6F, 1.0F, 0.2F, 20.0F, 2252.25F, 0.00488400506F, -0.0F, 4.0F } ; /* Variable: ctrlParams * Referenced by: * '<S1>/Controller_Mode_Scheduler' * '<S48>/ADC_Driver_Units_To_Amps' * '<S48>/ADC_Zero_Offset' * '<S49>/Offset_Between_Encoder_Zero_And_Mechanical_Zero' * '<S14>/number_of_pole_pairs' * '<S42>/Integral Gain' * '<S42>/Proportional Gain' * '<S45>/Integral Gain' * '<S45>/Proportional Gain' * '<S63>/Startup_Acceleration_Constant' * '<S17>/Integral Gain' * '<S17>/Proportional Gain' * '<S18>/Integral Gain' * '<S18>/Proportional Gain' */ /* Constant parameters (auto storage) */ const ConstParam ConstP = { /* Computed Parameter: Lookup_Table_table * Referenced by: '<S28>/Lookup_Table' */ { 2U, 2U, 6U, 1U, 4U, 3U, 5U } }; /* Block signals and states (auto storage) */ D_Work DWork; /* Real-time model */ RT_MODEL M_; RT_MODEL *const M = &M_; extern real32_T rt_roundf(real32_T u); extern real32_T rt_modf(real32_T u0, real32_T u1); static void rate_scheduler(void); /* * This function updates active task flag for each subrate. * The function is called at model base rate, hence the * generated code self-manages all its subrates. */ static void rate_scheduler(void) { /* Compute which subrates run during the next base time step. Subrates * are an integer multiple of the base rate counter. Therefore, the subtask * counter is reset when it reaches its limit (zero means run). */ (M->Timing.TaskCounters.TID[1])++; if ((M->Timing.TaskCounters.TID[1]) > 124) {/* Sample time: [0.005s, 0.0s] */ M->Timing.TaskCounters.TID[1] = 0; } } real32_T rt_roundf(real32_T u) { real32_T y; if (((real32_T)fabs(u)) < 8.388608E+6F) { if (u >= 0.5F) { y = (real32_T)floor(u + 0.5F); } else if (u > -0.5F) { y = 0.0F; } else { y = (real32_T)ceil(u - 0.5F); } } else { y = u; } return y; } real32_T rt_modf(real32_T u0, real32_T u1) { real32_T y; real32_T tmp; if (u1 == 0.0F) { y = u0; } else { tmp = u0 / u1; if (u1 <= ((real32_T)floor(u1))) { y = u0 - (((real32_T)floor(tmp)) * u1); } else if (((real32_T)fabs(tmp - rt_roundf(tmp))) <= (FLT_EPSILON * ((real32_T)fabs(tmp)))) { y = 0.0F; } else { y = (tmp - ((real32_T)floor(tmp))) * u1; } } return y; } /* Model step function */ extern "C" EnumErrorType Controller(uint16_T motor_on, EnumCommandType command_type, real32_T current_request, SENSORS_STRUCT *sensors, uint16_T pwm_compare[3]) { real32_T sin_coefficient; int32_T Wrap_To_Pi; real32_T electrical_angle; real32_T cos_coefficient; real32_T SignDeltaU_b; int16_T Enum_To_Int; uint8_T FixPtRelationalOperator; real32_T SignDeltaU; real32_T Gain1; int8_T rtPrevAction; real32_T alpha_voltage; real32_T SignPreIntegrator_f; real32_T IntegralGain_j; boolean_T RelationalOperator; boolean_T NotEqual_b; real32_T Switch_fr; real32_T Sectors_2_and_5_idx_1; real32_T Sectors_2_and_5_idx_2; int16_T u0; /* specified return value */ EnumErrorType error; /* Product: '<S48>/Product' incorporates: * Constant: '<S48>/ADC_Driver_Units_To_Amps' * Constant: '<S48>/ADC_Zero_Offset' * Inport: '<Root>/sensors' * Sum: '<S48>/Add' */ phase_currents[0] = (((real32_T)sensors->adc_phase_currents[0]) - ctrlParams.AdcZeroOffsetDriverUnits) * ctrlParams.AdcDriverUnitsToAmps; phase_currents[1] = (((real32_T)sensors->adc_phase_currents[1]) - ctrlParams.AdcZeroOffsetDriverUnits) * ctrlParams.AdcDriverUnitsToAmps; /* Chart: '<S49>/Wait_For_Valid_Position' incorporates: * Inport: '<Root>/sensors' */ /* Gateway: Motor_Control/Sensors_To_Engineering_Units/Encoder_To_Position/Wait_For_Valid_Position */ if (DWork.temporalCounter_i1 < 127U) { DWork.temporalCounter_i1++; } /* During: Motor_Control/Sensors_To_Engineering_Units/Encoder_To_Position/Wait_For_Valid_Position */ if (DWork.is_active_c2_rtwdemo_pmsmfoc == 0U) { /* Entry: Motor_Control/Sensors_To_Engineering_Units/Encoder_To_Position/Wait_For_Valid_Position */ DWork.is_active_c2_rtwdemo_pmsmfoc = 1U; /* Entry Internal: Motor_Control/Sensors_To_Engineering_Units/Encoder_To_Position/Wait_For_Valid_Position */ /* Transition: '<S52>:113' */ if (sensors->encoder_valid == 0) { /* Transition: '<S52>:114' */ /* Transition: '<S52>:115' */ DWork.is_c2_rtwdemo_pmsmfoc = IN_Wait_For_Valid_Position; } else { /* Transition: '<S52>:120' */ /* Transition: '<S52>:119' */ DWork.is_c2_rtwdemo_pmsmfoc = IN_Position_Valid; DWork.Position_Valid = 1U; } } else { switch (DWork.is_c2_rtwdemo_pmsmfoc) { case IN_Position_Valid: /* During 'Position_Valid': '<S52>:25' */ break; case IN_Wait_For_Valid_Position: /* During 'Wait_For_Valid_Position': '<S52>:99' */ if (sensors->encoder_valid != 0) { /* Transition: '<S52>:100' */ DWork.is_c2_rtwdemo_pmsmfoc = IN_Wait_For_Valid_Velocity; DWork.temporalCounter_i1 = 0U; } break; default: /* During 'Wait_For_Valid_Velocity': '<S52>:101' */ if (DWork.temporalCounter_i1 >= WaitForValidVelocityTicks) { /* Transition: '<S52>:102' */ DWork.is_c2_rtwdemo_pmsmfoc = IN_Position_Valid; DWork.Position_Valid = 1U; } break; } } /* End of Chart: '<S49>/Wait_For_Valid_Position' */ /* Sum: '<S49>/Add' incorporates: * Constant: '<S49>/Offset_Between_Encoder_Zero_And_Mechanical_Zero' * Gain: '<S49>/radians_per_counts' * Inport: '<Root>/sensors' */ rotor_position = (0.000785398181F * ((real32_T)sensors->encoder_counter)) + ctrlParams.EncoderToMechanicalZeroOffsetRads; if (M->Timing.TaskCounters.TID[1] == 0) { /* Gain: '<S55>/Wrap_To_Pi' incorporates: * DataTypeConversion: '<S55>/Convert_to_Uint32' * Gain: '<S55>/Scale_Input' */ Wrap_To_Pi = (((int32_T)(1.70891312E+8F * rotor_position)) << 2); /* Gain: '<S55>/Scale_Output' incorporates: * DataTypeConversion: '<S55>/Difference_to_Single' * Delay: '<S55>/Position_Delay' * Sum: '<S55>/Difference_Wrap' */ velocity_measured = ((real32_T)(Wrap_To_Pi - DWork.Position_Delay_DSTATE)) * 2.92583621E-7F; /* Chart: '<S1>/Controller_Mode_Scheduler' incorporates: * Inport: '<Root>/command_type' * Inport: '<Root>/command_value' * Inport: '<Root>/motor_on' */ /* Gateway: Mode_Scheduler/Controller_Mode_Scheduler */ /* During: Mode_Scheduler/Controller_Mode_Scheduler */ if (DWork.is_c1_rtwdemo_pmsmfoc == IN_Motor_On) { /* During 'Motor_On': '<S4>:338' */ if (DWork.error_l != 0) { /* Transition: '<S4>:339' */ /* Transition: '<S4>:353' */ /* Exit Internal 'Motor_On': '<S4>:338' */ /* Exit Internal 'Motor_Control': '<S4>:344' */ DWork.is_Motor_Control = IN_NO_ACTIVE_CHILD; DWork.is_Motor_On = IN_NO_ACTIVE_CHILD; DWork.is_c1_rtwdemo_pmsmfoc = IN_Stand_By; /* Entry 'Stand_By': '<S4>:154' */ controller_mode = StandBy; } else if (DWork.is_Motor_On == IN_Motor_Control) { /* During 'Motor_Control': '<S4>:344' */ if (!(motor_on != 0)) { /* Transition: '<S4>:282' */ /* Exit Internal 'Motor_Control': '<S4>:344' */ DWork.is_Motor_Control = IN_NO_ACTIVE_CHILD; DWork.is_Motor_On = IN_Ramp_To_Stop; /* Entry 'Ramp_To_Stop': '<S4>:270' */ controller_mode = VelocityControl; DWork.velocity_command = 0.0F; DWork.torque_command = 0.0F; } else { switch (DWork.is_Motor_Control) { case IN_Position_Control: /* During 'Position_Control': '<S4>:226' */ DWork.position_command = current_request; break; case IN_Startup_Open_Loop_Control: /* During 'Startup_Open_Loop_Control': '<S4>:103' */ if (DWork.Position_Valid != 0) { /* Transition: '<S4>:157' */ /* Transition: '<S4>:233' */ switch (command_type) { case Velocity: /* Transition: '<S4>:162' */ DWork.is_Motor_Control = IN_Velocity_Control; /* Entry 'Velocity_Control': '<S4>:108' */ controller_mode = VelocityControl; DWork.velocity_command = current_request; break; case Position: /* Transition: '<S4>:235' */ /* Transition: '<S4>:234' */ DWork.is_Motor_Control = IN_Position_Control; /* Entry 'Position_Control': '<S4>:226' */ controller_mode = PositionControl; DWork.position_command = current_request; break; default: /* Transition: '<S4>:237' */ /* Transition: '<S4>:158' */ /* [command_type==Torque] */ DWork.is_Motor_Control = IN_Torque_Control; /* Entry 'Torque_Control': '<S4>:220' */ controller_mode = TorqueControl; DWork.torque_command = current_request; break; } } break; case IN_Torque_Control: /* During 'Torque_Control': '<S4>:220' */ DWork.torque_command = current_request; break; default: /* During 'Velocity_Control': '<S4>:108' */ DWork.velocity_command = current_request; break; } } } else { /* During 'Ramp_To_Stop': '<S4>:270' */ if (((real32_T)fabs(velocity_measured)) < ctrlParams.RampToStopVelocity) { /* Transition: '<S4>:169' */ /* Transition: '<S4>:355' */ DWork.is_Motor_On = IN_NO_ACTIVE_CHILD; DWork.is_c1_rtwdemo_pmsmfoc = IN_Stand_By; /* Entry 'Stand_By': '<S4>:154' */ controller_mode = StandBy; } } } else { /* During 'Stand_By': '<S4>:154' */ if ((motor_on != 0) && (!(DWork.error_l != 0))) { /* Transition: '<S4>:164' */ if (!(DWork.Position_Valid != 0)) { /* Transition: '<S4>:133' */ DWork.is_c1_rtwdemo_pmsmfoc = IN_Motor_On; DWork.is_Motor_On = IN_Motor_Control; DWork.is_Motor_Control = IN_Startup_Open_Loop_Control; /* Entry 'Startup_Open_Loop_Control': '<S4>:103' */ controller_mode = Startup; DWork.torque_command = ctrlParams.StartupCurrent; } else { /* Transition: '<S4>:137' */ /* Transition: '<S4>:233' */ switch (command_type) { case Velocity: /* Transition: '<S4>:162' */ DWork.is_c1_rtwdemo_pmsmfoc = IN_Motor_On; DWork.is_Motor_On = IN_Motor_Control; DWork.is_Motor_Control = IN_Velocity_Control; /* Entry 'Velocity_Control': '<S4>:108' */ controller_mode = VelocityControl; DWork.velocity_command = current_request; break; case Position: /* Transition: '<S4>:235' */ /* Transition: '<S4>:234' */ DWork.is_c1_rtwdemo_pmsmfoc = IN_Motor_On; DWork.is_Motor_On = IN_Motor_Control; DWork.is_Motor_Control = IN_Position_Control; /* Entry 'Position_Control': '<S4>:226' */ controller_mode = PositionControl; DWork.position_command = current_request; break; default: /* Transition: '<S4>:237' */ /* Transition: '<S4>:158' */ /* [command_type==Torque] */ DWork.is_c1_rtwdemo_pmsmfoc = IN_Motor_On; DWork.is_Motor_On = IN_Motor_Control; DWork.is_Motor_Control = IN_Torque_Control; /* Entry 'Torque_Control': '<S4>:220' */ controller_mode = TorqueControl; DWork.torque_command = current_request; break; } } } } /* End of Chart: '<S1>/Controller_Mode_Scheduler' */ /* RelationalOperator: '<S51>/Relational Operator' incorporates: * Constant: '<S62>/Constant' */ DWork.RelationalOperator_a = (controller_mode == Startup); } /* Outputs for Enabled SubSystem: '<S51>/Open Loop Position' incorporates: * EnablePort: '<S63>/Enable' */ if (DWork.RelationalOperator_a) { if (!DWork.OpenLoopPosition_MODE) { /* InitializeConditions for DiscreteIntegrator: '<S63>/Integrate_To_Position' */ DWork.Integrate_To_Position_DSTATE = 0.0F; /* InitializeConditions for DiscreteIntegrator: '<S63>/Integrate_To_Velocity' */ DWork.Integrate_To_Velocity_DSTATE = 0.0F; DWork.OpenLoopPosition_MODE = true; } /* DiscreteIntegrator: '<S63>/Integrate_To_Position' */ DWork.position = DWork.Integrate_To_Position_DSTATE; /* Update for DiscreteIntegrator: '<S63>/Integrate_To_Position' incorporates: * DiscreteIntegrator: '<S63>/Integrate_To_Velocity' */ DWork.Integrate_To_Position_DSTATE += 4.0E-5F * DWork.Integrate_To_Velocity_DSTATE; /* Update for DiscreteIntegrator: '<S63>/Integrate_To_Velocity' incorporates: * Constant: '<S63>/Startup_Acceleration_Constant' */ DWork.Integrate_To_Velocity_DSTATE += 4.0E-5F * ctrlParams.StartupAcceleration; } else { if (DWork.OpenLoopPosition_MODE) { DWork.OpenLoopPosition_MODE = false; } } /* End of Outputs for SubSystem: '<S51>/Open Loop Position' */ /* Switch: '<S51>/Switch' */ if (DWork.RelationalOperator_a) { Switch_fr = DWork.position; } else { Switch_fr = rotor_position; } /* End of Switch: '<S51>/Switch' */ /* Gain: '<S14>/number_of_pole_pairs' */ electrical_angle = ctrlParams.PmsmPolePairs * Switch_fr; /* Trigonometry: '<S14>/sine_cosine' */ sin_coefficient = (real32_T)sin(electrical_angle); cos_coefficient = (real32_T)cos(electrical_angle); /* Gain: '<S21>/Beta_Gain' incorporates: * Gain: '<S21>/B_Gain' * Sum: '<S21>/Add' */ SignDeltaU_b = ((2.0F * phase_currents[1]) + phase_currents[0]) * 0.577350259F; /* Sum: '<S12>/Sum2' incorporates: * Constant: '<S12>/d_current_command (A)' * Product: '<S22>/Product2' * Product: '<S22>/Product3' * Sum: '<S22>/Add1' */ d_current_error = 0.0F - ((phase_currents[0] * cos_coefficient) + (SignDeltaU_b * sin_coefficient)); /* DataTypeConversion: '<S8>/Enum_To_Int' */ Enum_To_Int = (int16_T)controller_mode; /* RelationalOperator: '<S13>/FixPt Relational Operator' incorporates: * UnitDelay: '<S13>/Delay Input1' */ FixPtRelationalOperator = (uint8_T)(Enum_To_Int != DWork.DelayInput1_DSTATE); /* DiscreteIntegrator: '<S17>/Integrator' */ if ((FixPtRelationalOperator != 0) || (DWork.Integrator_PrevResetState != 0)) { DWork.Integrator_DSTATE = 0.0F; } /* Sum: '<S17>/Sum' incorporates: * DiscreteIntegrator: '<S17>/Integrator' * Gain: '<S17>/Proportional Gain' */ SignDeltaU = (ctrlParams.Current_P * d_current_error) + DWork.Integrator_DSTATE; /* Saturate: '<S17>/Saturate' */ if (SignDeltaU > 12.0F) { Gain1 = 12.0F; } else if (SignDeltaU < -12.0F) { Gain1 = -12.0F; } else { Gain1 = SignDeltaU; } /* End of Saturate: '<S17>/Saturate' */ if (M->Timing.TaskCounters.TID[1] == 0) { /* SwitchCase: '<S5>/Switch Case' incorporates: * Inport: '<S10>/torque_command' */ rtPrevAction = DWork.SwitchCase_ActiveSubsystem; switch (controller_mode) { case VelocityControl: DWork.SwitchCase_ActiveSubsystem = 0; break; case PositionControl: DWork.SwitchCase_ActiveSubsystem = 1; break; default: DWork.SwitchCase_ActiveSubsystem = 2; break; } switch (DWork.SwitchCase_ActiveSubsystem) { case 0: if (DWork.SwitchCase_ActiveSubsystem != rtPrevAction) { /* InitializeConditions for IfAction SubSystem: '<S5>/Velocity_Control' incorporates: * InitializeConditions for ActionPort: '<S11>/Action Port' */ /* InitializeConditions for SwitchCase: '<S5>/Switch Case' incorporates: * InitializeConditions for DiscreteIntegrator: '<S45>/Integrator' */ DWork.Integrator_DSTATE_f = 0.0F; /* End of InitializeConditions for SubSystem: '<S5>/Velocity_Control' */ } /* Outputs for IfAction SubSystem: '<S5>/Velocity_Control' incorporates: * ActionPort: '<S11>/Action Port' */ /* Sum: '<S11>/Sum2' */ velocity_error = DWork.velocity_command - velocity_measured; /* Sum: '<S45>/Sum' incorporates: * DiscreteIntegrator: '<S45>/Integrator' * Gain: '<S45>/Proportional Gain' */ electrical_angle = (ctrlParams.Velocity_P * velocity_error) + DWork.Integrator_DSTATE_f; /* Saturate: '<S45>/Saturate' */ if (electrical_angle > 2.0F) { /* SignalConversion: '<S11>/Isolate_For_Merge' */ DWork.Merge = 2.0F; } else if (electrical_angle < -2.0F) { /* SignalConversion: '<S11>/Isolate_For_Merge' */ DWork.Merge = -2.0F; } else { /* SignalConversion: '<S11>/Isolate_For_Merge' */ DWork.Merge = electrical_angle; } /* End of Saturate: '<S45>/Saturate' */ /* DeadZone: '<S46>/DeadZone' */ if (electrical_angle > 2.0F) { electrical_angle -= 2.0F; } else if (electrical_angle >= -2.0F) { electrical_angle = 0.0F; } else { electrical_angle -= -2.0F; } /* End of DeadZone: '<S46>/DeadZone' */ /* RelationalOperator: '<S46>/NotEqual' */ NotEqual_b = (0.0F != electrical_angle); /* Signum: '<S46>/SignDeltaU' */ if (electrical_angle < 0.0F) { electrical_angle = -1.0F; } else { if (electrical_angle > 0.0F) { electrical_angle = 1.0F; } } /* End of Signum: '<S46>/SignDeltaU' */ /* Gain: '<S45>/Integral Gain' */ Switch_fr = ctrlParams.Velocity_I * velocity_error; /* DataTypeConversion: '<S46>/DataTypeConv1' */ if (electrical_angle < 128.0F) { rtPrevAction = (int8_T)electrical_angle; } else { rtPrevAction = MAX_int8_T; } /* End of DataTypeConversion: '<S46>/DataTypeConv1' */ /* Signum: '<S46>/SignPreIntegrator' */ if (Switch_fr < 0.0F) { electrical_angle = -1.0F; } else if (Switch_fr > 0.0F) { electrical_angle = 1.0F; } else { electrical_angle = Switch_fr; } /* Switch: '<S45>/Switch' incorporates: * Constant: '<S45>/Constant' * DataTypeConversion: '<S46>/DataTypeConv2' * Logic: '<S46>/AND' * RelationalOperator: '<S46>/Equal' * Signum: '<S46>/SignPreIntegrator' */ if (NotEqual_b && (rtPrevAction == ((int8_T)electrical_angle))) { Switch_fr = 0.0F; } /* End of Switch: '<S45>/Switch' */ /* Update for DiscreteIntegrator: '<S45>/Integrator' */ DWork.Integrator_DSTATE_f += 0.005F * Switch_fr; /* End of Outputs for SubSystem: '<S5>/Velocity_Control' */ break; case 1: if (DWork.SwitchCase_ActiveSubsystem != rtPrevAction) { /* InitializeConditions for IfAction SubSystem: '<S5>/Position_Control' incorporates: * InitializeConditions for ActionPort: '<S9>/Action Port' */ /* InitializeConditions for SwitchCase: '<S5>/Switch Case' incorporates: * InitializeConditions for DiscreteIntegrator: '<S42>/Integrator' */ DWork.Integrator_DSTATE_lc = 0.0F; /* End of InitializeConditions for SubSystem: '<S5>/Position_Control' */ } /* Outputs for IfAction SubSystem: '<S5>/Position_Control' incorporates: * ActionPort: '<S9>/Action Port' */ /* Sum: '<S9>/Sum2' */ Switch_fr = DWork.position_command - Switch_fr; /* Switch: '<S43>/Select_Angle' incorporates: * Constant: '<S43>/Neg_Pi_Constant' * Constant: '<S43>/Pi_Constant_1' * Constant: '<S43>/Pi_Constant_2' * Constant: '<S43>/Pi_Constant_3' * Constant: '<S43>/Two_Pi_Constant' * Logic: '<S43>/OR' * Math: '<S43>/Modulus' * RelationalOperator: '<S43>/Greater_Than' * RelationalOperator: '<S43>/Less_Than' * Sum: '<S43>/Add' * Sum: '<S43>/Subtract' */ if ((Switch_fr < -1.57079637F) || (Switch_fr >= 1.57079637F)) { Switch_fr = rt_modf(Switch_fr + 1.57079637F, 3.14159274F) - 1.57079637F; } /* End of Switch: '<S43>/Select_Angle' */ /* Sum: '<S42>/Sum' incorporates: * DiscreteIntegrator: '<S42>/Integrator' * Gain: '<S42>/Proportional Gain' */ electrical_angle = (ctrlParams.Position_P * Switch_fr) + DWork.Integrator_DSTATE_lc; /* Saturate: '<S42>/Saturate' */ if (electrical_angle > 2.0F) { /* SignalConversion: '<S9>/Isolate_For_Merge' */ DWork.Merge = 2.0F; } else if (electrical_angle < -2.0F) { /* SignalConversion: '<S9>/Isolate_For_Merge' */ DWork.Merge = -2.0F; } else { /* SignalConversion: '<S9>/Isolate_For_Merge' */ DWork.Merge = electrical_angle; } /* End of Saturate: '<S42>/Saturate' */ /* DeadZone: '<S44>/DeadZone' */ if (electrical_angle > 2.0F) { electrical_angle -= 2.0F; } else if (electrical_angle >= -2.0F) { electrical_angle = 0.0F; } else { electrical_angle -= -2.0F; } /* End of DeadZone: '<S44>/DeadZone' */ /* RelationalOperator: '<S44>/NotEqual' */ NotEqual_b = (0.0F != electrical_angle); /* Signum: '<S44>/SignDeltaU' */ if (electrical_angle < 0.0F) { electrical_angle = -1.0F; } else { if (electrical_angle > 0.0F) { electrical_angle = 1.0F; } } /* End of Signum: '<S44>/SignDeltaU' */ /* Gain: '<S42>/Integral Gain' */ Switch_fr *= ctrlParams.Position_I; /* DataTypeConversion: '<S44>/DataTypeConv1' */ if (electrical_angle < 128.0F) { rtPrevAction = (int8_T)electrical_angle; } else { rtPrevAction = MAX_int8_T; } /* End of DataTypeConversion: '<S44>/DataTypeConv1' */ /* Signum: '<S44>/SignPreIntegrator' */ if (Switch_fr < 0.0F) { electrical_angle = -1.0F; } else if (Switch_fr > 0.0F) { electrical_angle = 1.0F; } else { electrical_angle = Switch_fr; } /* Switch: '<S42>/Switch' incorporates: * Constant: '<S42>/Constant' * DataTypeConversion: '<S44>/DataTypeConv2' * Logic: '<S44>/AND' * RelationalOperator: '<S44>/Equal' * Signum: '<S44>/SignPreIntegrator' */ if (NotEqual_b && (rtPrevAction == ((int8_T)electrical_angle))) { Switch_fr = 0.0F; } /* End of Switch: '<S42>/Switch' */ /* Update for DiscreteIntegrator: '<S42>/Integrator' */ DWork.Integrator_DSTATE_lc += 0.005F * Switch_fr; /* End of Outputs for SubSystem: '<S5>/Position_Control' */ break; case 2: /* Outputs for IfAction SubSystem: '<S5>/Torque_Control' incorporates: * ActionPort: '<S10>/Action Port' */ DWork.Merge = DWork.torque_command; /* End of Outputs for SubSystem: '<S5>/Torque_Control' */ break; } /* End of SwitchCase: '<S5>/Switch Case' */ } /* RateTransition: '<S5>/Lo_to_Hi_Rate_Transition1' */ q_current_command = DWork.Merge; /* Sum: '<S22>/Add' incorporates: * Product: '<S22>/Product' * Product: '<S22>/Product1' */ q_current_measured = (SignDeltaU_b * cos_coefficient) - (phase_currents[0] * sin_coefficient); /* Sum: '<S12>/Sum' */ q_current_error = q_current_command - q_current_measured; /* DiscreteIntegrator: '<S18>/Integrator' */ if ((FixPtRelationalOperator != 0) || (DWork.Integrator_PrevResetState_c != 0)) { DWork.Integrator_DSTATE_l = 0.0F; } /* Sum: '<S18>/Sum' incorporates: * DiscreteIntegrator: '<S18>/Integrator' * Gain: '<S18>/Proportional Gain' */ SignDeltaU_b = (ctrlParams.Current_P * q_current_error) + DWork.Integrator_DSTATE_l; /* Saturate: '<S18>/Saturate' */ if (SignDeltaU_b > 12.0F) { alpha_voltage = 12.0F; } else if (SignDeltaU_b < -12.0F) { alpha_voltage = -12.0F; } else { alpha_voltage = SignDeltaU_b; } /* End of Saturate: '<S18>/Saturate' */ /* Sum: '<S24>/Add' incorporates: * Product: '<S24>/Product' * Product: '<S24>/Product1' */ SignPreIntegrator_f = (Gain1 * sin_coefficient) + (alpha_voltage * cos_coefficient); /* Gain: '<S29>/Gain' */ IntegralGain_j = 0.5F * SignPreIntegrator_f; /* Sum: '<S24>/Add1' incorporates: * Product: '<S24>/Product2' * Product: '<S24>/Product3' */ alpha_voltage = (Gain1 * cos_coefficient) - (alpha_voltage * sin_coefficient); /* Gain: '<S29>/Gain1' */ Gain1 = 0.866025388F * alpha_voltage; /* Gain: '<S30>/Space_Vector_Gain' incorporates: * Gain: '<S30>/Alpha_Gain' * Sum: '<S30>/Add' */ electrical_angle = (((1.73205078F * alpha_voltage) + 33.941124F) + SignPreIntegrator_f) * 0.353553385F; /* Gain: '<S33>/Va_Gain' incorporates: * Gain: '<S33>/Alpha_Gain' * Gain: '<S33>/Beta_Gain' * Sum: '<S33>/Add' */ Switch_fr = ((33.941124F - (1.73205078F * alpha_voltage)) + (3.0F * SignPreIntegrator_f)) * 0.353553385F; /* Gain: '<S36>/Space_Vector_Gain' incorporates: * Gain: '<S36>/Alpha_Gain' * Sum: '<S36>/Add' */ cos_coefficient = ((33.941124F - (1.73205078F * alpha_voltage)) - SignPreIntegrator_f) * 0.353553385F; /* Gain: '<S31>/Space_Vector_Gain' incorporates: * Constant: '<S27>/Bus_Voltage' * Gain: '<S31>/Alpha_Gain' * Sum: '<S31>/Add' */ sin_coefficient = ((2.44948983F * alpha_voltage) + 24.0F) * 0.5F; /* Gain: '<S34>/Va_Gain' incorporates: * Constant: '<S27>/Bus_Voltage' * Gain: '<S34>/Beta_Gain' * Sum: '<S34>/Add' */ Sectors_2_and_5_idx_1 = ((1.41421354F * SignPreIntegrator_f) + 24.0F) * 0.5F; /* Gain: '<S37>/Space_Vector_Gain' incorporates: * Constant: '<S27>/Bus_Voltage' * Gain: '<S37>/Beta_Gain' * Sum: '<S37>/Add' */ Sectors_2_and_5_idx_2 = (24.0F - (1.41421354F * SignPreIntegrator_f)) * 0.5F; /* Gain: '<S32>/Space_Vector_Gain' incorporates: * Gain: '<S32>/Alpha_Gain' * Sum: '<S32>/Add' */ phase_voltages[0] = (((1.73205078F * alpha_voltage) + 33.941124F) - SignPreIntegrator_f) * 0.353553385F; /* Gain: '<S35>/Va_Gain' incorporates: * Gain: '<S35>/Alpha_Gain' * Sum: '<S35>/Add' */ phase_voltages[1] = ((33.941124F - (1.73205078F * alpha_voltage)) + SignPreIntegrator_f) * 0.353553385F; /* Gain: '<S38>/Space_Vector_Gain' incorporates: * Gain: '<S38>/Alpha_Gain' * Gain: '<S38>/Beta_Gain' * Sum: '<S38>/Add' */ phase_voltages[2] = ((33.941124F - (1.73205078F * alpha_voltage)) - (3.0F * SignPreIntegrator_f)) * 0.353553385F; /* LookupNDDirect: '<S28>/Lookup_Table' incorporates: * Constant: '<S39>/Constant' * Constant: '<S40>/Constant' * Constant: '<S41>/Constant' * Gain: '<S28>/Sector_Gain_VB' * Gain: '<S28>/Sector_Gain_VC' * RelationalOperator: '<S39>/Compare' * RelationalOperator: '<S40>/Compare' * RelationalOperator: '<S41>/Compare' * Sum: '<S28>/Calculate_Phase_Advanced_Sector' * Sum: '<S29>/Add' * Sum: '<S29>/Add1' * * About '<S28>/Lookup_Table': * 1-dimensional Direct Look-Up returning a Scalar */ u0 = (int16_T)(((((Gain1 - IntegralGain_j) > 0.0F) << 1) + (SignPreIntegrator_f > 0.0F)) + ((((0.0F - IntegralGain_j) - Gain1) > 0.0F) << 2)); if (u0 > 6) { u0 = 6; } /* MultiPortSwitch: '<S27>/Select_Sector' incorporates: * LookupNDDirect: '<S28>/Lookup_Table' * * About '<S28>/Lookup_Table': * 1-dimensional Direct Look-Up returning a Scalar */ switch (ConstP.Lookup_Table_table[u0]) { case 1: phase_voltages[0] = electrical_angle; phase_voltages[1] = Switch_fr; phase_voltages[2] = cos_coefficient; break; case 2: phase_voltages[0] = sin_coefficient; phase_voltages[1] = Sectors_2_and_5_idx_1; phase_voltages[2] = Sectors_2_and_5_idx_2; break; case 3: break; case 4: phase_voltages[0] = electrical_angle; phase_voltages[1] = Switch_fr; phase_voltages[2] = cos_coefficient; break; case 5: phase_voltages[0] = sin_coefficient; phase_voltages[1] = Sectors_2_and_5_idx_1; phase_voltages[2] = Sectors_2_and_5_idx_2; break; } /* End of MultiPortSwitch: '<S27>/Select_Sector' */ /* Switch: '<S6>/Switch' */ if (DWork.Lo_to_Hi_Rate_Transition3_Buffe) { /* Outport: '<Root>/pwm_compare' */ pwm_compare[0] = 1500U; pwm_compare[1] = 1500U; pwm_compare[2] = 1500U; } else { /* Gain: '<S6>/Voltage to PWM Compare Units' */ electrical_angle = 125.0F * phase_voltages[0]; /* Saturate: '<S6>/Saturation' */ if (electrical_angle > 2999.0F) { /* Outport: '<Root>/pwm_compare' incorporates: * DataTypeConversion: '<S6>/Quantize' */ pwm_compare[0] = 2999U; } else if (electrical_angle < 0.0F) { /* Outport: '<Root>/pwm_compare' incorporates: * DataTypeConversion: '<S6>/Quantize' */ pwm_compare[0] = 0U; } else { /* Outport: '<Root>/pwm_compare' incorporates: * DataTypeConversion: '<S6>/Quantize' */ pwm_compare[0] = (uint16_T)electrical_angle; } /* Gain: '<S6>/Voltage to PWM Compare Units' */ electrical_angle = 125.0F * phase_voltages[1]; /* Saturate: '<S6>/Saturation' */ if (electrical_angle > 2999.0F) { /* Outport: '<Root>/pwm_compare' incorporates: * DataTypeConversion: '<S6>/Quantize' */ pwm_compare[1] = 2999U; } else if (electrical_angle < 0.0F) { /* Outport: '<Root>/pwm_compare' incorporates: * DataTypeConversion: '<S6>/Quantize' */ pwm_compare[1] = 0U; } else { /* Outport: '<Root>/pwm_compare' incorporates: * DataTypeConversion: '<S6>/Quantize' */ pwm_compare[1] = (uint16_T)electrical_angle; } /* Gain: '<S6>/Voltage to PWM Compare Units' */ electrical_angle = 125.0F * phase_voltages[2]; /* Saturate: '<S6>/Saturation' */ if (electrical_angle > 2999.0F) { /* Outport: '<Root>/pwm_compare' incorporates: * DataTypeConversion: '<S6>/Quantize' */ pwm_compare[2] = 2999U; } else if (electrical_angle < 0.0F) { /* Outport: '<Root>/pwm_compare' incorporates: * DataTypeConversion: '<S6>/Quantize' */ pwm_compare[2] = 0U; } else { /* Outport: '<Root>/pwm_compare' incorporates: * DataTypeConversion: '<S6>/Quantize' */ pwm_compare[2] = (uint16_T)electrical_angle; } } /* End of Switch: '<S6>/Switch' */ /* DeadZone: '<S19>/DeadZone' */ if (SignDeltaU > 12.0F) { SignDeltaU -= 12.0F; } else if (SignDeltaU >= -12.0F) { SignDeltaU = 0.0F; } else { SignDeltaU -= -12.0F; } /* End of DeadZone: '<S19>/DeadZone' */ /* RelationalOperator: '<S19>/NotEqual' */ NotEqual_b = (0.0F != SignDeltaU); /* Signum: '<S19>/SignDeltaU' */ if (SignDeltaU < 0.0F) { SignDeltaU = -1.0F; } else { if (SignDeltaU > 0.0F) { SignDeltaU = 1.0F; } } /* End of Signum: '<S19>/SignDeltaU' */ /* Gain: '<S17>/Integral Gain' */ IntegralGain_j = ctrlParams.Current_I * d_current_error; /* DataTypeConversion: '<S19>/DataTypeConv1' */ if (SignDeltaU < 128.0F) { rtPrevAction = (int8_T)SignDeltaU; } else { rtPrevAction = MAX_int8_T; } /* End of DataTypeConversion: '<S19>/DataTypeConv1' */ /* Signum: '<S19>/SignPreIntegrator' */ if (IntegralGain_j < 0.0F) { electrical_angle = -1.0F; } else if (IntegralGain_j > 0.0F) { electrical_angle = 1.0F; } else { electrical_angle = IntegralGain_j; } /* Switch: '<S17>/Switch' incorporates: * Constant: '<S17>/Constant' * DataTypeConversion: '<S19>/DataTypeConv2' * Logic: '<S19>/AND' * RelationalOperator: '<S19>/Equal' * Signum: '<S19>/SignPreIntegrator' */ if (NotEqual_b && (rtPrevAction == ((int8_T)electrical_angle))) { electrical_angle = 0.0F; } else { electrical_angle = IntegralGain_j; } /* End of Switch: '<S17>/Switch' */ /* DeadZone: '<S20>/DeadZone' */ if (SignDeltaU_b > 12.0F) { SignDeltaU_b -= 12.0F; } else if (SignDeltaU_b >= -12.0F) { SignDeltaU_b = 0.0F; } else { SignDeltaU_b -= -12.0F; } /* End of DeadZone: '<S20>/DeadZone' */ /* RelationalOperator: '<S20>/NotEqual' */ NotEqual_b = (0.0F != SignDeltaU_b); /* Signum: '<S20>/SignDeltaU' */ if (SignDeltaU_b < 0.0F) { SignDeltaU_b = -1.0F; } else { if (SignDeltaU_b > 0.0F) { SignDeltaU_b = 1.0F; } } /* End of Signum: '<S20>/SignDeltaU' */ /* Gain: '<S18>/Integral Gain' */ IntegralGain_j = ctrlParams.Current_I * q_current_error; if (M->Timing.TaskCounters.TID[1] == 0) { /* RelationalOperator: '<S6>/Relational Operator' incorporates: * Constant: '<S47>/Constant' */ RelationalOperator = (controller_mode == StandBy); /* Outputs for Enabled SubSystem: '<S58>/Generate_Error' incorporates: * EnablePort: '<S60>/Enable' */ /* Logic: '<S58>/AND' incorporates: * Abs: '<S58>/Velocity_Abs' * Constant: '<S58>/Max_Valid_Velocity_Change' * Constant: '<S59>/Constant' * Delay: '<S58>/Velocity_Delay' * RelationalOperator: '<S58>/Excessive_Velocity_Change' * RelationalOperator: '<S58>/Relational_Operator' * Sum: '<S58>/Velocity_Difference' */ if ((controller_mode == VelocityControl) && (((real32_T)fabs (velocity_measured - DWork.Velocity_Delay_DSTATE)) >= 628.318542F)) { /* DataStoreWrite: '<S60>/Data_Store_Write' incorporates: * Constant: '<S61>/Constant' */ DWork.error_l = MeasuredVelocityError; } /* End of Logic: '<S58>/AND' */ /* End of Outputs for SubSystem: '<S58>/Generate_Error' */ } /* Outport: '<Root>/error' incorporates: * DataStoreRead: '<Root>/Data Store Read' */ error = DWork.error_l; /* Update for RateTransition: '<S6>/Lo_to_Hi_Rate_Transition3' */ if (M->Timing.TaskCounters.TID[1] == 0) { DWork.Lo_to_Hi_Rate_Transition3_Buffe = RelationalOperator; /* Update for Delay: '<S55>/Position_Delay' */ DWork.Position_Delay_DSTATE = Wrap_To_Pi; } /* End of Update for RateTransition: '<S6>/Lo_to_Hi_Rate_Transition3' */ /* Update for UnitDelay: '<S13>/Delay Input1' */ DWork.DelayInput1_DSTATE = Enum_To_Int; /* Update for DiscreteIntegrator: '<S17>/Integrator' */ if (FixPtRelationalOperator == 0) { DWork.Integrator_DSTATE += 4.0E-5F * electrical_angle; } if (FixPtRelationalOperator > 0) { DWork.Integrator_PrevResetState = 1; } else { DWork.Integrator_PrevResetState = 0; } /* End of Update for DiscreteIntegrator: '<S17>/Integrator' */ /* Update for DiscreteIntegrator: '<S18>/Integrator' */ if (FixPtRelationalOperator == 0) { /* DataTypeConversion: '<S20>/DataTypeConv1' */ if (SignDeltaU_b < 128.0F) { rtPrevAction = (int8_T)SignDeltaU_b; } else { rtPrevAction = MAX_int8_T; } /* End of DataTypeConversion: '<S20>/DataTypeConv1' */ /* Signum: '<S20>/SignPreIntegrator' */ if (IntegralGain_j < 0.0F) { electrical_angle = -1.0F; } else if (IntegralGain_j > 0.0F) { electrical_angle = 1.0F; } else { electrical_angle = IntegralGain_j; } /* Switch: '<S18>/Switch' incorporates: * Constant: '<S18>/Constant' * DataTypeConversion: '<S20>/DataTypeConv2' * Logic: '<S20>/AND' * RelationalOperator: '<S20>/Equal' * Signum: '<S20>/SignPreIntegrator' */ if (NotEqual_b && (rtPrevAction == ((int8_T)electrical_angle))) { IntegralGain_j = 0.0F; } /* End of Switch: '<S18>/Switch' */ DWork.Integrator_DSTATE_l += 4.0E-5F * IntegralGain_j; } if (FixPtRelationalOperator > 0) { DWork.Integrator_PrevResetState_c = 1; } else { DWork.Integrator_PrevResetState_c = 0; } /* End of Update for DiscreteIntegrator: '<S18>/Integrator' */ if (M->Timing.TaskCounters.TID[1] == 0) { /* Update for Delay: '<S58>/Velocity_Delay' */ DWork.Velocity_Delay_DSTATE = velocity_measured; } rate_scheduler(); return error; } /* Model initialize function */ extern "C" void Controller_Init(void) { /* Registration code */ /* initialize real-time model */ (void) memset((void *)M, 0, sizeof(RT_MODEL)); /* block I/O */ /* exported global signals */ phase_currents[0] = 0.0F; phase_currents[1] = 0.0F; rotor_position = 0.0F; velocity_measured = 0.0F; d_current_error = 0.0F; q_current_command = 0.0F; q_current_measured = 0.0F; q_current_error = 0.0F; phase_voltages[0] = 0.0F; phase_voltages[1] = 0.0F; phase_voltages[2] = 0.0F; velocity_error = 0.0F; controller_mode = StandBy; /* states (dwork) */ (void) memset((void *)&DWork, 0, sizeof(D_Work)); /* InitializeConditions for Enabled SubSystem: '<S51>/Open Loop Position' */ /* InitializeConditions for DiscreteIntegrator: '<S63>/Integrate_To_Position' */ DWork.Integrate_To_Position_DSTATE = 0.0F; /* InitializeConditions for DiscreteIntegrator: '<S63>/Integrate_To_Velocity' */ DWork.Integrate_To_Velocity_DSTATE = 0.0F; /* End of InitializeConditions for SubSystem: '<S51>/Open Loop Position' */ /* Start for SwitchCase: '<S5>/Switch Case' */ DWork.SwitchCase_ActiveSubsystem = -1; /* InitializeConditions for IfAction SubSystem: '<S5>/Velocity_Control' */ /* InitializeConditions for DiscreteIntegrator: '<S45>/Integrator' */ DWork.Integrator_DSTATE_f = 0.0F; /* End of InitializeConditions for SubSystem: '<S5>/Velocity_Control' */ /* InitializeConditions for IfAction SubSystem: '<S5>/Position_Control' */ /* InitializeConditions for DiscreteIntegrator: '<S42>/Integrator' */ DWork.Integrator_DSTATE_lc = 0.0F; /* End of InitializeConditions for SubSystem: '<S5>/Position_Control' */ /* InitializeConditions for Chart: '<S49>/Wait_For_Valid_Position' */ DWork.temporalCounter_i1 = 0U; DWork.is_active_c2_rtwdemo_pmsmfoc = 0U; DWork.is_c2_rtwdemo_pmsmfoc = IN_NO_ACTIVE_CHILD; DWork.Position_Valid = 0U; /* InitializeConditions for Chart: '<S1>/Controller_Mode_Scheduler' */ DWork.is_Motor_On = IN_NO_ACTIVE_CHILD; DWork.is_Motor_Control = IN_NO_ACTIVE_CHILD; DWork.velocity_command = 0.0F; DWork.position_command = 0.0F; DWork.torque_command = 0.0F; /* Entry: Mode_Scheduler/Controller_Mode_Scheduler */ /* Entry Internal: Mode_Scheduler/Controller_Mode_Scheduler */ /* Transition: '<S4>:9' */ DWork.is_c1_rtwdemo_pmsmfoc = IN_Stand_By; /* Entry 'Stand_By': '<S4>:154' */ controller_mode = StandBy; /* InitializeConditions for DiscreteIntegrator: '<S17>/Integrator' */ DWork.Integrator_PrevResetState = 0; /* InitializeConditions for DiscreteIntegrator: '<S18>/Integrator' */ DWork.Integrator_PrevResetState_c = 0; } /* * File trailer for generated code. * * [EOF] */