487 lines
13 KiB
C
487 lines
13 KiB
C
/*
|
|
* Academic License - for use in teaching, academic research, and meeting
|
|
* course requirements at degree granting institutions only. Not for
|
|
* government, commercial, or other organizational use.
|
|
*
|
|
* File: Controller.c
|
|
*
|
|
* Code generated for Simulink model 'Controller'.
|
|
*
|
|
* Model version : 2.23
|
|
* Simulink Coder version : 25.1 (R2025a) 21-Nov-2024
|
|
* C/C++ source code generated on : Sat Jun 14 13:33:39 2025
|
|
*
|
|
* Target selection: ert.tlc
|
|
* Embedded hardware selection: ARM Compatible->ARM Cortex-M
|
|
* Code generation objectives:
|
|
* 1. Execution efficiency
|
|
* 2. RAM efficiency
|
|
* Validation result: Not run
|
|
*/
|
|
|
|
#include "Controller.h"
|
|
#include "rtwtypes.h"
|
|
#include <math.h>
|
|
#include "arm_math.h"
|
|
|
|
/* Block signals and states (default storage) */
|
|
DW rtDW;
|
|
|
|
/* External inputs (root inport signals with default storage) */
|
|
ExtU rtU;
|
|
|
|
/* External outputs (root outports fed by signals with default storage) */
|
|
ExtY rtY;
|
|
|
|
/* Real-time model */
|
|
static RT_MODEL rtM_;
|
|
RT_MODEL *const rtM = &rtM_;
|
|
static real32_T IfActionSubsystem(real32_T rtu_In1);
|
|
static real32_T IfActionSubsystem1(real32_T rtu_In1);
|
|
|
|
/*
|
|
* Output and update for action system:
|
|
* '<S22>/If Action Subsystem'
|
|
* '<S193>/If Action Subsystem'
|
|
*/
|
|
static real32_T IfActionSubsystem(real32_T rtu_In1)
|
|
{
|
|
/* Sum: '<S25>/Sum' incorporates:
|
|
* DataTypeConversion: '<S25>/Convert_back'
|
|
* DataTypeConversion: '<S25>/Convert_uint16'
|
|
*/
|
|
return rtu_In1 - (real32_T)(int16_T)floorf(rtu_In1);
|
|
}
|
|
|
|
/*
|
|
* Output and update for action system:
|
|
* '<S22>/If Action Subsystem1'
|
|
* '<S193>/If Action Subsystem1'
|
|
*/
|
|
static real32_T IfActionSubsystem1(real32_T rtu_In1)
|
|
{
|
|
/* Sum: '<S26>/Sum' incorporates:
|
|
* DataTypeConversion: '<S26>/Convert_back'
|
|
* DataTypeConversion: '<S26>/Convert_uint16'
|
|
*/
|
|
return rtu_In1 - (real32_T)(int16_T)rtu_In1;
|
|
}
|
|
|
|
/* Model step function */
|
|
void Controller_step(void)
|
|
{
|
|
real32_T T_idx_0;
|
|
real32_T T_idx_1;
|
|
real32_T T_idx_2;
|
|
real32_T T_idx_3;
|
|
real32_T d_Temp;
|
|
real32_T rtb_Gain1;
|
|
real32_T rtb_Sum2;
|
|
real32_T rtb_Sum4_f;
|
|
real32_T rtb_algDD_o1;
|
|
real32_T rtb_algDD_o1_n;
|
|
real32_T rtb_algDD_o2_h;
|
|
real32_T rtb_convert_pu;
|
|
real32_T rtb_sum_alpha;
|
|
real32_T u0;
|
|
real32_T u0_0;
|
|
uint16_T rtb_Get_Integer_e;
|
|
|
|
/* Gain: '<S22>/convert_pu' incorporates:
|
|
* Gain: '<S1>/Gain'
|
|
* Inport: '<Root>/theta'
|
|
*/
|
|
rtb_convert_pu = 7.0F * rtU.Rotoranglethetamrad * 0.159154937F;
|
|
|
|
/* If: '<S22>/If' incorporates:
|
|
* Constant: '<S24>/Constant'
|
|
* RelationalOperator: '<S24>/Compare'
|
|
*/
|
|
if (rtb_convert_pu < 0.0F) {
|
|
/* Outputs for IfAction SubSystem: '<S22>/If Action Subsystem' incorporates:
|
|
* ActionPort: '<S25>/Action Port'
|
|
*/
|
|
T_idx_2 = IfActionSubsystem(rtb_convert_pu);
|
|
|
|
/* End of Outputs for SubSystem: '<S22>/If Action Subsystem' */
|
|
} else {
|
|
/* Outputs for IfAction SubSystem: '<S22>/If Action Subsystem1' incorporates:
|
|
* ActionPort: '<S26>/Action Port'
|
|
*/
|
|
T_idx_2 = IfActionSubsystem1(rtb_convert_pu);
|
|
|
|
/* End of Outputs for SubSystem: '<S22>/If Action Subsystem1' */
|
|
}
|
|
|
|
/* End of If: '<S22>/If' */
|
|
|
|
/* Gain: '<S20>/indexing' */
|
|
T_idx_2 *= 800.0F;
|
|
|
|
/* If: '<S193>/If' incorporates:
|
|
* Constant: '<S195>/Constant'
|
|
* RelationalOperator: '<S195>/Compare'
|
|
*/
|
|
if (rtb_convert_pu < 0.0F) {
|
|
/* Outputs for IfAction SubSystem: '<S193>/If Action Subsystem' incorporates:
|
|
* ActionPort: '<S196>/Action Port'
|
|
*/
|
|
rtb_Sum2 = IfActionSubsystem(rtb_convert_pu);
|
|
|
|
/* End of Outputs for SubSystem: '<S193>/If Action Subsystem' */
|
|
} else {
|
|
/* Outputs for IfAction SubSystem: '<S193>/If Action Subsystem1' incorporates:
|
|
* ActionPort: '<S197>/Action Port'
|
|
*/
|
|
rtb_Sum2 = IfActionSubsystem1(rtb_convert_pu);
|
|
|
|
/* End of Outputs for SubSystem: '<S193>/If Action Subsystem1' */
|
|
}
|
|
|
|
/* End of If: '<S193>/If' */
|
|
|
|
/* Gain: '<S191>/indexing' */
|
|
rtb_Sum2 *= 800.0F;
|
|
|
|
/* DataTypeConversion: '<S191>/Get_Integer' */
|
|
rtb_Get_Integer_e = (uint16_T)rtb_Sum2;
|
|
|
|
/* Sum: '<S191>/Sum2' incorporates:
|
|
* DataTypeConversion: '<S191>/Data Type Conversion1'
|
|
* DataTypeConversion: '<S191>/Get_Integer'
|
|
*/
|
|
rtb_Sum2 -= (real32_T)(uint16_T)rtb_Sum2;
|
|
|
|
/* Outputs for Atomic SubSystem: '<S13>/Two phase CRL wrap' */
|
|
/* AlgorithmDescriptorDelegate generated from: '<S14>/a16' incorporates:
|
|
* Inport: '<Root>/Ia'
|
|
* Inport: '<Root>/Ib'
|
|
*/
|
|
arm_clarke_f32(rtU.Ia, rtU.Ib, &rtb_convert_pu, &rtb_algDD_o2_h);
|
|
|
|
/* End of Outputs for SubSystem: '<S13>/Two phase CRL wrap' */
|
|
|
|
/* Outputs for Atomic SubSystem: '<S187>/Two inputs CRL' */
|
|
/* AlgorithmDescriptorDelegate generated from: '<S189>/a16' incorporates:
|
|
* Constant: '<S191>/offset'
|
|
* Constant: '<S191>/sine_table_values'
|
|
* Gain: '<S13>/Kalpha'
|
|
* Gain: '<S13>/Kbeta'
|
|
* Product: '<S192>/Product'
|
|
* Product: '<S192>/Product1'
|
|
* Selector: '<S191>/Lookup'
|
|
* Sum: '<S191>/Sum'
|
|
* Sum: '<S192>/Sum3'
|
|
* Sum: '<S192>/Sum4'
|
|
* Sum: '<S192>/Sum5'
|
|
* Sum: '<S192>/Sum6'
|
|
*/
|
|
arm_park_f32(1.22474492F * rtb_convert_pu, 1.22474492F * rtb_algDD_o2_h,
|
|
&rtb_algDD_o1, &T_idx_0, (rtConstP.pooled1[(int32_T)
|
|
(rtb_Get_Integer_e + 1U)] - rtConstP.pooled1[rtb_Get_Integer_e]) * rtb_Sum2
|
|
+ rtConstP.pooled1[rtb_Get_Integer_e], (rtConstP.pooled1[(int32_T)
|
|
(rtb_Get_Integer_e + 201U)] - rtConstP.pooled1[(int32_T)(rtb_Get_Integer_e +
|
|
200U)]) * rtb_Sum2 + rtConstP.pooled1[(int32_T)(rtb_Get_Integer_e + 200U)]);
|
|
|
|
/* End of Outputs for SubSystem: '<S187>/Two inputs CRL' */
|
|
|
|
/* SampleTimeMath: '<S3>/TSamp' incorporates:
|
|
* Inport: '<Root>/theta'
|
|
*
|
|
* About '<S3>/TSamp':
|
|
* y = u * K where K = 1 / ( w * Ts )
|
|
* */
|
|
rtb_convert_pu = rtU.Rotoranglethetamrad * 10000.0F;
|
|
|
|
/* Sum: '<S3>/Diff' incorporates:
|
|
* UnitDelay: '<S3>/UD'
|
|
*
|
|
* Block description for '<S3>/Diff':
|
|
*
|
|
* Add in CPU
|
|
*
|
|
* Block description for '<S3>/UD':
|
|
*
|
|
* Store in Global RAM
|
|
*/
|
|
rtb_Sum2 = rtb_convert_pu - rtDW.UD_DSTATE;
|
|
|
|
/* Gain: '<S1>/Gain1' */
|
|
rtb_Gain1 = 7.0F * rtb_Sum2;
|
|
|
|
/* Sum: '<S1>/Sum3' incorporates:
|
|
* Inport: '<Root>/Id_ref'
|
|
*/
|
|
rtb_algDD_o2_h = rtU.Id_ref - rtb_algDD_o1;
|
|
|
|
/* Sum: '<S1>/Sum4' incorporates:
|
|
* Gain: '<S1>/Gain2'
|
|
* Inport: '<Root>/Speed_ref'
|
|
*/
|
|
rtb_Sum4_f = rtU.Speed_ref - 9.54929638F * rtb_Sum2;
|
|
|
|
/* Sum: '<S1>/Sum2' incorporates:
|
|
* DiscreteIntegrator: '<S167>/Integrator'
|
|
* Gain: '<S172>/Proportional Gain'
|
|
* Sum: '<S176>/Sum'
|
|
*/
|
|
rtb_Sum2 = (0.026F * rtb_Sum4_f + rtDW.Integrator_DSTATE_n) - T_idx_0;
|
|
|
|
/* Sum: '<S20>/Sum2' incorporates:
|
|
* DataTypeConversion: '<S20>/Data Type Conversion1'
|
|
* DataTypeConversion: '<S20>/Get_Integer'
|
|
*/
|
|
rtb_sum_alpha = T_idx_2 - (real32_T)(uint16_T)T_idx_2;
|
|
|
|
/* Sum: '<S124>/Sum' incorporates:
|
|
* DiscreteIntegrator: '<S115>/Integrator'
|
|
* Gain: '<S120>/Proportional Gain'
|
|
*/
|
|
u0 = 0.558F * rtb_algDD_o2_h + rtDW.Integrator_DSTATE;
|
|
|
|
/* Sum: '<S72>/Sum' incorporates:
|
|
* DiscreteIntegrator: '<S63>/Integrator'
|
|
* Gain: '<S68>/Proportional Gain'
|
|
*/
|
|
u0_0 = 0.558F * rtb_Sum2 + rtDW.Integrator_DSTATE_h;
|
|
|
|
/* Saturate: '<S122>/Saturation' */
|
|
if (u0 > 6.92820311F) {
|
|
u0 = 6.92820311F;
|
|
} else if (u0 < -6.92820311F) {
|
|
u0 = -6.92820311F;
|
|
}
|
|
|
|
/* Saturate: '<S70>/Saturation' */
|
|
if (u0_0 > 6.92820311F) {
|
|
u0_0 = 6.92820311F;
|
|
} else if (u0_0 < -6.92820311F) {
|
|
u0_0 = -6.92820311F;
|
|
}
|
|
|
|
/* Outputs for Atomic SubSystem: '<S17>/Two inputs CRL' */
|
|
/* AlgorithmDescriptorDelegate generated from: '<S19>/a16' incorporates:
|
|
* Constant: '<S20>/offset'
|
|
* Constant: '<S20>/sine_table_values'
|
|
* DataTypeConversion: '<S20>/Get_Integer'
|
|
* Fcn: '<S1>/Fcn'
|
|
* Fcn: '<S1>/Fcn1'
|
|
* Product: '<S21>/Product'
|
|
* Product: '<S21>/Product1'
|
|
* Saturate: '<S122>/Saturation'
|
|
* Saturate: '<S70>/Saturation'
|
|
* Selector: '<S20>/Lookup'
|
|
* Sum: '<S1>/Sum'
|
|
* Sum: '<S1>/Sum1'
|
|
* Sum: '<S20>/Sum'
|
|
* Sum: '<S21>/Sum3'
|
|
* Sum: '<S21>/Sum4'
|
|
* Sum: '<S21>/Sum5'
|
|
* Sum: '<S21>/Sum6'
|
|
*/
|
|
arm_inv_park_f32(u0 + -rtb_Gain1 * T_idx_0 * 0.000279F, u0_0 + rtb_Gain1 *
|
|
(0.000279F * rtb_algDD_o1 + 0.005753F), &rtb_algDD_o1_n,
|
|
&T_idx_2, (rtConstP.pooled1[(int32_T)((uint16_T)T_idx_2 + 1U)]
|
|
- rtConstP.pooled1[(uint16_T)T_idx_2]) * rtb_sum_alpha + rtConstP.pooled1
|
|
[(uint16_T)T_idx_2], (rtConstP.pooled1[(int32_T)((uint16_T)
|
|
T_idx_2 + 201U)] - rtConstP.pooled1[(int32_T)((uint16_T)T_idx_2 + 200U)]) *
|
|
rtb_sum_alpha + rtConstP.pooled1[(int32_T)((uint16_T)T_idx_2
|
|
+ 200U)]);
|
|
|
|
/* End of Outputs for SubSystem: '<S17>/Two inputs CRL' */
|
|
|
|
/* MATLAB Function: '<S9>/MATLAB Function2' */
|
|
switch ((((T_idx_2 - rtb_algDD_o1_n * 1.73205078F > 0.0F) << 2) +
|
|
((rtb_algDD_o1_n * 1.73205078F + T_idx_2 > 0.0F) << 1)) + (T_idx_2 <=
|
|
0.0F)) {
|
|
case 1:
|
|
T_idx_0 = 5.0F;
|
|
break;
|
|
|
|
case 2:
|
|
T_idx_0 = 1.0F;
|
|
break;
|
|
|
|
case 3:
|
|
T_idx_0 = 6.0F;
|
|
break;
|
|
|
|
case 4:
|
|
T_idx_0 = 3.0F;
|
|
break;
|
|
|
|
case 5:
|
|
T_idx_0 = 4.0F;
|
|
break;
|
|
|
|
case 6:
|
|
T_idx_0 = 2.0F;
|
|
break;
|
|
|
|
default:
|
|
T_idx_0 = -1.0F;
|
|
break;
|
|
}
|
|
|
|
/* End of MATLAB Function: '<S9>/MATLAB Function2' */
|
|
|
|
/* MATLAB Function: '<S9>/MATLAB Function1' incorporates:
|
|
* Constant: '<S9>/Constant'
|
|
* Constant: '<S9>/Constant1'
|
|
*/
|
|
switch ((int32_T)T_idx_0) {
|
|
case 1:
|
|
rtb_algDD_o1 = 2.0F * T_idx_2 * 1800.0F / 1.73205078F / 8.0F;
|
|
T_idx_3 = (rtb_algDD_o1_n - T_idx_2 / 1.73205078F) * 1800.0F / 8.0F;
|
|
T_idx_1 = rtb_algDD_o1;
|
|
d_Temp = T_idx_3 + rtb_algDD_o1;
|
|
if (d_Temp > 1800.0F) {
|
|
d_Temp = 1800.0F / d_Temp;
|
|
T_idx_3 *= d_Temp;
|
|
T_idx_1 = rtb_algDD_o1 * d_Temp;
|
|
}
|
|
|
|
d_Temp = ((1800.0F - T_idx_3) - T_idx_1) / 2.0F;
|
|
T_idx_3 = (T_idx_3 + T_idx_1) + d_Temp;
|
|
T_idx_1 += d_Temp;
|
|
break;
|
|
|
|
case 2:
|
|
T_idx_3 = T_idx_2 / 1.73205078F;
|
|
T_idx_1 = (T_idx_3 - rtb_algDD_o1_n) * 1800.0F / 8.0F;
|
|
rtb_algDD_o1 = (T_idx_3 + rtb_algDD_o1_n) * 1800.0F / 8.0F;
|
|
T_idx_2 = rtb_algDD_o1;
|
|
d_Temp = T_idx_1 + rtb_algDD_o1;
|
|
if (d_Temp > 1800.0F) {
|
|
d_Temp = 1800.0F / d_Temp;
|
|
T_idx_1 *= d_Temp;
|
|
T_idx_2 = rtb_algDD_o1 * d_Temp;
|
|
}
|
|
|
|
d_Temp = ((1800.0F - T_idx_1) - T_idx_2) / 2.0F;
|
|
T_idx_3 = T_idx_2 + d_Temp;
|
|
T_idx_1 = (T_idx_1 + T_idx_2) + d_Temp;
|
|
break;
|
|
|
|
case 3:
|
|
T_idx_1 = 1.73205078F * T_idx_2 * 1800.0F / 12.0F;
|
|
T_idx_2 = -((1.5F * rtb_algDD_o1_n + 0.866025388F * T_idx_2) * 150.0F);
|
|
d_Temp = T_idx_1 + T_idx_2;
|
|
if (d_Temp > 1800.0F) {
|
|
d_Temp = 1800.0F / d_Temp;
|
|
T_idx_1 *= d_Temp;
|
|
T_idx_2 *= d_Temp;
|
|
}
|
|
|
|
d_Temp = ((1800.0F - T_idx_1) - T_idx_2) / 2.0F;
|
|
T_idx_3 = d_Temp;
|
|
T_idx_1 = (T_idx_1 + T_idx_2) + d_Temp;
|
|
d_Temp += T_idx_2;
|
|
break;
|
|
|
|
case 4:
|
|
T_idx_0 = -2.0F * T_idx_2 * 1800.0F / 1.73205078F / 8.0F;
|
|
T_idx_2 = (T_idx_2 / 1.73205078F - rtb_algDD_o1_n) * 1800.0F / 8.0F;
|
|
d_Temp = T_idx_0 + T_idx_2;
|
|
if (d_Temp > 1800.0F) {
|
|
d_Temp = 1800.0F / d_Temp;
|
|
T_idx_0 *= d_Temp;
|
|
T_idx_2 *= d_Temp;
|
|
}
|
|
|
|
d_Temp = ((1800.0F - T_idx_0) - T_idx_2) / 2.0F;
|
|
T_idx_3 = d_Temp;
|
|
T_idx_1 = T_idx_2 + d_Temp;
|
|
d_Temp += T_idx_0 + T_idx_2;
|
|
break;
|
|
|
|
case 5:
|
|
T_idx_3 = -T_idx_2 / 1.73205078F;
|
|
T_idx_0 = (T_idx_3 - rtb_algDD_o1_n) * 1800.0F / 8.0F;
|
|
T_idx_1 = (T_idx_3 + rtb_algDD_o1_n) * 1800.0F / 8.0F;
|
|
T_idx_2 = T_idx_1;
|
|
d_Temp = T_idx_0 + T_idx_1;
|
|
if (d_Temp > 1800.0F) {
|
|
d_Temp = 1800.0F / d_Temp;
|
|
T_idx_0 *= d_Temp;
|
|
T_idx_2 = T_idx_1 * d_Temp;
|
|
}
|
|
|
|
d_Temp = ((1800.0F - T_idx_0) - T_idx_2) / 2.0F;
|
|
T_idx_3 = T_idx_2 + d_Temp;
|
|
T_idx_1 = d_Temp;
|
|
d_Temp += T_idx_0 + T_idx_2;
|
|
break;
|
|
|
|
case 6:
|
|
T_idx_1 = -2.0F * T_idx_2 * 1800.0F / 1.73205078F / 8.0F;
|
|
T_idx_3 = (T_idx_2 / 1.73205078F + rtb_algDD_o1_n) * 1800.0F / 8.0F;
|
|
T_idx_2 = T_idx_1;
|
|
d_Temp = T_idx_3 + T_idx_1;
|
|
if (d_Temp > 1800.0F) {
|
|
d_Temp = 1800.0F / d_Temp;
|
|
T_idx_3 *= d_Temp;
|
|
T_idx_2 = T_idx_1 * d_Temp;
|
|
}
|
|
|
|
d_Temp = ((1800.0F - T_idx_3) - T_idx_2) / 2.0F;
|
|
T_idx_3 = (T_idx_3 + T_idx_2) + d_Temp;
|
|
T_idx_1 = d_Temp;
|
|
d_Temp += T_idx_2;
|
|
break;
|
|
}
|
|
|
|
/* Outport: '<Root>/CCR1' incorporates:
|
|
* Constant: '<S9>/Constant1'
|
|
* MATLAB Function: '<S9>/MATLAB Function1'
|
|
*/
|
|
rtY.CCR1 = (1800.0F - T_idx_3) / 2.0F;
|
|
|
|
/* Outport: '<Root>/CCR2' incorporates:
|
|
* Constant: '<S9>/Constant1'
|
|
* MATLAB Function: '<S9>/MATLAB Function1'
|
|
*/
|
|
rtY.CCR2 = (1800.0F - T_idx_1) / 2.0F;
|
|
|
|
/* Outport: '<Root>/CCR3' incorporates:
|
|
* Constant: '<S9>/Constant1'
|
|
* MATLAB Function: '<S9>/MATLAB Function1'
|
|
*/
|
|
rtY.CCR3 = (1800.0F - d_Temp) / 2.0F;
|
|
|
|
/* Update for UnitDelay: '<S3>/UD'
|
|
*
|
|
* Block description for '<S3>/UD':
|
|
*
|
|
* Store in Global RAM
|
|
*/
|
|
rtDW.UD_DSTATE = rtb_convert_pu;
|
|
|
|
/* Update for DiscreteIntegrator: '<S115>/Integrator' incorporates:
|
|
* Gain: '<S112>/Integral Gain'
|
|
*/
|
|
rtDW.Integrator_DSTATE += 1600.0F * rtb_algDD_o2_h * 0.0001F;
|
|
|
|
/* Update for DiscreteIntegrator: '<S167>/Integrator' incorporates:
|
|
* Gain: '<S164>/Integral Gain'
|
|
*/
|
|
rtDW.Integrator_DSTATE_n += 2.408F * rtb_Sum4_f * 0.0001F;
|
|
|
|
/* Update for DiscreteIntegrator: '<S63>/Integrator' incorporates:
|
|
* Gain: '<S60>/Integral Gain'
|
|
*/
|
|
rtDW.Integrator_DSTATE_h += 1600.0F * rtb_Sum2 * 0.0001F;
|
|
}
|
|
|
|
/* Model initialize function */
|
|
void Controller_initialize(void)
|
|
{
|
|
/* (no initialization code required) */
|
|
}
|
|
|
|
/*
|
|
* File trailer for generated code.
|
|
*
|
|
* [EOF]
|
|
*/
|