diff --git a/emBODY/eBcode/arch-arm/board/amcbldc/application/proj/amcbldc-application2.uvoptx b/emBODY/eBcode/arch-arm/board/amcbldc/application/proj/amcbldc-application2.uvoptx index f4d23d0f5..b329986a8 100644 --- a/emBODY/eBcode/arch-arm/board/amcbldc/application/proj/amcbldc-application2.uvoptx +++ b/emBODY/eBcode/arch-arm/board/amcbldc/application/proj/amcbldc-application2.uvoptx @@ -1352,7 +1352,7 @@ mbd - 0 + 1 0 0 0 @@ -1488,6 +1488,18 @@ 0 0 + + 18 + 75 + 8 + 0 + 0 + 0 + ..\..\..\..\mbd\amcbldc\sharedutils\rt_hypotf.cpp + rt_hypotf.cpp + 0 + 0 + @@ -1498,7 +1510,7 @@ 0 19 - 75 + 76 8 0 0 @@ -1518,7 +1530,7 @@ 0 20 - 76 + 77 8 0 0 @@ -1531,23 +1543,11 @@ - mbd::supervisor-rx + mbd::supervisor 0 0 0 0 - - 21 - 77 - 8 - 0 - 0 - 0 - ..\..\..\..\mbd\amcbldc\supervisor-rx\SupervisorFSM_RX.cpp - SupervisorFSM_RX.cpp - 0 - 0 - 21 78 @@ -1555,8 +1555,8 @@ 0 0 0 - ..\..\..\..\mbd\amcbldc\supervisor-rx\SupervisorFSM_RX_data.cpp - SupervisorFSM_RX_data.cpp + ..\..\..\..\mbd\amcbldc\supervisor\supervisor.cpp + supervisor.cpp 0 0 @@ -1632,18 +1632,6 @@ 0 0 - - 24 - 83 - 8 - 0 - 0 - 0 - ..\..\..\..\mbd\amcbldc\control-foc\control_foc_data.cpp - control_foc_data.cpp - 0 - 0 - @@ -1654,7 +1642,7 @@ 0 25 - 84 + 83 8 0 0 @@ -1674,7 +1662,7 @@ 0 26 - 85 + 84 8 0 0 @@ -1694,7 +1682,7 @@ 0 27 - 86 + 85 8 0 0 @@ -1706,6 +1694,38 @@ + + mbd::motion-controller + 0 + 0 + 0 + 0 + + 28 + 86 + 8 + 0 + 0 + 0 + ..\..\..\..\mbd\amcbldc\motion-controller\motion_controller.cpp + motion_controller.cpp + 0 + 0 + + + 28 + 87 + 8 + 0 + 0 + 0 + ..\..\..\..\mbd\amcbldc\motion-controller-single\motion_controller_single.cpp + motion_controller_single.cpp + 0 + 0 + + + ::CMSIS 0 diff --git a/emBODY/eBcode/arch-arm/board/amcbldc/application/proj/amcbldc-application2.uvprojx b/emBODY/eBcode/arch-arm/board/amcbldc/application/proj/amcbldc-application2.uvprojx index dfd920a9b..d68919186 100644 --- a/emBODY/eBcode/arch-arm/board/amcbldc/application/proj/amcbldc-application2.uvprojx +++ b/emBODY/eBcode/arch-arm/board/amcbldc/application/proj/amcbldc-application2.uvprojx @@ -340,7 +340,7 @@ -Wno-pragma-pack -Wno-deprecated-register -DEMBOT_USE_rtos_osal -DEMBOT_AMCBLDC_APP05 USE_STM32HAL STM32HAL_BOARD_AMCBLDC STM32HAL_DRIVER_V122 - ..\..\bsp;..\..\..\..\libs\highlevel\abslayer\osal\api;..\..\..\..\libs\midware\eventviewer\api;..\..\..\..\libs\lowlevel\stm32hal\api;..\..\..\..\embot\app;..\..\..\..\embot\i2h;..\..\..\..\embot\hw;..\..\..\..\embot\os;..\..\..\..\embot;..\..\..\..\embot\app\dsp;..\..\..\..\embot\app\skeleton;..\..\..\..\embot\prot\can;..\..\..\..\..\..\..\..\icub-firmware-shared\can\canProtocolLib;..\..\..\..\..\..\..\..\icub-firmware-shared\embot\core;..\..\..\..\..\..\..\..\icub-firmware-shared\embot\tools;..\..\..\..\libs\lowlevel\cmsis\dsp\v160\Include;..\..\..\..\libs\highlevel\abslayer\hal2\api;..\src\motorhal2;..\..\..\..\mbd\amcbldc\sharedutils;..\..\..\..\mbd\amcbldc\can-decoder;..\..\..\..\mbd\amcbldc\can-raw2struct;..\..\..\..\mbd\amcbldc\supervisor-rx;..\..\..\..\mbd\amcbldc\supervisor-tx;..\..\..\..\mbd\amcbldc\can-encoder;..\..\..\..\mbd\amcbldc\control-outer;..\..\..\..\mbd\amcbldc\control-foc;..\..\..\..\mbd\amcbldc\estimator;..\..\..\..\mbd\amcbldc\amc-bldc;..\..\..\..\mbd\amcbldc\filter-current;..\..\..\..\embot\app\bldc;..\src\app-board-amcbldc + ..\..\bsp;..\..\..\..\libs\highlevel\abslayer\osal\api;..\..\..\..\libs\midware\eventviewer\api;..\..\..\..\libs\lowlevel\stm32hal\api;..\..\..\..\embot\app;..\..\..\..\embot\i2h;..\..\..\..\embot\hw;..\..\..\..\embot\os;..\..\..\..\embot;..\..\..\..\embot\app\dsp;..\..\..\..\embot\app\skeleton;..\..\..\..\embot\prot\can;..\..\..\..\..\..\..\..\icub-firmware-shared\can\canProtocolLib;..\..\..\..\..\..\..\..\icub-firmware-shared\embot\core;..\..\..\..\..\..\..\..\icub-firmware-shared\embot\tools;..\..\..\..\libs\lowlevel\cmsis\dsp\v160\Include;..\..\..\..\libs\highlevel\abslayer\hal2\api;..\src\motorhal2;..\..\..\..\mbd\amcbldc\sharedutils;..\..\..\..\mbd\amcbldc\can-decoder;..\..\..\..\mbd\amcbldc\can-raw2struct;..\..\..\..\mbd\amcbldc\supervisor;..\..\..\..\mbd\amcbldc\supervisor-tx;..\..\..\..\mbd\amcbldc\can-encoder;..\..\..\..\mbd\amcbldc\control-outer;..\..\..\..\mbd\amcbldc\control-foc;..\..\..\..\mbd\amcbldc\estimator;..\..\..\..\mbd\amcbldc\amc-bldc;..\..\..\..\mbd\amcbldc\filter-current;..\..\..\..\mbd\amcbldc\motion-controller;..\..\..\..\mbd\amcbldc\motion-controller-single;..\..\..\..\embot\app\bldc;..\src\app-board-amcbldc @@ -1167,6 +1167,11 @@ 8 ..\..\..\..\mbd\amcbldc\sharedutils\uMultiWordShl.cpp + + rt_hypotf.cpp + 8 + ..\..\..\..\mbd\amcbldc\sharedutils\rt_hypotf.cpp + @@ -1190,17 +1195,12 @@ - mbd::supervisor-rx + mbd::supervisor - SupervisorFSM_RX.cpp - 8 - ..\..\..\..\mbd\amcbldc\supervisor-rx\SupervisorFSM_RX.cpp - - - SupervisorFSM_RX_data.cpp + supervisor.cpp 8 - ..\..\..\..\mbd\amcbldc\supervisor-rx\SupervisorFSM_RX_data.cpp + ..\..\..\..\mbd\amcbldc\supervisor\supervisor.cpp @@ -1306,11 +1306,6 @@ 8 ..\..\..\..\mbd\amcbldc\control-foc\FOCInnerLoop.cpp - - control_foc_data.cpp - 8 - ..\..\..\..\mbd\amcbldc\control-foc\control_foc_data.cpp - @@ -1343,6 +1338,21 @@ + + mbd::motion-controller + + + motion_controller.cpp + 8 + ..\..\..\..\mbd\amcbldc\motion-controller\motion_controller.cpp + + + motion_controller_single.cpp + 8 + ..\..\..\..\mbd\amcbldc\motion-controller-single\motion_controller_single.cpp + + + ::CMSIS @@ -2509,6 +2519,11 @@ 8 ..\..\..\..\mbd\amcbldc\sharedutils\uMultiWordShl.cpp + + rt_hypotf.cpp + 8 + ..\..\..\..\mbd\amcbldc\sharedutils\rt_hypotf.cpp + @@ -2532,17 +2547,12 @@ - mbd::supervisor-rx + mbd::supervisor - SupervisorFSM_RX.cpp - 8 - ..\..\..\..\mbd\amcbldc\supervisor-rx\SupervisorFSM_RX.cpp - - - SupervisorFSM_RX_data.cpp + supervisor.cpp 8 - ..\..\..\..\mbd\amcbldc\supervisor-rx\SupervisorFSM_RX_data.cpp + ..\..\..\..\mbd\amcbldc\supervisor\supervisor.cpp @@ -2579,11 +2589,6 @@ 8 ..\..\..\..\mbd\amcbldc\control-foc\FOCInnerLoop.cpp - - control_foc_data.cpp - 8 - ..\..\..\..\mbd\amcbldc\control-foc\control_foc_data.cpp - @@ -2616,6 +2621,21 @@ + + mbd::motion-controller + + + motion_controller.cpp + 8 + ..\..\..\..\mbd\amcbldc\motion-controller\motion_controller.cpp + + + motion_controller_single.cpp + 8 + ..\..\..\..\mbd\amcbldc\motion-controller-single\motion_controller_single.cpp + + + ::CMSIS diff --git a/emBODY/eBcode/arch-arm/board/amcbldc/application/src/app-board-amcbldc/embot_app_board_amcbldc_theMBD.cpp b/emBODY/eBcode/arch-arm/board/amcbldc/application/src/app-board-amcbldc/embot_app_board_amcbldc_theMBD.cpp index ca07d796e..fa14d1928 100644 --- a/emBODY/eBcode/arch-arm/board/amcbldc/application/src/app-board-amcbldc/embot_app_board_amcbldc_theMBD.cpp +++ b/emBODY/eBcode/arch-arm/board/amcbldc/application/src/app-board-amcbldc/embot_app_board_amcbldc_theMBD.cpp @@ -450,7 +450,7 @@ bool embot::app::board::amcbldc::theMBD::Impl::tick(const std::vector(electricalAngle)*0.0054931640625f; // (60 interval angle) @@ -549,13 +549,13 @@ void embot::app::board::amcbldc::theMBD::Impl::onCurrents_FOC_innerloop(void *ow // ----------------------------------------------------------------------------- - AMC_BLDC_U.SensorsData_p.jointpositions.position = static_cast(position) * 0.0054931640625f; // iCubDegree -> deg + AMC_BLDC_U.SensorsData_p.jointpositions = static_cast(position) * 0.0054931640625f; // iCubDegree -> deg // Set the voltages embot::hw::motor::PWMperc pwmperc { - AMC_BLDC_Y.ControlOutputs_p.Vabc[0], AMC_BLDC_Y.ControlOutputs_p.Vabc[1], AMC_BLDC_Y.ControlOutputs_p.Vabc[2] + AMC_BLDC_Y.ControlOutputs.Vabc[0], AMC_BLDC_Y.ControlOutputs.Vabc[1], AMC_BLDC_Y.ControlOutputs.Vabc[2] }; embot::hw::motor::setPWM(embot::hw::MOTOR::one, pwmperc); diff --git a/emBODY/eBcode/arch-arm/mbd/amcbldc/amc-bldc/AMC_BLDC.cpp b/emBODY/eBcode/arch-arm/mbd/amcbldc/amc-bldc/AMC_BLDC.cpp index e728fdf3c..68dd9a402 100644 --- a/emBODY/eBcode/arch-arm/mbd/amcbldc/amc-bldc/AMC_BLDC.cpp +++ b/emBODY/eBcode/arch-arm/mbd/amcbldc/amc-bldc/AMC_BLDC.cpp @@ -7,9 +7,9 @@ // // Code generated for Simulink model 'AMC_BLDC'. // -// Model version : 7.13 +// Model version : 7.54 // Simulink Coder version : 23.2 (R2023b) 01-Aug-2023 -// C/C++ source code generated on : Tue Feb 13 11:55:25 2024 +// C/C++ source code generated on : Wed Feb 28 11:29:48 2024 // // Target selection: ert.tlc // Embedded hardware selection: ARM Compatible->ARM Cortex-M @@ -20,26 +20,73 @@ #include "rtw_mutex.h" #include "rtwtypes.h" #include "AMC_BLDC_types.h" -#include "control_foc.h" -#include "estimation_velocity.h" -#include "filter_current.h" #include "can_decoder.h" -#include "SupervisorFSM_RX.h" #include "SupervisorFSM_TX.h" #include "can_encoder.h" -#include "control_outer.h" +#include "motion_controller_single.h" +#include "mc_supervisor_defines.h" // Exported block parameters -ConfigurationParameters InitConfParams = { +ActuatorConfiguration InitConfParams = { + { + 40000.0F, + 0.35F, + 0.7F, + 1.0F, + 0U, + 0.0F + }, + + { + { + ControlModes_Current, + 0.0F, + 0.0F, + 2.0F, + 500.0F, + 0.0F, + 10.0F, + 0.0F, + 0.0F, + 0U + }, + + { + ControlModes_Velocity, + 0.0F, + 0.0F, + -3.0e-5F, + -3.0e-5F, + 0.0F, + 10.0F, + 0.0F, + 0.0F, + 0U + }, + + { + ControlModes_Position, + 0.0F, + 0.0F, + 0.0F, + 0.0F, + 0.0F, + 0.0F, + 0.0F, + 0.0F, + 0U + } + }, + { - 30.0F, - true, true, false, false, - true, false, - 16000, + false, + false, + 0, + 0, 0, 0U, 7U, @@ -48,97 +95,28 @@ ConfigurationParameters InitConfParams = { 0.0F, 0.0F, 24.0F, + 44.0F, 25.9F, 271.0F, 16.0F, 797.5F - }, - - { - EstimationVelocityModes_MovingAverage, - 0.9995F - }, - - { - 0.0F, - 0.0F, - 2.0F, - 500.0F, - 0.0F, - 10.0F, - 0.0F, - 0.0F, - 0U - }, - - { - 0.0F, - 0.0F, - 0.0F, - 0.0F, - 0.0F, - 0.0F, - 0.0F, - 0.0F, - 0U - }, - - { - 0.0F, - 0.0F, - -3.0e-5F, - -3.0e-5F, - 0.0F, - 10.0F, - 0.0F, - 0.0F, - 0U - }, - - { - 0.0F, - 0.0F, - 0.0F, - 0.0F, - 0.0F, - 0.0F, - 0.0F, - 0.0F, - 0U - }, - - { - 1.0F, - 359.0F, - 0.0F, - 360.0F, - 0.0F, - 0.0F, - 40000.0F, - 10U, - 2.0F, - 5.0F, - 10.0F, - 32000U, - 70.0F - }, - 25.0F + } } ; // Variable: InitConfParams - // Referenced by: '/SupervisorFSM_RX' + // Referenced by: '/Motion Controller Single' real32_T CAN_ANGLE_DEG2ICUB = 182.044449F;// Variable: CAN_ANGLE_DEG2ICUB - // Referenced by: '/CAN_Encoder' + // Referenced by: '/CAN_Encoder' // 2^16/360 real32_T CAN_ANGLE_ICUB2DEG = 0.00549316406F;// Variable: CAN_ANGLE_ICUB2DEG - // Referenced by: '/CAN_Decoder' + // Referenced by: '/CAN_Decoder' // 360/2^16 uint8_T CAN_ID_AMC = 3U; // Variable: CAN_ID_AMC // Referenced by: - // '/CAN_Decoder' - // '/CAN_Encoder' + // '/CAN_Decoder' + // '/CAN_Encoder' // 4 bits defining the ID of the AMC_BLDC board. @@ -159,170 +137,35 @@ RT_MODEL_AMC_BLDC_T AMC_BLDC_M_ = RT_MODEL_AMC_BLDC_T(); RT_MODEL_AMC_BLDC_T *const AMC_BLDC_M = &AMC_BLDC_M_; // Model step function for TID0 -void AMC_BLDC_step0(void) // Sample time: [5.0E-6s, 0.0s] +void AMC_BLDC_step0(void) // Sample time: [1.1428571428571438E-6s, 0.0s] { - // (no output/update code required) + // ModelReference: '/Motion Controller Single' + mc_base_tick(); } // Model step function for TID1 -void AMC_BLDC_step_FOC(void) // Sample time: [4.5E-5s, 0.0s] +void AMC_BLDC_step_FOC(void) // Sample time: [3.65714285714286E-5s, 0.0s] { int8_T wrBufIdx; - // RateTransition generated from: '/Adapter1' incorporates: - // BusCreator: '/Bus Creator1' - // Constant: '/Constant' - - rtw_mutex_lock(); - wrBufIdx = static_cast - (AMC_BLDC_DW.RTBInsertedForAdapter_InsertedFor_Adapter1_at_outport_0_LstB + - 1); - if (wrBufIdx == 3) { - wrBufIdx = 0; - } - - if (wrBufIdx == - AMC_BLDC_DW.RTBInsertedForAdapter_InsertedFor_Adapter1_at_outport_0_RDBu) - { - wrBufIdx = static_cast(wrBufIdx + 1); - if (wrBufIdx == 3) { - wrBufIdx = 0; - } - } - - rtw_mutex_unlock(); - switch (wrBufIdx) { - case 0: - AMC_BLDC_DW.RTBInsertedForAdapter_InsertedFor_Adapter1_at_outport_0_Bu_d.temperature - = 0.0F; - break; - - case 1: - AMC_BLDC_DW.RTBInsertedForAdapter_InsertedFor_Adapter1_at_outport_0_Bu_j.temperature - = 0.0F; - break; - - case 2: - AMC_BLDC_DW.RTBInsertedForAdapter_InsertedFor_Adapter1_at_outport_0_Bu_o.temperature - = 0.0F; - break; - } - - AMC_BLDC_DW.RTBInsertedForAdapter_InsertedFor_Adapter1_at_outport_0_LstB = - wrBufIdx; - - // End of RateTransition generated from: '/Adapter1' - - // RateTransition generated from: '/Adapter2' - rtw_mutex_lock(); - AMC_BLDC_DW.RTBInsertedForAdapter_InsertedFor_Adapter2_at_outport_0_1_RD = - AMC_BLDC_DW.RTBInsertedForAdapter_InsertedFor_Adapter2_at_outport_0_1_Ls; - rtw_mutex_unlock(); - AMC_BLDC_B.BusConversion_InsertedFor_FOC_at_inport_1_BusCreator1.flags = - AMC_BLDC_DW.RTBInsertedForAdapter_InsertedFor_Adapter2_at_outport_0_1_Bu[AMC_BLDC_DW.RTBInsertedForAdapter_InsertedFor_Adapter2_at_outport_0_1_RD]; - - // RateTransition generated from: '/Adapter2' - rtw_mutex_lock(); - AMC_BLDC_DW.RTBInsertedForAdapter_InsertedFor_Adapter2_at_outport_0_2_RD = - AMC_BLDC_DW.RTBInsertedForAdapter_InsertedFor_Adapter2_at_outport_0_2_Ls; - rtw_mutex_unlock(); - AMC_BLDC_B.BusConversion_InsertedFor_FOC_at_inport_1_BusCreator1.configurationparameters - = - AMC_BLDC_DW.RTBInsertedForAdapter_InsertedFor_Adapter2_at_outport_0_2_Bu[AMC_BLDC_DW.RTBInsertedForAdapter_InsertedFor_Adapter2_at_outport_0_2_RD]; - - // RateTransition generated from: '/Adapter2' - rtw_mutex_lock(); - AMC_BLDC_DW.RTBInsertedForAdapter_InsertedFor_Adapter2_at_outport_0_3_RD = - AMC_BLDC_DW.RTBInsertedForAdapter_InsertedFor_Adapter2_at_outport_0_3_Ls; - rtw_mutex_unlock(); - AMC_BLDC_B.BusConversion_InsertedFor_FOC_at_inport_1_BusCreator1.estimateddata - = - AMC_BLDC_DW.RTBInsertedForAdapter_InsertedFor_Adapter2_at_outport_0_3_Bu[AMC_BLDC_DW.RTBInsertedForAdapter_InsertedFor_Adapter2_at_outport_0_3_RD]; - - // RateTransition generated from: '/Adapter2' - rtw_mutex_lock(); - AMC_BLDC_DW.RTBInsertedForAdapter_InsertedFor_Adapter2_at_outport_0_4_RD = - AMC_BLDC_DW.RTBInsertedForAdapter_InsertedFor_Adapter2_at_outport_0_4_Ls; - rtw_mutex_unlock(); - AMC_BLDC_B.BusConversion_InsertedFor_FOC_at_inport_1_BusCreator1.targets = - AMC_BLDC_DW.RTBInsertedForAdapter_InsertedFor_Adapter2_at_outport_0_4_Bu[AMC_BLDC_DW.RTBInsertedForAdapter_InsertedFor_Adapter2_at_outport_0_4_RD]; - - // RateTransition generated from: '/Adapter2' - rtw_mutex_lock(); - AMC_BLDC_DW.RTBInsertedForAdapter_InsertedFor_Adapter2_at_outport_0_5_RD = - AMC_BLDC_DW.RTBInsertedForAdapter_InsertedFor_Adapter2_at_outport_0_5_Ls; - rtw_mutex_unlock(); - AMC_BLDC_B.BusConversion_InsertedFor_FOC_at_inport_1_BusCreator1.controlouteroutputs - = - AMC_BLDC_DW.RTBInsertedForAdapter_InsertedFor_Adapter2_at_outport_0_5_Bu[AMC_BLDC_DW.RTBInsertedForAdapter_InsertedFor_Adapter2_at_outport_0_5_RD]; - - // ModelReference: '/FOC' incorporates: - // Inport generated from: '/In Bus Element5' + // ModelReference: '/Motion Controller Single' incorporates: + // Inport generated from: '/In Bus Element3' // Outport generated from: '/Out Bus Element' - control_foc(&AMC_BLDC_U.SensorsData_p, - &AMC_BLDC_B.BusConversion_InsertedFor_FOC_at_inport_1_BusCreator1, - &AMC_BLDC_Y.ControlOutputs_p, &(AMC_BLDC_DW.FOC_InstanceData.rtb), - &(AMC_BLDC_DW.FOC_InstanceData.rtdw), - &(AMC_BLDC_DW.FOC_InstanceData.rtzce)); + mc_foc_tick(&AMC_BLDC_U.SensorsData_p, &AMC_BLDC_Y.ControlOutputs, + &(AMC_BLDC_DW.MotionControllerSingle_InstanceData.rtdw)); - // RateTransition generated from: '/Adapter1' incorporates: + // RateTransition generated from: '/SupervisorFSM_TX' incorporates: // Outport generated from: '/Out Bus Element' rtw_mutex_lock(); wrBufIdx = static_cast - (AMC_BLDC_DW.RTBInsertedForAdapter_InsertedFor_Adapter1_at_outport_0_Ls_j + - 1); - if (wrBufIdx == 3) { - wrBufIdx = 0; - } - - if (wrBufIdx == - AMC_BLDC_DW.RTBInsertedForAdapter_InsertedFor_Adapter1_at_outport_0_RD_a) - { - wrBufIdx = static_cast(wrBufIdx + 1); - if (wrBufIdx == 3) { - wrBufIdx = 0; - } - } - - rtw_mutex_unlock(); - switch (wrBufIdx) { - case 0: - AMC_BLDC_DW.RTBInsertedForAdapter_InsertedFor_Adapter1_at_outport_0_Buf0 = - AMC_BLDC_Y.ControlOutputs_p; - break; - - case 1: - AMC_BLDC_DW.RTBInsertedForAdapter_InsertedFor_Adapter1_at_outport_0_Buf1 = - AMC_BLDC_Y.ControlOutputs_p; - break; - - case 2: - AMC_BLDC_DW.RTBInsertedForAdapter_InsertedFor_Adapter1_at_outport_0_Buf2 = - AMC_BLDC_Y.ControlOutputs_p; - break; - } - - AMC_BLDC_DW.RTBInsertedForAdapter_InsertedFor_Adapter1_at_outport_0_Ls_j = - wrBufIdx; - - // End of RateTransition generated from: '/Adapter1' - - // RateTransition generated from: '/Adapter3' incorporates: - // Inport generated from: '/In Bus Element5' - - rtw_mutex_lock(); - wrBufIdx = static_cast - (AMC_BLDC_DW.RTBInsertedForAdapter_InsertedFor_Adapter3_at_outport_0_LstB + - 1); + (AMC_BLDC_DW.TmpRTBAtSupervisorFSM_TXInport4_LstBufWR + 1); if (wrBufIdx == 3) { wrBufIdx = 0; } - if (wrBufIdx == - AMC_BLDC_DW.RTBInsertedForAdapter_InsertedFor_Adapter3_at_outport_0_RDBu) - { + if (wrBufIdx == AMC_BLDC_DW.TmpRTBAtSupervisorFSM_TXInport4_RDBuf) { wrBufIdx = static_cast(wrBufIdx + 1); if (wrBufIdx == 3) { wrBufIdx = 0; @@ -332,445 +175,158 @@ void AMC_BLDC_step_FOC(void) // Sample time: [4.5E-5s, 0.0s] rtw_mutex_unlock(); switch (wrBufIdx) { case 0: - AMC_BLDC_DW.RTBInsertedForAdapter_InsertedFor_Adapter3_at_outport_0_Buf0 = - AMC_BLDC_U.SensorsData_p; + AMC_BLDC_DW.TmpRTBAtSupervisorFSM_TXInport4_Buf0 = AMC_BLDC_Y.ControlOutputs; break; case 1: - AMC_BLDC_DW.RTBInsertedForAdapter_InsertedFor_Adapter3_at_outport_0_Buf1 = - AMC_BLDC_U.SensorsData_p; + AMC_BLDC_DW.TmpRTBAtSupervisorFSM_TXInport4_Buf1 = AMC_BLDC_Y.ControlOutputs; break; case 2: - AMC_BLDC_DW.RTBInsertedForAdapter_InsertedFor_Adapter3_at_outport_0_Buf2 = - AMC_BLDC_U.SensorsData_p; + AMC_BLDC_DW.TmpRTBAtSupervisorFSM_TXInport4_Buf2 = AMC_BLDC_Y.ControlOutputs; break; } - AMC_BLDC_DW.RTBInsertedForAdapter_InsertedFor_Adapter3_at_outport_0_LstB = - wrBufIdx; + AMC_BLDC_DW.TmpRTBAtSupervisorFSM_TXInport4_LstBufWR = wrBufIdx; - // End of RateTransition generated from: '/Adapter3' + // End of RateTransition generated from: '/SupervisorFSM_TX' } // Model step function for TID2 void AMC_BLDC_step_Time_1ms(void) // Sample time: [0.001s, 0.0s] { // local block i/o variables - Targets rtb_SupervisorFSM_RX_o2; - ControlOuterOutputs rtb_OuterControl; + BUS_CAN_RX_ERRORS_MULTIPLE rtb_CAN_Decoder_o2; BUS_STATUS_TX rtb_SupervisorFSM_TX_o2; - int8_T wrBufIdx; - - // UnitDelay generated from: '/Adapter4' - AMC_BLDC_B.ZOHBlockInsertedForAdapter_InsertedFor_Adapter4_at_outport_0 = - AMC_BLDC_DW.ZOHBlockInsertedForAdapter_InsertedFor_Adapter4_at_outport_0; - - // RateTransition generated from: '/Adapter3' - rtw_mutex_lock(); - AMC_BLDC_DW.RTBInsertedForAdapter_InsertedFor_Adapter3_at_outport_0_RDBu = - AMC_BLDC_DW.RTBInsertedForAdapter_InsertedFor_Adapter3_at_outport_0_LstB; - rtw_mutex_unlock(); - switch - (AMC_BLDC_DW.RTBInsertedForAdapter_InsertedFor_Adapter3_at_outport_0_RDBu) { - case 0: - // RateTransition generated from: '/Adapter3' - AMC_BLDC_B.RTBInsertedForAdapter_InsertedFor_Adapter3_at_outport_0 = - AMC_BLDC_DW.RTBInsertedForAdapter_InsertedFor_Adapter3_at_outport_0_Buf0; - break; - - case 1: - // RateTransition generated from: '/Adapter3' - AMC_BLDC_B.RTBInsertedForAdapter_InsertedFor_Adapter3_at_outport_0 = - AMC_BLDC_DW.RTBInsertedForAdapter_InsertedFor_Adapter3_at_outport_0_Buf1; - break; - - case 2: - // RateTransition generated from: '/Adapter3' - AMC_BLDC_B.RTBInsertedForAdapter_InsertedFor_Adapter3_at_outport_0 = - AMC_BLDC_DW.RTBInsertedForAdapter_InsertedFor_Adapter3_at_outport_0_Buf2; - break; - } + FOCOutputs rtb_TmpRTBAtSupervisorFSM_TXInport4; - // End of RateTransition generated from: '/Adapter3' + // UnitDelay generated from: '/Adapter' + AMC_BLDC_B.ZOHBlockInsertedForAdapter_InsertedFor_Adapter_at_outport_0 = + AMC_BLDC_DW.ZOHBlockInsertedForAdapter_InsertedFor_Adapter_at_outport_0_; - // ModelReference: '/Estimation_Velocity' - estimation_velocity - (&AMC_BLDC_B.RTBInsertedForAdapter_InsertedFor_Adapter3_at_outport_0, - &AMC_BLDC_B.ZOHBlockInsertedForAdapter_InsertedFor_Adapter4_at_outport_0, - &AMC_BLDC_Y.EstimatedData_p.jointvelocities, - &(AMC_BLDC_DW.Estimation_Velocity_InstanceData.rtdw)); - - // RateTransition generated from: '/Adapter1' - rtw_mutex_lock(); - AMC_BLDC_DW.RTBInsertedForAdapter_InsertedFor_Adapter1_at_outport_0_RD_a = - AMC_BLDC_DW.RTBInsertedForAdapter_InsertedFor_Adapter1_at_outport_0_Ls_j; - rtw_mutex_unlock(); - switch - (AMC_BLDC_DW.RTBInsertedForAdapter_InsertedFor_Adapter1_at_outport_0_RD_a) { - case 0: - // RateTransition generated from: '/Adapter1' - AMC_BLDC_B.RTBInsertedForAdapter_InsertedFor_Adapter1_at_outport_0 = - AMC_BLDC_DW.RTBInsertedForAdapter_InsertedFor_Adapter1_at_outport_0_Buf0; - break; - - case 1: - // RateTransition generated from: '/Adapter1' - AMC_BLDC_B.RTBInsertedForAdapter_InsertedFor_Adapter1_at_outport_0 = - AMC_BLDC_DW.RTBInsertedForAdapter_InsertedFor_Adapter1_at_outport_0_Buf1; - break; + // ModelReference: '/CAN_Decoder' incorporates: + // Inport generated from: '/In Bus Element2' - case 2: - // RateTransition generated from: '/Adapter1' - AMC_BLDC_B.RTBInsertedForAdapter_InsertedFor_Adapter1_at_outport_0 = - AMC_BLDC_DW.RTBInsertedForAdapter_InsertedFor_Adapter1_at_outport_0_Buf2; - break; - } + can_decoder(&AMC_BLDC_U.PacketsRx, + &AMC_BLDC_B.ZOHBlockInsertedForAdapter_InsertedFor_Adapter_at_outport_0, + &AMC_BLDC_B.CAN_Decoder_o1[0], &rtb_CAN_Decoder_o2, + &(AMC_BLDC_DW.CAN_Decoder_InstanceData.rtb), + &(AMC_BLDC_DW.CAN_Decoder_InstanceData.rtdw)); - // End of RateTransition generated from: '/Adapter1' + // ModelReference: '/Motion Controller Single' incorporates: + // Inport generated from: '/In Bus Element4' + // Outport generated from: '/Out Bus Element3' + // Outport generated from: '/Out Bus Element2' + // Outport generated from: '/Out Bus Element4' - // ModelReference: '/Filter_Current' - filter_current - (&AMC_BLDC_B.RTBInsertedForAdapter_InsertedFor_Adapter1_at_outport_0, - &AMC_BLDC_Y.EstimatedData_p.Iq_filtered, - &(AMC_BLDC_DW.Filter_Current_InstanceData.rtdw)); + mc_1ms_tick(&AMC_BLDC_B.CAN_Decoder_o1[0], &AMC_BLDC_U.ExternalFlags_p, + &AMC_BLDC_Y.EstimatedData_p, &AMC_BLDC_Y.Flags_p, + &AMC_BLDC_Y.ConfigurationParameters, + &(AMC_BLDC_DW.MotionControllerSingle_InstanceData.rtb), + &(AMC_BLDC_DW.MotionControllerSingle_InstanceData.rtdw)); - // RateTransition generated from: '/Adapter1' + // RateTransition generated from: '/SupervisorFSM_TX' rtw_mutex_lock(); - AMC_BLDC_DW.RTBInsertedForAdapter_InsertedFor_Adapter1_at_outport_0_RDBu = - AMC_BLDC_DW.RTBInsertedForAdapter_InsertedFor_Adapter1_at_outport_0_LstB; + AMC_BLDC_DW.TmpRTBAtSupervisorFSM_TXInport4_RDBuf = + AMC_BLDC_DW.TmpRTBAtSupervisorFSM_TXInport4_LstBufWR; rtw_mutex_unlock(); - switch - (AMC_BLDC_DW.RTBInsertedForAdapter_InsertedFor_Adapter1_at_outport_0_RDBu) { + switch (AMC_BLDC_DW.TmpRTBAtSupervisorFSM_TXInport4_RDBuf) { case 0: - AMC_BLDC_Y.EstimatedData_p.motor_temperature = - AMC_BLDC_DW.RTBInsertedForAdapter_InsertedFor_Adapter1_at_outport_0_Bu_d; + rtb_TmpRTBAtSupervisorFSM_TXInport4 = + AMC_BLDC_DW.TmpRTBAtSupervisorFSM_TXInport4_Buf0; break; case 1: - AMC_BLDC_Y.EstimatedData_p.motor_temperature = - AMC_BLDC_DW.RTBInsertedForAdapter_InsertedFor_Adapter1_at_outport_0_Bu_j; + rtb_TmpRTBAtSupervisorFSM_TXInport4 = + AMC_BLDC_DW.TmpRTBAtSupervisorFSM_TXInport4_Buf1; break; case 2: - AMC_BLDC_Y.EstimatedData_p.motor_temperature = - AMC_BLDC_DW.RTBInsertedForAdapter_InsertedFor_Adapter1_at_outport_0_Bu_o; + rtb_TmpRTBAtSupervisorFSM_TXInport4 = + AMC_BLDC_DW.TmpRTBAtSupervisorFSM_TXInport4_Buf2; break; } - // End of RateTransition generated from: '/Adapter1' - - // ModelReference: '/CAN_Decoder' incorporates: - // Inport generated from: '/In Bus Element2' - - can_decoder(&AMC_BLDC_U.PacketsRx, - &AMC_BLDC_B.ZOHBlockInsertedForAdapter_InsertedFor_Adapter4_at_outport_0, - &AMC_BLDC_B.CAN_Decoder_o1, &AMC_BLDC_B.CAN_Decoder_o2, - &AMC_BLDC_B.CAN_Decoder_o3); + // End of RateTransition generated from: '/SupervisorFSM_TX' - // ModelReference: '/SupervisorFSM_RX' incorporates: - // Inport generated from: '/In Bus Element' - // Outport generated from: '/Out Bus Element3' - // Outport generated from: '/Out Bus Element2' - // Outport generated from: '/Out Bus Element4' - - SupervisorFSM_RX - (&AMC_BLDC_B.RTBInsertedForAdapter_InsertedFor_Adapter3_at_outport_0, - &AMC_BLDC_U.ExternalFlags_p, - &AMC_BLDC_B.RTBInsertedForAdapter_InsertedFor_Adapter1_at_outport_0, - &AMC_BLDC_B.CAN_Decoder_o1, &AMC_BLDC_Y.EstimatedData_p, - &AMC_BLDC_B.CAN_Decoder_o2, &AMC_BLDC_B.CAN_Decoder_o3, &AMC_BLDC_Y.Flags_p, - &rtb_SupervisorFSM_RX_o2, &AMC_BLDC_Y.ConfigurationParameters_p); - - // ModelReference: '/SupervisorFSM_TX' incorporates: - // Outport generated from: '/Out Bus Element3' + // ModelReference generated from: '/SupervisorFSM_TX' incorporates: + // Inport generated from: '/In Bus Element4' + // Inport generated from: '/In Bus Element3' // Outport generated from: '/Out Bus Element2' // Outport generated from: '/Out Bus Element4' - SupervisorFSM_TX - (&AMC_BLDC_B.RTBInsertedForAdapter_InsertedFor_Adapter3_at_outport_0, - &AMC_BLDC_Y.EstimatedData_p, &AMC_BLDC_Y.Flags_p, - &AMC_BLDC_B.RTBInsertedForAdapter_InsertedFor_Adapter1_at_outport_0, - &AMC_BLDC_B.MessagesTx, &rtb_SupervisorFSM_TX_o2); + SupervisorFSM_TX(&AMC_BLDC_U.SensorsData_p, &AMC_BLDC_Y.EstimatedData_p, + &AMC_BLDC_Y.Flags_p, &rtb_TmpRTBAtSupervisorFSM_TXInport4, + &AMC_BLDC_U.ExternalFlags_p.fault_button, + &AMC_BLDC_B.MessagesTx, &rtb_SupervisorFSM_TX_o2, + &(AMC_BLDC_DW.SupervisorFSM_TX_InstanceData.rtb), + &(AMC_BLDC_DW.SupervisorFSM_TX_InstanceData.rtdw)); - // ModelReference: '/CAN_Encoder' incorporates: + // ModelReference: '/CAN_Encoder' incorporates: // Outport generated from: '/Out Bus Element1' can_encoder(&AMC_BLDC_B.MessagesTx, &rtb_SupervisorFSM_TX_o2, - &AMC_BLDC_B.ZOHBlockInsertedForAdapter_InsertedFor_Adapter4_at_outport_0, - &AMC_BLDC_Y.PacketsTx); + &AMC_BLDC_B.ZOHBlockInsertedForAdapter_InsertedFor_Adapter_at_outport_0, + &AMC_BLDC_Y.PacketsTx, &(AMC_BLDC_DW.CAN_Encoder_InstanceData.rtb)); - // ModelReference: '/OuterControl' incorporates: + // Update for UnitDelay generated from: '/Adapter' incorporates: // Outport generated from: '/Out Bus Element3' - // Outport generated from: '/Out Bus Element2' - // Outport generated from: '/Out Bus Element4' - control_outer(&AMC_BLDC_Y.Flags_p, &AMC_BLDC_Y.ConfigurationParameters_p, - &rtb_SupervisorFSM_RX_o2, - &AMC_BLDC_B.RTBInsertedForAdapter_InsertedFor_Adapter3_at_outport_0, - &AMC_BLDC_Y.EstimatedData_p, &rtb_OuterControl, - &(AMC_BLDC_DW.OuterControl_InstanceData.rtb), - &(AMC_BLDC_DW.OuterControl_InstanceData.rtdw), - &(AMC_BLDC_DW.OuterControl_InstanceData.rtzce)); - - // RateTransition generated from: '/Adapter2' - rtw_mutex_lock(); - wrBufIdx = static_cast - (AMC_BLDC_DW.RTBInsertedForAdapter_InsertedFor_Adapter2_at_outport_0_5_Ls + - 1); - if (wrBufIdx == 3) { - wrBufIdx = 0; - } - - if (wrBufIdx == - AMC_BLDC_DW.RTBInsertedForAdapter_InsertedFor_Adapter2_at_outport_0_5_RD) - { - wrBufIdx = static_cast(wrBufIdx + 1); - if (wrBufIdx == 3) { - wrBufIdx = 0; - } - } - - rtw_mutex_unlock(); - AMC_BLDC_DW.RTBInsertedForAdapter_InsertedFor_Adapter2_at_outport_0_5_Bu[wrBufIdx] - = rtb_OuterControl; - AMC_BLDC_DW.RTBInsertedForAdapter_InsertedFor_Adapter2_at_outport_0_5_Ls = - wrBufIdx; - - // RateTransition generated from: '/Adapter2' incorporates: - // Outport generated from: '/Out Bus Element4' - - rtw_mutex_lock(); - wrBufIdx = static_cast - (AMC_BLDC_DW.RTBInsertedForAdapter_InsertedFor_Adapter2_at_outport_0_1_Ls + - 1); - if (wrBufIdx == 3) { - wrBufIdx = 0; - } - - if (wrBufIdx == - AMC_BLDC_DW.RTBInsertedForAdapter_InsertedFor_Adapter2_at_outport_0_1_RD) - { - wrBufIdx = static_cast(wrBufIdx + 1); - if (wrBufIdx == 3) { - wrBufIdx = 0; - } - } - - rtw_mutex_unlock(); - AMC_BLDC_DW.RTBInsertedForAdapter_InsertedFor_Adapter2_at_outport_0_1_Bu[wrBufIdx] - = AMC_BLDC_Y.Flags_p; - AMC_BLDC_DW.RTBInsertedForAdapter_InsertedFor_Adapter2_at_outport_0_1_Ls = - wrBufIdx; - - // RateTransition generated from: '/Adapter2' incorporates: - // Outport generated from: '/Out Bus Element3' - - rtw_mutex_lock(); - wrBufIdx = static_cast - (AMC_BLDC_DW.RTBInsertedForAdapter_InsertedFor_Adapter2_at_outport_0_2_Ls + - 1); - if (wrBufIdx == 3) { - wrBufIdx = 0; - } - - if (wrBufIdx == - AMC_BLDC_DW.RTBInsertedForAdapter_InsertedFor_Adapter2_at_outport_0_2_RD) - { - wrBufIdx = static_cast(wrBufIdx + 1); - if (wrBufIdx == 3) { - wrBufIdx = 0; - } - } - - rtw_mutex_unlock(); - AMC_BLDC_DW.RTBInsertedForAdapter_InsertedFor_Adapter2_at_outport_0_2_Bu[wrBufIdx] - = AMC_BLDC_Y.ConfigurationParameters_p; - AMC_BLDC_DW.RTBInsertedForAdapter_InsertedFor_Adapter2_at_outport_0_2_Ls = - wrBufIdx; - - // RateTransition generated from: '/Adapter2' - rtw_mutex_lock(); - wrBufIdx = static_cast - (AMC_BLDC_DW.RTBInsertedForAdapter_InsertedFor_Adapter2_at_outport_0_4_Ls + - 1); - if (wrBufIdx == 3) { - wrBufIdx = 0; - } - - if (wrBufIdx == - AMC_BLDC_DW.RTBInsertedForAdapter_InsertedFor_Adapter2_at_outport_0_4_RD) - { - wrBufIdx = static_cast(wrBufIdx + 1); - if (wrBufIdx == 3) { - wrBufIdx = 0; - } - } - - rtw_mutex_unlock(); - AMC_BLDC_DW.RTBInsertedForAdapter_InsertedFor_Adapter2_at_outport_0_4_Bu[wrBufIdx] - = rtb_SupervisorFSM_RX_o2; - AMC_BLDC_DW.RTBInsertedForAdapter_InsertedFor_Adapter2_at_outport_0_4_Ls = - wrBufIdx; - - // RateTransition generated from: '/Adapter2' incorporates: - // Outport generated from: '/Out Bus Element2' - - rtw_mutex_lock(); - wrBufIdx = static_cast - (AMC_BLDC_DW.RTBInsertedForAdapter_InsertedFor_Adapter2_at_outport_0_3_Ls + - 1); - if (wrBufIdx == 3) { - wrBufIdx = 0; - } - - if (wrBufIdx == - AMC_BLDC_DW.RTBInsertedForAdapter_InsertedFor_Adapter2_at_outport_0_3_RD) - { - wrBufIdx = static_cast(wrBufIdx + 1); - if (wrBufIdx == 3) { - wrBufIdx = 0; - } - } - - rtw_mutex_unlock(); - AMC_BLDC_DW.RTBInsertedForAdapter_InsertedFor_Adapter2_at_outport_0_3_Bu[wrBufIdx] - = AMC_BLDC_Y.EstimatedData_p; - AMC_BLDC_DW.RTBInsertedForAdapter_InsertedFor_Adapter2_at_outport_0_3_Ls = - wrBufIdx; - - // Update for UnitDelay generated from: '/Adapter4' incorporates: - // Outport generated from: '/Out Bus Element3' - - AMC_BLDC_DW.ZOHBlockInsertedForAdapter_InsertedFor_Adapter4_at_outport_0 = - AMC_BLDC_Y.ConfigurationParameters_p; + AMC_BLDC_DW.ZOHBlockInsertedForAdapter_InsertedFor_Adapter_at_outport_0_ = + AMC_BLDC_Y.ConfigurationParameters; } // Model initialize function void AMC_BLDC_initialize(void) { - // Model Initialize function for ModelReference Block: '/Estimation_Velocity' - estimation_velocity_initialize(rtmGetErrorStatusPointer(AMC_BLDC_M), - &(AMC_BLDC_DW.Estimation_Velocity_InstanceData.rtm), - &(AMC_BLDC_DW.Estimation_Velocity_InstanceData.rtdw)); - - // Model Initialize function for ModelReference Block: '/Filter_Current' - filter_current_initialize(rtmGetErrorStatusPointer(AMC_BLDC_M), - &(AMC_BLDC_DW.Filter_Current_InstanceData.rtm), - &(AMC_BLDC_DW.Filter_Current_InstanceData.rtdw)); - - // Model Initialize function for ModelReference Block: '/FOC' - control_foc_initialize(rtmGetErrorStatusPointer(AMC_BLDC_M), - &(AMC_BLDC_DW.FOC_InstanceData.rtm), &(AMC_BLDC_DW.FOC_InstanceData.rtb), - &(AMC_BLDC_DW.FOC_InstanceData.rtdw), &(AMC_BLDC_DW.FOC_InstanceData.rtzce)); - - // Model Initialize function for ModelReference Block: '/CAN_Decoder' - can_decoder_initialize(rtmGetErrorStatusPointer(AMC_BLDC_M)); + // Model Initialize function for ModelReference Block: '/CAN_Decoder' + can_decoder_initialize(rtmGetErrorStatusPointer(AMC_BLDC_M), + &(AMC_BLDC_DW.CAN_Decoder_InstanceData.rtm)); - // Model Initialize function for ModelReference Block: '/CAN_Encoder' - can_encoder_initialize(rtmGetErrorStatusPointer(AMC_BLDC_M)); + // Model Initialize function for ModelReference Block: '/CAN_Encoder' + can_encoder_initialize(rtmGetErrorStatusPointer(AMC_BLDC_M), + &(AMC_BLDC_DW.CAN_Encoder_InstanceData.rtm)); - // Model Initialize function for ModelReference Block: '/OuterControl' - control_outer_initialize(rtmGetErrorStatusPointer(AMC_BLDC_M), - &(AMC_BLDC_DW.OuterControl_InstanceData.rtm), - &(AMC_BLDC_DW.OuterControl_InstanceData.rtb), - &(AMC_BLDC_DW.OuterControl_InstanceData.rtdw), - &(AMC_BLDC_DW.OuterControl_InstanceData.rtzce)); + // Model Initialize function for ModelReference Block: '/Motion Controller Single' + mc_initialize(rtmGetErrorStatusPointer(AMC_BLDC_M), + &(AMC_BLDC_DW.MotionControllerSingle_InstanceData.rtm), + &(AMC_BLDC_DW.MotionControllerSingle_InstanceData.rtdw)); - // Model Initialize function for ModelReference Block: '/SupervisorFSM_RX' - SupervisorFSM_RX_initialize(rtmGetErrorStatusPointer(AMC_BLDC_M)); + // Model Initialize function for ModelReference Block: '/SupervisorFSM_TX' + SupervisorFSM_TX_initialize(rtmGetErrorStatusPointer(AMC_BLDC_M), + &(AMC_BLDC_DW.SupervisorFSM_TX_InstanceData.rtm)); - // Model Initialize function for ModelReference Block: '/SupervisorFSM_TX' - SupervisorFSM_TX_initialize(rtmGetErrorStatusPointer(AMC_BLDC_M)); - - // Start for RateTransition generated from: '/Adapter1' - rtw_mutex_init(); - - // Start for RateTransition generated from: '/Adapter2' - rtw_mutex_init(); - - // Start for RateTransition generated from: '/Adapter2' + // Start for RateTransition generated from: '/SupervisorFSM_TX' rtw_mutex_init(); - // Start for RateTransition generated from: '/Adapter2' - rtw_mutex_init(); - - // Start for RateTransition generated from: '/Adapter2' - rtw_mutex_init(); - - // Start for RateTransition generated from: '/Adapter2' - rtw_mutex_init(); - - // Start for RateTransition generated from: '/Adapter1' - rtw_mutex_init(); - - // Start for RateTransition generated from: '/Adapter3' - rtw_mutex_init(); - - // SystemInitialize for ModelReference: '/Estimation_Velocity' - estimation_velocity_Init(&(AMC_BLDC_DW.Estimation_Velocity_InstanceData.rtdw)); - - // SystemInitialize for ModelReference: '/Filter_Current' - filter_current_Init(&(AMC_BLDC_DW.Filter_Current_InstanceData.rtdw)); - - // SystemInitialize for ModelReference: '/FOC' - control_foc_Init(&(AMC_BLDC_DW.FOC_InstanceData.rtdw)); - - // SystemInitialize for ModelReference: '/CAN_Decoder' incorporates: - // Inport generated from: '/In Bus Element2' - - can_decoder_Init(); - - // SystemInitialize for ModelReference: '/OuterControl' - control_outer_Init(&(AMC_BLDC_DW.OuterControl_InstanceData.rtdw)); + // SystemInitialize for ModelReference: '/CAN_Decoder' + can_decoder_Init(&(AMC_BLDC_DW.CAN_Decoder_InstanceData.rtb), + &(AMC_BLDC_DW.CAN_Decoder_InstanceData.rtdw)); - // SystemInitialize for ModelReference: '/SupervisorFSM_RX' incorporates: - // Inport generated from: '/In Bus Element' + // SystemInitialize for ModelReference: '/Motion Controller Single' incorporates: // Outport generated from: '/Out Bus Element3' // Outport generated from: '/Out Bus Element4' - SupervisorFSM_RX_Init(&AMC_BLDC_Y.Flags_p, - &AMC_BLDC_Y.ConfigurationParameters_p); - - // SystemInitialize for ModelReference: '/SupervisorFSM_TX' incorporates: - // Outport generated from: '/Out Bus Element3' - // Outport generated from: '/Out Bus Element4' + motion_controller_single_Init(&AMC_BLDC_Y.Flags_p, + &AMC_BLDC_Y.ConfigurationParameters, + &(AMC_BLDC_DW.MotionControllerSingle_InstanceData.rtdw)); + // SystemInitialize for ModelReference generated from: '/SupervisorFSM_TX' SupervisorFSM_TX_Init(&AMC_BLDC_B.MessagesTx); - // Enable for ModelReference: '/OuterControl' - control_outer_Enable(&(AMC_BLDC_DW.OuterControl_InstanceData.rtdw)); + // Enable for ModelReference: '/Motion Controller Single' + motion_controller_single_Enable + (&(AMC_BLDC_DW.MotionControllerSingle_InstanceData.rtdw)); } // Model terminate function void AMC_BLDC_terminate(void) { - // Terminate for RateTransition generated from: '/Adapter1' - rtw_mutex_destroy(); - - // Terminate for RateTransition generated from: '/Adapter2' - rtw_mutex_destroy(); - - // Terminate for RateTransition generated from: '/Adapter2' - rtw_mutex_destroy(); + // Terminate for ModelReference: '/Motion Controller Single' + mc_terminate(&(AMC_BLDC_DW.MotionControllerSingle_InstanceData.rtdw)); - // Terminate for RateTransition generated from: '/Adapter2' + // Terminate for RateTransition generated from: '/SupervisorFSM_TX' rtw_mutex_destroy(); - - // Terminate for RateTransition generated from: '/Adapter2' - rtw_mutex_destroy(); - - // Terminate for RateTransition generated from: '/Adapter2' - rtw_mutex_destroy(); - - // Terminate for RateTransition generated from: '/Adapter1' - rtw_mutex_destroy(); - - // Terminate for RateTransition generated from: '/Adapter3' - rtw_mutex_destroy(); - - // Terminate for ModelReference: '/Filter_Current' - filter_current_Term(&(AMC_BLDC_DW.Filter_Current_InstanceData.rtdw)); } // diff --git a/emBODY/eBcode/arch-arm/mbd/amcbldc/amc-bldc/AMC_BLDC.h b/emBODY/eBcode/arch-arm/mbd/amcbldc/amc-bldc/AMC_BLDC.h index 1f4da48af..8dc061ac1 100644 --- a/emBODY/eBcode/arch-arm/mbd/amcbldc/amc-bldc/AMC_BLDC.h +++ b/emBODY/eBcode/arch-arm/mbd/amcbldc/amc-bldc/AMC_BLDC.h @@ -7,9 +7,9 @@ // // Code generated for Simulink model 'AMC_BLDC'. // -// Model version : 7.13 +// Model version : 7.54 // Simulink Coder version : 23.2 (R2023b) 01-Aug-2023 -// C/C++ source code generated on : Tue Feb 13 11:55:25 2024 +// C/C++ source code generated on : Wed Feb 28 11:29:48 2024 // // Target selection: ert.tlc // Embedded hardware selection: ARM Compatible->ARM Cortex-M @@ -20,11 +20,12 @@ #define RTW_HEADER_AMC_BLDC_h_ #include "rtwtypes.h" #include "AMC_BLDC_types.h" -#include "control_foc.h" -#include "estimation_velocity.h" -#include "filter_current.h" -#include "control_outer.h" +#include "motion_controller_single.h" +#include "can_decoder.h" +#include "SupervisorFSM_TX.h" +#include "can_encoder.h" #include +#include "mc_supervisor_defines.h" #include "zero_crossing_types.h" // Macros for accessing real-time model data structure @@ -50,65 +51,26 @@ // Block signals (default storage) struct B_AMC_BLDC_T { - FOCSlowInputs BusConversion_InsertedFor_FOC_at_inport_1_BusCreator1; - BUS_MESSAGES_RX_MULTIPLE CAN_Decoder_o1;// '/CAN_Decoder' - ConfigurationParameters - ZOHBlockInsertedForAdapter_InsertedFor_Adapter4_at_outport_0;// '/Adapter4' - BUS_MESSAGES_TX MessagesTx; // '/SupervisorFSM_TX' - SensorsData RTBInsertedForAdapter_InsertedFor_Adapter3_at_outport_0;// '/Adapter3' - ControlOutputs RTBInsertedForAdapter_InsertedFor_Adapter1_at_outport_0;// '/Adapter1' - BUS_STATUS_RX_MULTIPLE CAN_Decoder_o2;// '/CAN_Decoder' - BUS_CAN_RX_ERRORS_MULTIPLE CAN_Decoder_o3;// '/CAN_Decoder' + ActuatorConfiguration + ZOHBlockInsertedForAdapter_InsertedFor_Adapter_at_outport_0;// '/Adapter' + SupervisorReceivedEvents CAN_Decoder_o1[MAX_EVENTS_PER_TICK];// '/CAN_Decoder' + BUS_MESSAGES_TX MessagesTx; // '/SupervisorFSM_TX' }; // Block states (default storage) for system '' struct DW_AMC_BLDC_T { - ConfigurationParameters - ZOHBlockInsertedForAdapter_InsertedFor_Adapter4_at_outport_0;// synthesized block - ConfigurationParameters - RTBInsertedForAdapter_InsertedFor_Adapter2_at_outport_0_2_Bu[3];// synthesized block - SensorsData RTBInsertedForAdapter_InsertedFor_Adapter3_at_outport_0_Buf0;// synthesized block - SensorsData RTBInsertedForAdapter_InsertedFor_Adapter3_at_outport_0_Buf1;// synthesized block - SensorsData RTBInsertedForAdapter_InsertedFor_Adapter3_at_outport_0_Buf2;// synthesized block - ControlOutputs RTBInsertedForAdapter_InsertedFor_Adapter1_at_outport_0_Buf0;// synthesized block - ControlOutputs RTBInsertedForAdapter_InsertedFor_Adapter1_at_outport_0_Buf1;// synthesized block - ControlOutputs RTBInsertedForAdapter_InsertedFor_Adapter1_at_outport_0_Buf2;// synthesized block - Targets RTBInsertedForAdapter_InsertedFor_Adapter2_at_outport_0_4_Bu[3];// synthesized block - EstimatedData RTBInsertedForAdapter_InsertedFor_Adapter2_at_outport_0_3_Bu[3];// synthesized block - ControlOuterOutputs - RTBInsertedForAdapter_InsertedFor_Adapter2_at_outport_0_5_Bu[3];// synthesized block - Flags RTBInsertedForAdapter_InsertedFor_Adapter2_at_outport_0_1_Bu[3];// synthesized block - MotorTemperature RTBInsertedForAdapter_InsertedFor_Adapter1_at_outport_0_Bu_d;// synthesized block - MotorTemperature RTBInsertedForAdapter_InsertedFor_Adapter1_at_outport_0_Bu_j;// synthesized block - MotorTemperature RTBInsertedForAdapter_InsertedFor_Adapter1_at_outport_0_Bu_o;// synthesized block - void* RTBInsertedForAdapter_InsertedFor_Adapter1_at_outport_0_SEMA;// synthesized block - void* RTBInsertedForAdapter_InsertedFor_Adapter2_at_outport_0_1_SE;// synthesized block - void* RTBInsertedForAdapter_InsertedFor_Adapter2_at_outport_0_2_SE;// synthesized block - void* RTBInsertedForAdapter_InsertedFor_Adapter2_at_outport_0_3_SE;// synthesized block - void* RTBInsertedForAdapter_InsertedFor_Adapter2_at_outport_0_4_SE;// synthesized block - void* RTBInsertedForAdapter_InsertedFor_Adapter2_at_outport_0_5_SE;// synthesized block - void* RTBInsertedForAdapter_InsertedFor_Adapter1_at_outport_0_SE_h;// synthesized block - void* RTBInsertedForAdapter_InsertedFor_Adapter3_at_outport_0_SEMA;// synthesized block - int8_T RTBInsertedForAdapter_InsertedFor_Adapter1_at_outport_0_LstB;// synthesized block - int8_T RTBInsertedForAdapter_InsertedFor_Adapter1_at_outport_0_RDBu;// synthesized block - int8_T RTBInsertedForAdapter_InsertedFor_Adapter2_at_outport_0_1_Ls;// synthesized block - int8_T RTBInsertedForAdapter_InsertedFor_Adapter2_at_outport_0_1_RD;// synthesized block - int8_T RTBInsertedForAdapter_InsertedFor_Adapter2_at_outport_0_2_Ls;// synthesized block - int8_T RTBInsertedForAdapter_InsertedFor_Adapter2_at_outport_0_2_RD;// synthesized block - int8_T RTBInsertedForAdapter_InsertedFor_Adapter2_at_outport_0_3_Ls;// synthesized block - int8_T RTBInsertedForAdapter_InsertedFor_Adapter2_at_outport_0_3_RD;// synthesized block - int8_T RTBInsertedForAdapter_InsertedFor_Adapter2_at_outport_0_4_Ls;// synthesized block - int8_T RTBInsertedForAdapter_InsertedFor_Adapter2_at_outport_0_4_RD;// synthesized block - int8_T RTBInsertedForAdapter_InsertedFor_Adapter2_at_outport_0_5_Ls;// synthesized block - int8_T RTBInsertedForAdapter_InsertedFor_Adapter2_at_outport_0_5_RD;// synthesized block - int8_T RTBInsertedForAdapter_InsertedFor_Adapter1_at_outport_0_Ls_j;// synthesized block - int8_T RTBInsertedForAdapter_InsertedFor_Adapter1_at_outport_0_RD_a;// synthesized block - int8_T RTBInsertedForAdapter_InsertedFor_Adapter3_at_outport_0_LstB;// synthesized block - int8_T RTBInsertedForAdapter_InsertedFor_Adapter3_at_outport_0_RDBu;// synthesized block - MdlrefDW_control_foc_T FOC_InstanceData;// '/FOC' - MdlrefDW_estimation_velocity_T Estimation_Velocity_InstanceData;// '/Estimation_Velocity' - MdlrefDW_filter_current_T Filter_Current_InstanceData;// '/Filter_Current' - MdlrefDW_control_outer_T OuterControl_InstanceData;// '/OuterControl' + ActuatorConfiguration + ZOHBlockInsertedForAdapter_InsertedFor_Adapter_at_outport_0_;// synthesized block + FOCOutputs TmpRTBAtSupervisorFSM_TXInport4_Buf0;// synthesized block + FOCOutputs TmpRTBAtSupervisorFSM_TXInport4_Buf1;// synthesized block + FOCOutputs TmpRTBAtSupervisorFSM_TXInport4_Buf2;// synthesized block + void* TmpRTBAtSupervisorFSM_TXInport4_SEMAPHORE;// synthesized block + int8_T TmpRTBAtSupervisorFSM_TXInport4_LstBufWR;// synthesized block + int8_T TmpRTBAtSupervisorFSM_TXInport4_RDBuf;// synthesized block + MdlrefDW_motion_controller_single_T MotionControllerSingle_InstanceData;// '/Motion Controller Single' + MdlrefDW_can_decoder_T CAN_Decoder_InstanceData;// '/CAN_Decoder' + MdlrefDW_SupervisorFSM_TX_T SupervisorFSM_TX_InstanceData;// '/SupervisorFSM_TX' + MdlrefDW_can_encoder_T CAN_Encoder_InstanceData;// '/CAN_Encoder' }; // External inputs (root inport signals with default storage) @@ -120,8 +82,8 @@ struct ExtU_AMC_BLDC_T { // External outputs (root outports fed by signals with default storage) struct ExtY_AMC_BLDC_T { - ControlOutputs ControlOutputs_p; // '/ControlOutputs' - ConfigurationParameters ConfigurationParameters_p;// '/ConfigurationParameters' + FOCOutputs ControlOutputs; // '/ControlOutputs' + ActuatorConfiguration ConfigurationParameters;// '/ConfigurationParameters' Flags Flags_p; // '/Flags' EstimatedData EstimatedData_p; // '/EstimatedData' BUS_CAN_MULTIPLE PacketsTx; // '/PacketsTx' @@ -189,21 +151,21 @@ extern "C" // these parameters and exports their symbols. // -extern ConfigurationParameters InitConfParams;// Variable: InitConfParams - // Referenced by: '/SupervisorFSM_RX' +extern ActuatorConfiguration InitConfParams;// Variable: InitConfParams + // Referenced by: '/Motion Controller Single' extern real32_T CAN_ANGLE_DEG2ICUB; // Variable: CAN_ANGLE_DEG2ICUB - // Referenced by: '/CAN_Encoder' + // Referenced by: '/CAN_Encoder' // 2^16/360 extern real32_T CAN_ANGLE_ICUB2DEG; // Variable: CAN_ANGLE_ICUB2DEG - // Referenced by: '/CAN_Decoder' + // Referenced by: '/CAN_Decoder' // 360/2^16 extern uint8_T CAN_ID_AMC; // Variable: CAN_ID_AMC // Referenced by: - // '/CAN_Decoder' - // '/CAN_Encoder' + // '/CAN_Decoder' + // '/CAN_Encoder' // 4 bits defining the ID of the AMC_BLDC board. @@ -258,19 +220,8 @@ extern "C" // Here is the system hierarchy for this model // // '' : 'AMC_BLDC' -// '' : 'AMC_BLDC/Adapter1' -// '' : 'AMC_BLDC/Adapter2' -// '' : 'AMC_BLDC/Adapter3' -// '' : 'AMC_BLDC/Adapter4' -// '' : 'AMC_BLDC/Estimation' -// '' : 'AMC_BLDC/Messaging' -// '' : 'AMC_BLDC/Supervision' -// '' : 'AMC_BLDC/Estimation/Adapter' -// '' : 'AMC_BLDC/Estimation/Adapter1' -// '' : 'AMC_BLDC/Estimation/Adapter3' -// '' : 'AMC_BLDC/Estimation/Estimation_Temperature' -// '' : 'AMC_BLDC/Estimation/Mux' -// '' : 'AMC_BLDC/Estimation/Estimation_Temperature/Disabled' +// '' : 'AMC_BLDC/Adapter' +// '' : 'AMC_BLDC/Messaging' #endif // RTW_HEADER_AMC_BLDC_h_ diff --git a/emBODY/eBcode/arch-arm/mbd/amcbldc/amc-bldc/AMC_BLDC_private.h b/emBODY/eBcode/arch-arm/mbd/amcbldc/amc-bldc/AMC_BLDC_private.h index 1de82f4e4..15073bb13 100644 --- a/emBODY/eBcode/arch-arm/mbd/amcbldc/amc-bldc/AMC_BLDC_private.h +++ b/emBODY/eBcode/arch-arm/mbd/amcbldc/amc-bldc/AMC_BLDC_private.h @@ -7,9 +7,9 @@ // // Code generated for Simulink model 'AMC_BLDC'. // -// Model version : 7.13 +// Model version : 7.54 // Simulink Coder version : 23.2 (R2023b) 01-Aug-2023 -// C/C++ source code generated on : Tue Feb 13 11:55:25 2024 +// C/C++ source code generated on : Wed Feb 28 11:29:48 2024 // // Target selection: ert.tlc // Embedded hardware selection: ARM Compatible->ARM Cortex-M diff --git a/emBODY/eBcode/arch-arm/mbd/amcbldc/amc-bldc/AMC_BLDC_types.h b/emBODY/eBcode/arch-arm/mbd/amcbldc/amc-bldc/AMC_BLDC_types.h index 980edb620..c118d3568 100644 --- a/emBODY/eBcode/arch-arm/mbd/amcbldc/amc-bldc/AMC_BLDC_types.h +++ b/emBODY/eBcode/arch-arm/mbd/amcbldc/amc-bldc/AMC_BLDC_types.h @@ -7,9 +7,9 @@ // // Code generated for Simulink model 'AMC_BLDC'. // -// Model version : 7.13 +// Model version : 7.54 // Simulink Coder version : 23.2 (R2023b) 01-Aug-2023 -// C/C++ source code generated on : Tue Feb 13 11:55:25 2024 +// C/C++ source code generated on : Wed Feb 28 11:29:48 2024 // // Target selection: ert.tlc // Embedded hardware selection: ARM Compatible->ARM Cortex-M @@ -22,6 +22,7 @@ // Includes for objects with custom storage classes #include "rtw_defines.h" +#include "mc_supervisor_defines.h" // // Registered constraints for dimension variants @@ -30,17 +31,24 @@ # error "The preprocessor definition 'CAN_MAX_NUM_PACKETS' must be greater than '0'" #endif +#if MAX_EVENTS_PER_TICK <= 0 +# error "The preprocessor definition 'MAX_EVENTS_PER_TICK' must be greater than '0'" +#endif + #if CAN_MAX_NUM_PACKETS >= 16 # error "The preprocessor definition 'CAN_MAX_NUM_PACKETS' must be less than '16'" #endif -#ifndef DEFINED_TYPEDEF_FOR_JointPositions_ -#define DEFINED_TYPEDEF_FOR_JointPositions_ +#if MAX_EVENTS_PER_TICK >= 5 +# error "The preprocessor definition 'MAX_EVENTS_PER_TICK' must be less than '5'" +#endif + +#ifndef DEFINED_TYPEDEF_FOR_DriverSensors_ +#define DEFINED_TYPEDEF_FOR_DriverSensors_ -struct JointPositions +struct DriverSensors { - // joint positions - real32_T position; + real32_T Vcc; }; #endif @@ -62,29 +70,28 @@ struct MotorSensors #endif -#ifndef DEFINED_TYPEDEF_FOR_SupplyVoltage_ -#define DEFINED_TYPEDEF_FOR_SupplyVoltage_ - -struct SupplyVoltage -{ - real32_T voltage; -}; - -#endif - #ifndef DEFINED_TYPEDEF_FOR_SensorsData_ #define DEFINED_TYPEDEF_FOR_SensorsData_ struct SensorsData { // position encoders - JointPositions jointpositions; - - // motor probes + real32_T jointpositions; + DriverSensors driversensors; MotorSensors motorsensors; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_Targets_ +#define DEFINED_TYPEDEF_FOR_Targets_ - // supply probes - SupplyVoltage supplyvoltagesensors; +struct Targets +{ + real32_T jointpositions; + real32_T jointvelocities; + real32_T motorcurrent; + real32_T motorvoltage; }; #endif @@ -100,39 +107,69 @@ typedef enum { ControlModes_Current, ControlModes_Velocity, ControlModes_Voltage, - ControlModes_Torque, ControlModes_HwFaultCM } ControlModes; #endif -#ifndef DEFINED_TYPEDEF_FOR_Flags_ -#define DEFINED_TYPEDEF_FOR_Flags_ +#ifndef DEFINED_TYPEDEF_FOR_PID_ +#define DEFINED_TYPEDEF_FOR_PID_ -struct Flags +struct PID { - // control mode - ControlModes control_mode; - boolean_T enable_sending_msg_status; - boolean_T fault_button; - boolean_T enable_thermal_protection; + ControlModes type; + real32_T OutMax; + real32_T OutMin; + real32_T P; + real32_T I; + real32_T D; + real32_T N; + real32_T I0; + real32_T D0; + uint8_T shift_factor; }; #endif -#ifndef DEFINED_TYPEDEF_FOR_MotorConfig_ -#define DEFINED_TYPEDEF_FOR_MotorConfig_ +#ifndef DEFINED_TYPEDEF_FOR_SupervisorEvents_ +#define DEFINED_TYPEDEF_FOR_SupervisorEvents_ -struct MotorConfig +typedef enum { + SupervisorEvents_None = 0, // Default value + SupervisorEvents_SetLimit, + SupervisorEvents_SetControlMode, + SupervisorEvents_SetMotorConfig, + SupervisorEvents_SetPid, + SupervisorEvents_SetTarget +} SupervisorEvents; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_SupervisorInputLimits_ +#define DEFINED_TYPEDEF_FOR_SupervisorInputLimits_ + +struct SupervisorInputLimits +{ + real32_T overload; + real32_T peak; + real32_T nominal; + ControlModes type; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_MotorConfiguration_ +#define DEFINED_TYPEDEF_FOR_MotorConfiguration_ + +struct MotorConfiguration { - // Angular offset in degrees between the stator windings and the hall sensors. - real32_T hall_sens_offset; boolean_T has_hall_sens; boolean_T has_quadrature_encoder; boolean_T has_speed_quadrature_encoder; boolean_T has_torque_sens; boolean_T use_index; boolean_T enable_verbosity; + int16_T hall_sensors_offset; int16_T rotor_encoder_resolution; int16_T rotor_index_offset; uint8_T encoder_tolerance; @@ -141,6 +178,7 @@ struct MotorConfig real32_T Rphase; real32_T Imin; real32_T Imax; + real32_T Vcc; real32_T Vmax; real32_T resistance; real32_T inductance; @@ -150,77 +188,96 @@ struct MotorConfig #endif -#ifndef DEFINED_TYPEDEF_FOR_EstimationVelocityModes_ -#define DEFINED_TYPEDEF_FOR_EstimationVelocityModes_ +#ifndef DEFINED_TYPEDEF_FOR_SupervisorReceivedEvents_ +#define DEFINED_TYPEDEF_FOR_SupervisorReceivedEvents_ -typedef enum { - EstimationVelocityModes_Disabled = 0,// Default value - EstimationVelocityModes_MovingAverage, - EstimationVelocityModes_LeastSquares -} EstimationVelocityModes; +struct SupervisorReceivedEvents +{ + Targets targets_content; + PID pid_content; + SupervisorEvents event_type; + ControlModes control_mode_content; + SupervisorInputLimits limits_content; + MotorConfiguration motor_config_content; +}; #endif -#ifndef DEFINED_TYPEDEF_FOR_EstimationConfig_ -#define DEFINED_TYPEDEF_FOR_EstimationConfig_ +#ifndef DEFINED_TYPEDEF_FOR_ExternalFlags_ +#define DEFINED_TYPEDEF_FOR_ExternalFlags_ -struct EstimationConfig +struct ExternalFlags { - EstimationVelocityModes velocity_mode; - - // Forgetting factor in [0, 1] for exponential weighting-based estimation of RMS current value - real32_T current_rms_lambda; + // External Fault Button (1 == pressed) + boolean_T fault_button; }; #endif -#ifndef DEFINED_TYPEDEF_FOR_PIDConfig_ -#define DEFINED_TYPEDEF_FOR_PIDConfig_ +#ifndef DEFINED_TYPEDEF_FOR_FOCOutputs_ +#define DEFINED_TYPEDEF_FOR_FOCOutputs_ -struct PIDConfig +struct FOCOutputs { - real32_T OutMax; - real32_T OutMin; - real32_T P; - real32_T I; - real32_T D; - real32_T N; - real32_T I0; - real32_T D0; - uint8_T shift_factor; + // control effort (quadrature) + real32_T Vq; + + // control effort (3-phases) + real32_T Vabc[3]; + + // quadrature current + real32_T Iq_fbk; + + // direct current + real32_T Id_fbk; + + // RMS of Iq + real32_T Iq_rms; + + // RMS of Id + real32_T Id_rms; }; #endif -#ifndef DEFINED_TYPEDEF_FOR_Thresholds_ -#define DEFINED_TYPEDEF_FOR_Thresholds_ +#ifndef DEFINED_TYPEDEF_FOR_EstimatedData_ +#define DEFINED_TYPEDEF_FOR_EstimatedData_ -struct Thresholds +struct EstimatedData { - // It shall be greater than hardwareJntPosMin - real32_T jntPosMin; + // velocity + real32_T jointvelocities; + + // filtered motor current + real32_T Iq_filtered; + + // motor temperature + real32_T motor_temperature; +}; - // It shall be smaller than hardwareJntPosMax - real32_T jntPosMax; +#endif - // Imposed by hardware constraint - real32_T hardwareJntPosMin; +#ifndef DEFINED_TYPEDEF_FOR_Flags_ +#define DEFINED_TYPEDEF_FOR_Flags_ - // Imposed by hardware constraint - real32_T hardwareJntPosMax; +struct Flags +{ + boolean_T enable_sending_msg_status; - // If robotMin == rotorMax == 0, there's no check - real32_T rotorPosMin; + // control mode + ControlModes control_mode; + boolean_T enable_thermal_protection; +}; - // If robotMin == rotorMax == 0, there's no check - real32_T rotorPosMax; +#endif - // Can be only non-negative - real32_T jntVelMax; +#ifndef DEFINED_TYPEDEF_FOR_Thresholds_ +#define DEFINED_TYPEDEF_FOR_Thresholds_ - // Timeout on reception of velocity setpoint +struct Thresholds +{ // Can be only non-negative - uint32_T velocityTimeout; + real32_T jntVelMax; // Current that can be kept for an indefinite period of time w/o damaging the motor // Expressed in [A] as all the internal computations are done this way @@ -248,159 +305,44 @@ struct Thresholds #endif -#ifndef DEFINED_TYPEDEF_FOR_ConfigurationParameters_ -#define DEFINED_TYPEDEF_FOR_ConfigurationParameters_ +#ifndef DEFINED_TYPEDEF_FOR_PIDsConfiguration_ +#define DEFINED_TYPEDEF_FOR_PIDsConfiguration_ -struct ConfigurationParameters +struct PIDsConfiguration { - MotorConfig motorconfig; - EstimationConfig estimationconfig; - PIDConfig CurLoopPID; - PIDConfig PosLoopPID; - PIDConfig VelLoopPID; - PIDConfig DirLoopPID; - Thresholds thresholds; - real32_T environment_temperature; + PID currentPID; + PID velocityPID; + PID positionPID; }; #endif -#ifndef DEFINED_TYPEDEF_FOR_JointVelocities_ -#define DEFINED_TYPEDEF_FOR_JointVelocities_ +#ifndef DEFINED_TYPEDEF_FOR_ActuatorConfiguration_ +#define DEFINED_TYPEDEF_FOR_ActuatorConfiguration_ -struct JointVelocities +struct ActuatorConfiguration { - // joint velocities - real32_T velocity; + Thresholds thresholds; + PIDsConfiguration pids; + MotorConfiguration motor; }; #endif -#ifndef DEFINED_TYPEDEF_FOR_MotorCurrent_ -#define DEFINED_TYPEDEF_FOR_MotorCurrent_ +#ifndef DEFINED_TYPEDEF_FOR_BUS_MSG_FOC_ +#define DEFINED_TYPEDEF_FOR_BUS_MSG_FOC_ -struct MotorCurrent +// Fields of a FOC message. +struct BUS_MSG_FOC { - // motor current + // Current feedback in A. real32_T current; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_MotorTemperature_ -#define DEFINED_TYPEDEF_FOR_MotorTemperature_ -struct MotorTemperature -{ - // motor temperature - real32_T temperature; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_EstimatedData_ -#define DEFINED_TYPEDEF_FOR_EstimatedData_ - -struct EstimatedData -{ - // velocity - JointVelocities jointvelocities; - - // filtered motor current - MotorCurrent Iq_filtered; - - // motor temperature - MotorTemperature motor_temperature; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_MotorVoltage_ -#define DEFINED_TYPEDEF_FOR_MotorVoltage_ - -struct MotorVoltage -{ - // motor voltage - real32_T voltage; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_Targets_ -#define DEFINED_TYPEDEF_FOR_Targets_ - -struct Targets -{ - JointPositions jointpositions; - JointVelocities jointvelocities; - MotorCurrent motorcurrent; - MotorVoltage motorvoltage; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_ControlOuterOutputs_ -#define DEFINED_TYPEDEF_FOR_ControlOuterOutputs_ - -struct ControlOuterOutputs -{ - boolean_T vel_en; - boolean_T cur_en; - boolean_T out_en; - boolean_T pid_reset; - MotorCurrent motorcurrent; - real32_T current_limiter; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_FOCSlowInputs_ -#define DEFINED_TYPEDEF_FOR_FOCSlowInputs_ - -struct FOCSlowInputs -{ - Flags flags; - ConfigurationParameters configurationparameters; - EstimatedData estimateddata; - Targets targets; - ControlOuterOutputs controlouteroutputs; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_ControlOutputs_ -#define DEFINED_TYPEDEF_FOR_ControlOutputs_ - -struct ControlOutputs -{ - // control effort (quadrature) - real32_T Vq; - - // control effort (3-phases) - real32_T Vabc[3]; - - // quadrature current - MotorCurrent Iq_fbk; - - // direct current - MotorCurrent Id_fbk; - - // RMS of Iq - MotorCurrent Iq_rms; - - // RMS of Id - MotorCurrent Id_rms; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_ExternalFlags_ -#define DEFINED_TYPEDEF_FOR_ExternalFlags_ + // Position feedback in deg. + real32_T position; -struct ExternalFlags -{ - // External Fault Button (1 == pressed) - boolean_T fault_button; + // Velocity feedback in deg/s. + real32_T velocity; }; #endif @@ -420,217 +362,6 @@ typedef enum { #endif -#ifndef DEFINED_TYPEDEF_FOR_BUS_MSG_CONTROL_MODE_ -#define DEFINED_TYPEDEF_FOR_BUS_MSG_CONTROL_MODE_ - -// Fields of a CONTROL_MODE message. -struct BUS_MSG_CONTROL_MODE -{ - // Motor selector. - boolean_T motor; - - // Control mode. - MCControlModes mode; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_BUS_MSG_CURRENT_LIMIT_ -#define DEFINED_TYPEDEF_FOR_BUS_MSG_CURRENT_LIMIT_ - -// Fields of a CURRENT_LIMIT message. -struct BUS_MSG_CURRENT_LIMIT -{ - // Motor selector. - boolean_T motor; - - // Nominal current in A. - real32_T nominal; - - // Peak current in A. - real32_T peak; - - // Overload current in A. - real32_T overload; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_BUS_MSG_DESIRED_TARGETS_ -#define DEFINED_TYPEDEF_FOR_BUS_MSG_DESIRED_TARGETS_ - -// Fields of a DESIRED_TARGETS message. -struct BUS_MSG_DESIRED_TARGETS -{ - // Target current in A. - real32_T current; - - // Target voltage in %. - real32_T voltage; - - // Target veocity in deg/s. - real32_T velocity; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_BUS_MSG_PID_ -#define DEFINED_TYPEDEF_FOR_BUS_MSG_PID_ - -// Fields of a CURRENT_PID message. -struct BUS_MSG_PID -{ - // Motor selector. - boolean_T motor; - - // Proportional gain. - real32_T Kp; - - // Integral gain. - real32_T Ki; - - // Derivative gain. - real32_T Kd; - - // Shift factor. - uint8_T Ks; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_BUS_MSG_MOTOR_CONFIG_ -#define DEFINED_TYPEDEF_FOR_BUS_MSG_MOTOR_CONFIG_ - -struct BUS_MSG_MOTOR_CONFIG -{ - boolean_T has_hall_sens; - boolean_T has_quadrature_encoder; - boolean_T has_speed_quadrature_encoder; - boolean_T has_torque_sens; - boolean_T use_index; - boolean_T enable_verbosity; - - // Number of polese of the motor. - uint8_T number_poles; - - // Encoder tolerance. - uint8_T encoder_tolerance; - - // Resolution of rotor encoder. - int16_T rotor_encoder_resolution; - - // Offset of the rotor encoder. - int16_T rotor_index_offset; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_BUS_MESSAGES_RX_ -#define DEFINED_TYPEDEF_FOR_BUS_MESSAGES_RX_ - -// Aggregate of all CAN received messages. -struct BUS_MESSAGES_RX -{ - BUS_MSG_CONTROL_MODE control_mode; - BUS_MSG_CURRENT_LIMIT current_limit; - BUS_MSG_DESIRED_TARGETS desired_targets; - BUS_MSG_PID pid; - BUS_MSG_MOTOR_CONFIG motor_config; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_BUS_MESSAGES_RX_MULTIPLE_ -#define DEFINED_TYPEDEF_FOR_BUS_MESSAGES_RX_MULTIPLE_ - -struct BUS_MESSAGES_RX_MULTIPLE -{ - BUS_MESSAGES_RX messages[CAN_MAX_NUM_PACKETS]; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_BUS_STATUS_RX_ -#define DEFINED_TYPEDEF_FOR_BUS_STATUS_RX_ - -// Aggregate of all events specifying types of received messages. -struct BUS_STATUS_RX -{ - boolean_T control_mode; - boolean_T current_limit; - boolean_T desired_targets; - boolean_T current_pid; - boolean_T velocity_pid; - boolean_T motor_config; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_BUS_STATUS_RX_MULTIPLE_ -#define DEFINED_TYPEDEF_FOR_BUS_STATUS_RX_MULTIPLE_ - -struct BUS_STATUS_RX_MULTIPLE -{ - BUS_STATUS_RX status[CAN_MAX_NUM_PACKETS]; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_CANErrorTypes_ -#define DEFINED_TYPEDEF_FOR_CANErrorTypes_ - -typedef enum { - CANErrorTypes_No_Error = 0, // Default value - CANErrorTypes_Packet_Not4Us, - CANErrorTypes_Packet_Unrecognized, - CANErrorTypes_Packet_Malformed, - CANErrorTypes_Packet_MultiFunctionsDetected, - CANErrorTypes_Mode_Unrecognized -} CANErrorTypes; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_BUS_CAN_RX_ERRORS_ -#define DEFINED_TYPEDEF_FOR_BUS_CAN_RX_ERRORS_ - -// Specifies the CAN error types. -struct BUS_CAN_RX_ERRORS -{ - // True if an error has been detected. - boolean_T event; - CANErrorTypes type; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_BUS_CAN_RX_ERRORS_MULTIPLE_ -#define DEFINED_TYPEDEF_FOR_BUS_CAN_RX_ERRORS_MULTIPLE_ - -struct BUS_CAN_RX_ERRORS_MULTIPLE -{ - BUS_CAN_RX_ERRORS errors[CAN_MAX_NUM_PACKETS]; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_BUS_MSG_FOC_ -#define DEFINED_TYPEDEF_FOR_BUS_MSG_FOC_ - -// Fields of a FOC message. -struct BUS_MSG_FOC -{ - // Current feedback in A. - real32_T current; - - // Position feedback in deg. - real32_T position; - - // Velocity feedback in deg/s. - real32_T velocity; -}; - -#endif - #ifndef DEFINED_TYPEDEF_FOR_BUS_FLAGS_TX_ #define DEFINED_TYPEDEF_FOR_BUS_FLAGS_TX_ @@ -752,6 +483,43 @@ struct BUS_CAN_MULTIPLE #endif +#ifndef DEFINED_TYPEDEF_FOR_CANErrorTypes_ +#define DEFINED_TYPEDEF_FOR_CANErrorTypes_ + +typedef enum { + CANErrorTypes_No_Error = 0, // Default value + CANErrorTypes_Packet_Not4Us, + CANErrorTypes_Packet_Unrecognized, + CANErrorTypes_Packet_Malformed, + CANErrorTypes_Packet_MultiFunctionsDetected, + CANErrorTypes_Mode_Unrecognized +} CANErrorTypes; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_BUS_CAN_RX_ERRORS_ +#define DEFINED_TYPEDEF_FOR_BUS_CAN_RX_ERRORS_ + +// Specifies the CAN error types. +struct BUS_CAN_RX_ERRORS +{ + // True if an error has been detected. + boolean_T event; + CANErrorTypes type; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_BUS_CAN_RX_ERRORS_MULTIPLE_ +#define DEFINED_TYPEDEF_FOR_BUS_CAN_RX_ERRORS_MULTIPLE_ + +struct BUS_CAN_RX_ERRORS_MULTIPLE +{ + BUS_CAN_RX_ERRORS errors[CAN_MAX_NUM_PACKETS]; +}; + +#endif + #ifndef DEFINED_TYPEDEF_FOR_CANClassTypes_ #define DEFINED_TYPEDEF_FOR_CANClassTypes_ @@ -767,6 +535,17 @@ typedef enum { #endif +#ifndef DEFINED_TYPEDEF_FOR_EstimationVelocityModes_ +#define DEFINED_TYPEDEF_FOR_EstimationVelocityModes_ + +typedef enum { + EstimationVelocityModes_Disabled = 0,// Default value + EstimationVelocityModes_MovingAverage, + EstimationVelocityModes_LeastSquares +} EstimationVelocityModes; + +#endif + // Forward declaration for rtModel typedef struct tag_RTM_AMC_BLDC_T RT_MODEL_AMC_BLDC_T; diff --git a/emBODY/eBcode/arch-arm/mbd/amcbldc/can-decoder/can_decoder.cpp b/emBODY/eBcode/arch-arm/mbd/amcbldc/can-decoder/can_decoder.cpp index 3a6aa79f2..4545a135c 100644 --- a/emBODY/eBcode/arch-arm/mbd/amcbldc/can-decoder/can_decoder.cpp +++ b/emBODY/eBcode/arch-arm/mbd/amcbldc/can-decoder/can_decoder.cpp @@ -7,9 +7,9 @@ // // Code generated for Simulink model 'can_decoder'. // -// Model version : 6.3 +// Model version : 6.43 // Simulink Coder version : 23.2 (R2023b) 01-Aug-2023 -// C/C++ source code generated on : Tue Feb 13 11:54:24 2024 +// C/C++ source code generated on : Wed Feb 28 11:27:59 2024 // // Target selection: ert.tlc // Embedded hardware selection: ARM Compatible->ARM Cortex-M @@ -17,12 +17,12 @@ // Validation result: Not run // #include "can_decoder.h" -#include "can_decoder_private.h" #include "rtwtypes.h" #include "can_decoder_types.h" #include #include -#include "rtw_defines.h" +#include "mc_supervisor_defines.h" +#include "can_decoder_private.h" // Named constants for Chart: '/Decoding Logic' const int32_T can_decoder_CALL_EVENT = -1; @@ -32,13 +32,6 @@ const uint8_T can_decoder_IN_Home_p = 2U; const int32_T can_decoder_event_ev_error_mode_unrecognized = 0; const int32_T can_decoder_event_ev_error_pck_malformed = 1; const int32_T can_decoder_event_ev_error_pck_not4us = 2; -MdlrefDW_can_decoder_T can_decoder_MdlrefDW; - -// Block signals (default storage) -B_can_decoder_c_T can_decoder_B; - -// Block states (default storage) -DW_can_decoder_f_T can_decoder_DW; // Forward declaration for local functions static int32_T can_decoder_safe_cast_to_MCStreaming(int32_T input); @@ -47,6 +40,7 @@ static void can_decoder_ERROR_HANDLING(boolean_T rtu_pck_available, static int16_T can_decoder_merge_2bytes_signed(uint16_T bl, uint16_T bh); static boolean_T can_decoder_is_controlmode_recognized(int32_T mode); static int32_T can_decoder_safe_cast_to_MCControlModes(int32_T input); +static ControlModes can_decoder_convertMode(MCControlModes mccontrolmode); static uint16_T can_decoder_merge_2bytes_unsigned(uint16_T bl, uint16_T bh); // Forward declaration for local functions @@ -151,6 +145,35 @@ static int32_T can_decoder_safe_cast_to_MCControlModes(int32_T input) return output; } +// Function for Chart: '/Decoding Logic' +static ControlModes can_decoder_convertMode(MCControlModes mccontrolmode) +{ + ControlModes controlmode; + switch (mccontrolmode) { + case MCControlModes_Idle: + controlmode = ControlModes_Idle; + break; + + case MCControlModes_Current: + controlmode = ControlModes_Current; + break; + + case MCControlModes_SpeedVoltage: + controlmode = ControlModes_Velocity; + break; + + case MCControlModes_OpenLoop: + controlmode = ControlModes_Voltage; + break; + + default: + controlmode = ControlModes_Idle; + break; + } + + return controlmode; +} + // Function for Chart: '/Decoding Logic' static uint16_T can_decoder_merge_2bytes_unsigned(uint16_T bl, uint16_T bh) { @@ -165,7 +188,7 @@ void can_decoder_DecodingLogic_Init(DW_DecodingLogic_can_decoder_T *localDW) // Output and update for atomic system: '/Decoding Logic' void can_decoder_DecodingLogic(boolean_T rtu_pck_available, const - BUS_CAN_PACKET_RX *rtu_pck_input, const ConfigurationParameters + BUS_CAN_PACKET_RX *rtu_pck_input, const ActuatorConfiguration *rtu_ConfigurationParameters, uint8_T rtu_CAN_ID_DST, uint8_T rtu_CAN_VOLT_REF_SHIFT, real32_T rtu_CAN_VOLT_REF_GAIN, B_DecodingLogic_can_decoder_T *localB, DW_DecodingLogic_can_decoder_T *localDW) @@ -207,14 +230,14 @@ void can_decoder_DecodingLogic(boolean_T rtu_pck_available, const if (rtu_pck_input->PAYLOAD.LEN >= 2) { if (can_decoder_is_controlmode_recognized(static_cast (rtu_pck_input->PAYLOAD.ARG[1]))) { - localB->msg_set_control_mode.motor = - rtu_pck_input->PAYLOAD.CMD.M; - localB->msg_set_control_mode.mode = static_cast + // msg_set_control_mode.motor = pck_input.PAYLOAD.CMD.M; + localB->msg_set_control_mode = can_decoder_convertMode( + static_cast (can_decoder_safe_cast_to_MCControlModes - (rtu_pck_input->PAYLOAD.ARG[1])); + (rtu_pck_input->PAYLOAD.ARG[1]))); localDW->cmd_processed = static_cast (localDW->cmd_processed + 1); - localDW->ev_set_control_modeEventCounter++; + localB->event_type = SupervisorEvents_SetControlMode; } else { localDW->sfEvent = can_decoder_event_ev_error_mode_unrecognized; if (localDW->is_active_ERROR_HANDLING != 0U) { @@ -264,17 +287,17 @@ void can_decoder_DecodingLogic(boolean_T rtu_pck_available, const tmp_merged = can_decoder_merge_2bytes_signed(static_cast (rtu_pck_input->PAYLOAD.ARG[idx]), static_cast (rtu_pck_input->PAYLOAD.ARG[idx + 1])); - localB->msg_desired_targets.current = 0.001F * static_cast + localB->msg_desired_targets.motorcurrent = 0.001F * static_cast (tmp_merged); - localB->msg_desired_targets.voltage = static_cast - (static_cast(tmp_merged >> (rtu_CAN_VOLT_REF_SHIFT - - rtu_ConfigurationParameters->CurLoopPID.shift_factor))) / + localB->msg_desired_targets.motorvoltage = static_cast( + static_cast(tmp_merged >> (rtu_CAN_VOLT_REF_SHIFT - + rtu_ConfigurationParameters->pids.currentPID.shift_factor))) / rtu_CAN_VOLT_REF_GAIN; - localB->msg_desired_targets.velocity = 1000.0F * static_cast - (tmp_merged) * CAN_ANGLE_ICUB2DEG; + localB->msg_desired_targets.jointvelocities = 1000.0F * + static_cast(tmp_merged) * CAN_ANGLE_ICUB2DEG; localDW->cmd_processed = static_cast(localDW->cmd_processed + 1); - localDW->ev_desired_targetsEventCounter++; + localB->event_type = SupervisorEvents_SetTarget; } else { localDW->sfEvent = can_decoder_event_ev_error_pck_malformed; if (localDW->is_active_ERROR_HANDLING != 0U) { @@ -294,7 +317,7 @@ void can_decoder_DecodingLogic(boolean_T rtu_pck_available, const if (rtu_pck_input->PAYLOAD.CMD.OPC == static_cast (MCOPC_Set_Current_Limit)) { if (rtu_pck_input->PAYLOAD.LEN == 8) { - localB->msg_set_current_limit.motor = rtu_pck_input->PAYLOAD.CMD.M; + // msg_set_current_limit.motor = pck_input.PAYLOAD.CMD.M; localB->msg_set_current_limit.nominal = 0.001F * static_cast(can_decoder_merge_2bytes_signed( static_cast(rtu_pck_input->PAYLOAD.ARG[2]), @@ -309,7 +332,7 @@ void can_decoder_DecodingLogic(boolean_T rtu_pck_available, const static_cast(rtu_pck_input->PAYLOAD.ARG[7]))); localDW->cmd_processed = static_cast (localDW->cmd_processed + 1); - localDW->ev_set_current_limitEventCounter++; + localB->event_type = SupervisorEvents_SetLimit; } else { guard1 = true; } @@ -318,19 +341,20 @@ void can_decoder_DecodingLogic(boolean_T rtu_pck_available, const (rtu_pck_input->PAYLOAD.CMD.OPC == static_cast (MCOPC_Set_Velocity_PID))) { if (rtu_pck_input->PAYLOAD.LEN == 8) { - localB->msg_set_pid.motor = rtu_pck_input->PAYLOAD.CMD.M; - localB->msg_set_pid.Ks = rtu_pck_input->PAYLOAD.ARG[7]; - c = static_cast(0x01 << (15 - localB->msg_set_pid.Ks)) / - 32768.0F; - localB->msg_set_pid.Kp = c * static_cast + // msg_set_pid.motor = pck_input.PAYLOAD.CMD.M; + localB->msg_set_pid.shift_factor = rtu_pck_input->PAYLOAD.ARG[7]; + localB->msg_set_pid.type = ControlModes_Current; + c = static_cast(0x01 << (15 - + localB->msg_set_pid.shift_factor)) / 32768.0F; + localB->msg_set_pid.P = c * static_cast (can_decoder_merge_2bytes_signed(static_cast (rtu_pck_input->PAYLOAD.ARG[1]), static_cast (rtu_pck_input->PAYLOAD.ARG[2]))); - localB->msg_set_pid.Ki = c * static_cast + localB->msg_set_pid.I = c * static_cast (can_decoder_merge_2bytes_signed(static_cast (rtu_pck_input->PAYLOAD.ARG[3]), static_cast (rtu_pck_input->PAYLOAD.ARG[4]))); - localB->msg_set_pid.Kd = c * static_cast + localB->msg_set_pid.D = c * static_cast (can_decoder_merge_2bytes_signed(static_cast (rtu_pck_input->PAYLOAD.ARG[5]), static_cast (rtu_pck_input->PAYLOAD.ARG[6]))); @@ -338,12 +362,13 @@ void can_decoder_DecodingLogic(boolean_T rtu_pck_available, const (localDW->cmd_processed + 1); if (rtu_pck_input->PAYLOAD.CMD.OPC == static_cast (MCOPC_Set_Current_PID)) { - localDW->ev_set_current_pidEventCounter++; + localB->event_type = SupervisorEvents_SetPid; } else { - localB->msg_set_pid.Kp *= 0.001F; - localB->msg_set_pid.Ki *= 0.001F; - localB->msg_set_pid.Kd *= 0.001F; - localDW->ev_set_velocity_pidEventCounter++; + localB->msg_set_pid.P *= 0.001F; + localB->msg_set_pid.I *= 0.001F; + localB->msg_set_pid.D *= 0.001F; + localB->msg_set_pid.type = ControlModes_Velocity; + localB->event_type = SupervisorEvents_SetPid; } } else { guard1 = true; @@ -406,13 +431,13 @@ void can_decoder_DecodingLogic(boolean_T rtu_pck_available, const can_decoder_merge_2bytes_signed(static_cast (rtu_pck_input->PAYLOAD.ARG[4]), static_cast (rtu_pck_input->PAYLOAD.ARG[5])); - localB->msg_set_motor_config.number_poles = + localB->msg_set_motor_config.pole_pairs = rtu_pck_input->PAYLOAD.ARG[6]; localB->msg_set_motor_config.encoder_tolerance = rtu_pck_input->PAYLOAD.ARG[7]; localDW->cmd_processed = static_cast (localDW->cmd_processed + 1); - localDW->ev_set_motor_configEventCounter++; + localB->event_type = SupervisorEvents_SetMotorConfig; } else { localDW->sfEvent = can_decoder_event_ev_error_pck_malformed; if (localDW->is_active_ERROR_HANDLING != 0U) { @@ -453,36 +478,6 @@ void can_decoder_DecodingLogic(boolean_T rtu_pck_available, const localB->ev_error = !localB->ev_error; localDW->ev_errorEventCounter--; } - - if (localDW->ev_set_control_modeEventCounter > 0U) { - localB->ev_set_control_mode = !localB->ev_set_control_mode; - localDW->ev_set_control_modeEventCounter--; - } - - if (localDW->ev_set_current_limitEventCounter > 0U) { - localB->ev_set_current_limit = !localB->ev_set_current_limit; - localDW->ev_set_current_limitEventCounter--; - } - - if (localDW->ev_desired_targetsEventCounter > 0U) { - localB->ev_desired_targets = !localB->ev_desired_targets; - localDW->ev_desired_targetsEventCounter--; - } - - if (localDW->ev_set_current_pidEventCounter > 0U) { - localB->ev_set_current_pid = !localB->ev_set_current_pid; - localDW->ev_set_current_pidEventCounter--; - } - - if (localDW->ev_set_velocity_pidEventCounter > 0U) { - localB->ev_set_velocity_pid = !localB->ev_set_velocity_pid; - localDW->ev_set_velocity_pidEventCounter--; - } - - if (localDW->ev_set_motor_configEventCounter > 0U) { - localB->ev_set_motor_config = !localB->ev_set_motor_config; - localDW->ev_set_motor_configEventCounter--; - } } // Function for MATLAB Function: '/RAW2STRUCT Decoding Logic' @@ -501,7 +496,7 @@ static CANClassTypes can_decoder_convert_to_enum_CANClassTypes(int32_T input) } // System initialize for referenced model: 'can_decoder' -void can_decoder_Init(void) +void can_decoder_Init(B_can_decoder_c_T *localB, DW_can_decoder_f_T *localDW) { int32_T ForEach_itr; static const BUS_CAN_RX tmp = { false,// available @@ -521,16 +516,16 @@ void can_decoder_Init(void) }; // SystemInitialize for Iterator SubSystem: '/Cycling Decoder' - for (ForEach_itr = 0; ForEach_itr < CAN_MAX_NUM_PACKETS; ForEach_itr++) { + for (ForEach_itr = 0; ForEach_itr < MAX_EVENTS_PER_TICK; ForEach_itr++) { // SystemInitialize for Atomic SubSystem: '/CAN_RX_RAW2STRUCT' // SystemInitialize for MATLAB Function: '/RAW2STRUCT Decoding Logic' - can_decoder_B.CoreSubsys[ForEach_itr].pck_rx_struct = tmp; + localB->CoreSubsys[ForEach_itr].pck_rx_struct = tmp; // End of SystemInitialize for SubSystem: '/CAN_RX_RAW2STRUCT' // SystemInitialize for Atomic SubSystem: '/CAN_Decoder' // SystemInitialize for Chart: '/Decoding Logic' - can_decoder_DecodingLogic_Init(&can_decoder_DW.CoreSubsys[ForEach_itr]. + can_decoder_DecodingLogic_Init(&localDW->CoreSubsys[ForEach_itr]. sf_DecodingLogic); // End of SystemInitialize for SubSystem: '/CAN_Decoder' @@ -541,10 +536,10 @@ void can_decoder_Init(void) // Output and update for referenced model: 'can_decoder' void can_decoder(const BUS_CAN_MULTIPLE *rtu_pck_rx_raw, const - ConfigurationParameters *rtu_ConfigurationParameters, - BUS_MESSAGES_RX_MULTIPLE *rty_messages_rx, - BUS_STATUS_RX_MULTIPLE *rty_status_rx, - BUS_CAN_RX_ERRORS_MULTIPLE *rty_errors_rx) + ActuatorConfiguration *rtu_ConfigurationParameters, + SupervisorReceivedEvents rty_messages_rx[MAX_EVENTS_PER_TICK], + BUS_CAN_RX_ERRORS_MULTIPLE *rty_errors_rx, B_can_decoder_c_T + *localB, DW_can_decoder_f_T *localDW) { int32_T ForEach_itr; int32_T i; @@ -552,30 +547,24 @@ void can_decoder(const BUS_CAN_MULTIPLE *rtu_pck_rx_raw, const uint8_T minval; uint8_T x_idx_1; boolean_T rtb_FixPtRelationalOperator; - boolean_T rtb_FixPtRelationalOperator_a; - boolean_T rtb_FixPtRelationalOperator_d; - boolean_T rtb_FixPtRelationalOperator_e; - boolean_T rtb_FixPtRelationalOperator_i; - boolean_T rtb_FixPtRelationalOperator_m; - boolean_T rtb_FixPtRelationalOperator_p; // Outputs for Iterator SubSystem: '/Cycling Decoder' incorporates: // ForEach: '/For Each' - for (ForEach_itr = 0; ForEach_itr < CAN_MAX_NUM_PACKETS; ForEach_itr++) { + for (ForEach_itr = 0; ForEach_itr < MAX_EVENTS_PER_TICK; ForEach_itr++) { // Outputs for Atomic SubSystem: '/CAN_RX_RAW2STRUCT' // MATLAB Function: '/RAW2STRUCT Decoding Logic' incorporates: // ForEachSliceSelector generated from: '/pck_rx_raw' - can_decoder_B.CoreSubsys[ForEach_itr].pck_rx_struct.available = + localB->CoreSubsys[ForEach_itr].pck_rx_struct.available = rtu_pck_rx_raw->packets[ForEach_itr].available; rtu_pck_rx_raw_0 = rtu_pck_rx_raw->packets[ForEach_itr].packet.ID; - can_decoder_B.CoreSubsys[ForEach_itr].pck_rx_struct.packet.ID.CLS = + localB->CoreSubsys[ForEach_itr].pck_rx_struct.packet.ID.CLS = can_decoder_convert_to_enum_CANClassTypes(static_cast( static_cast(rtu_pck_rx_raw_0 & 1792) >> 8)); - can_decoder_B.CoreSubsys[ForEach_itr].pck_rx_struct.packet.ID.SRC = + localB->CoreSubsys[ForEach_itr].pck_rx_struct.packet.ID.SRC = static_cast(static_cast(rtu_pck_rx_raw_0 & 240) >> 4); - can_decoder_B.CoreSubsys[ForEach_itr].pck_rx_struct.packet.ID.DST_TYP = + localB->CoreSubsys[ForEach_itr].pck_rx_struct.packet.ID.DST_TYP = static_cast(rtu_pck_rx_raw_0 & 15); x_idx_1 = rtu_pck_rx_raw->packets[ForEach_itr].length; minval = 8U; @@ -583,19 +572,18 @@ void can_decoder(const BUS_CAN_MULTIPLE *rtu_pck_rx_raw, const minval = x_idx_1; } - can_decoder_B.CoreSubsys[ForEach_itr].pck_rx_struct.packet.PAYLOAD.LEN = 0U; + localB->CoreSubsys[ForEach_itr].pck_rx_struct.packet.PAYLOAD.LEN = 0U; if (minval > 0) { - can_decoder_B.CoreSubsys[ForEach_itr].pck_rx_struct.packet.PAYLOAD.LEN = - minval; + localB->CoreSubsys[ForEach_itr].pck_rx_struct.packet.PAYLOAD.LEN = minval; } x_idx_1 = rtu_pck_rx_raw->packets[ForEach_itr].packet.PAYLOAD[0]; - can_decoder_B.CoreSubsys[ForEach_itr].pck_rx_struct.packet.PAYLOAD.CMD.M = + localB->CoreSubsys[ForEach_itr].pck_rx_struct.packet.PAYLOAD.CMD.M = ((x_idx_1 & 128U) != 0U); - can_decoder_B.CoreSubsys[ForEach_itr].pck_rx_struct.packet.PAYLOAD.CMD.OPC = + localB->CoreSubsys[ForEach_itr].pck_rx_struct.packet.PAYLOAD.CMD.OPC = static_cast(x_idx_1 & 127); for (i = 0; i < 8; i++) { - can_decoder_B.CoreSubsys[ForEach_itr].pck_rx_struct.packet.PAYLOAD.ARG[i] = + localB->CoreSubsys[ForEach_itr].pck_rx_struct.packet.PAYLOAD.ARG[i] = rtu_pck_rx_raw->packets[ForEach_itr].packet.PAYLOAD[i]; } @@ -608,77 +596,11 @@ void can_decoder(const BUS_CAN_MULTIPLE *rtu_pck_rx_raw, const // Constant: '/Constant1' // Constant: '/Constant2' - can_decoder_DecodingLogic(can_decoder_B.CoreSubsys[ForEach_itr]. - pck_rx_struct.available, &can_decoder_B.CoreSubsys[ForEach_itr]. + can_decoder_DecodingLogic(localB->CoreSubsys[ForEach_itr]. + pck_rx_struct.available, &localB->CoreSubsys[ForEach_itr]. pck_rx_struct.packet, rtu_ConfigurationParameters, CAN_ID_AMC, 5, 10.0F, - &can_decoder_B.CoreSubsys[ForEach_itr].sf_DecodingLogic, - &can_decoder_DW.CoreSubsys[ForEach_itr].sf_DecodingLogic); - - // RelationalOperator: '/FixPt Relational Operator' incorporates: - // UnitDelay: '/Delay Input1' - // - // Block description for '/Delay Input1': - // - // Store in Global RAM - - rtb_FixPtRelationalOperator = (can_decoder_B.CoreSubsys[ForEach_itr]. - sf_DecodingLogic.ev_set_control_mode != - can_decoder_DW.CoreSubsys[ForEach_itr].DelayInput1_DSTATE); - - // RelationalOperator: '/FixPt Relational Operator' incorporates: - // UnitDelay: '/Delay Input1' - // - // Block description for '/Delay Input1': - // - // Store in Global RAM - - rtb_FixPtRelationalOperator_d = (can_decoder_B.CoreSubsys[ForEach_itr]. - sf_DecodingLogic.ev_set_current_limit != - can_decoder_DW.CoreSubsys[ForEach_itr].DelayInput1_DSTATE_n); - - // RelationalOperator: '/FixPt Relational Operator' incorporates: - // UnitDelay: '/Delay Input1' - // - // Block description for '/Delay Input1': - // - // Store in Global RAM - - rtb_FixPtRelationalOperator_a = (can_decoder_B.CoreSubsys[ForEach_itr]. - sf_DecodingLogic.ev_desired_targets != - can_decoder_DW.CoreSubsys[ForEach_itr].DelayInput1_DSTATE_nk); - - // RelationalOperator: '/FixPt Relational Operator' incorporates: - // UnitDelay: '/Delay Input1' - // - // Block description for '/Delay Input1': - // - // Store in Global RAM - - rtb_FixPtRelationalOperator_p = (can_decoder_B.CoreSubsys[ForEach_itr]. - sf_DecodingLogic.ev_set_current_pid != - can_decoder_DW.CoreSubsys[ForEach_itr].DelayInput1_DSTATE_h); - - // RelationalOperator: '/FixPt Relational Operator' incorporates: - // UnitDelay: '/Delay Input1' - // - // Block description for '/Delay Input1': - // - // Store in Global RAM - - rtb_FixPtRelationalOperator_m = (can_decoder_B.CoreSubsys[ForEach_itr]. - sf_DecodingLogic.ev_set_velocity_pid != - can_decoder_DW.CoreSubsys[ForEach_itr].DelayInput1_DSTATE_l); - - // RelationalOperator: '/FixPt Relational Operator' incorporates: - // UnitDelay: '/Delay Input1' - // - // Block description for '/Delay Input1': - // - // Store in Global RAM - - rtb_FixPtRelationalOperator_e = (can_decoder_B.CoreSubsys[ForEach_itr]. - sf_DecodingLogic.ev_set_motor_config != - can_decoder_DW.CoreSubsys[ForEach_itr].DelayInput1_DSTATE_d); + &localB->CoreSubsys[ForEach_itr].sf_DecodingLogic, &localDW-> + CoreSubsys[ForEach_itr].sf_DecodingLogic); // RelationalOperator: '/FixPt Relational Operator' incorporates: // UnitDelay: '/Delay Input1' @@ -687,64 +609,9 @@ void can_decoder(const BUS_CAN_MULTIPLE *rtu_pck_rx_raw, const // // Store in Global RAM - rtb_FixPtRelationalOperator_i = (can_decoder_B.CoreSubsys[ForEach_itr]. - sf_DecodingLogic.ev_error != can_decoder_DW.CoreSubsys[ForEach_itr]. - DelayInput1_DSTATE_j); - - // Update for UnitDelay: '/Delay Input1' - // - // Block description for '/Delay Input1': - // - // Store in Global RAM - - can_decoder_DW.CoreSubsys[ForEach_itr].DelayInput1_DSTATE = - can_decoder_B.CoreSubsys[ForEach_itr].sf_DecodingLogic.ev_set_control_mode; - - // Update for UnitDelay: '/Delay Input1' - // - // Block description for '/Delay Input1': - // - // Store in Global RAM - - can_decoder_DW.CoreSubsys[ForEach_itr].DelayInput1_DSTATE_n = - can_decoder_B.CoreSubsys[ForEach_itr]. - sf_DecodingLogic.ev_set_current_limit; - - // Update for UnitDelay: '/Delay Input1' - // - // Block description for '/Delay Input1': - // - // Store in Global RAM - - can_decoder_DW.CoreSubsys[ForEach_itr].DelayInput1_DSTATE_nk = - can_decoder_B.CoreSubsys[ForEach_itr].sf_DecodingLogic.ev_desired_targets; - - // Update for UnitDelay: '/Delay Input1' - // - // Block description for '/Delay Input1': - // - // Store in Global RAM - - can_decoder_DW.CoreSubsys[ForEach_itr].DelayInput1_DSTATE_h = - can_decoder_B.CoreSubsys[ForEach_itr].sf_DecodingLogic.ev_set_current_pid; - - // Update for UnitDelay: '/Delay Input1' - // - // Block description for '/Delay Input1': - // - // Store in Global RAM - - can_decoder_DW.CoreSubsys[ForEach_itr].DelayInput1_DSTATE_l = - can_decoder_B.CoreSubsys[ForEach_itr].sf_DecodingLogic.ev_set_velocity_pid; - - // Update for UnitDelay: '/Delay Input1' - // - // Block description for '/Delay Input1': - // - // Store in Global RAM - - can_decoder_DW.CoreSubsys[ForEach_itr].DelayInput1_DSTATE_d = - can_decoder_B.CoreSubsys[ForEach_itr].sf_DecodingLogic.ev_set_motor_config; + rtb_FixPtRelationalOperator = (localB->CoreSubsys[ForEach_itr]. + sf_DecodingLogic.ev_error != localDW->CoreSubsys[ForEach_itr]. + DelayInput1_DSTATE); // Update for UnitDelay: '/Delay Input1' // @@ -752,48 +619,31 @@ void can_decoder(const BUS_CAN_MULTIPLE *rtu_pck_rx_raw, const // // Store in Global RAM - can_decoder_DW.CoreSubsys[ForEach_itr].DelayInput1_DSTATE_j = - can_decoder_B.CoreSubsys[ForEach_itr].sf_DecodingLogic.ev_error; + localDW->CoreSubsys[ForEach_itr].DelayInput1_DSTATE = localB-> + CoreSubsys[ForEach_itr].sf_DecodingLogic.ev_error; // ForEachSliceAssignment generated from: '/messages_rx' incorporates: // BusCreator: '/Bus Creator2' - rty_messages_rx->messages[ForEach_itr].control_mode = - can_decoder_B.CoreSubsys[ForEach_itr]. - sf_DecodingLogic.msg_set_control_mode; - rty_messages_rx->messages[ForEach_itr].current_limit = - can_decoder_B.CoreSubsys[ForEach_itr]. - sf_DecodingLogic.msg_set_current_limit; - rty_messages_rx->messages[ForEach_itr].desired_targets = - can_decoder_B.CoreSubsys[ForEach_itr].sf_DecodingLogic.msg_desired_targets; - rty_messages_rx->messages[ForEach_itr].pid = - can_decoder_B.CoreSubsys[ForEach_itr].sf_DecodingLogic.msg_set_pid; - rty_messages_rx->messages[ForEach_itr].motor_config = - can_decoder_B.CoreSubsys[ForEach_itr]. - sf_DecodingLogic.msg_set_motor_config; - - // ForEachSliceAssignment generated from: '/events_rx' incorporates: - // BusCreator: '/Bus Creator1' - - rty_status_rx->status[ForEach_itr].control_mode = - rtb_FixPtRelationalOperator; - rty_status_rx->status[ForEach_itr].current_limit = - rtb_FixPtRelationalOperator_d; - rty_status_rx->status[ForEach_itr].desired_targets = - rtb_FixPtRelationalOperator_a; - rty_status_rx->status[ForEach_itr].current_pid = - rtb_FixPtRelationalOperator_p; - rty_status_rx->status[ForEach_itr].velocity_pid = - rtb_FixPtRelationalOperator_m; - rty_status_rx->status[ForEach_itr].motor_config = - rtb_FixPtRelationalOperator_e; + rty_messages_rx[ForEach_itr].targets_content = localB-> + CoreSubsys[ForEach_itr].sf_DecodingLogic.msg_desired_targets; + rty_messages_rx[ForEach_itr].pid_content = localB->CoreSubsys[ForEach_itr]. + sf_DecodingLogic.msg_set_pid; + rty_messages_rx[ForEach_itr].event_type = localB->CoreSubsys[ForEach_itr]. + sf_DecodingLogic.event_type; + rty_messages_rx[ForEach_itr].control_mode_content = localB-> + CoreSubsys[ForEach_itr].sf_DecodingLogic.msg_set_control_mode; + rty_messages_rx[ForEach_itr].limits_content = localB->CoreSubsys[ForEach_itr] + .sf_DecodingLogic.msg_set_current_limit; + rty_messages_rx[ForEach_itr].motor_config_content = localB-> + CoreSubsys[ForEach_itr].sf_DecodingLogic.msg_set_motor_config; // ForEachSliceAssignment generated from: '/errors_rx' incorporates: // BusCreator: '/Bus Creator3' - rty_errors_rx->errors[ForEach_itr].event = rtb_FixPtRelationalOperator_i; - rty_errors_rx->errors[ForEach_itr].type = - can_decoder_B.CoreSubsys[ForEach_itr].sf_DecodingLogic.error_type; + rty_errors_rx->errors[ForEach_itr].event = rtb_FixPtRelationalOperator; + rty_errors_rx->errors[ForEach_itr].type = localB->CoreSubsys[ForEach_itr]. + sf_DecodingLogic.error_type; // End of Outputs for SubSystem: '/CAN_Decoder' } @@ -802,10 +652,9 @@ void can_decoder(const BUS_CAN_MULTIPLE *rtu_pck_rx_raw, const } // Model initialize function -void can_decoder_initialize(const char_T **rt_errorStatus) +void can_decoder_initialize(const char_T **rt_errorStatus, + RT_MODEL_can_decoder_T *const can_decoder_M) { - RT_MODEL_can_decoder_T *const can_decoder_M = &(can_decoder_MdlrefDW.rtm); - // Registration code // initialize error status diff --git a/emBODY/eBcode/arch-arm/mbd/amcbldc/can-decoder/can_decoder.h b/emBODY/eBcode/arch-arm/mbd/amcbldc/can-decoder/can_decoder.h index 3a89bbb0b..4283d3527 100644 --- a/emBODY/eBcode/arch-arm/mbd/amcbldc/can-decoder/can_decoder.h +++ b/emBODY/eBcode/arch-arm/mbd/amcbldc/can-decoder/can_decoder.h @@ -7,9 +7,9 @@ // // Code generated for Simulink model 'can_decoder'. // -// Model version : 6.3 +// Model version : 6.43 // Simulink Coder version : 23.2 (R2023b) 01-Aug-2023 -// C/C++ source code generated on : Tue Feb 13 11:54:24 2024 +// C/C++ source code generated on : Wed Feb 28 11:27:59 2024 // // Target selection: ert.tlc // Embedded hardware selection: ARM Compatible->ARM Cortex-M @@ -20,6 +20,71 @@ #define RTW_HEADER_can_decoder_h_ #include "rtwtypes.h" #include "can_decoder_types.h" +#include "mc_supervisor_defines.h" + +// Block signals for system '/Decoding Logic' +struct B_DecodingLogic_can_decoder_T { + MotorConfiguration msg_set_motor_config;// '/Decoding Logic' + PID msg_set_pid; // '/Decoding Logic' + Targets msg_desired_targets; // '/Decoding Logic' + SupervisorInputLimits msg_set_current_limit;// '/Decoding Logic' + SupervisorEvents event_type; // '/Decoding Logic' + ControlModes msg_set_control_mode; // '/Decoding Logic' + CANErrorTypes error_type; // '/Decoding Logic' + boolean_T ev_error; // '/Decoding Logic' +}; + +// Block states (default storage) for system '/Decoding Logic' +struct DW_DecodingLogic_can_decoder_T { + int32_T sfEvent; // '/Decoding Logic' + uint32_T ev_errorEventCounter; // '/Decoding Logic' + uint16_T cmd_processed; // '/Decoding Logic' + uint8_T is_active_c3_can_decoder; // '/Decoding Logic' + uint8_T is_active_SET_CONTROL_MODE; // '/Decoding Logic' + uint8_T is_SET_CONTROL_MODE; // '/Decoding Logic' + uint8_T is_active_DESIRED_TARGETS; // '/Decoding Logic' + uint8_T is_DESIRED_TARGETS; // '/Decoding Logic' + uint8_T is_active_SET_OPTIONS; // '/Decoding Logic' + uint8_T is_SET_OPTIONS; // '/Decoding Logic' + uint8_T is_active_SET_MOTOR_CONFIG; // '/Decoding Logic' + uint8_T is_SET_MOTOR_CONFIG; // '/Decoding Logic' + uint8_T is_active_ERROR_HANDLING; // '/Decoding Logic' + uint8_T is_ERROR_HANDLING; // '/Decoding Logic' + boolean_T ev_async; // '/Decoding Logic' +}; + +// Block signals for system '/Cycling Decoder' +struct B_CoreSubsys_can_decoder_T { + BUS_CAN_RX pck_rx_struct; // '/RAW2STRUCT Decoding Logic' + B_DecodingLogic_can_decoder_T sf_DecodingLogic;// '/Decoding Logic' +}; + +// Block states (default storage) for system '/Cycling Decoder' +struct DW_CoreSubsys_can_decoder_T { + boolean_T DelayInput1_DSTATE; // '/Delay Input1' + DW_DecodingLogic_can_decoder_T sf_DecodingLogic;// '/Decoding Logic' +}; + +// Block signals for model 'can_decoder' +struct B_can_decoder_c_T { + B_CoreSubsys_can_decoder_T CoreSubsys[MAX_EVENTS_PER_TICK];// '/Cycling Decoder' +}; + +// Block states (default storage) for model 'can_decoder' +struct DW_can_decoder_f_T { + DW_CoreSubsys_can_decoder_T CoreSubsys[MAX_EVENTS_PER_TICK];// '/Cycling Decoder' +}; + +// Real-time Model Data Structure +struct tag_RTM_can_decoder_T { + const char_T **errorStatus; +}; + +struct MdlrefDW_can_decoder_T { + B_can_decoder_c_T rtb; + DW_can_decoder_f_T rtdw; + RT_MODEL_can_decoder_T rtm; +}; // // Exported Global Parameters @@ -37,14 +102,23 @@ extern uint8_T CAN_ID_AMC; // Variable: CAN_ID_AMC // Referenced by: '/Constant' // 4 bits defining the ID of the AMC_BLDC board. -extern void can_decoder_Init(void); -extern void can_decoder(const BUS_CAN_MULTIPLE *rtu_pck_rx_raw, const - ConfigurationParameters *rtu_ConfigurationParameters, BUS_MESSAGES_RX_MULTIPLE - *rty_messages_rx, BUS_STATUS_RX_MULTIPLE *rty_status_rx, - BUS_CAN_RX_ERRORS_MULTIPLE *rty_errors_rx); // Model reference registration function -extern void can_decoder_initialize(const char_T **rt_errorStatus); +extern void can_decoder_initialize(const char_T **rt_errorStatus, + RT_MODEL_can_decoder_T *const can_decoder_M); +extern void can_decoder_DecodingLogic_Init(DW_DecodingLogic_can_decoder_T + *localDW); +extern void can_decoder_DecodingLogic(boolean_T rtu_pck_available, const + BUS_CAN_PACKET_RX *rtu_pck_input, const ActuatorConfiguration + *rtu_ConfigurationParameters, uint8_T rtu_CAN_ID_DST, uint8_T + rtu_CAN_VOLT_REF_SHIFT, real32_T rtu_CAN_VOLT_REF_GAIN, + B_DecodingLogic_can_decoder_T *localB, DW_DecodingLogic_can_decoder_T *localDW); +extern void can_decoder_Init(B_can_decoder_c_T *localB, DW_can_decoder_f_T + *localDW); +extern void can_decoder(const BUS_CAN_MULTIPLE *rtu_pck_rx_raw, const + ActuatorConfiguration *rtu_ConfigurationParameters, SupervisorReceivedEvents + rty_messages_rx[MAX_EVENTS_PER_TICK], BUS_CAN_RX_ERRORS_MULTIPLE + *rty_errors_rx, B_can_decoder_c_T *localB, DW_can_decoder_f_T *localDW); //- // The generated code includes comments that allow you to trace directly @@ -66,13 +140,7 @@ extern void can_decoder_initialize(const char_T **rt_errorStatus); // '' : 'can_decoder/Cycling Decoder/CAN_RX_RAW2STRUCT' // '' : 'can_decoder/Cycling Decoder/CAN_Decoder/Decoding Logic' // '' : 'can_decoder/Cycling Decoder/CAN_Decoder/Detect Change' -// '' : 'can_decoder/Cycling Decoder/CAN_Decoder/Detect Change1' -// '' : 'can_decoder/Cycling Decoder/CAN_Decoder/Detect Change2' -// '' : 'can_decoder/Cycling Decoder/CAN_Decoder/Detect Change3' -// '' : 'can_decoder/Cycling Decoder/CAN_Decoder/Detect Change4' -// '' : 'can_decoder/Cycling Decoder/CAN_Decoder/Detect Change5' -// '' : 'can_decoder/Cycling Decoder/CAN_Decoder/Detect Change6' -// '' : 'can_decoder/Cycling Decoder/CAN_RX_RAW2STRUCT/RAW2STRUCT Decoding Logic' +// '' : 'can_decoder/Cycling Decoder/CAN_RX_RAW2STRUCT/RAW2STRUCT Decoding Logic' #endif // RTW_HEADER_can_decoder_h_ diff --git a/emBODY/eBcode/arch-arm/mbd/amcbldc/can-decoder/can_decoder_private.h b/emBODY/eBcode/arch-arm/mbd/amcbldc/can-decoder/can_decoder_private.h index a552d6e97..33cc4cbfc 100644 --- a/emBODY/eBcode/arch-arm/mbd/amcbldc/can-decoder/can_decoder_private.h +++ b/emBODY/eBcode/arch-arm/mbd/amcbldc/can-decoder/can_decoder_private.h @@ -7,9 +7,9 @@ // // Code generated for Simulink model 'can_decoder'. // -// Model version : 6.3 +// Model version : 6.43 // Simulink Coder version : 23.2 (R2023b) 01-Aug-2023 -// C/C++ source code generated on : Tue Feb 13 11:54:24 2024 +// C/C++ source code generated on : Wed Feb 28 11:27:59 2024 // // Target selection: ert.tlc // Embedded hardware selection: ARM Compatible->ARM Cortex-M @@ -20,82 +20,6 @@ #define RTW_HEADER_can_decoder_private_h_ #include "rtwtypes.h" #include "can_decoder_types.h" -#include "rtw_defines.h" - -// Block signals for system '/Decoding Logic' -struct B_DecodingLogic_can_decoder_T { - BUS_MSG_PID msg_set_pid; // '/Decoding Logic' - BUS_MSG_MOTOR_CONFIG msg_set_motor_config;// '/Decoding Logic' - BUS_MSG_DESIRED_TARGETS msg_desired_targets;// '/Decoding Logic' - BUS_MSG_CURRENT_LIMIT msg_set_current_limit;// '/Decoding Logic' - BUS_MSG_CONTROL_MODE msg_set_control_mode;// '/Decoding Logic' - CANErrorTypes error_type; // '/Decoding Logic' - boolean_T ev_error; // '/Decoding Logic' - boolean_T ev_set_control_mode; // '/Decoding Logic' - boolean_T ev_set_current_limit; // '/Decoding Logic' - boolean_T ev_desired_targets; // '/Decoding Logic' - boolean_T ev_set_current_pid; // '/Decoding Logic' - boolean_T ev_set_velocity_pid; // '/Decoding Logic' - boolean_T ev_set_motor_config; // '/Decoding Logic' -}; - -// Block states (default storage) for system '/Decoding Logic' -struct DW_DecodingLogic_can_decoder_T { - int32_T sfEvent; // '/Decoding Logic' - uint32_T ev_errorEventCounter; // '/Decoding Logic' - uint32_T ev_set_control_modeEventCounter;// '/Decoding Logic' - uint32_T ev_set_current_limitEventCounter;// '/Decoding Logic' - uint32_T ev_desired_targetsEventCounter;// '/Decoding Logic' - uint32_T ev_set_current_pidEventCounter;// '/Decoding Logic' - uint32_T ev_set_velocity_pidEventCounter;// '/Decoding Logic' - uint32_T ev_set_motor_configEventCounter;// '/Decoding Logic' - uint16_T cmd_processed; // '/Decoding Logic' - uint8_T is_active_c3_can_decoder; // '/Decoding Logic' - uint8_T is_active_SET_CONTROL_MODE; // '/Decoding Logic' - uint8_T is_SET_CONTROL_MODE; // '/Decoding Logic' - uint8_T is_active_DESIRED_TARGETS; // '/Decoding Logic' - uint8_T is_DESIRED_TARGETS; // '/Decoding Logic' - uint8_T is_active_SET_OPTIONS; // '/Decoding Logic' - uint8_T is_SET_OPTIONS; // '/Decoding Logic' - uint8_T is_active_SET_MOTOR_CONFIG; // '/Decoding Logic' - uint8_T is_SET_MOTOR_CONFIG; // '/Decoding Logic' - uint8_T is_active_ERROR_HANDLING; // '/Decoding Logic' - uint8_T is_ERROR_HANDLING; // '/Decoding Logic' - boolean_T ev_async; // '/Decoding Logic' -}; - -// Block signals for system '/Cycling Decoder' -struct B_CoreSubsys_can_decoder_T { - BUS_CAN_RX pck_rx_struct; // '/RAW2STRUCT Decoding Logic' - B_DecodingLogic_can_decoder_T sf_DecodingLogic;// '/Decoding Logic' -}; - -// Block states (default storage) for system '/Cycling Decoder' -struct DW_CoreSubsys_can_decoder_T { - boolean_T DelayInput1_DSTATE; // '/Delay Input1' - boolean_T DelayInput1_DSTATE_n; // '/Delay Input1' - boolean_T DelayInput1_DSTATE_nk; // '/Delay Input1' - boolean_T DelayInput1_DSTATE_h; // '/Delay Input1' - boolean_T DelayInput1_DSTATE_l; // '/Delay Input1' - boolean_T DelayInput1_DSTATE_d; // '/Delay Input1' - boolean_T DelayInput1_DSTATE_j; // '/Delay Input1' - DW_DecodingLogic_can_decoder_T sf_DecodingLogic;// '/Decoding Logic' -}; - -// Block signals for model 'can_decoder' -struct B_can_decoder_c_T { - B_CoreSubsys_can_decoder_T CoreSubsys[CAN_MAX_NUM_PACKETS];// '/Cycling Decoder' -}; - -// Block states (default storage) for model 'can_decoder' -struct DW_can_decoder_f_T { - DW_CoreSubsys_can_decoder_T CoreSubsys[CAN_MAX_NUM_PACKETS];// '/Cycling Decoder' -}; - -// Real-time Model Data Structure -struct tag_RTM_can_decoder_T { - const char_T **errorStatus; -}; // Macros for accessing real-time model data structure #ifndef rtmGetErrorStatus @@ -113,26 +37,6 @@ struct tag_RTM_can_decoder_T { #ifndef rtmSetErrorStatusPointer #define rtmSetErrorStatusPointer(rtm, val) ((rtm)->errorStatus = (val)) #endif - -struct MdlrefDW_can_decoder_T { - RT_MODEL_can_decoder_T rtm; -}; - -extern void can_decoder_DecodingLogic_Init(DW_DecodingLogic_can_decoder_T - *localDW); -extern void can_decoder_DecodingLogic(boolean_T rtu_pck_available, const - BUS_CAN_PACKET_RX *rtu_pck_input, const ConfigurationParameters - *rtu_ConfigurationParameters, uint8_T rtu_CAN_ID_DST, uint8_T - rtu_CAN_VOLT_REF_SHIFT, real32_T rtu_CAN_VOLT_REF_GAIN, - B_DecodingLogic_can_decoder_T *localB, DW_DecodingLogic_can_decoder_T *localDW); -extern MdlrefDW_can_decoder_T can_decoder_MdlrefDW; - -// Block signals (default storage) -extern B_can_decoder_c_T can_decoder_B; - -// Block states (default storage) -extern DW_can_decoder_f_T can_decoder_DW; - #endif // RTW_HEADER_can_decoder_private_h_ // diff --git a/emBODY/eBcode/arch-arm/mbd/amcbldc/can-decoder/can_decoder_types.h b/emBODY/eBcode/arch-arm/mbd/amcbldc/can-decoder/can_decoder_types.h index b85e19a2a..c7e1f739b 100644 --- a/emBODY/eBcode/arch-arm/mbd/amcbldc/can-decoder/can_decoder_types.h +++ b/emBODY/eBcode/arch-arm/mbd/amcbldc/can-decoder/can_decoder_types.h @@ -7,9 +7,9 @@ // // Code generated for Simulink model 'can_decoder'. // -// Model version : 6.3 +// Model version : 6.43 // Simulink Coder version : 23.2 (R2023b) 01-Aug-2023 -// C/C++ source code generated on : Tue Feb 13 11:54:24 2024 +// C/C++ source code generated on : Wed Feb 28 11:27:59 2024 // // Target selection: ert.tlc // Embedded hardware selection: ARM Compatible->ARM Cortex-M @@ -23,6 +23,7 @@ // Includes for objects with custom storage classes #include "rtw_defines.h" +#include "mc_supervisor_defines.h" // // Constraints for division operations in dimension variants @@ -34,12 +35,31 @@ // // Registered constraints for dimension variants +// Constraint 'MAX_EVENTS_PER_TICK == CAN_MAX_NUM_PACKETS' registered by: +// '/errors_rx' + +#if MAX_EVENTS_PER_TICK != CAN_MAX_NUM_PACKETS +# error "The preprocessor definition 'MAX_EVENTS_PER_TICK' must be equal to 'CAN_MAX_NUM_PACKETS'" +#endif + +#if MAX_EVENTS_PER_TICK <= 0 +# error "The preprocessor definition 'MAX_EVENTS_PER_TICK' must be greater than '0'" +#endif + #if CAN_MAX_NUM_PACKETS <= 0 # error "The preprocessor definition 'CAN_MAX_NUM_PACKETS' must be greater than '0'" #endif -#if CAN_MAX_NUM_PACKETS >= 16 -# error "The preprocessor definition 'CAN_MAX_NUM_PACKETS' must be less than '16'" +#if (CAN_MAX_NUM_PACKETS+1) <= MAX_EVENTS_PER_TICK +# error "The preprocessor definition '(CAN_MAX_NUM_PACKETS+1)' must be greater than 'MAX_EVENTS_PER_TICK'" +#endif + +#if MAX_EVENTS_PER_TICK >= 5 +# error "The preprocessor definition 'MAX_EVENTS_PER_TICK' must be less than '5'" +#endif + +#if CAN_MAX_NUM_PACKETS >= 5 +# error "The preprocessor definition 'CAN_MAX_NUM_PACKETS' must be less than '5'" #endif #ifndef DEFINED_TYPEDEF_FOR_BUS_CAN_PACKET_ @@ -80,108 +100,14 @@ struct BUS_CAN_MULTIPLE #endif -#ifndef DEFINED_TYPEDEF_FOR_MotorConfig_ -#define DEFINED_TYPEDEF_FOR_MotorConfig_ - -struct MotorConfig -{ - // Angular offset in degrees between the stator windings and the hall sensors. - real32_T hall_sens_offset; - boolean_T has_hall_sens; - boolean_T has_quadrature_encoder; - boolean_T has_speed_quadrature_encoder; - boolean_T has_torque_sens; - boolean_T use_index; - boolean_T enable_verbosity; - int16_T rotor_encoder_resolution; - int16_T rotor_index_offset; - uint8_T encoder_tolerance; - uint8_T pole_pairs; - real32_T Kbemf; - real32_T Rphase; - real32_T Imin; - real32_T Imax; - real32_T Vmax; - real32_T resistance; - real32_T inductance; - real32_T thermal_resistance; - real32_T thermal_time_constant; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_EstimationVelocityModes_ -#define DEFINED_TYPEDEF_FOR_EstimationVelocityModes_ - -typedef enum { - EstimationVelocityModes_Disabled = 0,// Default value - EstimationVelocityModes_MovingAverage, - EstimationVelocityModes_LeastSquares -} EstimationVelocityModes; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_EstimationConfig_ -#define DEFINED_TYPEDEF_FOR_EstimationConfig_ - -struct EstimationConfig -{ - EstimationVelocityModes velocity_mode; - - // Forgetting factor in [0, 1] for exponential weighting-based estimation of RMS current value - real32_T current_rms_lambda; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_PIDConfig_ -#define DEFINED_TYPEDEF_FOR_PIDConfig_ - -struct PIDConfig -{ - real32_T OutMax; - real32_T OutMin; - real32_T P; - real32_T I; - real32_T D; - real32_T N; - real32_T I0; - real32_T D0; - uint8_T shift_factor; -}; - -#endif - #ifndef DEFINED_TYPEDEF_FOR_Thresholds_ #define DEFINED_TYPEDEF_FOR_Thresholds_ struct Thresholds { - // It shall be greater than hardwareJntPosMin - real32_T jntPosMin; - - // It shall be smaller than hardwareJntPosMax - real32_T jntPosMax; - - // Imposed by hardware constraint - real32_T hardwareJntPosMin; - - // Imposed by hardware constraint - real32_T hardwareJntPosMax; - - // If robotMin == rotorMax == 0, there's no check - real32_T rotorPosMin; - - // If robotMin == rotorMax == 0, there's no check - real32_T rotorPosMax; - // Can be only non-negative real32_T jntVelMax; - // Timeout on reception of velocity setpoint - // Can be only non-negative - uint32_T velocityTimeout; - // Current that can be kept for an indefinite period of time w/o damaging the motor // Expressed in [A] as all the internal computations are done this way // Can be only non-negative @@ -208,120 +134,57 @@ struct Thresholds #endif -#ifndef DEFINED_TYPEDEF_FOR_ConfigurationParameters_ -#define DEFINED_TYPEDEF_FOR_ConfigurationParameters_ - -struct ConfigurationParameters -{ - MotorConfig motorconfig; - EstimationConfig estimationconfig; - PIDConfig CurLoopPID; - PIDConfig PosLoopPID; - PIDConfig VelLoopPID; - PIDConfig DirLoopPID; - Thresholds thresholds; - real32_T environment_temperature; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_MCControlModes_ -#define DEFINED_TYPEDEF_FOR_MCControlModes_ +#ifndef DEFINED_TYPEDEF_FOR_ControlModes_ +#define DEFINED_TYPEDEF_FOR_ControlModes_ typedef enum { - MCControlModes_Idle = 0, // Default value - MCControlModes_OpenLoop = 80, - MCControlModes_SpeedVoltage = 10, - MCControlModes_SpeedCurrent = 11, - MCControlModes_Current = 6, - MCControlModes_NotConfigured = 176, - MCControlModes_HWFault = 160 -} MCControlModes; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_BUS_MSG_CONTROL_MODE_ -#define DEFINED_TYPEDEF_FOR_BUS_MSG_CONTROL_MODE_ - -// Fields of a CONTROL_MODE message. -struct BUS_MSG_CONTROL_MODE -{ - // Motor selector. - boolean_T motor; - - // Control mode. - MCControlModes mode; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_BUS_MSG_CURRENT_LIMIT_ -#define DEFINED_TYPEDEF_FOR_BUS_MSG_CURRENT_LIMIT_ - -// Fields of a CURRENT_LIMIT message. -struct BUS_MSG_CURRENT_LIMIT -{ - // Motor selector. - boolean_T motor; - - // Nominal current in A. - real32_T nominal; - - // Peak current in A. - real32_T peak; - - // Overload current in A. - real32_T overload; -}; + ControlModes_NotConfigured = 0, // Default value + ControlModes_Idle, + ControlModes_Position, + ControlModes_PositionDirect, + ControlModes_Current, + ControlModes_Velocity, + ControlModes_Voltage, + ControlModes_HwFaultCM +} ControlModes; #endif -#ifndef DEFINED_TYPEDEF_FOR_BUS_MSG_DESIRED_TARGETS_ -#define DEFINED_TYPEDEF_FOR_BUS_MSG_DESIRED_TARGETS_ +#ifndef DEFINED_TYPEDEF_FOR_PID_ +#define DEFINED_TYPEDEF_FOR_PID_ -// Fields of a DESIRED_TARGETS message. -struct BUS_MSG_DESIRED_TARGETS +struct PID { - // Target current in A. - real32_T current; - - // Target voltage in %. - real32_T voltage; - - // Target veocity in deg/s. - real32_T velocity; + ControlModes type; + real32_T OutMax; + real32_T OutMin; + real32_T P; + real32_T I; + real32_T D; + real32_T N; + real32_T I0; + real32_T D0; + uint8_T shift_factor; }; #endif -#ifndef DEFINED_TYPEDEF_FOR_BUS_MSG_PID_ -#define DEFINED_TYPEDEF_FOR_BUS_MSG_PID_ +#ifndef DEFINED_TYPEDEF_FOR_PIDsConfiguration_ +#define DEFINED_TYPEDEF_FOR_PIDsConfiguration_ -// Fields of a CURRENT_PID message. -struct BUS_MSG_PID +struct PIDsConfiguration { - // Motor selector. - boolean_T motor; - - // Proportional gain. - real32_T Kp; - - // Integral gain. - real32_T Ki; - - // Derivative gain. - real32_T Kd; - - // Shift factor. - uint8_T Ks; + PID currentPID; + PID velocityPID; + PID positionPID; }; #endif -#ifndef DEFINED_TYPEDEF_FOR_BUS_MSG_MOTOR_CONFIG_ -#define DEFINED_TYPEDEF_FOR_BUS_MSG_MOTOR_CONFIG_ +#ifndef DEFINED_TYPEDEF_FOR_MotorConfiguration_ +#define DEFINED_TYPEDEF_FOR_MotorConfiguration_ -struct BUS_MSG_MOTOR_CONFIG +struct MotorConfiguration { boolean_T has_hall_sens; boolean_T has_quadrature_encoder; @@ -329,69 +192,33 @@ struct BUS_MSG_MOTOR_CONFIG boolean_T has_torque_sens; boolean_T use_index; boolean_T enable_verbosity; - - // Number of polese of the motor. - uint8_T number_poles; - - // Encoder tolerance. - uint8_T encoder_tolerance; - - // Resolution of rotor encoder. + int16_T hall_sensors_offset; int16_T rotor_encoder_resolution; - - // Offset of the rotor encoder. int16_T rotor_index_offset; + uint8_T encoder_tolerance; + uint8_T pole_pairs; + real32_T Kbemf; + real32_T Rphase; + real32_T Imin; + real32_T Imax; + real32_T Vcc; + real32_T Vmax; + real32_T resistance; + real32_T inductance; + real32_T thermal_resistance; + real32_T thermal_time_constant; }; #endif -#ifndef DEFINED_TYPEDEF_FOR_BUS_MESSAGES_RX_ -#define DEFINED_TYPEDEF_FOR_BUS_MESSAGES_RX_ - -// Aggregate of all CAN received messages. -struct BUS_MESSAGES_RX -{ - BUS_MSG_CONTROL_MODE control_mode; - BUS_MSG_CURRENT_LIMIT current_limit; - BUS_MSG_DESIRED_TARGETS desired_targets; - BUS_MSG_PID pid; - BUS_MSG_MOTOR_CONFIG motor_config; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_BUS_MESSAGES_RX_MULTIPLE_ -#define DEFINED_TYPEDEF_FOR_BUS_MESSAGES_RX_MULTIPLE_ - -struct BUS_MESSAGES_RX_MULTIPLE -{ - BUS_MESSAGES_RX messages[CAN_MAX_NUM_PACKETS]; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_BUS_STATUS_RX_ -#define DEFINED_TYPEDEF_FOR_BUS_STATUS_RX_ - -// Aggregate of all events specifying types of received messages. -struct BUS_STATUS_RX -{ - boolean_T control_mode; - boolean_T current_limit; - boolean_T desired_targets; - boolean_T current_pid; - boolean_T velocity_pid; - boolean_T motor_config; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_BUS_STATUS_RX_MULTIPLE_ -#define DEFINED_TYPEDEF_FOR_BUS_STATUS_RX_MULTIPLE_ +#ifndef DEFINED_TYPEDEF_FOR_ActuatorConfiguration_ +#define DEFINED_TYPEDEF_FOR_ActuatorConfiguration_ -struct BUS_STATUS_RX_MULTIPLE +struct ActuatorConfiguration { - BUS_STATUS_RX status[CAN_MAX_NUM_PACKETS]; + Thresholds thresholds; + PIDsConfiguration pids; + MotorConfiguration motor; }; #endif @@ -522,6 +349,76 @@ struct BUS_CAN_RX #endif +#ifndef DEFINED_TYPEDEF_FOR_Targets_ +#define DEFINED_TYPEDEF_FOR_Targets_ + +struct Targets +{ + real32_T jointpositions; + real32_T jointvelocities; + real32_T motorcurrent; + real32_T motorvoltage; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_SupervisorEvents_ +#define DEFINED_TYPEDEF_FOR_SupervisorEvents_ + +typedef enum { + SupervisorEvents_None = 0, // Default value + SupervisorEvents_SetLimit, + SupervisorEvents_SetControlMode, + SupervisorEvents_SetMotorConfig, + SupervisorEvents_SetPid, + SupervisorEvents_SetTarget +} SupervisorEvents; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_SupervisorInputLimits_ +#define DEFINED_TYPEDEF_FOR_SupervisorInputLimits_ + +struct SupervisorInputLimits +{ + real32_T overload; + real32_T peak; + real32_T nominal; + ControlModes type; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_SupervisorReceivedEvents_ +#define DEFINED_TYPEDEF_FOR_SupervisorReceivedEvents_ + +struct SupervisorReceivedEvents +{ + Targets targets_content; + PID pid_content; + SupervisorEvents event_type; + ControlModes control_mode_content; + SupervisorInputLimits limits_content; + MotorConfiguration motor_config_content; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_MCControlModes_ +#define DEFINED_TYPEDEF_FOR_MCControlModes_ + +typedef enum { + MCControlModes_Idle = 0, // Default value + MCControlModes_OpenLoop = 80, + MCControlModes_SpeedVoltage = 10, + MCControlModes_SpeedCurrent = 11, + MCControlModes_Current = 6, + MCControlModes_NotConfigured = 176, + MCControlModes_HWFault = 160 +} MCControlModes; + +#endif + #ifndef DEFINED_TYPEDEF_FOR_MCOPC_ #define DEFINED_TYPEDEF_FOR_MCOPC_ diff --git a/emBODY/eBcode/arch-arm/mbd/amcbldc/can-encoder/can_encoder.cpp b/emBODY/eBcode/arch-arm/mbd/amcbldc/can-encoder/can_encoder.cpp index dfa9b3f20..8ba151191 100644 --- a/emBODY/eBcode/arch-arm/mbd/amcbldc/can-encoder/can_encoder.cpp +++ b/emBODY/eBcode/arch-arm/mbd/amcbldc/can-encoder/can_encoder.cpp @@ -7,9 +7,9 @@ // // Code generated for Simulink model 'can_encoder'. // -// Model version : 6.3 +// Model version : 6.8 // Simulink Coder version : 23.2 (R2023b) 01-Aug-2023 -// C/C++ source code generated on : Tue Feb 13 11:54:30 2024 +// C/C++ source code generated on : Wed Feb 28 11:28:10 2024 // // Target selection: ert.tlc // Embedded hardware selection: ARM Compatible->ARM Cortex-M @@ -19,30 +19,29 @@ #include "can_encoder.h" #include "rtwtypes.h" #include "can_encoder_types.h" -#include "can_encoder_private.h" #include #include +#include "can_encoder_private.h" #include "rtw_defines.h" -MdlrefDW_can_encoder_T can_encoder_MdlrefDW; - // // Output and update for atomic system: // '/format_can_id' // '/format_can_id' // void can_encoder_format_can_id(uint8_T rtu_class, uint8_T rtu_can_id_amc, - uint8_T rtu_dst_typ, uint16_T *rty_pkt_id) + uint8_T rtu_dst_typ, B_format_can_id_can_encoder_T *localB) { - *rty_pkt_id = static_cast(rtu_class << 8); - *rty_pkt_id = static_cast(rtu_can_id_amc << 4 | *rty_pkt_id); - *rty_pkt_id = static_cast(rtu_dst_typ | *rty_pkt_id); + localB->pkt_id = static_cast(rtu_class << 8); + localB->pkt_id = static_cast(rtu_can_id_amc << 4 | localB->pkt_id); + localB->pkt_id = static_cast(rtu_dst_typ | localB->pkt_id); } // Output and update for referenced model: 'can_encoder' void can_encoder(const BUS_MESSAGES_TX *rtu_messages_tx, const BUS_STATUS_TX - *rtu_status_tx, const ConfigurationParameters - *rtu_ConfigurationParameters, BUS_CAN_MULTIPLE *rty_pck_tx) + *rtu_status_tx, const ActuatorConfiguration + *rtu_ConfigurationParameters, BUS_CAN_MULTIPLE *rty_pck_tx, + B_can_encoder_c_T *localB) { BUS_CAN rtb_BusCreator_n_0[4]; BUS_CAN rtb_BusCreator_hs; @@ -63,8 +62,8 @@ void can_encoder(const BUS_MESSAGES_TX *rtu_messages_tx, const BUS_STATUS_TX rtb_BusCreator_m.packet.PAYLOAD[0] = static_cast (rtu_messages_tx->status.control_mode); rtb_BusCreator_m.packet.PAYLOAD[1] = 0U; - qY = 5U - rtu_ConfigurationParameters->CurLoopPID.shift_factor; - if (5U - rtu_ConfigurationParameters->CurLoopPID.shift_factor > 5U) { + qY = 5U - rtu_ConfigurationParameters->pids.currentPID.shift_factor; + if (5U - rtu_ConfigurationParameters->pids.currentPID.shift_factor > 5U) { qY = 0U; } @@ -161,27 +160,35 @@ void can_encoder(const BUS_MESSAGES_TX *rtu_messages_tx, const BUS_STATUS_TX rtb_BusCreator_n.packet.PAYLOAD[6] = b_tmp[2]; rtb_BusCreator_n.packet.PAYLOAD[7] = b_tmp[3]; - // BusCreator: '/Bus Creator' incorporates: + // MATLAB Function: '/format_can_id' incorporates: // Constant: '/Constant' // Constant: '/Motor Control Streaming' // Constant: '/TYPESTATUS' + + can_encoder_format_can_id(1, CAN_ID_AMC, 3, &localB->sf_format_can_id_n); + + // BusCreator: '/Bus Creator' incorporates: + // BusCreator: '/Bus Creator1' // Constant: '/length' - // MATLAB Function: '/format_can_id' - can_encoder_format_can_id(1, CAN_ID_AMC, 3, &rtb_BusCreator_m.packet.ID); rtb_BusCreator_m.available = rtu_status_tx->status; rtb_BusCreator_m.length = 8U; + rtb_BusCreator_m.packet.ID = localB->sf_format_can_id_n.pkt_id; - // BusCreator: '/Bus Creator' incorporates: + // MATLAB Function: '/format_can_id' incorporates: // Constant: '/Constant' // Constant: '/Motor Control Streaming' // Constant: '/TYPE2FOC' + + can_encoder_format_can_id(1, CAN_ID_AMC, 0, &localB->sf_format_can_id); + + // BusCreator: '/Bus Creator' incorporates: + // BusCreator: '/Bus Creator1' // Constant: '/length' - // MATLAB Function: '/format_can_id' - can_encoder_format_can_id(1, CAN_ID_AMC, 0, &rtb_BusCreator_n.packet.ID); rtb_BusCreator_n.available = rtu_status_tx->foc; rtb_BusCreator_n.length = 8U; + rtb_BusCreator_n.packet.ID = localB->sf_format_can_id.pkt_id; // BusCreator: '/Bus Creator' incorporates: // BusCreator: '/Bus Creator6' @@ -212,10 +219,9 @@ void can_encoder(const BUS_MESSAGES_TX *rtu_messages_tx, const BUS_STATUS_TX } // Model initialize function -void can_encoder_initialize(const char_T **rt_errorStatus) +void can_encoder_initialize(const char_T **rt_errorStatus, + RT_MODEL_can_encoder_T *const can_encoder_M) { - RT_MODEL_can_encoder_T *const can_encoder_M = &(can_encoder_MdlrefDW.rtm); - // Registration code // initialize error status diff --git a/emBODY/eBcode/arch-arm/mbd/amcbldc/can-encoder/can_encoder.h b/emBODY/eBcode/arch-arm/mbd/amcbldc/can-encoder/can_encoder.h index c24878889..f8b7e549b 100644 --- a/emBODY/eBcode/arch-arm/mbd/amcbldc/can-encoder/can_encoder.h +++ b/emBODY/eBcode/arch-arm/mbd/amcbldc/can-encoder/can_encoder.h @@ -7,9 +7,9 @@ // // Code generated for Simulink model 'can_encoder'. // -// Model version : 6.3 +// Model version : 6.8 // Simulink Coder version : 23.2 (R2023b) 01-Aug-2023 -// C/C++ source code generated on : Tue Feb 13 11:54:30 2024 +// C/C++ source code generated on : Wed Feb 28 11:28:10 2024 // // Target selection: ert.tlc // Embedded hardware selection: ARM Compatible->ARM Cortex-M @@ -21,6 +21,27 @@ #include "rtwtypes.h" #include "can_encoder_types.h" +// Block signals for system '/format_can_id' +struct B_format_can_id_can_encoder_T { + uint16_T pkt_id; // '/format_can_id' +}; + +// Block signals for model 'can_encoder' +struct B_can_encoder_c_T { + B_format_can_id_can_encoder_T sf_format_can_id_n;// '/format_can_id' + B_format_can_id_can_encoder_T sf_format_can_id;// '/format_can_id' +}; + +// Real-time Model Data Structure +struct tag_RTM_can_encoder_T { + const char_T **errorStatus; +}; + +struct MdlrefDW_can_encoder_T { + B_can_encoder_c_T rtb; + RT_MODEL_can_encoder_T rtm; +}; + // // Exported Global Parameters // @@ -41,12 +62,16 @@ extern uint8_T CAN_ID_AMC; // Variable: CAN_ID_AMC // '/Constant' // 4 bits defining the ID of the AMC_BLDC board. -extern void can_encoder(const BUS_MESSAGES_TX *rtu_messages_tx, const - BUS_STATUS_TX *rtu_status_tx, const ConfigurationParameters - *rtu_ConfigurationParameters, BUS_CAN_MULTIPLE *rty_pck_tx); // Model reference registration function -extern void can_encoder_initialize(const char_T **rt_errorStatus); +extern void can_encoder_initialize(const char_T **rt_errorStatus, + RT_MODEL_can_encoder_T *const can_encoder_M); +extern void can_encoder_format_can_id(uint8_T rtu_class, uint8_T rtu_can_id_amc, + uint8_T rtu_dst_typ, B_format_can_id_can_encoder_T *localB); +extern void can_encoder(const BUS_MESSAGES_TX *rtu_messages_tx, const + BUS_STATUS_TX *rtu_status_tx, const ActuatorConfiguration + *rtu_ConfigurationParameters, BUS_CAN_MULTIPLE *rty_pck_tx, B_can_encoder_c_T * + localB); //- // The generated code includes comments that allow you to trace directly diff --git a/emBODY/eBcode/arch-arm/mbd/amcbldc/can-encoder/can_encoder_private.h b/emBODY/eBcode/arch-arm/mbd/amcbldc/can-encoder/can_encoder_private.h index 2e3487724..e44300366 100644 --- a/emBODY/eBcode/arch-arm/mbd/amcbldc/can-encoder/can_encoder_private.h +++ b/emBODY/eBcode/arch-arm/mbd/amcbldc/can-encoder/can_encoder_private.h @@ -7,9 +7,9 @@ // // Code generated for Simulink model 'can_encoder'. // -// Model version : 6.3 +// Model version : 6.8 // Simulink Coder version : 23.2 (R2023b) 01-Aug-2023 -// C/C++ source code generated on : Tue Feb 13 11:54:30 2024 +// C/C++ source code generated on : Wed Feb 28 11:28:10 2024 // // Target selection: ert.tlc // Embedded hardware selection: ARM Compatible->ARM Cortex-M @@ -21,11 +21,6 @@ #include "rtwtypes.h" #include "can_encoder_types.h" -// Real-time Model Data Structure -struct tag_RTM_can_encoder_T { - const char_T **errorStatus; -}; - // Macros for accessing real-time model data structure #ifndef rtmGetErrorStatus #define rtmGetErrorStatus(rtm) (*((rtm)->errorStatus)) @@ -42,15 +37,6 @@ struct tag_RTM_can_encoder_T { #ifndef rtmSetErrorStatusPointer #define rtmSetErrorStatusPointer(rtm, val) ((rtm)->errorStatus = (val)) #endif - -struct MdlrefDW_can_encoder_T { - RT_MODEL_can_encoder_T rtm; -}; - -extern void can_encoder_format_can_id(uint8_T rtu_class, uint8_T rtu_can_id_amc, - uint8_T rtu_dst_typ, uint16_T *rty_pkt_id); -extern MdlrefDW_can_encoder_T can_encoder_MdlrefDW; - #endif // RTW_HEADER_can_encoder_private_h_ // diff --git a/emBODY/eBcode/arch-arm/mbd/amcbldc/can-encoder/can_encoder_types.h b/emBODY/eBcode/arch-arm/mbd/amcbldc/can-encoder/can_encoder_types.h index 3835e59c5..6f2c73e45 100644 --- a/emBODY/eBcode/arch-arm/mbd/amcbldc/can-encoder/can_encoder_types.h +++ b/emBODY/eBcode/arch-arm/mbd/amcbldc/can-encoder/can_encoder_types.h @@ -7,9 +7,9 @@ // // Code generated for Simulink model 'can_encoder'. // -// Model version : 6.3 +// Model version : 6.8 // Simulink Coder version : 23.2 (R2023b) 01-Aug-2023 -// C/C++ source code generated on : Tue Feb 13 11:54:30 2024 +// C/C++ source code generated on : Wed Feb 28 11:28:10 2024 // // Target selection: ert.tlc // Embedded hardware selection: ARM Compatible->ARM Cortex-M @@ -150,65 +150,62 @@ struct BUS_STATUS_TX #endif -#ifndef DEFINED_TYPEDEF_FOR_MotorConfig_ -#define DEFINED_TYPEDEF_FOR_MotorConfig_ +#ifndef DEFINED_TYPEDEF_FOR_Thresholds_ +#define DEFINED_TYPEDEF_FOR_Thresholds_ -struct MotorConfig +struct Thresholds { - // Angular offset in degrees between the stator windings and the hall sensors. - real32_T hall_sens_offset; - boolean_T has_hall_sens; - boolean_T has_quadrature_encoder; - boolean_T has_speed_quadrature_encoder; - boolean_T has_torque_sens; - boolean_T use_index; - boolean_T enable_verbosity; - int16_T rotor_encoder_resolution; - int16_T rotor_index_offset; - uint8_T encoder_tolerance; - uint8_T pole_pairs; - real32_T Kbemf; - real32_T Rphase; - real32_T Imin; - real32_T Imax; - real32_T Vmax; - real32_T resistance; - real32_T inductance; - real32_T thermal_resistance; - real32_T thermal_time_constant; -}; + // Can be only non-negative + real32_T jntVelMax; -#endif + // Current that can be kept for an indefinite period of time w/o damaging the motor + // Expressed in [A] as all the internal computations are done this way + // Can be only non-negative + real32_T motorNominalCurrents; -#ifndef DEFINED_TYPEDEF_FOR_EstimationVelocityModes_ -#define DEFINED_TYPEDEF_FOR_EstimationVelocityModes_ + // Current that can be applied for a short period of time + // Expressed in [A] as all the internal computations are done this way + // Can be only non-negative + real32_T motorPeakCurrents; -typedef enum { - EstimationVelocityModes_Disabled = 0,// Default value - EstimationVelocityModes_MovingAverage, - EstimationVelocityModes_LeastSquares -} EstimationVelocityModes; + // Currents over this threshold can instantaneously damages the motor + // Expressed in [A] as all the internal computations are done this way + // Can be only non-negative + real32_T motorOverloadCurrents; -#endif + // Expressed in ticks + // Max value is 32000 + // Can be only non-negative + uint32_T motorPwmLimit; -#ifndef DEFINED_TYPEDEF_FOR_EstimationConfig_ -#define DEFINED_TYPEDEF_FOR_EstimationConfig_ + // The critical temperature of the motor that triggers i2t current protection. + real32_T motorCriticalTemperature; +}; -struct EstimationConfig -{ - EstimationVelocityModes velocity_mode; +#endif - // Forgetting factor in [0, 1] for exponential weighting-based estimation of RMS current value - real32_T current_rms_lambda; -}; +#ifndef DEFINED_TYPEDEF_FOR_ControlModes_ +#define DEFINED_TYPEDEF_FOR_ControlModes_ + +typedef enum { + ControlModes_NotConfigured = 0, // Default value + ControlModes_Idle, + ControlModes_Position, + ControlModes_PositionDirect, + ControlModes_Current, + ControlModes_Velocity, + ControlModes_Voltage, + ControlModes_HwFaultCM +} ControlModes; #endif -#ifndef DEFINED_TYPEDEF_FOR_PIDConfig_ -#define DEFINED_TYPEDEF_FOR_PIDConfig_ +#ifndef DEFINED_TYPEDEF_FOR_PID_ +#define DEFINED_TYPEDEF_FOR_PID_ -struct PIDConfig +struct PID { + ControlModes type; real32_T OutMax; real32_T OutMin; real32_T P; @@ -222,75 +219,56 @@ struct PIDConfig #endif -#ifndef DEFINED_TYPEDEF_FOR_Thresholds_ -#define DEFINED_TYPEDEF_FOR_Thresholds_ +#ifndef DEFINED_TYPEDEF_FOR_PIDsConfiguration_ +#define DEFINED_TYPEDEF_FOR_PIDsConfiguration_ -struct Thresholds +struct PIDsConfiguration { - // It shall be greater than hardwareJntPosMin - real32_T jntPosMin; - - // It shall be smaller than hardwareJntPosMax - real32_T jntPosMax; - - // Imposed by hardware constraint - real32_T hardwareJntPosMin; - - // Imposed by hardware constraint - real32_T hardwareJntPosMax; - - // If robotMin == rotorMax == 0, there's no check - real32_T rotorPosMin; - - // If robotMin == rotorMax == 0, there's no check - real32_T rotorPosMax; - - // Can be only non-negative - real32_T jntVelMax; - - // Timeout on reception of velocity setpoint - // Can be only non-negative - uint32_T velocityTimeout; - - // Current that can be kept for an indefinite period of time w/o damaging the motor - // Expressed in [A] as all the internal computations are done this way - // Can be only non-negative - real32_T motorNominalCurrents; + PID currentPID; + PID velocityPID; + PID positionPID; +}; - // Current that can be applied for a short period of time - // Expressed in [A] as all the internal computations are done this way - // Can be only non-negative - real32_T motorPeakCurrents; +#endif - // Currents over this threshold can instantaneously damages the motor - // Expressed in [A] as all the internal computations are done this way - // Can be only non-negative - real32_T motorOverloadCurrents; +#ifndef DEFINED_TYPEDEF_FOR_MotorConfiguration_ +#define DEFINED_TYPEDEF_FOR_MotorConfiguration_ - // Expressed in ticks - // Max value is 32000 - // Can be only non-negative - uint32_T motorPwmLimit; - - // The critical temperature of the motor that triggers i2t current protection. - real32_T motorCriticalTemperature; +struct MotorConfiguration +{ + boolean_T has_hall_sens; + boolean_T has_quadrature_encoder; + boolean_T has_speed_quadrature_encoder; + boolean_T has_torque_sens; + boolean_T use_index; + boolean_T enable_verbosity; + int16_T hall_sensors_offset; + int16_T rotor_encoder_resolution; + int16_T rotor_index_offset; + uint8_T encoder_tolerance; + uint8_T pole_pairs; + real32_T Kbemf; + real32_T Rphase; + real32_T Imin; + real32_T Imax; + real32_T Vcc; + real32_T Vmax; + real32_T resistance; + real32_T inductance; + real32_T thermal_resistance; + real32_T thermal_time_constant; }; #endif -#ifndef DEFINED_TYPEDEF_FOR_ConfigurationParameters_ -#define DEFINED_TYPEDEF_FOR_ConfigurationParameters_ +#ifndef DEFINED_TYPEDEF_FOR_ActuatorConfiguration_ +#define DEFINED_TYPEDEF_FOR_ActuatorConfiguration_ -struct ConfigurationParameters +struct ActuatorConfiguration { - MotorConfig motorconfig; - EstimationConfig estimationconfig; - PIDConfig CurLoopPID; - PIDConfig PosLoopPID; - PIDConfig VelLoopPID; - PIDConfig DirLoopPID; Thresholds thresholds; - real32_T environment_temperature; + PIDsConfiguration pids; + MotorConfiguration motor; }; #endif diff --git a/emBODY/eBcode/arch-arm/mbd/amcbldc/control-foc/FOCInnerLoop.cpp b/emBODY/eBcode/arch-arm/mbd/amcbldc/control-foc/FOCInnerLoop.cpp index b883df187..8ee2387a5 100644 --- a/emBODY/eBcode/arch-arm/mbd/amcbldc/control-foc/FOCInnerLoop.cpp +++ b/emBODY/eBcode/arch-arm/mbd/amcbldc/control-foc/FOCInnerLoop.cpp @@ -7,9 +7,9 @@ // // Code generated for Simulink model 'control_foc'. // -// Model version : 6.18 +// Model version : 6.68 // Simulink Coder version : 23.2 (R2023b) 01-Aug-2023 -// C/C++ source code generated on : Tue Feb 13 11:54:36 2024 +// C/C++ source code generated on : Wed Feb 28 11:28:20 2024 // // Target selection: ert.tlc // Embedded hardware selection: ARM Compatible->ARM Cortex-M @@ -18,92 +18,85 @@ // #include "FOCInnerLoop.h" #include "control_foc_types.h" - -extern "C" -{ - -#include "rt_nonfinite.h" - -} - +#include "rtwtypes.h" #include "arm_math.h" #include -#include "rtwtypes.h" #include "control_foc_private.h" #include "zero_crossing_types.h" // System initialize for atomic system: '/FOC inner loop' void FOCInnerLoop_Init(DW_FOCInnerLoop_T *localDW) { - // InitializeConditions for DiscreteTransferFcn: '/Filter Differentiator TF' - localDW->FilterDifferentiatorTF_states = 0.0F; - // InitializeConditions for DiscreteIntegrator: '/Integrator' - localDW->Integrator_DSTATE = 0.0F; localDW->Integrator_PrevResetState = 2; - // InitializeConditions for DiscreteTransferFcn: '/Filter Differentiator TF' - localDW->FilterDifferentiatorTF_states_k = 0.0F; - // InitializeConditions for DiscreteIntegrator: '/Integrator' - localDW->Integrator_DSTATE_o = 0.0F; localDW->Integrator_PrevResetState_k = 2; } // Outputs for atomic system: '/FOC inner loop' -void FOCInnerLoop(const ConfigurationParameters *rtu_ConfigurationParameters, - const SensorsData *rtu_Sensors, const EstimatedData - *rtu_Estimates, const Targets *rtu_Targets, const - ControlOuterOutputs *rtu_OuterOutputs, ControlOutputs - *rty_FOCOutputs, B_FOCInnerLoop_T *localB, const - ConstB_FOCInnerLoop_T *localC, DW_FOCInnerLoop_T *localDW, +void FOCInnerLoop(const SensorsData *rtu_Sensors, const real32_T + *rtu_ConfigurationParameters, const real32_T + *rtu_ConfigurationParameters_f, const real32_T + *rtu_ConfigurationParameters_i, const real32_T + *rtu_ConfigurationParameters_iy, const real32_T + *rtu_ConfigurationParameters_c, const real32_T + *rtu_ConfigurationParameters_o, const real32_T + *rtu_ConfigurationParameters_j, const real32_T *rtu_Estimates, + const real32_T *rtu_Targets, const real32_T *rtu_OuterOutputs, + const boolean_T *rtu_OuterOutputs_i, const boolean_T + *rtu_OuterOutputs_ig, const boolean_T *rtu_OuterOutputs_a, + const real32_T *rtu_OuterOutputs_c, FOCOutputs *rty_FOCOutputs, + B_FOCInnerLoop_T *localB, DW_FOCInnerLoop_T *localDW, ZCE_FOCInnerLoop_T *localZCE) { real32_T rtb_IaIbIc0[2]; - real32_T DProdOut; - real32_T rtb_Add; - real32_T rtb_FilterDifferentiatorTF_f; real32_T rtb_PProdOut_k; real32_T rtb_Product; + real32_T rtb_Product1; + real32_T rtb_Product2; real32_T rtb_SinCos_o1; real32_T rtb_SinCos_o2; real32_T rtb_algDD_o1; real32_T rtb_algDD_o1_g; - real32_T rtb_algDD_o2; real32_T rtb_algDD_o2_l; + real32_T rtb_sum_alpha; + real32_T rtb_sum_beta; int8_T tmp; int8_T tmp_0; + boolean_T zcEvent; + + // Product: '/Product2' + rtb_Product2 = *rtu_OuterOutputs * *rtu_ConfigurationParameters; - // Sum: '/Add' incorporates: - // Product: '/Product1' - // Product: '/Product2' + // Product: '/Product1' + rtb_Product1 = *rtu_Estimates * *rtu_ConfigurationParameters_f; - rtb_Add = rtu_OuterOutputs->motorcurrent.current * - rtu_ConfigurationParameters->motorconfig.Rphase + - rtu_Estimates->jointvelocities.velocity * - rtu_ConfigurationParameters->motorconfig.Kbemf; + // Sum: '/Add' + rtb_Product2 += rtb_Product1; // MinMax: '/Min' - if ((rtu_ConfigurationParameters->motorconfig.Vmax <= - rtu_Sensors->supplyvoltagesensors.voltage) || rtIsNaNF - (rtu_Sensors->supplyvoltagesensors.voltage)) { - rtb_algDD_o2_l = rtu_ConfigurationParameters->motorconfig.Vmax; - } else { - rtb_algDD_o2_l = rtu_Sensors->supplyvoltagesensors.voltage; + rtb_Product1 = *rtu_ConfigurationParameters_i; + if (rtu_Sensors->driversensors.Vcc <= rtb_Product1) { + rtb_Product1 = rtu_Sensors->driversensors.Vcc; } // Product: '/Product' incorporates: + // Constant: '/Constant2' + // Constant: '/Constant3' // Gain: '/Gain4' + // Gain: '/Gain5' // MinMax: '/Min' + // Sum: '/Sum5' - rtb_Product = 0.5F * rtb_algDD_o2_l * localC->Sum5; + rtb_Product = 0.5F * rtb_Product1 * 0.975F; // Gain: '/Ia+Ib+Ic=0' - rtb_algDD_o1_g = rtu_Sensors->motorsensors.Iabc[1]; + rtb_Product1 = rtu_Sensors->motorsensors.Iabc[1]; rtb_SinCos_o1 = rtu_Sensors->motorsensors.Iabc[0]; rtb_SinCos_o2 = rtu_Sensors->motorsensors.Iabc[2]; for (int32_T i = 0; i < 2; i++) { - rtb_IaIbIc0[i] = (rtCP_IaIbIc0_Gain[i + 2] * rtb_algDD_o1_g + + rtb_IaIbIc0[i] = (rtCP_IaIbIc0_Gain[i + 2] * rtb_Product1 + rtCP_IaIbIc0_Gain[i] * rtb_SinCos_o1) + rtCP_IaIbIc0_Gain[i + 4] * rtb_SinCos_o2; } @@ -126,19 +119,22 @@ void FOCInnerLoop(const ConfigurationParameters *rtu_ConfigurationParameters, // Outputs for Atomic SubSystem: '/Two inputs CRL' // AlgorithmDescriptorDelegate generated from: '/a16' - arm_park_f32(rtb_algDD_o1_g, rtb_algDD_o2_l, &rtb_algDD_o1, &rtb_algDD_o2, + arm_park_f32(rtb_algDD_o1_g, rtb_algDD_o2_l, &rtb_algDD_o1, &rtb_Product1, rtb_SinCos_o1, rtb_SinCos_o2); // End of Outputs for SubSystem: '/Two inputs CRL' // Sum: '/Sum' - rtb_PProdOut_k = rtu_OuterOutputs->motorcurrent.current - rtb_algDD_o2; + rtb_PProdOut_k = *rtu_OuterOutputs - rtb_Product1; // Product: '/PProd Out' - rtb_algDD_o1_g = rtb_PProdOut_k * rtu_ConfigurationParameters->CurLoopPID.P; + rtb_algDD_o2_l = rtb_PProdOut_k * *rtu_ConfigurationParameters_iy; // Product: '/IProd Out' - rtb_algDD_o2_l = rtb_PProdOut_k * rtu_ConfigurationParameters->CurLoopPID.I; + rtb_algDD_o1_g = rtb_PProdOut_k * *rtu_ConfigurationParameters_c; + + // Product: '/DProd Out' + rtb_PProdOut_k *= *rtu_ConfigurationParameters_o; // SampleTimeMath: '/Tsamp' incorporates: // SampleTimeMath: '/Tsamp' @@ -149,8 +145,7 @@ void FOCInnerLoop(const ConfigurationParameters *rtu_ConfigurationParameters, // About '/Tsamp': // y = u * K where K = ( w * Ts ) - rtb_FilterDifferentiatorTF_f = rtu_ConfigurationParameters->CurLoopPID.N * - 2.25E-5F; + rtb_sum_beta = *rtu_ConfigurationParameters_j * 1.82857148E-5F; // Math: '/Reciprocal' incorporates: // Constant: '/Filter Den Constant' @@ -167,15 +162,16 @@ void FOCInnerLoop(const ConfigurationParameters *rtu_ConfigurationParameters, // About '/Tsamp': // y = u * K where K = ( w * Ts ) - DProdOut = 1.0F / (rtb_FilterDifferentiatorTF_f + 1.0F); + rtb_sum_alpha = 1.0F / (rtb_sum_beta + 1.0F); // DiscreteTransferFcn: '/Filter Differentiator TF' - if (rtu_OuterOutputs->pid_reset && (localZCE->FilterDifferentiatorTF_Reset_ZCE - != POS_ZCSIG)) { + zcEvent = ((*rtu_OuterOutputs_i) && + (localZCE->FilterDifferentiatorTF_Reset_ZCE != POS_ZCSIG)); + if (zcEvent) { localDW->FilterDifferentiatorTF_states = 0.0F; } - localZCE->FilterDifferentiatorTF_Reset_ZCE = rtu_OuterOutputs->pid_reset; + localZCE->FilterDifferentiatorTF_Reset_ZCE = *rtu_OuterOutputs_i; // Product: '/Divide' incorporates: // Constant: '/Filter Den Constant' @@ -190,15 +186,12 @@ void FOCInnerLoop(const ConfigurationParameters *rtu_ConfigurationParameters, // About '/Tsamp': // y = u * K where K = ( w * Ts ) - rtb_FilterDifferentiatorTF_f = (rtb_FilterDifferentiatorTF_f - 1.0F) * - DProdOut; + rtb_sum_beta = (rtb_sum_beta - 1.0F) * rtb_sum_alpha; // DiscreteTransferFcn: '/Filter Differentiator TF' incorporates: - // Product: '/DProd Out' // Product: '/Divide' - localDW->FilterDifferentiatorTF_tmp = rtb_PProdOut_k * - rtu_ConfigurationParameters->CurLoopPID.D - rtb_FilterDifferentiatorTF_f * + localDW->FilterDifferentiatorTF_tmp = rtb_PProdOut_k - rtb_sum_beta * localDW->FilterDifferentiatorTF_states; // Product: '/NProd Out' incorporates: @@ -210,39 +203,41 @@ void FOCInnerLoop(const ConfigurationParameters *rtu_ConfigurationParameters, // Operator: reciprocal rtb_PProdOut_k = (localDW->FilterDifferentiatorTF_tmp - - localDW->FilterDifferentiatorTF_states) * DProdOut * - rtu_ConfigurationParameters->CurLoopPID.N; + localDW->FilterDifferentiatorTF_states) * rtb_sum_alpha * + *rtu_ConfigurationParameters_j; // Sum: '/SumI1' incorporates: // Sum: '/Sum Fdbk' // Sum: '/SumI3' // UnitDelay: '/Unit Delay' - localB->SumI1 = (localDW->UnitDelay_DSTATE - ((rtb_algDD_o1_g + - localDW->Integrator_DSTATE) + rtb_PProdOut_k)) + rtb_algDD_o2_l; + localB->SumI1 = (localDW->UnitDelay_DSTATE - ((rtb_algDD_o2_l + + localDW->Integrator_DSTATE) + rtb_PProdOut_k)) + rtb_algDD_o1_g; // DiscreteIntegrator: '/Integrator' - if (rtu_OuterOutputs->pid_reset && (localDW->Integrator_PrevResetState <= 0)) - { + if ((*rtu_OuterOutputs_i) && (localDW->Integrator_PrevResetState <= 0)) { localDW->Integrator_DSTATE = 0.0F; } // DiscreteIntegrator: '/Integrator' - localB->Integrator = 2.25E-5F * localB->SumI1 + localDW->Integrator_DSTATE; + localB->Integrator = 1.82857148E-5F * localB->SumI1 + + localDW->Integrator_DSTATE; // Switch: '/Switch1' incorporates: - // Gain: '/Gain6' - // Product: '/Divide2' // Sum: '/Sum' // Sum: '/Sum2' - // Sum: '/Sum6' - if (rtu_OuterOutputs->cur_en) { - rtb_algDD_o1_g = ((rtb_algDD_o1_g + localB->Integrator) + rtb_PProdOut_k) + - rtb_Add; + if (*rtu_OuterOutputs_ig) { + rtb_algDD_o2_l = ((rtb_algDD_o2_l + localB->Integrator) + rtb_PProdOut_k) + + rtb_Product2; } else { - rtb_algDD_o1_g = rtu_Targets->motorvoltage.voltage * rtb_Product * 0.01F + - rtu_OuterOutputs->current_limiter; + // Product: '/Divide2' + rtb_algDD_o2_l = *rtu_Targets * rtb_Product; + + // Sum: '/Sum6' incorporates: + // Gain: '/Gain6' + + rtb_algDD_o2_l = 0.01F * rtb_algDD_o2_l + *rtu_OuterOutputs_c; } // End of Switch: '/Switch1' @@ -253,13 +248,13 @@ void FOCInnerLoop(const ConfigurationParameters *rtu_ConfigurationParameters, // RelationalOperator: '/UpperRelop' // Switch: '/Switch' - if (rtb_algDD_o1_g > rtb_Product) { - rtb_algDD_o1_g = rtb_Product; - } else if (rtb_algDD_o1_g < -rtb_Product) { + if (rtb_algDD_o2_l > rtb_Product) { + rtb_algDD_o2_l = rtb_Product; + } else if (rtb_algDD_o2_l < -rtb_Product) { // Switch: '/Switch' incorporates: // Gain: '/Gain2' - rtb_algDD_o1_g = -rtb_Product; + rtb_algDD_o2_l = -rtb_Product; } // End of Switch: '/Switch2' @@ -267,34 +262,36 @@ void FOCInnerLoop(const ConfigurationParameters *rtu_ConfigurationParameters, // Product: '/PProd Out' incorporates: // Gain: '/Gain' - rtb_PProdOut_k = -rtb_algDD_o1 * rtu_ConfigurationParameters->CurLoopPID.P; + rtb_PProdOut_k = -rtb_algDD_o1 * *rtu_ConfigurationParameters_iy; - // DiscreteTransferFcn: '/Filter Differentiator TF' incorporates: - // DiscreteTransferFcn: '/Filter Differentiator TF' + // Product: '/DProd Out' incorporates: // Gain: '/Gain' - // Product: '/DProd Out' - if (rtu_OuterOutputs->pid_reset && - (localZCE->FilterDifferentiatorTF_Reset_ZCE_o != POS_ZCSIG)) { + rtb_algDD_o1_g = -rtb_algDD_o1 * *rtu_ConfigurationParameters_o; + + // DiscreteTransferFcn: '/Filter Differentiator TF' + zcEvent = ((*rtu_OuterOutputs_i) && + (localZCE->FilterDifferentiatorTF_Reset_ZCE_o != POS_ZCSIG)); + if (zcEvent) { localDW->FilterDifferentiatorTF_states_k = 0.0F; } - localZCE->FilterDifferentiatorTF_Reset_ZCE_o = rtu_OuterOutputs->pid_reset; - localDW->FilterDifferentiatorTF_tmp_c = -rtb_algDD_o1 * - rtu_ConfigurationParameters->CurLoopPID.D - rtb_FilterDifferentiatorTF_f * + localZCE->FilterDifferentiatorTF_Reset_ZCE_o = + localZCE->FilterDifferentiatorTF_Reset_ZCE; + localDW->FilterDifferentiatorTF_tmp_c = rtb_algDD_o1_g - rtb_sum_beta * localDW->FilterDifferentiatorTF_states_k; // Product: '/NProd Out' incorporates: // DiscreteTransferFcn: '/Filter Differentiator TF' // Product: '/DenCoefOut' - DProdOut = (localDW->FilterDifferentiatorTF_tmp_c - - localDW->FilterDifferentiatorTF_states_k) * DProdOut * - rtu_ConfigurationParameters->CurLoopPID.N; + rtb_sum_beta = (localDW->FilterDifferentiatorTF_tmp_c - + localDW->FilterDifferentiatorTF_states_k) * rtb_sum_alpha * + *rtu_ConfigurationParameters_j; // Sum: '/Sum Fdbk' - rtb_FilterDifferentiatorTF_f = (rtb_PProdOut_k + localDW->Integrator_DSTATE_o) - + DProdOut; + rtb_algDD_o1_g = (rtb_PProdOut_k + localDW->Integrator_DSTATE_o) + + rtb_sum_beta; // Switch: '/Switch' incorporates: // Gain: '/Gain2' @@ -302,25 +299,24 @@ void FOCInnerLoop(const ConfigurationParameters *rtu_ConfigurationParameters, // RelationalOperator: '/u_GT_lo' // Switch: '/Switch1' - if (rtb_FilterDifferentiatorTF_f >= rtb_Product) { - rtb_algDD_o2_l = rtb_Product; - } else if (rtb_FilterDifferentiatorTF_f > -rtb_Product) { + if (rtb_algDD_o1_g >= rtb_Product) { + rtb_sum_alpha = rtb_Product; + } else if (rtb_algDD_o1_g > -rtb_Product) { // Switch: '/Switch1' - rtb_algDD_o2_l = rtb_FilterDifferentiatorTF_f; + rtb_sum_alpha = rtb_algDD_o1_g; } else { - rtb_algDD_o2_l = -rtb_Product; + rtb_sum_alpha = -rtb_Product; } // Sum: '/Diff' incorporates: // Switch: '/Switch' - rtb_algDD_o2_l = rtb_FilterDifferentiatorTF_f - rtb_algDD_o2_l; + rtb_sum_alpha = rtb_algDD_o1_g - rtb_sum_alpha; // Product: '/IProd Out' incorporates: // Gain: '/Gain' - rtb_FilterDifferentiatorTF_f = -rtb_algDD_o1 * - rtu_ConfigurationParameters->CurLoopPID.I; + localB->Switch = -rtb_algDD_o1 * *rtu_ConfigurationParameters_c; // Switch: '/Switch1' incorporates: // Constant: '/Clamping_zero' @@ -328,7 +324,7 @@ void FOCInnerLoop(const ConfigurationParameters *rtu_ConfigurationParameters, // Constant: '/Constant2' // RelationalOperator: '/fix for DT propagation issue' - if (rtb_algDD_o2_l > 0.0F) { + if (rtb_sum_alpha > 0.0F) { tmp = 1; } else { tmp = -1; @@ -340,7 +336,7 @@ void FOCInnerLoop(const ConfigurationParameters *rtu_ConfigurationParameters, // Constant: '/Constant4' // RelationalOperator: '/fix for DT propagation issue1' - if (rtb_FilterDifferentiatorTF_f > 0.0F) { + if (localB->Switch > 0.0F) { tmp_0 = 1; } else { tmp_0 = -1; @@ -354,97 +350,94 @@ void FOCInnerLoop(const ConfigurationParameters *rtu_ConfigurationParameters, // Switch: '/Switch1' // Switch: '/Switch2' - if ((rtb_algDD_o2_l != 0.0F) && (tmp == tmp_0)) { - // Switch: '/Switch' incorporates: + if ((rtb_sum_alpha != 0.0F) && (tmp == tmp_0)) { + // Product: '/IProd Out' incorporates: // Constant: '/Constant1' + // Switch: '/Switch' localB->Switch = 0.0F; - } else { - // Switch: '/Switch' - localB->Switch = rtb_FilterDifferentiatorTF_f; } // End of Switch: '/Switch' // DiscreteIntegrator: '/Integrator' - if (rtu_OuterOutputs->pid_reset && (localDW->Integrator_PrevResetState_k <= 0)) - { + if ((*rtu_OuterOutputs_i) && (localDW->Integrator_PrevResetState_k <= 0)) { localDW->Integrator_DSTATE_o = 0.0F; } // DiscreteIntegrator: '/Integrator' - localB->Integrator_j = 2.25E-5F * localB->Switch + + localB->Integrator_j = 1.82857148E-5F * localB->Switch + localDW->Integrator_DSTATE_o; // Sum: '/Sum' - rtb_FilterDifferentiatorTF_f = (rtb_PProdOut_k + localB->Integrator_j) + - DProdOut; + rtb_PProdOut_k = (rtb_PProdOut_k + localB->Integrator_j) + rtb_sum_beta; // Switch: '/Switch2' incorporates: + // Gain: '/Gain2' // RelationalOperator: '/LowerRelop1' + // RelationalOperator: '/UpperRelop' + // Switch: '/Switch' - if (!(rtb_FilterDifferentiatorTF_f > rtb_Product)) { + if (rtb_PProdOut_k > rtb_Product) { + rtb_PProdOut_k = rtb_Product; + } else if (rtb_PProdOut_k < -rtb_Product) { // Switch: '/Switch' incorporates: // Gain: '/Gain2' - // RelationalOperator: '/UpperRelop' - if (rtb_FilterDifferentiatorTF_f < -rtb_Product) { - rtb_Product = -rtb_Product; - } else { - rtb_Product = rtb_FilterDifferentiatorTF_f; - } - - // End of Switch: '/Switch' + rtb_PProdOut_k = -rtb_Product; } - // End of Switch: '/Switch2' - // Outputs for Atomic SubSystem: '/Two inputs CRL' - // AlgorithmDescriptorDelegate generated from: '/a16' - arm_inv_park_f32(rtb_Product, rtb_algDD_o1_g, &rtb_FilterDifferentiatorTF_f, - &rtb_PProdOut_k, rtb_SinCos_o1, rtb_SinCos_o2); + // AlgorithmDescriptorDelegate generated from: '/a16' incorporates: + // Switch: '/Switch2' + + arm_inv_park_f32(rtb_PProdOut_k, rtb_algDD_o2_l, &rtb_Product, &rtb_sum_alpha, + rtb_SinCos_o1, rtb_SinCos_o2); // End of Outputs for SubSystem: '/Two inputs CRL' // Switch: '/Switch2' incorporates: // Constant: '/Constant1' - if (rtu_OuterOutputs->out_en) { + if (*rtu_OuterOutputs_a) { // Gain: '/sqrt3_by_two' - rtb_SinCos_o2 = 0.866025388F * rtb_PProdOut_k; + rtb_SinCos_o1 = 0.866025388F * rtb_sum_alpha; // Gain: '/one_by_two' - rtb_Product = 0.5F * rtb_FilterDifferentiatorTF_f; + rtb_PProdOut_k = 0.5F * rtb_Product; // Sum: '/add_c' - rtb_SinCos_o1 = (0.0F - rtb_Product) - rtb_SinCos_o2; + rtb_sum_beta = (0.0F - rtb_PProdOut_k) - rtb_SinCos_o1; // Sum: '/add_b' - rtb_Product = rtb_SinCos_o2 - rtb_Product; + rtb_PProdOut_k = rtb_SinCos_o1 - rtb_PProdOut_k; // MinMax: '/Min1' - if ((rtb_FilterDifferentiatorTF_f <= rtb_Product) || rtIsNaNF(rtb_Product)) - { - rtb_SinCos_o2 = rtb_FilterDifferentiatorTF_f; + if (rtb_Product <= rtb_PProdOut_k) { + rtb_SinCos_o1 = rtb_Product; } else { - rtb_SinCos_o2 = rtb_Product; + rtb_SinCos_o1 = rtb_PProdOut_k; } - if ((!(rtb_SinCos_o2 <= rtb_SinCos_o1)) && (!rtIsNaNF(rtb_SinCos_o1))) { - rtb_SinCos_o2 = rtb_SinCos_o1; + if (rtb_SinCos_o1 > rtb_sum_beta) { + rtb_SinCos_o1 = rtb_sum_beta; } // Gain: '/Gain3' incorporates: // Product: '/Divide1' - rtb_algDD_o2_l = rtb_algDD_o1_g / rtu_Sensors->supplyvoltagesensors.voltage * - 100.0F; + rtb_SinCos_o2 = rtb_algDD_o2_l / rtu_Sensors->driversensors.Vcc * 100.0F; // Saturate: '/Saturation1' - if (rtb_algDD_o2_l > 100.0F) { - rtb_algDD_o2_l = 100.0F; - } else if (rtb_algDD_o2_l < -100.0F) { - rtb_algDD_o2_l = -100.0F; + if (rtb_SinCos_o2 > 100.0F) { + // BusCreator: '/Bus Creator' + rty_FOCOutputs->Vq = 100.0F; + } else if (rtb_SinCos_o2 < -100.0F) { + // BusCreator: '/Bus Creator' + rty_FOCOutputs->Vq = -100.0F; + } else { + // BusCreator: '/Bus Creator' + rty_FOCOutputs->Vq = rtb_SinCos_o2; } // End of Saturate: '/Saturation1' @@ -456,14 +449,14 @@ void FOCInnerLoop(const ConfigurationParameters *rtu_ConfigurationParameters, // Product: '/Divide' // Sum: '/Sum4' - rtb_FilterDifferentiatorTF_f = (rtb_FilterDifferentiatorTF_f - rtb_SinCos_o2) - / rtu_Sensors->supplyvoltagesensors.voltage * 100.0F + 5.0F; + rtb_Product = (rtb_Product - rtb_SinCos_o1) / rtu_Sensors->driversensors.Vcc + * 100.0F + 5.0F; // Saturate: '/Saturation' - if (rtb_FilterDifferentiatorTF_f > 100.0F) { - rtb_FilterDifferentiatorTF_f = 100.0F; - } else if (rtb_FilterDifferentiatorTF_f < 0.0F) { - rtb_FilterDifferentiatorTF_f = 0.0F; + if (rtb_Product > 100.0F) { + rtb_Product = 100.0F; + } else if (rtb_Product < 0.0F) { + rtb_Product = 0.0F; } // Sum: '/Sum1' incorporates: @@ -473,14 +466,14 @@ void FOCInnerLoop(const ConfigurationParameters *rtu_ConfigurationParameters, // Product: '/Divide' // Sum: '/Sum4' - rtb_Product = (rtb_Product - rtb_SinCos_o2) / - rtu_Sensors->supplyvoltagesensors.voltage * 100.0F + 5.0F; + rtb_PProdOut_k = (rtb_PProdOut_k - rtb_SinCos_o1) / + rtu_Sensors->driversensors.Vcc * 100.0F + 5.0F; // Saturate: '/Saturation' - if (rtb_Product > 100.0F) { - rtb_Product = 100.0F; - } else if (rtb_Product < 0.0F) { - rtb_Product = 0.0F; + if (rtb_PProdOut_k > 100.0F) { + rtb_PProdOut_k = 100.0F; + } else if (rtb_PProdOut_k < 0.0F) { + rtb_PProdOut_k = 0.0F; } // Sum: '/Sum1' incorporates: @@ -490,76 +483,65 @@ void FOCInnerLoop(const ConfigurationParameters *rtu_ConfigurationParameters, // Product: '/Divide' // Sum: '/Sum4' - rtb_SinCos_o1 = (rtb_SinCos_o1 - rtb_SinCos_o2) / - rtu_Sensors->supplyvoltagesensors.voltage * 100.0F + 5.0F; + rtb_sum_beta = (rtb_sum_beta - rtb_SinCos_o1) / + rtu_Sensors->driversensors.Vcc * 100.0F + 5.0F; // Saturate: '/Saturation' - if (rtb_SinCos_o1 > 100.0F) { - rtb_SinCos_o1 = 100.0F; - } else if (rtb_SinCos_o1 < 0.0F) { - rtb_SinCos_o1 = 0.0F; + if (rtb_sum_beta > 100.0F) { + rtb_sum_beta = 100.0F; + } else if (rtb_sum_beta < 0.0F) { + rtb_sum_beta = 0.0F; } } else { - rtb_algDD_o2_l = 0.0F; - rtb_FilterDifferentiatorTF_f = 0.0F; + // BusCreator: '/Bus Creator' incorporates: + // Constant: '/Constant1' + + rty_FOCOutputs->Vq = 0.0F; rtb_Product = 0.0F; - rtb_SinCos_o1 = 0.0F; + rtb_PProdOut_k = 0.0F; + rtb_sum_beta = 0.0F; } // End of Switch: '/Switch2' - // BusCreator: '/Bus Creator1' - rty_FOCOutputs->Iq_fbk.current = rtb_algDD_o2; - - // BusCreator: '/Bus Creator2' - rty_FOCOutputs->Id_fbk.current = rtb_algDD_o1; - - // BusCreator: '/Bus Creator3' incorporates: + // BusCreator: '/Bus Creator' incorporates: // Constant: '/Constant' - - rty_FOCOutputs->Iq_rms.current = 0.0F; - - // BusCreator: '/Bus Creator4' incorporates: // Constant: '/Constant1' - rty_FOCOutputs->Id_rms.current = 0.0F; + rty_FOCOutputs->Vabc[0] = rtb_Product; + rty_FOCOutputs->Vabc[1] = rtb_PProdOut_k; + rty_FOCOutputs->Vabc[2] = rtb_sum_beta; + rty_FOCOutputs->Iq_fbk = rtb_Product1; + rty_FOCOutputs->Id_fbk = rtb_algDD_o1; + rty_FOCOutputs->Iq_rms = 0.0F; + rty_FOCOutputs->Id_rms = 0.0F; - // BusCreator: '/Bus Creator' - rty_FOCOutputs->Vq = rtb_algDD_o2_l; - rty_FOCOutputs->Vabc[0] = rtb_FilterDifferentiatorTF_f; - rty_FOCOutputs->Vabc[1] = rtb_Product; - rty_FOCOutputs->Vabc[2] = rtb_SinCos_o1; + // Sum: '/Sum3' incorporates: + // UnitDelay: '/Unit Delay' - // Sum: '/Sum3' - localB->Sum3 = rtb_algDD_o1_g - rtb_Add; + localDW->UnitDelay_DSTATE = rtb_algDD_o2_l - rtb_Product2; } // Update for atomic system: '/FOC inner loop' -void FOCInnerLoop_Update(const ControlOuterOutputs *rtu_OuterOutputs, - B_FOCInnerLoop_T *localB, DW_FOCInnerLoop_T *localDW) +void FOCInnerLoop_Update(const boolean_T *rtu_OuterOutputs_i, B_FOCInnerLoop_T + *localB, DW_FOCInnerLoop_T *localDW) { // Update for DiscreteTransferFcn: '/Filter Differentiator TF' localDW->FilterDifferentiatorTF_states = localDW->FilterDifferentiatorTF_tmp; - // Update for UnitDelay: '/Unit Delay' - localDW->UnitDelay_DSTATE = localB->Sum3; - // Update for DiscreteIntegrator: '/Integrator' - localDW->Integrator_DSTATE = 2.25E-5F * localB->SumI1 + localB->Integrator; - localDW->Integrator_PrevResetState = static_cast - (rtu_OuterOutputs->pid_reset); + localDW->Integrator_DSTATE = 1.82857148E-5F * localB->SumI1 + + localB->Integrator; + localDW->Integrator_PrevResetState = static_cast(*rtu_OuterOutputs_i); // Update for DiscreteTransferFcn: '/Filter Differentiator TF' localDW->FilterDifferentiatorTF_states_k = localDW->FilterDifferentiatorTF_tmp_c; - // Update for DiscreteIntegrator: '/Integrator' incorporates: - // DiscreteIntegrator: '/Integrator' - - localDW->Integrator_DSTATE_o = 2.25E-5F * localB->Switch + + // Update for DiscreteIntegrator: '/Integrator' + localDW->Integrator_DSTATE_o = 1.82857148E-5F * localB->Switch + localB->Integrator_j; - localDW->Integrator_PrevResetState_k = static_cast - (rtu_OuterOutputs->pid_reset); + localDW->Integrator_PrevResetState_k = static_cast(*rtu_OuterOutputs_i); } // diff --git a/emBODY/eBcode/arch-arm/mbd/amcbldc/control-foc/FOCInnerLoop.h b/emBODY/eBcode/arch-arm/mbd/amcbldc/control-foc/FOCInnerLoop.h index 5ac7a1952..ef6ffaf54 100644 --- a/emBODY/eBcode/arch-arm/mbd/amcbldc/control-foc/FOCInnerLoop.h +++ b/emBODY/eBcode/arch-arm/mbd/amcbldc/control-foc/FOCInnerLoop.h @@ -7,9 +7,9 @@ // // Code generated for Simulink model 'control_foc'. // -// Model version : 6.18 +// Model version : 6.68 // Simulink Coder version : 23.2 (R2023b) 01-Aug-2023 -// C/C++ source code generated on : Tue Feb 13 11:54:36 2024 +// C/C++ source code generated on : Wed Feb 28 11:28:20 2024 // // Target selection: ert.tlc // Embedded hardware selection: ARM Compatible->ARM Cortex-M @@ -28,7 +28,6 @@ struct B_FOCInnerLoop_T { real32_T Integrator; // '/Integrator' real32_T Switch; // '/Switch' real32_T Integrator_j; // '/Integrator' - real32_T Sum3; // '/Sum3' }; // Block states (default storage) for system '/FOC inner loop' @@ -50,21 +49,20 @@ struct ZCE_FOCInnerLoop_T { ZCSigState FilterDifferentiatorTF_Reset_ZCE_o;// '/Filter Differentiator TF' }; -// Invariant block signals for system '/FOC inner loop' -struct ConstB_FOCInnerLoop_T { - real32_T Gain5; // '/Gain5' - real32_T Sum5; // '/Sum5' -}; - extern void FOCInnerLoop_Init(DW_FOCInnerLoop_T *localDW); -extern void FOCInnerLoop_Update(const ControlOuterOutputs *rtu_OuterOutputs, +extern void FOCInnerLoop_Update(const boolean_T *rtu_OuterOutputs_i, B_FOCInnerLoop_T *localB, DW_FOCInnerLoop_T *localDW); -extern void FOCInnerLoop(const ConfigurationParameters - *rtu_ConfigurationParameters, const SensorsData *rtu_Sensors, const - EstimatedData *rtu_Estimates, const Targets *rtu_Targets, const - ControlOuterOutputs *rtu_OuterOutputs, ControlOutputs *rty_FOCOutputs, - B_FOCInnerLoop_T *localB, const ConstB_FOCInnerLoop_T *localC, - DW_FOCInnerLoop_T *localDW, ZCE_FOCInnerLoop_T *localZCE); +extern void FOCInnerLoop(const SensorsData *rtu_Sensors, const real32_T + *rtu_ConfigurationParameters, const real32_T *rtu_ConfigurationParameters_f, + const real32_T *rtu_ConfigurationParameters_i, const real32_T + *rtu_ConfigurationParameters_iy, const real32_T *rtu_ConfigurationParameters_c, + const real32_T *rtu_ConfigurationParameters_o, const real32_T + *rtu_ConfigurationParameters_j, const real32_T *rtu_Estimates, const real32_T * + rtu_Targets, const real32_T *rtu_OuterOutputs, const boolean_T + *rtu_OuterOutputs_i, const boolean_T *rtu_OuterOutputs_ig, const boolean_T + *rtu_OuterOutputs_a, const real32_T *rtu_OuterOutputs_c, FOCOutputs + *rty_FOCOutputs, B_FOCInnerLoop_T *localB, DW_FOCInnerLoop_T *localDW, + ZCE_FOCInnerLoop_T *localZCE); #endif // RTW_HEADER_FOCInnerLoop_h_ diff --git a/emBODY/eBcode/arch-arm/mbd/amcbldc/control-foc/control_foc.cpp b/emBODY/eBcode/arch-arm/mbd/amcbldc/control-foc/control_foc.cpp index a04c59838..3b80dcca9 100644 --- a/emBODY/eBcode/arch-arm/mbd/amcbldc/control-foc/control_foc.cpp +++ b/emBODY/eBcode/arch-arm/mbd/amcbldc/control-foc/control_foc.cpp @@ -7,9 +7,9 @@ // // Code generated for Simulink model 'control_foc'. // -// Model version : 6.18 +// Model version : 6.68 // Simulink Coder version : 23.2 (R2023b) 01-Aug-2023 -// C/C++ source code generated on : Tue Feb 13 11:54:36 2024 +// C/C++ source code generated on : Wed Feb 28 11:28:20 2024 // // Target selection: ert.tlc // Embedded hardware selection: ARM Compatible->ARM Cortex-M @@ -18,16 +18,8 @@ // #include "control_foc.h" #include "control_foc_types.h" +#include "rtwtypes.h" #include "FOCInnerLoop.h" -#include "control_foc_private.h" -#include - -extern "C" -{ - -#include "rt_nonfinite.h" - -} // System initialize for referenced model: 'control_foc' void control_foc_Init(DW_control_foc_f_T *localDW) @@ -39,47 +31,62 @@ void control_foc_Init(DW_control_foc_f_T *localDW) } // Output and update for referenced model: 'control_foc' -void control_foc(const SensorsData *rtu_Sensors, const FOCSlowInputs - *rtu_FOCSlowInputs, ControlOutputs *rty_FOCOutputs, - B_control_foc_c_T *localB, DW_control_foc_f_T *localDW, - ZCE_control_foc_T *localZCE) +void control_foc(const SensorsData *rtu_Sensors, const real32_T + *rtu_FocSlowInputs_actuator_configuration_pids_currentPID_P, + const real32_T + *rtu_FocSlowInputs_actuator_configuration_pids_currentPID_I, + const real32_T + *rtu_FocSlowInputs_actuator_configuration_pids_currentPID_D, + const real32_T + *rtu_FocSlowInputs_actuator_configuration_pids_currentPID_N, + const real32_T + *rtu_FocSlowInputs_actuator_configuration_motor_Kbemf, const + real32_T *rtu_FocSlowInputs_actuator_configuration_motor_Rphase, + const real32_T + *rtu_FocSlowInputs_actuator_configuration_motor_Vmax, const + real32_T *rtu_FocSlowInputs_estimated_data_jointvelocities, + const real32_T *rtu_FocSlowInputs_targets_motorvoltage, const + boolean_T *rtu_FocSlowInputs_control_outer_outputs_cur_en, + const boolean_T *rtu_FocSlowInputs_control_outer_outputs_out_en, + const boolean_T + *rtu_FocSlowInputs_control_outer_outputs_pid_reset, const + real32_T *rtu_FocSlowInputs_control_outer_outputs_motorcurrent, + const real32_T + *rtu_FocSlowInputs_control_outer_outputs_current_limiter, + FOCOutputs *rty_FOCOutputs, B_control_foc_c_T *localB, + DW_control_foc_f_T *localDW, ZCE_control_foc_T *localZCE) { // Outputs for Atomic SubSystem: '/FOC inner loop' - FOCInnerLoop(&rtu_FOCSlowInputs->configurationparameters, rtu_Sensors, - &rtu_FOCSlowInputs->estimateddata, &rtu_FOCSlowInputs->targets, - &rtu_FOCSlowInputs->controlouteroutputs, rty_FOCOutputs, - &localB->FOCinnerloop, &control_foc_ConstB.FOCinnerloop, - &localDW->FOCinnerloop, &localZCE->FOCinnerloop); + FOCInnerLoop(rtu_Sensors, + rtu_FocSlowInputs_actuator_configuration_motor_Rphase, + rtu_FocSlowInputs_actuator_configuration_motor_Kbemf, + rtu_FocSlowInputs_actuator_configuration_motor_Vmax, + rtu_FocSlowInputs_actuator_configuration_pids_currentPID_P, + rtu_FocSlowInputs_actuator_configuration_pids_currentPID_I, + rtu_FocSlowInputs_actuator_configuration_pids_currentPID_D, + rtu_FocSlowInputs_actuator_configuration_pids_currentPID_N, + rtu_FocSlowInputs_estimated_data_jointvelocities, + rtu_FocSlowInputs_targets_motorvoltage, + rtu_FocSlowInputs_control_outer_outputs_motorcurrent, + rtu_FocSlowInputs_control_outer_outputs_pid_reset, + rtu_FocSlowInputs_control_outer_outputs_cur_en, + rtu_FocSlowInputs_control_outer_outputs_out_en, + rtu_FocSlowInputs_control_outer_outputs_current_limiter, + rty_FOCOutputs, &localB->FOCinnerloop, &localDW->FOCinnerloop, + &localZCE->FOCinnerloop); // End of Outputs for SubSystem: '/FOC inner loop' // Update for Atomic SubSystem: '/FOC inner loop' - FOCInnerLoop_Update(&rtu_FOCSlowInputs->controlouteroutputs, + FOCInnerLoop_Update(rtu_FocSlowInputs_control_outer_outputs_pid_reset, &localB->FOCinnerloop, &localDW->FOCinnerloop); // End of Update for SubSystem: '/FOC inner loop' } // Model initialize function -void control_foc_initialize(const char_T **rt_errorStatus, - RT_MODEL_control_foc_T *const control_foc_M, B_control_foc_c_T *localB, - DW_control_foc_f_T *localDW, ZCE_control_foc_T *localZCE) +void control_foc_initialize(ZCE_control_foc_T *localZCE) { - // Registration code - - // initialize non-finites - rt_InitInfAndNaN(sizeof(real_T)); - - // initialize error status - rtmSetErrorStatusPointer(control_foc_M, rt_errorStatus); - - // block I/O - (void) std::memset((static_cast(localB)), 0, - sizeof(B_control_foc_c_T)); - - // states (dwork) - (void) std::memset(static_cast(localDW), 0, - sizeof(DW_control_foc_f_T)); localZCE->FOCinnerloop.FilterDifferentiatorTF_Reset_ZCE = POS_ZCSIG; localZCE->FOCinnerloop.FilterDifferentiatorTF_Reset_ZCE_o = POS_ZCSIG; } diff --git a/emBODY/eBcode/arch-arm/mbd/amcbldc/control-foc/control_foc.h b/emBODY/eBcode/arch-arm/mbd/amcbldc/control-foc/control_foc.h index a9fc2ef04..7c18626fa 100644 --- a/emBODY/eBcode/arch-arm/mbd/amcbldc/control-foc/control_foc.h +++ b/emBODY/eBcode/arch-arm/mbd/amcbldc/control-foc/control_foc.h @@ -7,9 +7,9 @@ // // Code generated for Simulink model 'control_foc'. // -// Model version : 6.18 +// Model version : 6.68 // Simulink Coder version : 23.2 (R2023b) 01-Aug-2023 -// C/C++ source code generated on : Tue Feb 13 11:54:36 2024 +// C/C++ source code generated on : Wed Feb 28 11:28:20 2024 // // Target selection: ert.tlc // Embedded hardware selection: ARM Compatible->ARM Cortex-M @@ -21,14 +21,6 @@ #include "rtwtypes.h" #include "control_foc_types.h" #include "FOCInnerLoop.h" -#include - -extern "C" -{ - -#include "rt_nonfinite.h" - -} // Block signals for model 'control_foc' struct B_control_foc_c_T { @@ -45,31 +37,32 @@ struct ZCE_control_foc_T { ZCE_FOCInnerLoop_T FOCinnerloop; // '/FOC inner loop' }; -// Invariant block signals for model 'control_foc' -struct ConstB_control_foc_h_T { - ConstB_FOCInnerLoop_T FOCinnerloop; // '/FOC inner loop' -}; - -// Real-time Model Data Structure -struct tag_RTM_control_foc_T { - const char_T **errorStatus; -}; - struct MdlrefDW_control_foc_T { B_control_foc_c_T rtb; DW_control_foc_f_T rtdw; - RT_MODEL_control_foc_T rtm; ZCE_control_foc_T rtzce; }; // Model reference registration function -extern void control_foc_initialize(const char_T **rt_errorStatus, - RT_MODEL_control_foc_T *const control_foc_M, B_control_foc_c_T *localB, - DW_control_foc_f_T *localDW, ZCE_control_foc_T *localZCE); +extern void control_foc_initialize(ZCE_control_foc_T *localZCE); extern void control_foc_Init(DW_control_foc_f_T *localDW); -extern void control_foc(const SensorsData *rtu_Sensors, const FOCSlowInputs - *rtu_FOCSlowInputs, ControlOutputs *rty_FOCOutputs, B_control_foc_c_T *localB, - DW_control_foc_f_T *localDW, ZCE_control_foc_T *localZCE); +extern void control_foc(const SensorsData *rtu_Sensors, const real32_T + *rtu_FocSlowInputs_actuator_configuration_pids_currentPID_P, const real32_T + *rtu_FocSlowInputs_actuator_configuration_pids_currentPID_I, const real32_T + *rtu_FocSlowInputs_actuator_configuration_pids_currentPID_D, const real32_T + *rtu_FocSlowInputs_actuator_configuration_pids_currentPID_N, const real32_T + *rtu_FocSlowInputs_actuator_configuration_motor_Kbemf, const real32_T + *rtu_FocSlowInputs_actuator_configuration_motor_Rphase, const real32_T + *rtu_FocSlowInputs_actuator_configuration_motor_Vmax, const real32_T + *rtu_FocSlowInputs_estimated_data_jointvelocities, const real32_T + *rtu_FocSlowInputs_targets_motorvoltage, const boolean_T + *rtu_FocSlowInputs_control_outer_outputs_cur_en, const boolean_T + *rtu_FocSlowInputs_control_outer_outputs_out_en, const boolean_T + *rtu_FocSlowInputs_control_outer_outputs_pid_reset, const real32_T + *rtu_FocSlowInputs_control_outer_outputs_motorcurrent, const real32_T + *rtu_FocSlowInputs_control_outer_outputs_current_limiter, FOCOutputs + *rty_FOCOutputs, B_control_foc_c_T *localB, DW_control_foc_f_T *localDW, + ZCE_control_foc_T *localZCE); //- // These blocks were eliminated from the model due to optimizations: diff --git a/emBODY/eBcode/arch-arm/mbd/amcbldc/control-foc/control_foc_data.cpp b/emBODY/eBcode/arch-arm/mbd/amcbldc/control-foc/control_foc_data.cpp deleted file mode 100644 index 3d587e982..000000000 --- a/emBODY/eBcode/arch-arm/mbd/amcbldc/control-foc/control_foc_data.cpp +++ /dev/null @@ -1,37 +0,0 @@ -// -// Non-Degree Granting Education License -- for use at non-degree -// granting, nonprofit, education, and research organizations only. Not -// for commercial or industrial use. -// -// File: control_foc_data.cpp -// -// Code generated for Simulink model 'control_foc'. -// -// Model version : 6.18 -// Simulink Coder version : 23.2 (R2023b) 01-Aug-2023 -// C/C++ source code generated on : Tue Feb 13 11:54:36 2024 -// -// Target selection: ert.tlc -// Embedded hardware selection: ARM Compatible->ARM Cortex-M -// Code generation objectives: Unspecified -// Validation result: Not run -// -#include "control_foc_private.h" - -// Invariant block signals (default storage) -const ConstB_control_foc_h_T control_foc_ConstB = { - // Start of '/FOC inner loop' - { - 0.0249999985F - , // '/Gain5' - 0.975F - // '/Sum5' - } - // End of '/FOC inner loop' -}; - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/emBODY/eBcode/arch-arm/mbd/amcbldc/control-foc/control_foc_private.h b/emBODY/eBcode/arch-arm/mbd/amcbldc/control-foc/control_foc_private.h index f08774999..3e870cd32 100644 --- a/emBODY/eBcode/arch-arm/mbd/amcbldc/control-foc/control_foc_private.h +++ b/emBODY/eBcode/arch-arm/mbd/amcbldc/control-foc/control_foc_private.h @@ -7,9 +7,9 @@ // // Code generated for Simulink model 'control_foc'. // -// Model version : 6.18 +// Model version : 6.68 // Simulink Coder version : 23.2 (R2023b) 01-Aug-2023 -// C/C++ source code generated on : Tue Feb 13 11:54:36 2024 +// C/C++ source code generated on : Wed Feb 28 11:28:20 2024 // // Target selection: ert.tlc // Embedded hardware selection: ARM Compatible->ARM Cortex-M @@ -20,35 +20,13 @@ #define RTW_HEADER_control_foc_private_h_ #include "rtwtypes.h" #include "zero_crossing_types.h" -#include "control_foc.h" #include "control_foc_types.h" -// Macros for accessing real-time model data structure -#ifndef rtmGetErrorStatus -#define rtmGetErrorStatus(rtm) (*((rtm)->errorStatus)) -#endif - -#ifndef rtmSetErrorStatus -#define rtmSetErrorStatus(rtm, val) (*((rtm)->errorStatus) = (val)) -#endif - -#ifndef rtmGetErrorStatusPointer -#define rtmGetErrorStatusPointer(rtm) (rtm)->errorStatus -#endif - -#ifndef rtmSetErrorStatusPointer -#define rtmSetErrorStatusPointer(rtm, val) ((rtm)->errorStatus = (val)) -#endif - extern const real32_T rtCP_pooled_IgamRjjg0YgF[6]; #define rtCP_IaIbIc0_Gain rtCP_pooled_IgamRjjg0YgF // Computed Parameter: rtCP_IaIbIc0_Gain // Referenced by: '/Ia+Ib+Ic=0' - -// Invariant block signals (default storage) -extern const ConstB_control_foc_h_T control_foc_ConstB; - #endif // RTW_HEADER_control_foc_private_h_ // diff --git a/emBODY/eBcode/arch-arm/mbd/amcbldc/control-foc/control_foc_types.h b/emBODY/eBcode/arch-arm/mbd/amcbldc/control-foc/control_foc_types.h index c06f073e8..1b57c8899 100644 --- a/emBODY/eBcode/arch-arm/mbd/amcbldc/control-foc/control_foc_types.h +++ b/emBODY/eBcode/arch-arm/mbd/amcbldc/control-foc/control_foc_types.h @@ -7,9 +7,9 @@ // // Code generated for Simulink model 'control_foc'. // -// Model version : 6.18 +// Model version : 6.68 // Simulink Coder version : 23.2 (R2023b) 01-Aug-2023 -// C/C++ source code generated on : Tue Feb 13 11:54:36 2024 +// C/C++ source code generated on : Wed Feb 28 11:28:20 2024 // // Target selection: ert.tlc // Embedded hardware selection: ARM Compatible->ARM Cortex-M @@ -19,13 +19,23 @@ #ifndef RTW_HEADER_control_foc_types_h_ #define RTW_HEADER_control_foc_types_h_ #include "rtwtypes.h" -#ifndef DEFINED_TYPEDEF_FOR_JointPositions_ -#define DEFINED_TYPEDEF_FOR_JointPositions_ +#ifndef DEFINED_TYPEDEF_FOR_EstimationVelocityModes_ +#define DEFINED_TYPEDEF_FOR_EstimationVelocityModes_ -struct JointPositions +typedef enum { + EstimationVelocityModes_Disabled = 0,// Default value + EstimationVelocityModes_MovingAverage, + EstimationVelocityModes_LeastSquares +} EstimationVelocityModes; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_DriverSensors_ +#define DEFINED_TYPEDEF_FOR_DriverSensors_ + +struct DriverSensors { - // joint positions - real32_T position; + real32_T Vcc; }; #endif @@ -47,29 +57,15 @@ struct MotorSensors #endif -#ifndef DEFINED_TYPEDEF_FOR_SupplyVoltage_ -#define DEFINED_TYPEDEF_FOR_SupplyVoltage_ - -struct SupplyVoltage -{ - real32_T voltage; -}; - -#endif - #ifndef DEFINED_TYPEDEF_FOR_SensorsData_ #define DEFINED_TYPEDEF_FOR_SensorsData_ struct SensorsData { // position encoders - JointPositions jointpositions; - - // motor probes + real32_T jointpositions; + DriverSensors driversensors; MotorSensors motorsensors; - - // supply probes - SupplyVoltage supplyvoltagesensors; }; #endif @@ -85,278 +81,15 @@ typedef enum { ControlModes_Current, ControlModes_Velocity, ControlModes_Voltage, - ControlModes_Torque, ControlModes_HwFaultCM } ControlModes; #endif -#ifndef DEFINED_TYPEDEF_FOR_Flags_ -#define DEFINED_TYPEDEF_FOR_Flags_ - -struct Flags -{ - // control mode - ControlModes control_mode; - boolean_T enable_sending_msg_status; - boolean_T fault_button; - boolean_T enable_thermal_protection; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_MotorConfig_ -#define DEFINED_TYPEDEF_FOR_MotorConfig_ - -struct MotorConfig -{ - // Angular offset in degrees between the stator windings and the hall sensors. - real32_T hall_sens_offset; - boolean_T has_hall_sens; - boolean_T has_quadrature_encoder; - boolean_T has_speed_quadrature_encoder; - boolean_T has_torque_sens; - boolean_T use_index; - boolean_T enable_verbosity; - int16_T rotor_encoder_resolution; - int16_T rotor_index_offset; - uint8_T encoder_tolerance; - uint8_T pole_pairs; - real32_T Kbemf; - real32_T Rphase; - real32_T Imin; - real32_T Imax; - real32_T Vmax; - real32_T resistance; - real32_T inductance; - real32_T thermal_resistance; - real32_T thermal_time_constant; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_EstimationVelocityModes_ -#define DEFINED_TYPEDEF_FOR_EstimationVelocityModes_ - -typedef enum { - EstimationVelocityModes_Disabled = 0,// Default value - EstimationVelocityModes_MovingAverage, - EstimationVelocityModes_LeastSquares -} EstimationVelocityModes; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_EstimationConfig_ -#define DEFINED_TYPEDEF_FOR_EstimationConfig_ - -struct EstimationConfig -{ - EstimationVelocityModes velocity_mode; - - // Forgetting factor in [0, 1] for exponential weighting-based estimation of RMS current value - real32_T current_rms_lambda; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_PIDConfig_ -#define DEFINED_TYPEDEF_FOR_PIDConfig_ - -struct PIDConfig -{ - real32_T OutMax; - real32_T OutMin; - real32_T P; - real32_T I; - real32_T D; - real32_T N; - real32_T I0; - real32_T D0; - uint8_T shift_factor; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_Thresholds_ -#define DEFINED_TYPEDEF_FOR_Thresholds_ +#ifndef DEFINED_TYPEDEF_FOR_FOCOutputs_ +#define DEFINED_TYPEDEF_FOR_FOCOutputs_ -struct Thresholds -{ - // It shall be greater than hardwareJntPosMin - real32_T jntPosMin; - - // It shall be smaller than hardwareJntPosMax - real32_T jntPosMax; - - // Imposed by hardware constraint - real32_T hardwareJntPosMin; - - // Imposed by hardware constraint - real32_T hardwareJntPosMax; - - // If robotMin == rotorMax == 0, there's no check - real32_T rotorPosMin; - - // If robotMin == rotorMax == 0, there's no check - real32_T rotorPosMax; - - // Can be only non-negative - real32_T jntVelMax; - - // Timeout on reception of velocity setpoint - // Can be only non-negative - uint32_T velocityTimeout; - - // Current that can be kept for an indefinite period of time w/o damaging the motor - // Expressed in [A] as all the internal computations are done this way - // Can be only non-negative - real32_T motorNominalCurrents; - - // Current that can be applied for a short period of time - // Expressed in [A] as all the internal computations are done this way - // Can be only non-negative - real32_T motorPeakCurrents; - - // Currents over this threshold can instantaneously damages the motor - // Expressed in [A] as all the internal computations are done this way - // Can be only non-negative - real32_T motorOverloadCurrents; - - // Expressed in ticks - // Max value is 32000 - // Can be only non-negative - uint32_T motorPwmLimit; - - // The critical temperature of the motor that triggers i2t current protection. - real32_T motorCriticalTemperature; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_ConfigurationParameters_ -#define DEFINED_TYPEDEF_FOR_ConfigurationParameters_ - -struct ConfigurationParameters -{ - MotorConfig motorconfig; - EstimationConfig estimationconfig; - PIDConfig CurLoopPID; - PIDConfig PosLoopPID; - PIDConfig VelLoopPID; - PIDConfig DirLoopPID; - Thresholds thresholds; - real32_T environment_temperature; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_JointVelocities_ -#define DEFINED_TYPEDEF_FOR_JointVelocities_ - -struct JointVelocities -{ - // joint velocities - real32_T velocity; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_MotorCurrent_ -#define DEFINED_TYPEDEF_FOR_MotorCurrent_ - -struct MotorCurrent -{ - // motor current - real32_T current; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_MotorTemperature_ -#define DEFINED_TYPEDEF_FOR_MotorTemperature_ - -struct MotorTemperature -{ - // motor temperature - real32_T temperature; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_EstimatedData_ -#define DEFINED_TYPEDEF_FOR_EstimatedData_ - -struct EstimatedData -{ - // velocity - JointVelocities jointvelocities; - - // filtered motor current - MotorCurrent Iq_filtered; - - // motor temperature - MotorTemperature motor_temperature; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_MotorVoltage_ -#define DEFINED_TYPEDEF_FOR_MotorVoltage_ - -struct MotorVoltage -{ - // motor voltage - real32_T voltage; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_Targets_ -#define DEFINED_TYPEDEF_FOR_Targets_ - -struct Targets -{ - JointPositions jointpositions; - JointVelocities jointvelocities; - MotorCurrent motorcurrent; - MotorVoltage motorvoltage; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_ControlOuterOutputs_ -#define DEFINED_TYPEDEF_FOR_ControlOuterOutputs_ - -struct ControlOuterOutputs -{ - boolean_T vel_en; - boolean_T cur_en; - boolean_T out_en; - boolean_T pid_reset; - MotorCurrent motorcurrent; - real32_T current_limiter; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_FOCSlowInputs_ -#define DEFINED_TYPEDEF_FOR_FOCSlowInputs_ - -struct FOCSlowInputs -{ - Flags flags; - ConfigurationParameters configurationparameters; - EstimatedData estimateddata; - Targets targets; - ControlOuterOutputs controlouteroutputs; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_ControlOutputs_ -#define DEFINED_TYPEDEF_FOR_ControlOutputs_ - -struct ControlOutputs +struct FOCOutputs { // control effort (quadrature) real32_T Vq; @@ -365,23 +98,19 @@ struct ControlOutputs real32_T Vabc[3]; // quadrature current - MotorCurrent Iq_fbk; + real32_T Iq_fbk; // direct current - MotorCurrent Id_fbk; + real32_T Id_fbk; // RMS of Iq - MotorCurrent Iq_rms; + real32_T Iq_rms; // RMS of Id - MotorCurrent Id_rms; + real32_T Id_rms; }; #endif - -// Forward declaration for rtModel -typedef struct tag_RTM_control_foc_T RT_MODEL_control_foc_T; - #endif // RTW_HEADER_control_foc_types_h_ // diff --git a/emBODY/eBcode/arch-arm/mbd/amcbldc/control-outer/control_outer.cpp b/emBODY/eBcode/arch-arm/mbd/amcbldc/control-outer/control_outer.cpp index da1dc2b70..00219175b 100644 --- a/emBODY/eBcode/arch-arm/mbd/amcbldc/control-outer/control_outer.cpp +++ b/emBODY/eBcode/arch-arm/mbd/amcbldc/control-outer/control_outer.cpp @@ -7,9 +7,9 @@ // // Code generated for Simulink model 'control_outer'. // -// Model version : 6.3 +// Model version : 6.29 // Simulink Coder version : 23.2 (R2023b) 01-Aug-2023 -// C/C++ source code generated on : Tue Feb 13 11:54:43 2024 +// C/C++ source code generated on : Wed Feb 28 11:28:34 2024 // // Target selection: ert.tlc // Embedded hardware selection: ARM Compatible->ARM Cortex-M @@ -18,11 +18,10 @@ // #include "control_outer.h" #include "control_outer_types.h" -#include #include "rtwtypes.h" +#include #include "control_outer_private.h" #include "zero_crossing_types.h" -#include // System initialize for referenced model: 'control_outer' void control_outer_Init(DW_control_outer_f_T *localDW) @@ -35,29 +34,14 @@ void control_outer_Init(DW_control_outer_f_T *localDW) localDW->DelayInput1_DSTATE = ControlModes_Idle; - // InitializeConditions for DiscreteTransferFcn: '/Filter Differentiator TF' - localDW->FilterDifferentiatorTF_states = 0.0F; - // InitializeConditions for DiscreteIntegrator: '/Integrator' - localDW->Integrator_DSTATE = 0.0F; localDW->Integrator_PrevResetState = 2; - // InitializeConditions for DiscreteTransferFcn: '/Filter Differentiator TF' - localDW->FilterDifferentiatorTF_states_i = 0.0F; - // InitializeConditions for DiscreteIntegrator: '/Integrator' - localDW->Integrator_DSTATE_i = 0.0F; localDW->Integrator_PrevResetState_n = 2; - // InitializeConditions for DiscreteTransferFcn: '/Filter Differentiator TF' - localDW->FilterDifferentiatorTF_states_c = 0.0F; - // InitializeConditions for DiscreteIntegrator: '/Integrator' - localDW->Integrator_DSTATE_b = 0.0F; localDW->Integrator_PrevResetState_c = 2; - - // InitializeConditions for DiscreteIntegrator: '/Discrete-Time Integrator' - localDW->DiscreteTimeIntegrator_PrevResetState = 0; } // Enable for referenced model: 'control_outer' @@ -76,30 +60,34 @@ void control_outer_Disable(B_control_outer_c_T *localB, DW_control_outer_f_T } // Output and update for referenced model: 'control_outer' -void control_outer(const Flags *rtu_Flags, const ConfigurationParameters - *rtu_ConfigurationParameters, const Targets *rtu_Targets, - const SensorsData *rtu_Sensors, const EstimatedData - *rtu_Estimates, ControlOuterOutputs *rty_OuterOutputs, - B_control_outer_c_T *localB, DW_control_outer_f_T *localDW, - ZCE_control_outer_T *localZCE) +void control_outer(const Flags *rtu_Flags, const ActuatorConfiguration + *rtu_ConfigurationParameters, const real32_T + *rtu_Estimates_jointvelocities, const real32_T + *rtu_Estimates_Iq_filtered, const real32_T + *rtu_Sensors_jointpositions, const real32_T + *rtu_Targets_jointpositions, const real32_T + *rtu_Targets_jointvelocities, const real32_T + *rtu_Targets_motorcurrent, ControlOuterOutputs + *rty_OuterOutputs, B_control_outer_c_T *localB, + DW_control_outer_f_T *localDW, ZCE_control_outer_T *localZCE) { int32_T rowIdx; int32_T tmp; + real32_T Integrator; + real32_T Integrator_p; real32_T rtb_Abs; - real32_T rtb_DProdOut; - real32_T rtb_DProdOut_f; + real32_T rtb_DProdOut_b; real32_T rtb_DenCoefOut; - real32_T rtb_FilterDifferentiatorTF; - real32_T rtb_FilterDifferentiatorTF_n; - real32_T rtb_Gain1_h; - real32_T rtb_PProdOut; - real32_T rtb_PProdOut_o; - real32_T rtb_Product; + real32_T rtb_NProdOut; + real32_T rtb_NProdOut_f; + real32_T rtb_Sum2; + real32_T rtb_SumI1; + real32_T rtb_SumI1_e; real32_T rtb_Switch2; - real32_T rtb_Switch2_f; boolean_T rtb_Compare; boolean_T rtb_Compare_m; boolean_T rtb_FixPtRelationalOperator; + boolean_T rtb_Logic_idx_1; // Abs: '/Abs' rtb_Abs = std::abs(rtu_ConfigurationParameters->thresholds.jntVelMax); @@ -146,61 +134,103 @@ void control_outer(const Flags *rtu_Flags, const ConfigurationParameters // End of Switch: '/Switch2' // Sum: '/Sum3' - rtb_DenCoefOut = rtu_Targets->jointpositions.position - - rtu_Sensors->jointpositions.position; + rtb_DenCoefOut = *rtu_Targets_jointpositions - *rtu_Sensors_jointpositions; - // Product: '/PProd Out' - rtb_PProdOut = rtb_DenCoefOut * rtu_ConfigurationParameters->PosLoopPID.P; + // Product: '/PProd Out' incorporates: + // Product: '/PProd Out' - // SampleTimeMath: '/Tsamp' + rtb_Sum2 = rtb_DenCoefOut * rtu_ConfigurationParameters->pids.positionPID.P; + + // SampleTimeMath: '/Tsamp' incorporates: + // SampleTimeMath: '/Tsamp' // // About '/Tsamp': // y = u * K where K = ( w * Ts ) + // + // About '/Tsamp': + // y = u * K where K = ( w * Ts ) - rtb_Product = rtu_ConfigurationParameters->PosLoopPID.N * 0.0005F; + rtb_SumI1 = rtu_ConfigurationParameters->pids.positionPID.N * 0.0005F; // Math: '/Reciprocal' incorporates: // Constant: '/Filter Den Constant' + // Math: '/Reciprocal' + // SampleTimeMath: '/Tsamp' // Sum: '/SumDen' // // About '/Reciprocal': // Operator: reciprocal + // + // About '/Reciprocal': + // Operator: reciprocal + // + // About '/Tsamp': + // y = u * K where K = ( w * Ts ) - rtb_Switch2_f = 1.0F / (rtb_Product + 1.0F); - - // DiscreteTransferFcn: '/Filter Differentiator TF' incorporates: - // Constant: '/Filter Den Constant' - // Product: '/DProd Out' - // Product: '/Divide' - // Sum: '/SumNum' + rtb_SumI1_e = 1.0F / (rtb_SumI1 + 1.0F); + // DiscreteTransferFcn: '/Filter Differentiator TF' if (rtb_FixPtRelationalOperator && (localZCE->FilterDifferentiatorTF_Reset_ZCE != POS_ZCSIG)) { localDW->FilterDifferentiatorTF_states = 0.0F; } localZCE->FilterDifferentiatorTF_Reset_ZCE = rtb_FixPtRelationalOperator; - localDW->FilterDifferentiatorTF_tmp = rtb_DenCoefOut * - rtu_ConfigurationParameters->PosLoopPID.D - (rtb_Product - 1.0F) * - rtb_Switch2_f * localDW->FilterDifferentiatorTF_states; + + // Product: '/DProd Out' incorporates: + // Product: '/DProd Out' + + Integrator_p = rtb_DenCoefOut * + rtu_ConfigurationParameters->pids.positionPID.D; + + // Product: '/Divide' incorporates: + // Constant: '/Filter Den Constant' + // Math: '/Reciprocal' + // Product: '/Divide' + // SampleTimeMath: '/Tsamp' + // Sum: '/SumNum' + // + // About '/Reciprocal': + // Operator: reciprocal + // + // About '/Tsamp': + // y = u * K where K = ( w * Ts ) + + rtb_NProdOut_f = (rtb_SumI1 - 1.0F) * rtb_SumI1_e; + + // DiscreteTransferFcn: '/Filter Differentiator TF' incorporates: + // Product: '/DProd Out' + // Product: '/Divide' + + localDW->FilterDifferentiatorTF_tmp = Integrator_p - rtb_NProdOut_f * + localDW->FilterDifferentiatorTF_states; // Product: '/NProd Out' incorporates: // DiscreteTransferFcn: '/Filter Differentiator TF' + // Math: '/Reciprocal' // Product: '/DenCoefOut' + // + // About '/Reciprocal': + // Operator: reciprocal + + rtb_NProdOut = (localDW->FilterDifferentiatorTF_tmp - + localDW->FilterDifferentiatorTF_states) * rtb_SumI1_e * + rtu_ConfigurationParameters->pids.positionPID.N; - rtb_Switch2_f = (localDW->FilterDifferentiatorTF_tmp - - localDW->FilterDifferentiatorTF_states) * rtb_Switch2_f * - rtu_ConfigurationParameters->PosLoopPID.N; + // Product: '/IProd Out' incorporates: + // Product: '/IProd Out' + + rtb_DenCoefOut *= rtu_ConfigurationParameters->pids.positionPID.I; // Sum: '/SumI1' incorporates: // Product: '/IProd Out' + // Product: '/PProd Out' // Sum: '/Sum Fdbk' // Sum: '/SumI3' // UnitDelay: '/Unit Delay' - rtb_DProdOut = (localDW->UnitDelay_DSTATE - ((rtb_PProdOut + - localDW->Integrator_DSTATE) + rtb_Switch2_f)) + rtb_DenCoefOut * - rtu_ConfigurationParameters->PosLoopPID.I; + rtb_SumI1 = (localDW->UnitDelay_DSTATE - ((rtb_Sum2 + + localDW->Integrator_DSTATE) + rtb_NProdOut)) + rtb_DenCoefOut; // DiscreteIntegrator: '/Integrator' if (rtb_FixPtRelationalOperator && (localDW->Integrator_PrevResetState <= 0)) @@ -209,44 +239,15 @@ void control_outer(const Flags *rtu_Flags, const ConfigurationParameters } // DiscreteIntegrator: '/Integrator' - rtb_FilterDifferentiatorTF = 0.0005F * rtb_DProdOut + - localDW->Integrator_DSTATE; + Integrator = 0.0005F * rtb_SumI1 + localDW->Integrator_DSTATE; // RelationalOperator: '/Compare' incorporates: // Constant: '/Constant' rtb_Compare = (rtu_Flags->control_mode == ControlModes_Position); - // Product: '/PProd Out' - rtb_PProdOut_o = rtb_DenCoefOut * rtu_ConfigurationParameters->DirLoopPID.P; - - // Product: '/IProd Out' - rtb_Product = rtb_DenCoefOut * rtu_ConfigurationParameters->DirLoopPID.I; - - // Product: '/DProd Out' - rtb_DProdOut_f = rtb_DenCoefOut * rtu_ConfigurationParameters->DirLoopPID.D; - - // SampleTimeMath: '/Tsamp' - // - // About '/Tsamp': - // y = u * K where K = ( w * Ts ) - - rtb_Gain1_h = rtu_ConfigurationParameters->DirLoopPID.N * 0.0005F; - - // Math: '/Reciprocal' incorporates: - // Constant: '/Filter Den Constant' - // Sum: '/SumDen' - // - // About '/Reciprocal': - // Operator: reciprocal - - rtb_DenCoefOut = 1.0F / (rtb_Gain1_h + 1.0F); - // DiscreteTransferFcn: '/Filter Differentiator TF' incorporates: - // Constant: '/Filter Den Constant' // DiscreteTransferFcn: '/Filter Differentiator TF' - // Product: '/Divide' - // Sum: '/SumNum' if (rtb_FixPtRelationalOperator && (localZCE->FilterDifferentiatorTF_Reset_ZCE_m != POS_ZCSIG)) { @@ -254,24 +255,24 @@ void control_outer(const Flags *rtu_Flags, const ConfigurationParameters } localZCE->FilterDifferentiatorTF_Reset_ZCE_m = rtb_FixPtRelationalOperator; - localDW->FilterDifferentiatorTF_tmp_m = rtb_DProdOut_f - (rtb_Gain1_h - 1.0F) * - rtb_DenCoefOut * localDW->FilterDifferentiatorTF_states_i; + localDW->FilterDifferentiatorTF_tmp_m = Integrator_p - rtb_NProdOut_f * + localDW->FilterDifferentiatorTF_states_i; // Product: '/NProd Out' incorporates: // DiscreteTransferFcn: '/Filter Differentiator TF' // Product: '/DenCoefOut' - rtb_DenCoefOut = (localDW->FilterDifferentiatorTF_tmp_m - - localDW->FilterDifferentiatorTF_states_i) * rtb_DenCoefOut * - rtu_ConfigurationParameters->DirLoopPID.N; + rtb_NProdOut_f = (localDW->FilterDifferentiatorTF_tmp_m - + localDW->FilterDifferentiatorTF_states_i) * rtb_SumI1_e * + rtu_ConfigurationParameters->pids.positionPID.N; // Sum: '/SumI1' incorporates: // Sum: '/Sum Fdbk' // Sum: '/SumI3' // UnitDelay: '/Unit Delay' - rtb_FilterDifferentiatorTF_n = (localDW->UnitDelay_DSTATE - ((rtb_PProdOut_o + - localDW->Integrator_DSTATE_i) + rtb_DenCoefOut)) + rtb_Product; + rtb_SumI1_e = (localDW->UnitDelay_DSTATE - ((rtb_Sum2 + + localDW->Integrator_DSTATE_i) + rtb_NProdOut_f)) + rtb_DenCoefOut; // DiscreteIntegrator: '/Integrator' if (rtb_FixPtRelationalOperator && (localDW->Integrator_PrevResetState_n <= 0)) @@ -280,8 +281,7 @@ void control_outer(const Flags *rtu_Flags, const ConfigurationParameters } // DiscreteIntegrator: '/Integrator' - rtb_DProdOut_f = 0.0005F * rtb_FilterDifferentiatorTF_n + - localDW->Integrator_DSTATE_i; + Integrator_p = 0.0005F * rtb_SumI1_e + localDW->Integrator_DSTATE_i; // Switch: '/Switch3' incorporates: // Constant: '/Constant1' @@ -294,14 +294,14 @@ void control_outer(const Flags *rtu_Flags, const ConfigurationParameters if (rtb_Compare || (rtu_Flags->control_mode == ControlModes_PositionDirect) || (rtu_Flags->control_mode == ControlModes_Velocity)) { // Switch: '/Switch5' incorporates: + // Product: '/PProd Out' // Sum: '/Sum' // Sum: '/Sum' if (rtb_Compare) { - rtb_DenCoefOut = (rtb_PProdOut + rtb_FilterDifferentiatorTF) + - rtb_Switch2_f; + rtb_DenCoefOut = (rtb_Sum2 + Integrator) + rtb_NProdOut; } else { - rtb_DenCoefOut += rtb_PProdOut_o + rtb_DProdOut_f; + rtb_DenCoefOut = (rtb_Sum2 + Integrator_p) + rtb_NProdOut_f; } // End of Switch: '/Switch5' @@ -312,7 +312,7 @@ void control_outer(const Flags *rtu_Flags, const ConfigurationParameters // Sum: '/Sum2' incorporates: // Switch: '/Switch3' - rtb_Switch2_f = rtb_DenCoefOut + rtu_Targets->jointvelocities.velocity; + rtb_Sum2 = rtb_DenCoefOut + *rtu_Targets_jointvelocities; // Switch: '/Switch2' incorporates: // Gain: '/Gain' @@ -320,35 +320,35 @@ void control_outer(const Flags *rtu_Flags, const ConfigurationParameters // RelationalOperator: '/UpperRelop' // Switch: '/Switch' - if (rtb_Switch2_f > rtb_Abs) { - rtb_Switch2_f = rtb_Abs; - } else if (rtb_Switch2_f < -rtb_Abs) { + if (rtb_Sum2 > rtb_Abs) { + rtb_Sum2 = rtb_Abs; + } else if (rtb_Sum2 < -rtb_Abs) { // Switch: '/Switch' incorporates: // Gain: '/Gain' - rtb_Switch2_f = -rtb_Abs; + rtb_Sum2 = -rtb_Abs; } // End of Switch: '/Switch2' // Sum: '/Sum1' - rtb_Product = rtb_Switch2_f - rtu_Estimates->jointvelocities.velocity; + rtb_Abs = rtb_Sum2 - *rtu_Estimates_jointvelocities; // Product: '/PProd Out' - rtb_Abs = rtb_Product * rtu_ConfigurationParameters->VelLoopPID.P; + rtb_NProdOut = rtb_Abs * rtu_ConfigurationParameters->pids.velocityPID.P; // Product: '/IProd Out' - rtb_Gain1_h = rtb_Product * rtu_ConfigurationParameters->VelLoopPID.I; + rtb_NProdOut_f = rtb_Abs * rtu_ConfigurationParameters->pids.velocityPID.I; // Product: '/DProd Out' - rtb_PProdOut = rtb_Product * rtu_ConfigurationParameters->VelLoopPID.D; + rtb_DProdOut_b = rtb_Abs * rtu_ConfigurationParameters->pids.velocityPID.D; // SampleTimeMath: '/Tsamp' // // About '/Tsamp': // y = u * K where K = ( w * Ts ) - rtb_Product = rtu_ConfigurationParameters->VelLoopPID.N * 0.0005F; + rtb_Abs = rtu_ConfigurationParameters->pids.velocityPID.N * 0.0005F; // Math: '/Reciprocal' incorporates: // Constant: '/Filter Den Constant' @@ -357,7 +357,7 @@ void control_outer(const Flags *rtu_Flags, const ConfigurationParameters // About '/Reciprocal': // Operator: reciprocal - rtb_DenCoefOut = 1.0F / (rtb_Product + 1.0F); + rtb_DenCoefOut = 1.0F / (rtb_Abs + 1.0F); // DiscreteTransferFcn: '/Filter Differentiator TF' incorporates: // Constant: '/Filter Den Constant' @@ -371,24 +371,24 @@ void control_outer(const Flags *rtu_Flags, const ConfigurationParameters } localZCE->FilterDifferentiatorTF_Reset_ZCE_e = rtb_FixPtRelationalOperator; - localDW->FilterDifferentiatorTF_tmp_p = rtb_PProdOut - (rtb_Product - 1.0F) * + localDW->FilterDifferentiatorTF_tmp_p = rtb_DProdOut_b - (rtb_Abs - 1.0F) * rtb_DenCoefOut * localDW->FilterDifferentiatorTF_states_c; // Product: '/NProd Out' incorporates: // DiscreteTransferFcn: '/Filter Differentiator TF' // Product: '/DenCoefOut' - rtb_DenCoefOut = (localDW->FilterDifferentiatorTF_tmp_p - + rtb_DProdOut_b = (localDW->FilterDifferentiatorTF_tmp_p - localDW->FilterDifferentiatorTF_states_c) * rtb_DenCoefOut * - rtu_ConfigurationParameters->VelLoopPID.N; + rtu_ConfigurationParameters->pids.velocityPID.N; // Sum: '/SumI1' incorporates: // Sum: '/Sum Fdbk' // Sum: '/SumI3' // UnitDelay: '/Unit Delay1' - rtb_PProdOut = (localDW->UnitDelay1_DSTATE - ((rtb_Abs + - localDW->Integrator_DSTATE_b) + rtb_DenCoefOut)) + rtb_Gain1_h; + rtb_DenCoefOut = (localDW->UnitDelay1_DSTATE - ((rtb_NProdOut + + localDW->Integrator_DSTATE_b) + rtb_DProdOut_b)) + rtb_NProdOut_f; // DiscreteIntegrator: '/Integrator' if (rtb_FixPtRelationalOperator && (localDW->Integrator_PrevResetState_c <= 0)) @@ -397,46 +397,51 @@ void control_outer(const Flags *rtu_Flags, const ConfigurationParameters } // DiscreteIntegrator: '/Integrator' - rtb_Product = 0.0005F * rtb_PProdOut + localDW->Integrator_DSTATE_b; - - // Switch: '/Switch1' incorporates: - // Sum: '/Sum' + rtb_Abs = 0.0005F * rtb_DenCoefOut + localDW->Integrator_DSTATE_b; + // Switch: '/Switch1' if (rtb_Compare_m) { - rtb_Abs = (rtb_Abs + rtb_Product) + rtb_DenCoefOut; + // Switch: '/Switch1' incorporates: + // Sum: '/Sum' + + localDW->UnitDelay1_DSTATE = (rtb_NProdOut + rtb_Abs) + rtb_DProdOut_b; } else { - rtb_Abs = rtu_Targets->motorcurrent.current; + // Switch: '/Switch1' + localDW->UnitDelay1_DSTATE = *rtu_Targets_motorcurrent; } // End of Switch: '/Switch1' // Switch: '/Switch2' incorporates: + // Gain: '/Gain1' // RelationalOperator: '/LowerRelop1' - - if (!(rtb_Abs > rtb_Switch2)) { - // Gain: '/Gain1' - rtb_Switch2 = -rtb_Switch2; - - // Switch: '/Switch' incorporates: - // RelationalOperator: '/UpperRelop' - - if (!(rtb_Abs < rtb_Switch2)) { - rtb_Switch2 = rtb_Abs; - } - - // End of Switch: '/Switch' + // RelationalOperator: '/UpperRelop' + // Switch: '/Switch' + + if (localDW->UnitDelay1_DSTATE > rtb_Switch2) { + // Switch: '/Switch1' + localDW->UnitDelay1_DSTATE = rtb_Switch2; + } else if (localDW->UnitDelay1_DSTATE < -rtb_Switch2) { + // Switch: '/Switch1' incorporates: + // Gain: '/Gain1' + // Switch: '/Switch' + + localDW->UnitDelay1_DSTATE = -rtb_Switch2; } // End of Switch: '/Switch2' - // BusCreator: '/Bus Creator2' - rty_OuterOutputs->motorcurrent.current = rtb_Switch2; + // RelationalOperator: '/Compare' incorporates: + // Constant: '/Constant' + + rtb_Compare = (*rtu_Estimates_Iq_filtered < 0.0F); - // Sum: '/Sum' incorporates: - // Abs: '/Abs1' + // Abs: '/Abs1' + rtb_NProdOut_f = std::abs(*rtu_Estimates_Iq_filtered); - rtb_Abs = rtu_ConfigurationParameters->thresholds.motorPeakCurrents - std::abs - (rtu_Estimates->Iq_filtered.current); + // Sum: '/Sum' + rtb_Switch2 = rtu_ConfigurationParameters->thresholds.motorPeakCurrents - + rtb_NProdOut_f; // CombinatorialLogic: '/Logic' incorporates: // Constant: '/Constant' @@ -446,39 +451,40 @@ void control_outer(const Flags *rtu_Flags, const ConfigurationParameters // RelationalOperator: '/Relational Operator' // RelationalOperator: '/Compare' - rowIdx = static_cast(((((rtb_Abs > 0.1F * + rowIdx = static_cast(((((rtb_Switch2 > 0.1F * rtu_ConfigurationParameters->thresholds.motorPeakCurrents) || - rtb_FixPtRelationalOperator) + (static_cast(rtb_Abs <= 0.0F) << 1)) - << 1) + localDW->Memory_PreviousInput); - rtb_Compare = rtCP_Logic_table[static_cast(rowIdx) + 8U]; + rtb_FixPtRelationalOperator) + (static_cast(rtb_Switch2 <= 0.0F) << + 1)) << 1) + localDW->Memory_PreviousInput); + rtb_Logic_idx_1 = rtCP_Logic_table[static_cast(rowIdx) + 8U]; // DiscreteIntegrator: '/Discrete-Time Integrator' if (localDW->DiscreteTimeIntegrator_SYSTEM_ENABLE != 0) { // DiscreteIntegrator: '/Discrete-Time Integrator' localB->DiscreteTimeIntegrator = localDW->DiscreteTimeIntegrator_DSTATE; - } else if (rtb_Compare || (localDW->DiscreteTimeIntegrator_PrevResetState != 0)) - { + } else if (rtb_Logic_idx_1 || (localDW->DiscreteTimeIntegrator_PrevResetState + != 0)) { // DiscreteIntegrator: '/Discrete-Time Integrator' localB->DiscreteTimeIntegrator = 0.0F; } else { // DiscreteIntegrator: '/Discrete-Time Integrator' - localB->DiscreteTimeIntegrator = 0.0005F * rtb_Abs + + localB->DiscreteTimeIntegrator = 0.0005F * rtb_Switch2 + localDW->DiscreteTimeIntegrator_DSTATE; } // End of DiscreteIntegrator: '/Discrete-Time Integrator' - // BusCreator: '/Bus Creator1' + // BusCreator: '/Bus Creator1' incorporates: + // UnitDelay: '/Unit Delay1' + rty_OuterOutputs->vel_en = rtb_Compare_m; rty_OuterOutputs->pid_reset = rtb_FixPtRelationalOperator; + rty_OuterOutputs->motorcurrent = localDW->UnitDelay1_DSTATE; // Switch: '/Switch1' incorporates: // Constant: '/Constant1' // Constant: '/Constant2' - // Constant: '/Constant' - // RelationalOperator: '/Compare' - if (rtu_Estimates->Iq_filtered.current < 0.0F) { + if (rtb_Compare) { tmp = -1; } else { tmp = 1; @@ -489,7 +495,13 @@ void control_outer(const Flags *rtu_Flags, const ConfigurationParameters // Switch: '/Switch1' rty_OuterOutputs->current_limiter = static_cast(tmp) * - localB->DiscreteTimeIntegrator * rtu_ConfigurationParameters->CurLoopPID.I; + localB->DiscreteTimeIntegrator * + rtu_ConfigurationParameters->pids.currentPID.I; + + // Sum: '/Sum3' incorporates: + // UnitDelay: '/Unit Delay' + + localDW->UnitDelay_DSTATE = rtb_Sum2 - *rtu_Targets_jointvelocities; // Update for UnitDelay: '/Delay Input1' // @@ -502,15 +514,8 @@ void control_outer(const Flags *rtu_Flags, const ConfigurationParameters // Update for DiscreteTransferFcn: '/Filter Differentiator TF' localDW->FilterDifferentiatorTF_states = localDW->FilterDifferentiatorTF_tmp; - // Update for UnitDelay: '/Unit Delay' incorporates: - // Sum: '/Sum3' - - localDW->UnitDelay_DSTATE = rtb_Switch2_f - - rtu_Targets->jointvelocities.velocity; - // Update for DiscreteIntegrator: '/Integrator' - localDW->Integrator_DSTATE = 0.0005F * rtb_DProdOut + - rtb_FilterDifferentiatorTF; + localDW->Integrator_DSTATE = 0.0005F * rtb_SumI1 + Integrator; localDW->Integrator_PrevResetState = static_cast (rtb_FixPtRelationalOperator); @@ -521,8 +526,7 @@ void control_outer(const Flags *rtu_Flags, const ConfigurationParameters // Update for DiscreteIntegrator: '/Integrator' incorporates: // DiscreteIntegrator: '/Integrator' - localDW->Integrator_DSTATE_i = 0.0005F * rtb_FilterDifferentiatorTF_n + - rtb_DProdOut_f; + localDW->Integrator_DSTATE_i = 0.0005F * rtb_SumI1_e + Integrator_p; localDW->Integrator_PrevResetState_n = static_cast (rtb_FixPtRelationalOperator); @@ -530,13 +534,10 @@ void control_outer(const Flags *rtu_Flags, const ConfigurationParameters localDW->FilterDifferentiatorTF_states_c = localDW->FilterDifferentiatorTF_tmp_p; - // Update for UnitDelay: '/Unit Delay1' - localDW->UnitDelay1_DSTATE = rtb_Switch2; - // Update for DiscreteIntegrator: '/Integrator' incorporates: // DiscreteIntegrator: '/Integrator' - localDW->Integrator_DSTATE_b = 0.0005F * rtb_PProdOut + rtb_Product; + localDW->Integrator_DSTATE_b = 0.0005F * rtb_DenCoefOut + rtb_Abs; localDW->Integrator_PrevResetState_c = static_cast (rtb_FixPtRelationalOperator); @@ -547,29 +548,15 @@ void control_outer(const Flags *rtu_Flags, const ConfigurationParameters // Update for DiscreteIntegrator: '/Discrete-Time Integrator' localDW->DiscreteTimeIntegrator_SYSTEM_ENABLE = 0U; - localDW->DiscreteTimeIntegrator_DSTATE = 0.0005F * rtb_Abs + + localDW->DiscreteTimeIntegrator_DSTATE = 0.0005F * rtb_Switch2 + localB->DiscreteTimeIntegrator; localDW->DiscreteTimeIntegrator_PrevResetState = static_cast - (rtb_Compare); + (rtb_Logic_idx_1); } // Model initialize function -void control_outer_initialize(const char_T **rt_errorStatus, - RT_MODEL_control_outer_T *const control_outer_M, B_control_outer_c_T *localB, - DW_control_outer_f_T *localDW, ZCE_control_outer_T *localZCE) +void control_outer_initialize(ZCE_control_outer_T *localZCE) { - // Registration code - - // initialize error status - rtmSetErrorStatusPointer(control_outer_M, rt_errorStatus); - - // block I/O - (void) std::memset((static_cast(localB)), 0, - sizeof(B_control_outer_c_T)); - - // states (dwork) - (void) std::memset(static_cast(localDW), 0, - sizeof(DW_control_outer_f_T)); localZCE->FilterDifferentiatorTF_Reset_ZCE = POS_ZCSIG; localZCE->FilterDifferentiatorTF_Reset_ZCE_m = POS_ZCSIG; localZCE->FilterDifferentiatorTF_Reset_ZCE_e = POS_ZCSIG; diff --git a/emBODY/eBcode/arch-arm/mbd/amcbldc/control-outer/control_outer.h b/emBODY/eBcode/arch-arm/mbd/amcbldc/control-outer/control_outer.h index 0b9ced330..469eb7211 100644 --- a/emBODY/eBcode/arch-arm/mbd/amcbldc/control-outer/control_outer.h +++ b/emBODY/eBcode/arch-arm/mbd/amcbldc/control-outer/control_outer.h @@ -7,9 +7,9 @@ // // Code generated for Simulink model 'control_outer'. // -// Model version : 6.3 +// Model version : 6.29 // Simulink Coder version : 23.2 (R2023b) 01-Aug-2023 -// C/C++ source code generated on : Tue Feb 13 11:54:43 2024 +// C/C++ source code generated on : Wed Feb 28 11:28:34 2024 // // Target selection: ert.tlc // Embedded hardware selection: ARM Compatible->ARM Cortex-M @@ -20,7 +20,6 @@ #define RTW_HEADER_control_outer_h_ #include "rtwtypes.h" #include "control_outer_types.h" -#include #include "zero_crossing_types.h" // Block signals for model 'control_outer' @@ -58,30 +57,25 @@ struct ZCE_control_outer_T { ZCSigState FilterDifferentiatorTF_Reset_ZCE_e;// '/Filter Differentiator TF' }; -// Real-time Model Data Structure -struct tag_RTM_control_outer_T { - const char_T **errorStatus; -}; - struct MdlrefDW_control_outer_T { B_control_outer_c_T rtb; DW_control_outer_f_T rtdw; - RT_MODEL_control_outer_T rtm; ZCE_control_outer_T rtzce; }; // Model reference registration function -extern void control_outer_initialize(const char_T **rt_errorStatus, - RT_MODEL_control_outer_T *const control_outer_M, B_control_outer_c_T *localB, - DW_control_outer_f_T *localDW, ZCE_control_outer_T *localZCE); +extern void control_outer_initialize(ZCE_control_outer_T *localZCE); extern void control_outer_Init(DW_control_outer_f_T *localDW); extern void control_outer_Enable(DW_control_outer_f_T *localDW); extern void control_outer_Disable(B_control_outer_c_T *localB, DW_control_outer_f_T *localDW); -extern void control_outer(const Flags *rtu_Flags, const ConfigurationParameters * - rtu_ConfigurationParameters, const Targets *rtu_Targets, const SensorsData - *rtu_Sensors, const EstimatedData *rtu_Estimates, ControlOuterOutputs - *rty_OuterOutputs, B_control_outer_c_T *localB, DW_control_outer_f_T *localDW, +extern void control_outer(const Flags *rtu_Flags, const ActuatorConfiguration + *rtu_ConfigurationParameters, const real32_T *rtu_Estimates_jointvelocities, + const real32_T *rtu_Estimates_Iq_filtered, const real32_T + *rtu_Sensors_jointpositions, const real32_T *rtu_Targets_jointpositions, const + real32_T *rtu_Targets_jointvelocities, const real32_T + *rtu_Targets_motorcurrent, ControlOuterOutputs *rty_OuterOutputs, + B_control_outer_c_T *localB, DW_control_outer_f_T *localDW, ZCE_control_outer_T *localZCE); //- diff --git a/emBODY/eBcode/arch-arm/mbd/amcbldc/control-outer/control_outer_private.h b/emBODY/eBcode/arch-arm/mbd/amcbldc/control-outer/control_outer_private.h index 924188333..373a716f8 100644 --- a/emBODY/eBcode/arch-arm/mbd/amcbldc/control-outer/control_outer_private.h +++ b/emBODY/eBcode/arch-arm/mbd/amcbldc/control-outer/control_outer_private.h @@ -7,9 +7,9 @@ // // Code generated for Simulink model 'control_outer'. // -// Model version : 6.3 +// Model version : 6.29 // Simulink Coder version : 23.2 (R2023b) 01-Aug-2023 -// C/C++ source code generated on : Tue Feb 13 11:54:43 2024 +// C/C++ source code generated on : Wed Feb 28 11:28:34 2024 // // Target selection: ert.tlc // Embedded hardware selection: ARM Compatible->ARM Cortex-M @@ -22,23 +22,6 @@ #include "zero_crossing_types.h" #include "control_outer_types.h" -// Macros for accessing real-time model data structure -#ifndef rtmGetErrorStatus -#define rtmGetErrorStatus(rtm) (*((rtm)->errorStatus)) -#endif - -#ifndef rtmSetErrorStatus -#define rtmSetErrorStatus(rtm, val) (*((rtm)->errorStatus) = (val)) -#endif - -#ifndef rtmGetErrorStatusPointer -#define rtmGetErrorStatusPointer(rtm) (rtm)->errorStatus -#endif - -#ifndef rtmSetErrorStatusPointer -#define rtmSetErrorStatusPointer(rtm, val) ((rtm)->errorStatus = (val)) -#endif - extern const boolean_T rtCP_pooled_kUC6nmgO8rex[16]; #define rtCP_Logic_table rtCP_pooled_kUC6nmgO8rex // Computed Parameter: rtCP_Logic_table diff --git a/emBODY/eBcode/arch-arm/mbd/amcbldc/control-outer/control_outer_types.h b/emBODY/eBcode/arch-arm/mbd/amcbldc/control-outer/control_outer_types.h index 539dbee82..43fb2ecb7 100644 --- a/emBODY/eBcode/arch-arm/mbd/amcbldc/control-outer/control_outer_types.h +++ b/emBODY/eBcode/arch-arm/mbd/amcbldc/control-outer/control_outer_types.h @@ -7,9 +7,9 @@ // // Code generated for Simulink model 'control_outer'. // -// Model version : 6.3 +// Model version : 6.29 // Simulink Coder version : 23.2 (R2023b) 01-Aug-2023 -// C/C++ source code generated on : Tue Feb 13 11:54:43 2024 +// C/C++ source code generated on : Wed Feb 28 11:28:34 2024 // // Target selection: ert.tlc // Embedded hardware selection: ARM Compatible->ARM Cortex-M @@ -30,7 +30,6 @@ typedef enum { ControlModes_Current, ControlModes_Velocity, ControlModes_Voltage, - ControlModes_Torque, ControlModes_HwFaultCM } ControlModes; @@ -41,117 +40,23 @@ typedef enum { struct Flags { + boolean_T enable_sending_msg_status; + // control mode ControlModes control_mode; - boolean_T enable_sending_msg_status; - boolean_T fault_button; boolean_T enable_thermal_protection; }; #endif -#ifndef DEFINED_TYPEDEF_FOR_MotorConfig_ -#define DEFINED_TYPEDEF_FOR_MotorConfig_ - -struct MotorConfig -{ - // Angular offset in degrees between the stator windings and the hall sensors. - real32_T hall_sens_offset; - boolean_T has_hall_sens; - boolean_T has_quadrature_encoder; - boolean_T has_speed_quadrature_encoder; - boolean_T has_torque_sens; - boolean_T use_index; - boolean_T enable_verbosity; - int16_T rotor_encoder_resolution; - int16_T rotor_index_offset; - uint8_T encoder_tolerance; - uint8_T pole_pairs; - real32_T Kbemf; - real32_T Rphase; - real32_T Imin; - real32_T Imax; - real32_T Vmax; - real32_T resistance; - real32_T inductance; - real32_T thermal_resistance; - real32_T thermal_time_constant; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_EstimationVelocityModes_ -#define DEFINED_TYPEDEF_FOR_EstimationVelocityModes_ - -typedef enum { - EstimationVelocityModes_Disabled = 0,// Default value - EstimationVelocityModes_MovingAverage, - EstimationVelocityModes_LeastSquares -} EstimationVelocityModes; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_EstimationConfig_ -#define DEFINED_TYPEDEF_FOR_EstimationConfig_ - -struct EstimationConfig -{ - EstimationVelocityModes velocity_mode; - - // Forgetting factor in [0, 1] for exponential weighting-based estimation of RMS current value - real32_T current_rms_lambda; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_PIDConfig_ -#define DEFINED_TYPEDEF_FOR_PIDConfig_ - -struct PIDConfig -{ - real32_T OutMax; - real32_T OutMin; - real32_T P; - real32_T I; - real32_T D; - real32_T N; - real32_T I0; - real32_T D0; - uint8_T shift_factor; -}; - -#endif - #ifndef DEFINED_TYPEDEF_FOR_Thresholds_ #define DEFINED_TYPEDEF_FOR_Thresholds_ struct Thresholds { - // It shall be greater than hardwareJntPosMin - real32_T jntPosMin; - - // It shall be smaller than hardwareJntPosMax - real32_T jntPosMax; - - // Imposed by hardware constraint - real32_T hardwareJntPosMin; - - // Imposed by hardware constraint - real32_T hardwareJntPosMax; - - // If robotMin == rotorMax == 0, there's no check - real32_T rotorPosMin; - - // If robotMin == rotorMax == 0, there's no check - real32_T rotorPosMax; - // Can be only non-negative real32_T jntVelMax; - // Timeout on reception of velocity setpoint - // Can be only non-negative - uint32_T velocityTimeout; - // Current that can be kept for an indefinite period of time w/o damaging the motor // Expressed in [A] as all the internal computations are done this way // Can be only non-negative @@ -178,148 +83,75 @@ struct Thresholds #endif -#ifndef DEFINED_TYPEDEF_FOR_ConfigurationParameters_ -#define DEFINED_TYPEDEF_FOR_ConfigurationParameters_ - -struct ConfigurationParameters -{ - MotorConfig motorconfig; - EstimationConfig estimationconfig; - PIDConfig CurLoopPID; - PIDConfig PosLoopPID; - PIDConfig VelLoopPID; - PIDConfig DirLoopPID; - Thresholds thresholds; - real32_T environment_temperature; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_JointPositions_ -#define DEFINED_TYPEDEF_FOR_JointPositions_ - -struct JointPositions -{ - // joint positions - real32_T position; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_JointVelocities_ -#define DEFINED_TYPEDEF_FOR_JointVelocities_ - -struct JointVelocities -{ - // joint velocities - real32_T velocity; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_MotorCurrent_ -#define DEFINED_TYPEDEF_FOR_MotorCurrent_ - -struct MotorCurrent -{ - // motor current - real32_T current; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_MotorVoltage_ -#define DEFINED_TYPEDEF_FOR_MotorVoltage_ - -struct MotorVoltage -{ - // motor voltage - real32_T voltage; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_Targets_ -#define DEFINED_TYPEDEF_FOR_Targets_ - -struct Targets -{ - JointPositions jointpositions; - JointVelocities jointvelocities; - MotorCurrent motorcurrent; - MotorVoltage motorvoltage; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_MotorSensors_ -#define DEFINED_TYPEDEF_FOR_MotorSensors_ - -struct MotorSensors -{ - real32_T Iabc[3]; - - // electrical angle = angle * pole_pairs - real32_T angle; - real32_T temperature; - real32_T voltage; - real32_T current; - uint8_T hallABC; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_SupplyVoltage_ -#define DEFINED_TYPEDEF_FOR_SupplyVoltage_ +#ifndef DEFINED_TYPEDEF_FOR_PID_ +#define DEFINED_TYPEDEF_FOR_PID_ -struct SupplyVoltage +struct PID { - real32_T voltage; + ControlModes type; + real32_T OutMax; + real32_T OutMin; + real32_T P; + real32_T I; + real32_T D; + real32_T N; + real32_T I0; + real32_T D0; + uint8_T shift_factor; }; #endif -#ifndef DEFINED_TYPEDEF_FOR_SensorsData_ -#define DEFINED_TYPEDEF_FOR_SensorsData_ +#ifndef DEFINED_TYPEDEF_FOR_PIDsConfiguration_ +#define DEFINED_TYPEDEF_FOR_PIDsConfiguration_ -struct SensorsData +struct PIDsConfiguration { - // position encoders - JointPositions jointpositions; - - // motor probes - MotorSensors motorsensors; - - // supply probes - SupplyVoltage supplyvoltagesensors; + PID currentPID; + PID velocityPID; + PID positionPID; }; #endif -#ifndef DEFINED_TYPEDEF_FOR_MotorTemperature_ -#define DEFINED_TYPEDEF_FOR_MotorTemperature_ +#ifndef DEFINED_TYPEDEF_FOR_MotorConfiguration_ +#define DEFINED_TYPEDEF_FOR_MotorConfiguration_ -struct MotorTemperature +struct MotorConfiguration { - // motor temperature - real32_T temperature; + boolean_T has_hall_sens; + boolean_T has_quadrature_encoder; + boolean_T has_speed_quadrature_encoder; + boolean_T has_torque_sens; + boolean_T use_index; + boolean_T enable_verbosity; + int16_T hall_sensors_offset; + int16_T rotor_encoder_resolution; + int16_T rotor_index_offset; + uint8_T encoder_tolerance; + uint8_T pole_pairs; + real32_T Kbemf; + real32_T Rphase; + real32_T Imin; + real32_T Imax; + real32_T Vcc; + real32_T Vmax; + real32_T resistance; + real32_T inductance; + real32_T thermal_resistance; + real32_T thermal_time_constant; }; #endif -#ifndef DEFINED_TYPEDEF_FOR_EstimatedData_ -#define DEFINED_TYPEDEF_FOR_EstimatedData_ +#ifndef DEFINED_TYPEDEF_FOR_ActuatorConfiguration_ +#define DEFINED_TYPEDEF_FOR_ActuatorConfiguration_ -struct EstimatedData +struct ActuatorConfiguration { - // velocity - JointVelocities jointvelocities; - - // filtered motor current - MotorCurrent Iq_filtered; - - // motor temperature - MotorTemperature motor_temperature; + Thresholds thresholds; + PIDsConfiguration pids; + MotorConfiguration motor; }; #endif @@ -333,15 +165,11 @@ struct ControlOuterOutputs boolean_T cur_en; boolean_T out_en; boolean_T pid_reset; - MotorCurrent motorcurrent; + real32_T motorcurrent; real32_T current_limiter; }; #endif - -// Forward declaration for rtModel -typedef struct tag_RTM_control_outer_T RT_MODEL_control_outer_T; - #endif // RTW_HEADER_control_outer_types_h_ // diff --git a/emBODY/eBcode/arch-arm/mbd/amcbldc/estimator/estimation_velocity.cpp b/emBODY/eBcode/arch-arm/mbd/amcbldc/estimator/estimation_velocity.cpp index 836c01b4e..d179da941 100644 --- a/emBODY/eBcode/arch-arm/mbd/amcbldc/estimator/estimation_velocity.cpp +++ b/emBODY/eBcode/arch-arm/mbd/amcbldc/estimator/estimation_velocity.cpp @@ -7,9 +7,9 @@ // // Code generated for Simulink model 'estimation_velocity'. // -// Model version : 6.4 +// Model version : 6.19 // Simulink Coder version : 23.2 (R2023b) 01-Aug-2023 -// C/C++ source code generated on : Tue Feb 13 11:54:57 2024 +// C/C++ source code generated on : Wed Feb 28 11:28:54 2024 // // Target selection: ert.tlc // Embedded hardware selection: ARM Compatible->ARM Cortex-M @@ -21,17 +21,10 @@ #include "rtwtypes.h" #include #include -#include "rt_hypotf_snf.h" +#include "rt_hypotf.h" #include "mw_cmsis.h" #include "estimation_velocity_private.h" -extern "C" -{ - -#include "rt_nonfinite.h" - -} - // Forward declaration for local functions static real32_T estimation_velocity_xnrm2(int32_T n, const real32_T x[32], int32_T ix0); @@ -74,22 +67,22 @@ static real32_T estimation_velocity_xnrm2(int32_T n, const real32_T x[32], static void estimation_velocity_xgeqp3(const real32_T A[32], real32_T b_A[32], real32_T tau[2], int32_T jpvt[2]) { - int32_T b_k; + int32_T b_A_tmp; + int32_T b_j; int32_T exitg1; - int32_T ia; + int32_T i; + int32_T ii; + int32_T ijA; + int32_T itemp; int32_T ix; - int32_T iy; - int32_T kend; - int32_T knt; - int32_T lastv; + int32_T jA; int32_T pvt; - int32_T scale_tmp; real32_T vn1[2]; real32_T vn2[2]; real32_T work[2]; real32_T absxk; + real32_T beta1; real32_T scale; - real32_T smax; real32_T t; boolean_T exitg2; jpvt[0] = 1; @@ -103,142 +96,141 @@ static void estimation_velocity_xgeqp3(const real32_T A[32], real32_T b_A[32], std::memcpy(&b_A[0], &A[0], sizeof(real32_T) << 5U); work[0] = 0.0F; work[1] = 0.0F; - for (b_k = 0; b_k < 2; b_k++) { - ix = b_k << 4; - smax = 0.0F; + for (b_j = 0; b_j < 2; b_j++) { + i = b_j << 4; + beta1 = 0.0F; scale = 1.29246971E-26F; // Start for MATLABSystem: '/QR Solver' - for (lastv = ix + 1; lastv <= ix + 16; lastv++) { - absxk = std::abs(A[lastv - 1]); + for (itemp = i + 1; itemp <= i + 16; itemp++) { + absxk = std::abs(A[itemp - 1]); if (absxk > scale) { t = scale / absxk; - smax = smax * t * t + 1.0F; + beta1 = beta1 * t * t + 1.0F; scale = absxk; } else { t = absxk / scale; - smax += t * t; + beta1 += t * t; } } - mw_arm_sqrt_f32(smax, &smax); - smax *= scale; - vn1[b_k] = smax; - vn2[b_k] = smax; + mw_arm_sqrt_f32(beta1, &beta1); + scale *= beta1; + vn1[b_j] = scale; + vn2[b_j] = scale; } - for (b_k = 0; b_k < 2; b_k++) { + for (b_j = 0; b_j < 2; b_j++) { // Start for MATLABSystem: '/QR Solver' - knt = b_k << 4; - kend = knt + b_k; - pvt = 1; + i = b_j << 4; + ii = i + b_j; + itemp = 1; // Start for MATLABSystem: '/QR Solver' - if ((2 - b_k > 1) && (vn1[1] > vn1[b_k])) { - pvt = 2; + if ((2 - b_j > 1) && (vn1[1] > vn1[b_j])) { + itemp = 2; } - pvt = (b_k + pvt) - 1; - if (pvt != b_k) { + pvt = (b_j + itemp) - 1; + if (pvt != b_j) { // Start for MATLABSystem: '/QR Solver' ix = pvt << 4; - for (lastv = 0; lastv < 16; lastv++) { - scale_tmp = ix + lastv; - scale = b_A[scale_tmp]; - iy = knt + lastv; - b_A[scale_tmp] = b_A[iy]; - b_A[iy] = scale; + for (itemp = 0; itemp < 16; itemp++) { + jA = ix + itemp; + scale = b_A[jA]; + b_A_tmp = i + itemp; + b_A[jA] = b_A[b_A_tmp]; + b_A[b_A_tmp] = scale; } - ix = jpvt[pvt]; - jpvt[pvt] = jpvt[b_k]; - jpvt[b_k] = ix; - vn1[pvt] = vn1[b_k]; - vn2[pvt] = vn2[b_k]; + itemp = jpvt[pvt]; + jpvt[pvt] = jpvt[b_j]; + jpvt[b_j] = itemp; + vn1[pvt] = vn1[b_j]; + vn2[pvt] = vn2[b_j]; } // Start for MATLABSystem: '/QR Solver' - ix = kend + 2; - smax = b_A[kend]; - tau[b_k] = 0.0F; + i = ii + 2; + absxk = b_A[ii]; + tau[b_j] = 0.0F; // Start for MATLABSystem: '/QR Solver' - scale = estimation_velocity_xnrm2(15 - b_k, b_A, kend + 2); - if (scale != 0.0F) { + beta1 = estimation_velocity_xnrm2(15 - b_j, b_A, ii + 2); + if (beta1 != 0.0F) { // Start for MATLABSystem: '/QR Solver' - absxk = b_A[kend]; - scale = rt_hypotf_snf(absxk, scale); - if (absxk >= 0.0F) { - scale = -scale; + scale = b_A[ii]; + beta1 = rt_hypotf(scale, beta1); + if (scale >= 0.0F) { + beta1 = -beta1; } - if (std::abs(scale) < 9.86076132E-32F) { - knt = -1; + if (std::abs(beta1) < 9.86076132E-32F) { + pvt = -1; do { - knt++; - pvt = kend - b_k; - for (lastv = ix; lastv <= pvt + 16; lastv++) { - b_A[lastv - 1] *= 1.01412048E+31F; + pvt++; + ix = ii - b_j; + for (itemp = i; itemp <= ix + 16; itemp++) { + b_A[itemp - 1] *= 1.01412048E+31F; } - scale *= 1.01412048E+31F; - smax *= 1.01412048E+31F; - } while ((std::abs(scale) < 9.86076132E-32F) && (knt + 1 < 20)); + beta1 *= 1.01412048E+31F; + absxk *= 1.01412048E+31F; + } while ((std::abs(beta1) < 9.86076132E-32F) && (pvt + 1 < 20)); - scale = rt_hypotf_snf(smax, estimation_velocity_xnrm2(15 - b_k, b_A, - kend + 2)); - if (smax >= 0.0F) { - scale = -scale; + beta1 = rt_hypotf(absxk, estimation_velocity_xnrm2(15 - b_j, b_A, ii + 2)); + if (absxk >= 0.0F) { + beta1 = -beta1; } - tau[b_k] = (scale - smax) / scale; - smax = 1.0F / (smax - scale); - for (lastv = ix; lastv <= pvt + 16; lastv++) { - b_A[lastv - 1] *= smax; + tau[b_j] = (beta1 - absxk) / beta1; + absxk = 1.0F / (absxk - beta1); + for (itemp = i; itemp <= ix + 16; itemp++) { + b_A[itemp - 1] *= absxk; } - for (lastv = 0; lastv <= knt; lastv++) { - scale *= 9.86076132E-32F; + for (itemp = 0; itemp <= pvt; itemp++) { + beta1 *= 9.86076132E-32F; } - smax = scale; + absxk = beta1; } else { - tau[b_k] = (scale - absxk) / scale; - smax = 1.0F / (absxk - scale); - pvt = kend - b_k; - for (lastv = ix; lastv <= pvt + 16; lastv++) { - b_A[lastv - 1] *= smax; + tau[b_j] = (beta1 - scale) / beta1; + absxk = 1.0F / (scale - beta1); + pvt = ii - b_j; + for (itemp = i; itemp <= pvt + 16; itemp++) { + b_A[itemp - 1] *= absxk; } - smax = scale; + absxk = beta1; } } - b_A[kend] = smax; + b_A[ii] = absxk; // Start for MATLABSystem: '/QR Solver' - if (b_k + 1 < 2) { - b_A[kend] = 1.0F; - iy = kend + 17; + if (b_j + 1 < 2) { + b_A[ii] = 1.0F; + pvt = ii + 17; if (tau[0] != 0.0F) { - lastv = 16 - b_k; - pvt = kend - b_k; - while ((lastv > 0) && (b_A[pvt + 15] == 0.0F)) { - lastv--; - pvt--; + itemp = 16 - b_j; + i = ii - b_j; + while ((itemp > 0) && (b_A[i + 15] == 0.0F)) { + itemp--; + i--; } - ix = 1 - b_k; + ix = 1 - b_j; exitg2 = false; while ((!exitg2) && (ix > 0)) { - ia = kend; + b_A_tmp = ii; do { exitg1 = 0; - if (ia + 17 <= (kend + lastv) + 16) { - if (b_A[ia + 16] != 0.0F) { + if (b_A_tmp + 17 <= (ii + itemp) + 16) { + if (b_A[b_A_tmp + 16] != 0.0F) { exitg1 = 1; } else { - ia++; + b_A_tmp++; } } else { ix = 0; @@ -251,79 +243,78 @@ static void estimation_velocity_xgeqp3(const real32_T A[32], real32_T b_A[32], } } } else { - lastv = 0; + itemp = 0; ix = 0; } - if (lastv > 0) { + if (itemp > 0) { if (ix != 0) { work[0] = 0.0F; - for (scale_tmp = iy; scale_tmp <= iy; scale_tmp += 16) { + for (jA = pvt; jA <= pvt; jA += 16) { scale = 0.0F; - knt = scale_tmp + lastv; - for (ia = scale_tmp; ia < knt; ia++) { - scale += b_A[(kend + ia) - scale_tmp] * b_A[ia - 1]; + i = jA + itemp; + for (b_A_tmp = jA; b_A_tmp < i; b_A_tmp++) { + scale += b_A[(ii + b_A_tmp) - jA] * b_A[b_A_tmp - 1]; } - pvt = ((scale_tmp - kend) - 17) >> 4; - work[pvt] += scale; + i = ((jA - ii) - 17) >> 4; + work[i] += scale; } } - if (!(-tau[0] == 0.0F)) { - iy = kend; + if (-tau[0] != 0.0F) { + jA = ii; pvt = ix - 1; for (ix = 0; ix <= pvt; ix++) { if (work[0] != 0.0F) { scale = work[0] * -tau[0]; - ia = iy + 17; - knt = (lastv + iy) + 16; - for (scale_tmp = ia; scale_tmp <= knt; scale_tmp++) { - b_A[scale_tmp - 1] += b_A[((kend + scale_tmp) - iy) - 17] * - scale; + b_A_tmp = jA + 17; + i = (itemp + jA) + 16; + for (ijA = b_A_tmp; ijA <= i; ijA++) { + b_A[ijA - 1] += b_A[((ii + ijA) - jA) - 17] * scale; } } - iy += 16; + jA += 16; } } } - b_A[kend] = smax; + b_A[ii] = absxk; } - for (pvt = b_k + 2; pvt < 3; pvt++) { + for (ii = b_j + 2; ii < 3; ii++) { if (vn1[1] != 0.0F) { - smax = std::abs(b_A[b_k + 16]) / vn1[1]; - smax = 1.0F - smax * smax; - if (smax < 0.0F) { - smax = 0.0F; + absxk = std::abs(b_A[b_j + 16]) / vn1[1]; + absxk = 1.0F - absxk * absxk; + if (absxk < 0.0F) { + absxk = 0.0F; } scale = vn1[1] / vn2[1]; - scale = scale * scale * smax; + scale = scale * scale * absxk; if (scale <= 0.000345266977F) { - smax = 0.0F; + beta1 = 0.0F; scale = 1.29246971E-26F; - for (lastv = b_k + 18; lastv < 33; lastv++) { - absxk = std::abs(b_A[lastv - 1]); + for (itemp = b_j + 18; itemp < 33; itemp++) { + absxk = std::abs(b_A[itemp - 1]); if (absxk > scale) { t = scale / absxk; - smax = smax * t * t + 1.0F; + beta1 = beta1 * t * t + 1.0F; scale = absxk; } else { t = absxk / scale; - smax += t * t; + beta1 += t * t; } } - mw_arm_sqrt_f32(smax, &smax); - smax *= scale; - vn1[1] = smax; - vn2[1] = smax; + mw_arm_sqrt_f32(beta1, &beta1); + beta1 *= scale; + vn1[1] = beta1; + vn2[1] = beta1; } else { - mw_arm_sqrt_f32(smax, &smax); - vn1[1] *= smax; + mw_arm_sqrt_f32(absxk, &beta1); + vn1[1] *= beta1; } } } @@ -344,7 +335,7 @@ static void estimation_velocity_mldivide(const real32_T A_data[], const int32_T *Y_size = *B_size; std::memcpy(&Y_data[0], &B_data[0], static_cast(*B_size) * sizeof (real32_T)); - for (int32_T minmn = 1; minmn >= 1; minmn--) { + for (int32_T loop_ub = 1; loop_ub >= 1; loop_ub--) { if (Y_data[0] != 0.0F) { Y_data[0] /= A_data[0]; } @@ -357,39 +348,27 @@ static void estimation_velocity_mldivide(const real32_T A_data[], const int32_T // System initialize for referenced model: 'estimation_velocity' void estimation_velocity_Init(DW_estimation_velocity_f_T *localDW) { - // InitializeConditions for S-Function (sdspsreg2): '/Delay Line' - for (int32_T i = 0; i < 15; i++) { - localDW->DelayLine_Buff[i] = 0.0F; - } - - localDW->DelayLine_BUFF_OFFSET = 0; - - // End of InitializeConditions for S-Function (sdspsreg2): '/Delay Line' - - // InitializeConditions for Delay: '/Delay' - localDW->CircBufIdx = 0U; - // Start for MATLABSystem: '/QR Solver' localDW->objisempty = true; } // Output and update for referenced model: 'estimation_velocity' void estimation_velocity(const SensorsData *rtu_SensorsData, const - ConfigurationParameters *rtu_ConfigurationParameters, JointVelocities - *rty_EstimatedVelocity, DW_estimation_velocity_f_T *localDW) + EstimationVelocityModes *rtu_EstimationConfig, real32_T *rty_EstimatedVelocity, + DW_estimation_velocity_f_T *localDW) { int32_T b_R_size[2]; int32_T b_jpvt[2]; - int32_T bIndx; + int32_T c; int32_T d; int32_T exitg1; + int32_T i; + int32_T i_0; int32_T ia; int32_T iaii; - int32_T itau; int32_T jA; + int32_T k; int32_T lastc; - int32_T lastv; - int32_T yIndx; real32_T c_A[32]; real32_T rtb_DelayLine[16]; real32_T b_R[4]; @@ -400,29 +379,31 @@ void estimation_velocity(const SensorsData *rtu_SensorsData, const real32_T xi[2]; real32_T b_R_data; real32_T b_tau_data; - real32_T c; + real32_T c_0; real32_T tol; // S-Function (sdspsreg2): '/Delay Line' - for (lastc = 0; lastc < 15 - localDW->DelayLine_BUFF_OFFSET; lastc++) { - rtb_DelayLine[lastc] = localDW->DelayLine_Buff - [localDW->DelayLine_BUFF_OFFSET + lastc]; + for (k = 0; k < 15 - localDW->DelayLine_BUFF_OFFSET; k++) { + rtb_DelayLine[k] = localDW->DelayLine_Buff[localDW->DelayLine_BUFF_OFFSET + + k]; } - for (lastc = 0; lastc < localDW->DelayLine_BUFF_OFFSET; lastc++) { - rtb_DelayLine[(lastc - localDW->DelayLine_BUFF_OFFSET) + 15] = - localDW->DelayLine_Buff[lastc]; + for (k = 0; k < localDW->DelayLine_BUFF_OFFSET; k++) { + rtb_DelayLine[(k - localDW->DelayLine_BUFF_OFFSET) + 15] = + localDW->DelayLine_Buff[k]; } - rtb_DelayLine[15] = rtu_SensorsData->jointpositions.position; + rtb_DelayLine[15] = rtu_SensorsData->jointpositions; + + // End of S-Function (sdspsreg2): '/Delay Line' // MATLABSystem: '/QR Solver' incorporates: // Constant: '/Constant' estimation_velocity_xgeqp3(rtCP_Constant_Value_c, c_A, xi, b_jpvt); for (iaii = 0; iaii < 2; iaii++) { - for (itau = 0; itau <= iaii; itau++) { - b_R[itau + (iaii << 1)] = c_A[(iaii << 4) + itau]; + for (i = 0; i <= iaii; i++) { + b_R[i + (iaii << 1)] = c_A[(iaii << 4) + i]; } if (iaii + 2 <= 2) { @@ -432,24 +413,24 @@ void estimation_velocity(const SensorsData *rtu_SensorsData, const work[0] = 0.0F; work[1] = 0.0F; - for (itau = 1; itau >= 0; itau--) { - iaii = ((itau << 4) + itau) + 16; - if (itau + 1 < 2) { + for (i = 1; i >= 0; i--) { + iaii = ((i << 4) + i) + 16; + if (i + 1 < 2) { c_A[iaii - 16] = 1.0F; - yIndx = iaii + 1; - if (xi[itau] != 0.0F) { - lastv = 16; - bIndx = iaii - 1; - while ((lastv > 0) && (c_A[bIndx] == 0.0F)) { - lastv--; - bIndx--; + c = iaii + 1; + if (xi[i] != 0.0F) { + k = 16; + i_0 = iaii - 1; + while ((k > 0) && (c_A[i_0] == 0.0F)) { + k--; + i_0--; } lastc = 1; ia = iaii; do { exitg1 = 0; - if (ia + 1 <= iaii + lastv) { + if (ia + 1 <= iaii + k) { if (c_A[ia] != 0.0F) { exitg1 = 1; } else { @@ -461,35 +442,35 @@ void estimation_velocity(const SensorsData *rtu_SensorsData, const } } while (exitg1 == 0); } else { - lastv = 0; + k = 0; lastc = 0; } - if (lastv > 0) { + if (k > 0) { if (lastc != 0) { work[0] = 0.0F; - for (jA = yIndx; jA <= iaii + 1; jA += 16) { - c = 0.0F; - d = jA + lastv; - for (ia = jA; ia < d; ia++) { - c += c_A[((iaii + ia) - jA) - 16] * c_A[ia - 1]; + for (i_0 = c; i_0 <= iaii + 1; i_0 += 16) { + c_0 = 0.0F; + d = i_0 + k; + for (ia = i_0; ia < d; ia++) { + c_0 += c_A[((iaii + ia) - i_0) - 16] * c_A[ia - 1]; } - bIndx = ((jA - iaii) - 1) >> 4; - work[bIndx] += c; + d = ((i_0 - iaii) - 1) >> 4; + work[d] += c_0; } } - if (!(-xi[itau] == 0.0F)) { + if (-xi[i] != 0.0F) { jA = iaii; - bIndx = lastc - 1; - for (lastc = 0; lastc <= bIndx; lastc++) { + i_0 = lastc - 1; + for (lastc = 0; lastc <= i_0; lastc++) { if (work[0] != 0.0F) { - c = work[0] * -xi[itau]; - yIndx = jA + 1; - d = lastv + jA; - for (ia = yIndx; ia <= d; ia++) { - c_A[ia - 1] += c_A[((iaii + ia) - jA) - 17] * c; + c_0 = work[0] * -xi[i]; + c = jA + 1; + d = k + jA; + for (ia = c; ia <= d; ia++) { + c_A[ia - 1] += c_A[((iaii + ia) - jA) - 17] * c_0; } } @@ -499,121 +480,121 @@ void estimation_velocity(const SensorsData *rtu_SensorsData, const } } - bIndx = iaii - itau; - for (lastc = iaii - 14; lastc <= bIndx; lastc++) { - c_A[lastc - 1] *= -xi[itau]; + i_0 = iaii - i; + for (k = iaii - 14; k <= i_0; k++) { + c_A[k - 1] *= -xi[i]; } - c_A[iaii - 16] = 1.0F - xi[itau]; - if (itau - 1 >= 0) { + c_A[iaii - 16] = 1.0F - xi[i]; + if (i - 1 >= 0) { c_A[iaii - 17] = 0.0F; } } // Start for MATLABSystem: '/QR Solver' - c = std::abs(b_R[0]); + c_0 = std::abs(b_R[0]); // MATLABSystem: '/QR Solver' incorporates: // S-Function (sdspsreg2): '/Delay Line' - tol = 1.90734863E-6F * c; - lastc = 0; - while ((lastc < 2) && (!(std::abs(b_R[(lastc << 1) + lastc]) <= tol))) { - lastc++; + tol = 1.90734863E-6F * c_0; + k = 0; + while ((k < 2) && (std::abs(b_R[(k << 1) + k]) > tol)) { + k++; } - if (lastc < 1) { - yIndx = 0; + if (k < 1) { + c = 0; } else { - yIndx = lastc; + c = k; } - d = yIndx; - bIndx = yIndx - 1; - for (itau = 0; itau <= bIndx; itau++) { - jA = (itau << 4) - 1; + ia = c; + i_0 = c - 1; + for (i = 0; i <= i_0; i++) { + lastc = (i << 4) - 1; tol = 0.0F; - for (iaii = 0; iaii < 16; iaii++) { - lastv = (jA + iaii) + 1; - tol += c_A[((lastv / 16) << 4) + lastv % 16] * rtb_DelayLine[iaii]; + for (c = 0; c < 16; c++) { + iaii = (lastc + c) + 1; + tol += c_A[((iaii / 16) << 4) + iaii % 16] * rtb_DelayLine[c]; } - Qb_data[itau] = tol; + Qb_data[i] = tol; } - if (lastc == 2) { - if (std::abs(b_R[1]) > c) { - itau = 1; - yIndx = 0; + if (k == 2) { + if (std::abs(b_R[1]) > c_0) { + i = 1; + iaii = 0; } else { - itau = 0; - yIndx = 1; + i = 0; + iaii = 1; } - tol = b_R[yIndx] / b_R[itau]; - c = b_R[itau + 2]; - tol = (Qb_data[yIndx] - Qb_data[itau] * tol) / (b_R[yIndx + 2] - c * tol); + tol = b_R[iaii] / b_R[i]; + c_0 = b_R[i + 2]; + tol = (Qb_data[iaii] - Qb_data[i] * tol) / (b_R[iaii + 2] - c_0 * tol); xi[1] = tol; - xi[0] = (Qb_data[itau] - tol * c) / b_R[itau]; + xi[0] = (Qb_data[i] - tol * c_0) / b_R[i]; } else { - lastc = (lastc >= 1); - if (lastc - 1 >= 0) { + c = (k >= 1); + if (c - 1 >= 0) { xi[0] = b_R[0]; xi[1] = b_R[2]; } - ia = lastc; - itau = lastc << 1; - if (itau - 1 >= 0) { - std::memcpy(&c_A_data[0], &xi[0], static_cast(itau) * sizeof + k = c; + i = c << 1; + if (i - 1 >= 0) { + std::memcpy(&c_A_data[0], &xi[0], static_cast(i) * sizeof (real32_T)); } - if (lastc - 1 >= 0) { + if (c - 1 >= 0) { b_tau_data = 0.0F; } - if (lastc != 0) { + if (c != 0) { b_tau_data = 0.0F; for (iaii = 0; iaii < 2; iaii++) { c_A_data_0[iaii] = xi[iaii]; } - for (itau = 0; itau < 1; itau++) { + for (i = 0; i < 1; i++) { for (iaii = 0; iaii < 2; iaii++) { c_A_data[iaii] = c_A_data_0[iaii]; } - c = c_A_data_0[0]; + c_0 = c_A_data_0[0]; b_tau_data = 0.0F; tol = std::abs(c_A_data_0[1]); if (tol != 0.0F) { - tol = rt_hypotf_snf(c_A_data_0[0], tol); + tol = rt_hypotf(c_A_data_0[0], tol); if (c_A_data_0[0] >= 0.0F) { tol = -tol; } if (std::abs(tol) < 9.86076132E-32F) { - yIndx = -1; + iaii = -1; do { - yIndx++; + iaii++; c_A_data[1] *= 1.01412048E+31F; tol *= 1.01412048E+31F; - c *= 1.01412048E+31F; - } while ((std::abs(tol) < 9.86076132E-32F) && (yIndx + 1 < 20)); + c_0 *= 1.01412048E+31F; + } while ((std::abs(tol) < 9.86076132E-32F) && (iaii + 1 < 20)); - tol = rt_hypotf_snf(c, std::abs(c_A_data[1])); - if (c >= 0.0F) { + tol = rt_hypotf(c_0, std::abs(c_A_data[1])); + if (c_0 >= 0.0F) { tol = -tol; } - b_tau_data = (tol - c) / tol; - c_A_data[1] *= 1.0F / (c - tol); - for (lastc = 0; lastc <= yIndx; lastc++) { + b_tau_data = (tol - c_0) / tol; + c_A_data[1] *= 1.0F / (c_0 - tol); + for (k = 0; k <= iaii; k++) { tol *= 9.86076132E-32F; } - c = tol; + c_0 = tol; } else { b_tau_data = (tol - c_A_data_0[0]) / tol; for (iaii = 0; iaii < 2; iaii++) { @@ -621,7 +602,7 @@ void estimation_velocity(const SensorsData *rtu_SensorsData, const } c_A_data[1] *= 1.0F / (c_A_data_0[0] - tol); - c = tol; + c_0 = tol; } } @@ -629,28 +610,28 @@ void estimation_velocity(const SensorsData *rtu_SensorsData, const c_A_data_0[iaii] = c_A_data[iaii]; } - c_A_data_0[0] = c; + c_A_data_0[0] = c_0; } - ia = 1; + k = 1; for (iaii = 0; iaii < 2; iaii++) { c_A_data[iaii] = c_A_data_0[iaii]; } } - lastv = ia - 1; - if (ia - 1 >= 0) { + i = k - 1; + if (k - 1 >= 0) { b_R_data = c_A_data[0]; } - lastc = ia << 1; - if (lastc - 1 >= 0) { - std::memcpy(&xi[0], &c_A_data[0], static_cast(lastc) * sizeof + c = k << 1; + if (c - 1 >= 0) { + std::memcpy(&xi[0], &c_A_data[0], static_cast(c) * sizeof (real32_T)); } - if (ia >= 1) { - for (lastc = ia; lastc < 1; lastc++) { + if (k >= 1) { + for (lastc = k; lastc < 1; lastc++) { xi[1] = 0.0F; } @@ -658,18 +639,18 @@ void estimation_velocity(const SensorsData *rtu_SensorsData, const xi[0] = 1.0F - b_tau_data; } - if (ia - 1 >= 0) { + if (k - 1 >= 0) { c_A_data_0[0] = xi[0]; c_A_data_0[1] = xi[1]; } - b_R_size[0] = ia; - b_R_size[1] = ia; - estimation_velocity_mldivide(&b_R_data, b_R_size, Qb_data, &d, &b_tau_data, - &lastc); + b_R_size[0] = k; + b_R_size[1] = k; + estimation_velocity_mldivide(&b_R_data, b_R_size, Qb_data, &ia, &b_tau_data, + &k); xi[0] = 0.0F; xi[1] = 0.0F; - for (lastc = 0; lastc <= lastv; lastc++) { + for (k = 0; k <= i; k++) { xi[0] += c_A_data_0[0] * b_tau_data; xi[1] += b_tau_data * c_A_data_0[1]; } @@ -684,21 +665,20 @@ void estimation_velocity(const SensorsData *rtu_SensorsData, const // Delay: '/Delay' // Gain: '/Gain' // MATLABSystem: '/QR Solver' - // S-Function (sdspsreg2): '/Delay Line' // Sum: '/Sum' - switch (rtu_ConfigurationParameters->estimationconfig.velocity_mode) { + switch (*rtu_EstimationConfig) { case EstimationVelocityModes_Disabled: - rty_EstimatedVelocity->velocity = 0.0F; + *rty_EstimatedVelocity = 0.0F; break; case EstimationVelocityModes_MovingAverage: - rty_EstimatedVelocity->velocity = (rtu_SensorsData->jointpositions.position - - localDW->Delay_DSTATE[localDW->CircBufIdx]) * 62.5F; + *rty_EstimatedVelocity = (rtu_SensorsData->jointpositions - + localDW->Delay_DSTATE[localDW->CircBufIdx]) * 62.5F; break; default: - rty_EstimatedVelocity->velocity = work[0]; + *rty_EstimatedVelocity = work[0]; break; } @@ -706,7 +686,7 @@ void estimation_velocity(const SensorsData *rtu_SensorsData, const // Update for S-Function (sdspsreg2): '/Delay Line' localDW->DelayLine_Buff[localDW->DelayLine_BUFF_OFFSET] = - rtu_SensorsData->jointpositions.position; + rtu_SensorsData->jointpositions; localDW->DelayLine_BUFF_OFFSET++; while (localDW->DelayLine_BUFF_OFFSET >= 15) { localDW->DelayLine_BUFF_OFFSET -= 15; @@ -714,11 +694,8 @@ void estimation_velocity(const SensorsData *rtu_SensorsData, const // End of Update for S-Function (sdspsreg2): '/Delay Line' - // Update for Delay: '/Delay' incorporates: - // S-Function (sdspsreg2): '/Delay Line' - - localDW->Delay_DSTATE[localDW->CircBufIdx] = - rtu_SensorsData->jointpositions.position; + // Update for Delay: '/Delay' + localDW->Delay_DSTATE[localDW->CircBufIdx] = rtu_SensorsData->jointpositions; if (localDW->CircBufIdx < 15U) { localDW->CircBufIdx++; } else { @@ -728,24 +705,6 @@ void estimation_velocity(const SensorsData *rtu_SensorsData, const // End of Update for Delay: '/Delay' } -// Model initialize function -void estimation_velocity_initialize(const char_T **rt_errorStatus, - RT_MODEL_estimation_velocity_T *const estimation_velocity_M, - DW_estimation_velocity_f_T *localDW) -{ - // Registration code - - // initialize non-finites - rt_InitInfAndNaN(sizeof(real_T)); - - // initialize error status - rtmSetErrorStatusPointer(estimation_velocity_M, rt_errorStatus); - - // states (dwork) - (void) std::memset(static_cast(localDW), 0, - sizeof(DW_estimation_velocity_f_T)); -} - // // File trailer for generated code. // diff --git a/emBODY/eBcode/arch-arm/mbd/amcbldc/estimator/estimation_velocity.h b/emBODY/eBcode/arch-arm/mbd/amcbldc/estimator/estimation_velocity.h index 78e6325ff..0fb6ee5ea 100644 --- a/emBODY/eBcode/arch-arm/mbd/amcbldc/estimator/estimation_velocity.h +++ b/emBODY/eBcode/arch-arm/mbd/amcbldc/estimator/estimation_velocity.h @@ -7,9 +7,9 @@ // // Code generated for Simulink model 'estimation_velocity'. // -// Model version : 6.4 +// Model version : 6.19 // Simulink Coder version : 23.2 (R2023b) 01-Aug-2023 -// C/C++ source code generated on : Tue Feb 13 11:54:57 2024 +// C/C++ source code generated on : Wed Feb 28 11:28:54 2024 // // Target selection: ert.tlc // Embedded hardware selection: ARM Compatible->ARM Cortex-M @@ -21,16 +21,6 @@ #include "rtwtypes.h" #include "estimation_velocity_types.h" -extern "C" -{ - -#include "rt_nonfinite.h" - -} - -#include "rtGetNaN.h" -#include - // Block states (default storage) for model 'estimation_velocity' struct DW_estimation_velocity_f_T { real32_T DelayLine_Buff[15]; // '/Delay Line' @@ -41,24 +31,14 @@ struct DW_estimation_velocity_f_T { boolean_T objisempty; // '/QR Solver' }; -// Real-time Model Data Structure -struct tag_RTM_estimation_velocity_T { - const char_T **errorStatus; -}; - struct MdlrefDW_estimation_velocity_T { DW_estimation_velocity_f_T rtdw; - RT_MODEL_estimation_velocity_T rtm; }; -// Model reference registration function -extern void estimation_velocity_initialize(const char_T **rt_errorStatus, - RT_MODEL_estimation_velocity_T *const estimation_velocity_M, - DW_estimation_velocity_f_T *localDW); extern void estimation_velocity_Init(DW_estimation_velocity_f_T *localDW); extern void estimation_velocity(const SensorsData *rtu_SensorsData, const - ConfigurationParameters *rtu_ConfigurationParameters, JointVelocities - *rty_EstimatedVelocity, DW_estimation_velocity_f_T *localDW); + EstimationVelocityModes *rtu_EstimationConfig, real32_T *rty_EstimatedVelocity, + DW_estimation_velocity_f_T *localDW); //- // These blocks were eliminated from the model due to optimizations: diff --git a/emBODY/eBcode/arch-arm/mbd/amcbldc/estimator/estimation_velocity_private.h b/emBODY/eBcode/arch-arm/mbd/amcbldc/estimator/estimation_velocity_private.h index c1fbd6234..68dc2e2fe 100644 --- a/emBODY/eBcode/arch-arm/mbd/amcbldc/estimator/estimation_velocity_private.h +++ b/emBODY/eBcode/arch-arm/mbd/amcbldc/estimator/estimation_velocity_private.h @@ -7,9 +7,9 @@ // // Code generated for Simulink model 'estimation_velocity'. // -// Model version : 6.4 +// Model version : 6.19 // Simulink Coder version : 23.2 (R2023b) 01-Aug-2023 -// C/C++ source code generated on : Tue Feb 13 11:54:57 2024 +// C/C++ source code generated on : Wed Feb 28 11:28:54 2024 // // Target selection: ert.tlc // Embedded hardware selection: ARM Compatible->ARM Cortex-M @@ -21,23 +21,6 @@ #include "rtwtypes.h" #include "estimation_velocity_types.h" -// Macros for accessing real-time model data structure -#ifndef rtmGetErrorStatus -#define rtmGetErrorStatus(rtm) (*((rtm)->errorStatus)) -#endif - -#ifndef rtmSetErrorStatus -#define rtmSetErrorStatus(rtm, val) (*((rtm)->errorStatus) = (val)) -#endif - -#ifndef rtmGetErrorStatusPointer -#define rtmGetErrorStatusPointer(rtm) (rtm)->errorStatus -#endif - -#ifndef rtmSetErrorStatusPointer -#define rtmSetErrorStatusPointer(rtm, val) ((rtm)->errorStatus = (val)) -#endif - extern const real32_T rtCP_pooled_Az3IVI54Pn7X[32]; #define rtCP_Constant_Value_c rtCP_pooled_Az3IVI54Pn7X // Computed Parameter: rtCP_Constant_Value_c diff --git a/emBODY/eBcode/arch-arm/mbd/amcbldc/estimator/estimation_velocity_types.h b/emBODY/eBcode/arch-arm/mbd/amcbldc/estimator/estimation_velocity_types.h index 3e015c38f..dc11cd92c 100644 --- a/emBODY/eBcode/arch-arm/mbd/amcbldc/estimator/estimation_velocity_types.h +++ b/emBODY/eBcode/arch-arm/mbd/amcbldc/estimator/estimation_velocity_types.h @@ -7,9 +7,9 @@ // // Code generated for Simulink model 'estimation_velocity'. // -// Model version : 6.4 +// Model version : 6.19 // Simulink Coder version : 23.2 (R2023b) 01-Aug-2023 -// C/C++ source code generated on : Tue Feb 13 11:54:57 2024 +// C/C++ source code generated on : Wed Feb 28 11:28:54 2024 // // Target selection: ert.tlc // Embedded hardware selection: ARM Compatible->ARM Cortex-M @@ -19,13 +19,12 @@ #ifndef RTW_HEADER_estimation_velocity_types_h_ #define RTW_HEADER_estimation_velocity_types_h_ #include "rtwtypes.h" -#ifndef DEFINED_TYPEDEF_FOR_JointPositions_ -#define DEFINED_TYPEDEF_FOR_JointPositions_ +#ifndef DEFINED_TYPEDEF_FOR_DriverSensors_ +#define DEFINED_TYPEDEF_FOR_DriverSensors_ -struct JointPositions +struct DriverSensors { - // joint positions - real32_T position; + real32_T Vcc; }; #endif @@ -47,59 +46,15 @@ struct MotorSensors #endif -#ifndef DEFINED_TYPEDEF_FOR_SupplyVoltage_ -#define DEFINED_TYPEDEF_FOR_SupplyVoltage_ - -struct SupplyVoltage -{ - real32_T voltage; -}; - -#endif - #ifndef DEFINED_TYPEDEF_FOR_SensorsData_ #define DEFINED_TYPEDEF_FOR_SensorsData_ struct SensorsData { // position encoders - JointPositions jointpositions; - - // motor probes + real32_T jointpositions; + DriverSensors driversensors; MotorSensors motorsensors; - - // supply probes - SupplyVoltage supplyvoltagesensors; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_MotorConfig_ -#define DEFINED_TYPEDEF_FOR_MotorConfig_ - -struct MotorConfig -{ - // Angular offset in degrees between the stator windings and the hall sensors. - real32_T hall_sens_offset; - boolean_T has_hall_sens; - boolean_T has_quadrature_encoder; - boolean_T has_speed_quadrature_encoder; - boolean_T has_torque_sens; - boolean_T use_index; - boolean_T enable_verbosity; - int16_T rotor_encoder_resolution; - int16_T rotor_index_offset; - uint8_T encoder_tolerance; - uint8_T pole_pairs; - real32_T Kbemf; - real32_T Rphase; - real32_T Imin; - real32_T Imax; - real32_T Vmax; - real32_T resistance; - real32_T inductance; - real32_T thermal_resistance; - real32_T thermal_time_constant; }; #endif @@ -115,121 +70,6 @@ typedef enum { #endif -#ifndef DEFINED_TYPEDEF_FOR_EstimationConfig_ -#define DEFINED_TYPEDEF_FOR_EstimationConfig_ - -struct EstimationConfig -{ - EstimationVelocityModes velocity_mode; - - // Forgetting factor in [0, 1] for exponential weighting-based estimation of RMS current value - real32_T current_rms_lambda; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_PIDConfig_ -#define DEFINED_TYPEDEF_FOR_PIDConfig_ - -struct PIDConfig -{ - real32_T OutMax; - real32_T OutMin; - real32_T P; - real32_T I; - real32_T D; - real32_T N; - real32_T I0; - real32_T D0; - uint8_T shift_factor; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_Thresholds_ -#define DEFINED_TYPEDEF_FOR_Thresholds_ - -struct Thresholds -{ - // It shall be greater than hardwareJntPosMin - real32_T jntPosMin; - - // It shall be smaller than hardwareJntPosMax - real32_T jntPosMax; - - // Imposed by hardware constraint - real32_T hardwareJntPosMin; - - // Imposed by hardware constraint - real32_T hardwareJntPosMax; - - // If robotMin == rotorMax == 0, there's no check - real32_T rotorPosMin; - - // If robotMin == rotorMax == 0, there's no check - real32_T rotorPosMax; - - // Can be only non-negative - real32_T jntVelMax; - - // Timeout on reception of velocity setpoint - // Can be only non-negative - uint32_T velocityTimeout; - - // Current that can be kept for an indefinite period of time w/o damaging the motor - // Expressed in [A] as all the internal computations are done this way - // Can be only non-negative - real32_T motorNominalCurrents; - - // Current that can be applied for a short period of time - // Expressed in [A] as all the internal computations are done this way - // Can be only non-negative - real32_T motorPeakCurrents; - - // Currents over this threshold can instantaneously damages the motor - // Expressed in [A] as all the internal computations are done this way - // Can be only non-negative - real32_T motorOverloadCurrents; - - // Expressed in ticks - // Max value is 32000 - // Can be only non-negative - uint32_T motorPwmLimit; - - // The critical temperature of the motor that triggers i2t current protection. - real32_T motorCriticalTemperature; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_ConfigurationParameters_ -#define DEFINED_TYPEDEF_FOR_ConfigurationParameters_ - -struct ConfigurationParameters -{ - MotorConfig motorconfig; - EstimationConfig estimationconfig; - PIDConfig CurLoopPID; - PIDConfig PosLoopPID; - PIDConfig VelLoopPID; - PIDConfig DirLoopPID; - Thresholds thresholds; - real32_T environment_temperature; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_JointVelocities_ -#define DEFINED_TYPEDEF_FOR_JointVelocities_ - -struct JointVelocities -{ - // joint velocities - real32_T velocity; -}; - -#endif - #ifndef struct_dsp_simulink_QRSolver_estimation_velocity_T #define struct_dsp_simulink_QRSolver_estimation_velocity_T @@ -239,10 +79,6 @@ struct dsp_simulink_QRSolver_estimation_velocity_T }; #endif // struct_dsp_simulink_QRSolver_estimation_velocity_T - -// Forward declaration for rtModel -typedef struct tag_RTM_estimation_velocity_T RT_MODEL_estimation_velocity_T; - #endif // RTW_HEADER_estimation_velocity_types_h_ // diff --git a/emBODY/eBcode/arch-arm/mbd/amcbldc/filter-current/filter_current.cpp b/emBODY/eBcode/arch-arm/mbd/amcbldc/filter-current/filter_current.cpp index 0ee544f14..2447c6c6d 100644 --- a/emBODY/eBcode/arch-arm/mbd/amcbldc/filter-current/filter_current.cpp +++ b/emBODY/eBcode/arch-arm/mbd/amcbldc/filter-current/filter_current.cpp @@ -7,9 +7,9 @@ // // Code generated for Simulink model 'filter_current'. // -// Model version : 6.3 +// Model version : 6.14 // Simulink Coder version : 23.2 (R2023b) 01-Aug-2023 -// C/C++ source code generated on : Tue Feb 13 11:55:05 2024 +// C/C++ source code generated on : Wed Feb 28 11:29:07 2024 // // Target selection: ert.tlc // Embedded hardware selection: ARM Compatible->ARM Cortex-M @@ -21,7 +21,6 @@ #include "rtwtypes.h" #include #include -#include "filter_current_private.h" // Forward declaration for local functions static void filter_current_MedianFilterCG_resetImpl @@ -33,7 +32,7 @@ static void filter_current_MedianFilterCG_trickleDownMin static void filter_current_MedianFilterCG_resetImpl (c_dsp_internal_MedianFilterCG_filter_current_T *obj) { - real32_T cnt1; + int32_T cnt1; real32_T cnt2; // Start for MATLABSystem: '/Median Filter' @@ -45,26 +44,26 @@ static void filter_current_MedianFilterCG_resetImpl // Start for MATLABSystem: '/Median Filter' obj->pMidHeap = std::ceil((obj->pWinLen + 1.0F) / 2.0F); - cnt1 = (obj->pWinLen - 1.0F) / 2.0F; - if (cnt1 < 0.0F) { - obj->pMinHeapLength = std::ceil(cnt1); + cnt2 = (obj->pWinLen - 1.0F) / 2.0F; + if (cnt2 < 0.0F) { + obj->pMinHeapLength = std::ceil(cnt2); } else { - obj->pMinHeapLength = std::floor(cnt1); + obj->pMinHeapLength = std::floor(cnt2); } - cnt1 = obj->pWinLen / 2.0F; - if (cnt1 < 0.0F) { - obj->pMaxHeapLength = std::ceil(cnt1); + cnt2 = obj->pWinLen / 2.0F; + if (cnt2 < 0.0F) { + obj->pMaxHeapLength = std::ceil(cnt2); } else { - obj->pMaxHeapLength = std::floor(cnt1); + obj->pMaxHeapLength = std::floor(cnt2); } - cnt1 = 1.0F; + cnt1 = 1; cnt2 = obj->pWinLen; for (int32_T i = 0; i < 32; i++) { // Start for MATLABSystem: '/Median Filter' if (std::fmod(32.0F - static_cast(i), 2.0F) == 0.0F) { - obj->pPos[31 - i] = cnt1; + obj->pPos[31 - i] = static_cast(cnt1); cnt1++; } else { obj->pPos[31 - i] = cnt2; @@ -102,8 +101,8 @@ static void filter_current_MedianFilterCG_trickleDownMax ind2 = i + obj->pMidHeap; tmp = obj->pHeap[static_cast(ind2) - 1]; - if (!(obj->pBuf[static_cast(obj->pHeap[static_cast(temp + - obj->pMidHeap) - 1]) - 1] < obj->pBuf[static_cast(tmp) - 1])) + if (obj->pBuf[static_cast(obj->pHeap[static_cast(temp + + obj->pMidHeap) - 1]) - 1] >= obj->pBuf[static_cast(tmp) - 1]) { exitg1 = true; } else { @@ -152,9 +151,9 @@ static void filter_current_MedianFilterCG_trickleDownMin ind1 = i + obj->pMidHeap; tmp_0 = obj->pHeap[static_cast(ind1) - 1]; - if (!(obj->pBuf[static_cast(tmp_0) - 1] < obj->pBuf - [static_cast(obj->pHeap[static_cast(tmp + - obj->pMidHeap) - 1]) - 1])) { + if (obj->pBuf[static_cast(tmp_0) - 1] >= obj->pBuf + [static_cast(obj->pHeap[static_cast(tmp + + obj->pMidHeap) - 1]) - 1]) { exitg1 = true; } else { if (u_tmp < 0.0F) { @@ -189,15 +188,15 @@ void filter_current_Init(DW_filter_current_f_T *localDW) } // Output and update for referenced model: 'filter_current' -void filter_current(const ControlOutputs *rtu_ControlOutputs, MotorCurrent +void filter_current(const FOCOutputs *rtu_ControlOutputs, real32_T *rty_FilteredCurrent, DW_filter_current_f_T *localDW) { c_dsp_internal_MedianFilterCG_filter_current_T *obj; int32_T vprev_tmp; + real32_T ind1; real32_T ind2; real32_T p; real32_T temp; - real32_T u_tmp; real32_T vprev; boolean_T exitg1; boolean_T flag; @@ -212,7 +211,7 @@ void filter_current(const ControlOutputs *rtu_ControlOutputs, MotorCurrent vprev_tmp = static_cast(localDW->obj.pMID.pIdx) - 1; vprev = localDW->obj.pMID.pBuf[vprev_tmp]; - localDW->obj.pMID.pBuf[vprev_tmp] = rtu_ControlOutputs->Iq_fbk.current; + localDW->obj.pMID.pBuf[vprev_tmp] = rtu_ControlOutputs->Iq_fbk; p = localDW->obj.pMID.pPos[static_cast(localDW->obj.pMID.pIdx) - 1]; localDW->obj.pMID.pIdx++; if (localDW->obj.pMID.pWinLen + 1.0F == localDW->obj.pMID.pIdx) { @@ -220,7 +219,7 @@ void filter_current(const ControlOutputs *rtu_ControlOutputs, MotorCurrent } if (p > localDW->obj.pMID.pMidHeap) { - if (vprev < rtu_ControlOutputs->Iq_fbk.current) { + if (vprev < rtu_ControlOutputs->Iq_fbk) { vprev = p - localDW->obj.pMID.pMidHeap; filter_current_MedianFilterCG_trickleDownMin(&localDW->obj.pMID, vprev * 2.0F); @@ -228,22 +227,22 @@ void filter_current(const ControlOutputs *rtu_ControlOutputs, MotorCurrent vprev = p - localDW->obj.pMID.pMidHeap; exitg1 = false; while ((!exitg1) && (vprev > 0.0F)) { - u_tmp = std::floor(vprev / 2.0F); + p = std::floor(vprev / 2.0F); flag = (obj->pBuf[static_cast(obj->pHeap[static_cast (vprev + obj->pMidHeap) - 1]) - 1] < obj->pBuf - [static_cast(obj->pHeap[static_cast(u_tmp + + [static_cast(obj->pHeap[static_cast(p + obj->pMidHeap) - 1]) - 1]); if (!flag) { exitg1 = true; } else { - p = vprev + obj->pMidHeap; - ind2 = u_tmp + obj->pMidHeap; - temp = obj->pHeap[static_cast(p) - 1]; - obj->pHeap[static_cast(p) - 1] = obj->pHeap + ind1 = vprev + obj->pMidHeap; + ind2 = p + obj->pMidHeap; + temp = obj->pHeap[static_cast(ind1) - 1]; + obj->pHeap[static_cast(ind1) - 1] = obj->pHeap [static_cast(ind2) - 1]; obj->pHeap[static_cast(ind2) - 1] = temp; - obj->pPos[static_cast(obj->pHeap[static_cast(p) - 1]) - - 1] = p; + obj->pPos[static_cast(obj->pHeap[static_cast(ind1) - + 1]) - 1] = ind1; obj->pPos[static_cast(obj->pHeap[static_cast(ind2) - 1]) - 1] = ind2; vprev = std::floor(vprev / 2.0F); @@ -255,7 +254,7 @@ void filter_current(const ControlOutputs *rtu_ControlOutputs, MotorCurrent } } } else if (p < localDW->obj.pMID.pMidHeap) { - if (rtu_ControlOutputs->Iq_fbk.current < vprev) { + if (rtu_ControlOutputs->Iq_fbk < vprev) { vprev = p - localDW->obj.pMID.pMidHeap; filter_current_MedianFilterCG_trickleDownMax(&localDW->obj.pMID, vprev * 2.0F); @@ -263,38 +262,38 @@ void filter_current(const ControlOutputs *rtu_ControlOutputs, MotorCurrent vprev = p - localDW->obj.pMID.pMidHeap; exitg1 = false; while ((!exitg1) && (vprev < 0.0F)) { - u_tmp = vprev / 2.0F; - if (u_tmp < 0.0F) { - p = std::ceil(u_tmp); + p = vprev / 2.0F; + if (p < 0.0F) { + ind1 = std::ceil(p); } else { - p = -0.0F; + ind1 = -0.0F; } - flag = (obj->pBuf[static_cast(obj->pHeap[static_cast(p - + obj->pMidHeap) - 1]) - 1] < obj->pBuf[static_cast - (obj->pHeap[static_cast(vprev + obj->pMidHeap) - 1]) - - 1]); + flag = (obj->pBuf[static_cast(obj->pHeap[static_cast + (ind1 + obj->pMidHeap) - 1]) - 1] < obj->pBuf + [static_cast(obj->pHeap[static_cast(vprev + + obj->pMidHeap) - 1]) - 1]); if (!flag) { exitg1 = true; } else { - if (u_tmp < 0.0F) { - p = std::ceil(u_tmp); + if (p < 0.0F) { + ind1 = std::ceil(p); } else { - p = -0.0F; + ind1 = -0.0F; } - p += obj->pMidHeap; + ind1 += obj->pMidHeap; ind2 = vprev + obj->pMidHeap; - temp = obj->pHeap[static_cast(p) - 1]; - obj->pHeap[static_cast(p) - 1] = obj->pHeap - [static_cast(ind2) - 1]; + temp = obj->pHeap[static_cast(ind1) - 1]; + obj->pHeap[static_cast(ind1) - 1] = obj->pHeap[static_cast< + int32_T>(ind2) - 1]; obj->pHeap[static_cast(ind2) - 1] = temp; - obj->pPos[static_cast(obj->pHeap[static_cast(p) - 1]) - - 1] = p; + obj->pPos[static_cast(obj->pHeap[static_cast(ind1) - + 1]) - 1] = ind1; obj->pPos[static_cast(obj->pHeap[static_cast(ind2) - 1]) - 1] = ind2; - if (u_tmp < 0.0F) { - vprev = std::ceil(u_tmp); + if (p < 0.0F) { + vprev = std::ceil(p); } else { vprev = -0.0F; } @@ -315,13 +314,15 @@ void filter_current(const ControlOutputs *rtu_ControlOutputs, MotorCurrent } } - vprev = localDW->obj.pMID.pBuf[static_cast(localDW->obj.pMID.pHeap[ + ind1 = localDW->obj.pMID.pBuf[static_cast(localDW->obj.pMID.pHeap[ static_cast(localDW->obj.pMID.pMidHeap) - 1]) - 1]; - vprev += localDW->obj.pMID.pBuf[static_cast(localDW->obj.pMID.pHeap[ - static_cast(localDW->obj.pMID.pMidHeap - 1.0F) - 1]) - 1]; - rty_FilteredCurrent->current = vprev / 2.0F; + vprev = localDW->obj.pMID.pBuf[static_cast(localDW->obj.pMID.pHeap[ + static_cast(localDW->obj.pMID.pMidHeap - 1.0F) - 1]) - 1] + ind1; - // End of MATLABSystem: '/Median Filter' + // SignalConversion generated from: '/Median Filter' incorporates: + // MATLABSystem: '/Median Filter' + // + *rty_FilteredCurrent = vprev / 2.0F; } // Termination for referenced model: 'filter_current' @@ -341,21 +342,6 @@ void filter_current_Term(DW_filter_current_f_T *localDW) // End of Terminate for MATLABSystem: '/Median Filter' } -// Model initialize function -void filter_current_initialize(const char_T **rt_errorStatus, - RT_MODEL_filter_current_T *const filter_current_M, DW_filter_current_f_T - *localDW) -{ - // Registration code - - // initialize error status - rtmSetErrorStatusPointer(filter_current_M, rt_errorStatus); - - // states (dwork) - (void) std::memset(static_cast(localDW), 0, - sizeof(DW_filter_current_f_T)); -} - // // File trailer for generated code. // diff --git a/emBODY/eBcode/arch-arm/mbd/amcbldc/filter-current/filter_current.h b/emBODY/eBcode/arch-arm/mbd/amcbldc/filter-current/filter_current.h index f2e59f02c..344d5e593 100644 --- a/emBODY/eBcode/arch-arm/mbd/amcbldc/filter-current/filter_current.h +++ b/emBODY/eBcode/arch-arm/mbd/amcbldc/filter-current/filter_current.h @@ -7,9 +7,9 @@ // // Code generated for Simulink model 'filter_current'. // -// Model version : 6.3 +// Model version : 6.14 // Simulink Coder version : 23.2 (R2023b) 01-Aug-2023 -// C/C++ source code generated on : Tue Feb 13 11:55:05 2024 +// C/C++ source code generated on : Wed Feb 28 11:29:07 2024 // // Target selection: ert.tlc // Embedded hardware selection: ARM Compatible->ARM Cortex-M @@ -20,7 +20,6 @@ #define RTW_HEADER_filter_current_h_ #include "rtwtypes.h" #include "filter_current_types.h" -#include // Block states (default storage) for model 'filter_current' struct DW_filter_current_f_T { @@ -28,23 +27,13 @@ struct DW_filter_current_f_T { boolean_T objisempty; // '/Median Filter' }; -// Real-time Model Data Structure -struct tag_RTM_filter_current_T { - const char_T **errorStatus; -}; - struct MdlrefDW_filter_current_T { DW_filter_current_f_T rtdw; - RT_MODEL_filter_current_T rtm; }; -// Model reference registration function -extern void filter_current_initialize(const char_T **rt_errorStatus, - RT_MODEL_filter_current_T *const filter_current_M, DW_filter_current_f_T - *localDW); extern void filter_current_Init(DW_filter_current_f_T *localDW); -extern void filter_current(const ControlOutputs *rtu_ControlOutputs, - MotorCurrent *rty_FilteredCurrent, DW_filter_current_f_T *localDW); +extern void filter_current(const FOCOutputs *rtu_ControlOutputs, real32_T + *rty_FilteredCurrent, DW_filter_current_f_T *localDW); extern void filter_current_Term(DW_filter_current_f_T *localDW); //- diff --git a/emBODY/eBcode/arch-arm/mbd/amcbldc/filter-current/filter_current_private.h b/emBODY/eBcode/arch-arm/mbd/amcbldc/filter-current/filter_current_private.h index c7f2d5103..b39abd3d2 100644 --- a/emBODY/eBcode/arch-arm/mbd/amcbldc/filter-current/filter_current_private.h +++ b/emBODY/eBcode/arch-arm/mbd/amcbldc/filter-current/filter_current_private.h @@ -7,9 +7,9 @@ // // Code generated for Simulink model 'filter_current'. // -// Model version : 6.3 +// Model version : 6.14 // Simulink Coder version : 23.2 (R2023b) 01-Aug-2023 -// C/C++ source code generated on : Tue Feb 13 11:55:05 2024 +// C/C++ source code generated on : Wed Feb 28 11:29:07 2024 // // Target selection: ert.tlc // Embedded hardware selection: ARM Compatible->ARM Cortex-M @@ -20,23 +20,6 @@ #define RTW_HEADER_filter_current_private_h_ #include "rtwtypes.h" #include "filter_current_types.h" - -// Macros for accessing real-time model data structure -#ifndef rtmGetErrorStatus -#define rtmGetErrorStatus(rtm) (*((rtm)->errorStatus)) -#endif - -#ifndef rtmSetErrorStatus -#define rtmSetErrorStatus(rtm, val) (*((rtm)->errorStatus) = (val)) -#endif - -#ifndef rtmGetErrorStatusPointer -#define rtmGetErrorStatusPointer(rtm) (rtm)->errorStatus -#endif - -#ifndef rtmSetErrorStatusPointer -#define rtmSetErrorStatusPointer(rtm, val) ((rtm)->errorStatus = (val)) -#endif #endif // RTW_HEADER_filter_current_private_h_ // diff --git a/emBODY/eBcode/arch-arm/mbd/amcbldc/filter-current/filter_current_types.h b/emBODY/eBcode/arch-arm/mbd/amcbldc/filter-current/filter_current_types.h index ab06617b7..cbc745a7e 100644 --- a/emBODY/eBcode/arch-arm/mbd/amcbldc/filter-current/filter_current_types.h +++ b/emBODY/eBcode/arch-arm/mbd/amcbldc/filter-current/filter_current_types.h @@ -7,9 +7,9 @@ // // Code generated for Simulink model 'filter_current'. // -// Model version : 6.3 +// Model version : 6.14 // Simulink Coder version : 23.2 (R2023b) 01-Aug-2023 -// C/C++ source code generated on : Tue Feb 13 11:55:05 2024 +// C/C++ source code generated on : Wed Feb 28 11:29:07 2024 // // Target selection: ert.tlc // Embedded hardware selection: ARM Compatible->ARM Cortex-M @@ -19,21 +19,10 @@ #ifndef RTW_HEADER_filter_current_types_h_ #define RTW_HEADER_filter_current_types_h_ #include "rtwtypes.h" -#ifndef DEFINED_TYPEDEF_FOR_MotorCurrent_ -#define DEFINED_TYPEDEF_FOR_MotorCurrent_ +#ifndef DEFINED_TYPEDEF_FOR_FOCOutputs_ +#define DEFINED_TYPEDEF_FOR_FOCOutputs_ -struct MotorCurrent -{ - // motor current - real32_T current; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_ControlOutputs_ -#define DEFINED_TYPEDEF_FOR_ControlOutputs_ - -struct ControlOutputs +struct FOCOutputs { // control effort (quadrature) real32_T Vq; @@ -42,16 +31,16 @@ struct ControlOutputs real32_T Vabc[3]; // quadrature current - MotorCurrent Iq_fbk; + real32_T Iq_fbk; // direct current - MotorCurrent Id_fbk; + real32_T Id_fbk; // RMS of Iq - MotorCurrent Iq_rms; + real32_T Iq_rms; // RMS of Id - MotorCurrent Id_rms; + real32_T Id_rms; }; #endif @@ -99,10 +88,6 @@ struct dsp_simulink_MedianFilter_filter_current_T }; #endif // struct_dsp_simulink_MedianFilter_filter_current_T - -// Forward declaration for rtModel -typedef struct tag_RTM_filter_current_T RT_MODEL_filter_current_T; - #endif // RTW_HEADER_filter_current_types_h_ // diff --git a/emBODY/eBcode/arch-arm/mbd/amcbldc/motion-controller-single/motion_controller_single.cpp b/emBODY/eBcode/arch-arm/mbd/amcbldc/motion-controller-single/motion_controller_single.cpp new file mode 100644 index 000000000..1be2d085d --- /dev/null +++ b/emBODY/eBcode/arch-arm/mbd/amcbldc/motion-controller-single/motion_controller_single.cpp @@ -0,0 +1,120 @@ +// +// Non-Degree Granting Education License -- for use at non-degree +// granting, nonprofit, education, and research organizations only. Not +// for commercial or industrial use. +// +// File: motion_controller_single.cpp +// +// Code generated for Simulink model 'motion_controller_single'. +// +// Model version : 1.137 +// Simulink Coder version : 23.2 (R2023b) 01-Aug-2023 +// C/C++ source code generated on : Wed Feb 28 11:29:36 2024 +// +// Target selection: ert.tlc +// Embedded hardware selection: ARM Compatible->ARM Cortex-M +// Code generation objectives: Unspecified +// Validation result: Not run +// +#include "motion_controller_single.h" +#include "motion_controller_single_types.h" +#include "mc_supervisor_defines.h" +#include "motion_controller_single_private.h" +#include "motion_controller.h" + +// System initialize for referenced model: 'motion_controller_single' +void motion_controller_single_Init(Flags *rty_Flags, ActuatorConfiguration + *rty_ActuatorsConfiguration, DW_motion_controller_single_f_T *localDW) +{ + // SystemInitialize for ModelReference: '/Motion Controller' + motion_controller_Init(rty_Flags, rty_ActuatorsConfiguration, + &(localDW->MotionController_InstanceData.rtb), + &(localDW->MotionController_InstanceData.rtdw)); +} + +// Enable for referenced model: 'motion_controller_single' +void motion_controller_single_Enable(DW_motion_controller_single_f_T *localDW) +{ + // Enable for ModelReference: '/Motion Controller' + motion_controller_Enable(&(localDW->MotionController_InstanceData.rtdw)); +} + +// Disable for referenced model: 'motion_controller_single' +void motion_controller_single_Disable(DW_motion_controller_single_f_T *localDW) +{ + // Disable for ModelReference: '/Motion Controller' + motion_controller_Disable(&(localDW->MotionController_InstanceData.rtdw)); +} + +// Output and update for referenced model: 'motion_controller_single' +void mc_base_tick(void) +{ + // ModelReference: '/Motion Controller' + motion_controllerTID0(); +} + +// Output and update for referenced model: 'motion_controller_single' +void mc_foc_tick(const SensorsData *rtu_SensorData, FOCOutputs *rty_FOCOutputs, + DW_motion_controller_single_f_T *localDW) +{ + // ModelReference: '/Motion Controller' + mc_step_foc(rtu_SensorData, rty_FOCOutputs, + &(localDW->MotionController_InstanceData.rtb), + &(localDW->MotionController_InstanceData.rtdw)); +} + +// Output and update for referenced model: 'motion_controller_single' +void mc_1ms_tick(const SupervisorReceivedEvents rtu_Events[MAX_EVENTS_PER_TICK], + const ExternalFlags *rtu_ExternalFlags, EstimatedData + *rty_Estimates, Flags *rty_Flags, ActuatorConfiguration + *rty_ActuatorsConfiguration, B_motion_controller_single_c_T + *localB, DW_motion_controller_single_f_T *localDW) +{ + // BusCreator: '/Bus Creator1' incorporates: + // Constant: '/current_rms_lambda' + // Constant: '/environment_temperature' + // Constant: '/velocity_est_mode' + + localB->BusCreator.estimation.environment_temperature = 25.0F; + localB->BusCreator.estimation.current_rms_lambda = 0.99995F; + localB->BusCreator.estimation.velocity_est_mode = + EstimationVelocityModes_MovingAverage; + + // Constant: '/Constant' + localB->Constant = InitConfParams; + + // ModelReference: '/Motion Controller' + mc_step_1ms(rtu_ExternalFlags, &localB->BusCreator, &rtu_Events[0], + &localB->Constant, rty_Estimates, rty_Flags, + rty_ActuatorsConfiguration, + &(localDW->MotionController_InstanceData.rtb), + &(localDW->MotionController_InstanceData.rtdw)); +} + +// Termination for referenced model: 'motion_controller_single' +void mc_terminate(DW_motion_controller_single_f_T *localDW) +{ + // Terminate for ModelReference: '/Motion Controller' + motion_controller_Term(&(localDW->MotionController_InstanceData.rtdw)); +} + +// Model initialize function +void mc_initialize(const char_T **rt_errorStatus, + RT_MODEL_motion_controller_single_T *const + motion_controller_single_M, DW_motion_controller_single_f_T + *localDW) +{ + // Registration code + + // initialize error status + rtmSetErrorStatusPointer(motion_controller_single_M, rt_errorStatus); + + // Model Initialize function for ModelReference Block: '/Motion Controller' + motion_controller_initialize(&(localDW->MotionController_InstanceData.rtdw)); +} + +// +// File trailer for generated code. +// +// [EOF] +// diff --git a/emBODY/eBcode/arch-arm/mbd/amcbldc/motion-controller-single/motion_controller_single.h b/emBODY/eBcode/arch-arm/mbd/amcbldc/motion-controller-single/motion_controller_single.h new file mode 100644 index 000000000..db6e45b84 --- /dev/null +++ b/emBODY/eBcode/arch-arm/mbd/amcbldc/motion-controller-single/motion_controller_single.h @@ -0,0 +1,104 @@ +// +// Non-Degree Granting Education License -- for use at non-degree +// granting, nonprofit, education, and research organizations only. Not +// for commercial or industrial use. +// +// File: motion_controller_single.h +// +// Code generated for Simulink model 'motion_controller_single'. +// +// Model version : 1.137 +// Simulink Coder version : 23.2 (R2023b) 01-Aug-2023 +// C/C++ source code generated on : Wed Feb 28 11:29:36 2024 +// +// Target selection: ert.tlc +// Embedded hardware selection: ARM Compatible->ARM Cortex-M +// Code generation objectives: Unspecified +// Validation result: Not run +// +#ifndef RTW_HEADER_motion_controller_single_h_ +#define RTW_HEADER_motion_controller_single_h_ +#include "rtwtypes.h" +#include "motion_controller_single_types.h" +#include "motion_controller.h" +#include "mc_supervisor_defines.h" +#include "zero_crossing_types.h" + +// Block signals for model 'motion_controller_single' +struct B_motion_controller_single_c_T { + ActuatorConfiguration Constant; // '/Constant' + GlobalConfiguration BusCreator; // '/Bus Creator' +}; + +// Block states (default storage) for model 'motion_controller_single' +struct DW_motion_controller_single_f_T { + MdlrefDW_motion_controller_T MotionController_InstanceData;// '/Motion Controller' +}; + +// Real-time Model Data Structure +struct tag_RTM_motion_controller_single_T { + const char_T **errorStatus; +}; + +struct MdlrefDW_motion_controller_single_T { + B_motion_controller_single_c_T rtb; + DW_motion_controller_single_f_T rtdw; + RT_MODEL_motion_controller_single_T rtm; +}; + +// +// Exported Global Parameters +// +// Note: Exported global parameters are tunable parameters with an exported +// global storage class designation. Code generation will declare the memory for +// these parameters and exports their symbols. +// + +extern ActuatorConfiguration InitConfParams;// Variable: InitConfParams + // Referenced by: '/Constant' + + +// Model reference registration function +extern void mc_initialize(const char_T **rt_errorStatus, + RT_MODEL_motion_controller_single_T *const motion_controller_single_M, + DW_motion_controller_single_f_T *localDW); +extern void motion_controller_single_Init(Flags *rty_Flags, + ActuatorConfiguration *rty_ActuatorsConfiguration, + DW_motion_controller_single_f_T *localDW); +extern void motion_controller_single_Enable(DW_motion_controller_single_f_T + *localDW); +extern void motion_controller_single_Disable(DW_motion_controller_single_f_T + *localDW); +extern void mc_base_tick(void); +extern void mc_foc_tick(const SensorsData *rtu_SensorData, FOCOutputs + *rty_FOCOutputs, DW_motion_controller_single_f_T *localDW); +extern void mc_1ms_tick(const SupervisorReceivedEvents + rtu_Events[MAX_EVENTS_PER_TICK], const ExternalFlags *rtu_ExternalFlags, + EstimatedData *rty_Estimates, Flags *rty_Flags, ActuatorConfiguration + *rty_ActuatorsConfiguration, B_motion_controller_single_c_T *localB, + DW_motion_controller_single_f_T *localDW); +extern void mc_terminate(DW_motion_controller_single_f_T *localDW); + +//- +// The generated code includes comments that allow you to trace directly +// back to the appropriate location in the model. The basic format +// is /block_name, where system is the system number (uniquely +// assigned by Simulink) and block_name is the name of the block. +// +// Use the MATLAB hilite_system command to trace the generated code back +// to the model. For example, +// +// hilite_system('') - opens system 3 +// hilite_system('/Kp') - opens and selects block Kp which resides in S3 +// +// Here is the system hierarchy for this model +// +// '' : 'motion_controller_single' + +#endif // RTW_HEADER_motion_controller_single_h_ + +// +// File trailer for generated code. +// +// [EOF] +// diff --git a/emBODY/eBcode/arch-arm/mbd/amcbldc/motion-controller-single/motion_controller_single_private.h b/emBODY/eBcode/arch-arm/mbd/amcbldc/motion-controller-single/motion_controller_single_private.h new file mode 100644 index 000000000..7638f1b9d --- /dev/null +++ b/emBODY/eBcode/arch-arm/mbd/amcbldc/motion-controller-single/motion_controller_single_private.h @@ -0,0 +1,47 @@ +// +// Non-Degree Granting Education License -- for use at non-degree +// granting, nonprofit, education, and research organizations only. Not +// for commercial or industrial use. +// +// File: motion_controller_single_private.h +// +// Code generated for Simulink model 'motion_controller_single'. +// +// Model version : 1.137 +// Simulink Coder version : 23.2 (R2023b) 01-Aug-2023 +// C/C++ source code generated on : Wed Feb 28 11:29:36 2024 +// +// Target selection: ert.tlc +// Embedded hardware selection: ARM Compatible->ARM Cortex-M +// Code generation objectives: Unspecified +// Validation result: Not run +// +#ifndef RTW_HEADER_motion_controller_single_private_h_ +#define RTW_HEADER_motion_controller_single_private_h_ +#include "rtwtypes.h" +#include "zero_crossing_types.h" +#include "motion_controller_single_types.h" + +// Macros for accessing real-time model data structure +#ifndef rtmGetErrorStatus +#define rtmGetErrorStatus(rtm) (*((rtm)->errorStatus)) +#endif + +#ifndef rtmSetErrorStatus +#define rtmSetErrorStatus(rtm, val) (*((rtm)->errorStatus) = (val)) +#endif + +#ifndef rtmGetErrorStatusPointer +#define rtmGetErrorStatusPointer(rtm) (rtm)->errorStatus +#endif + +#ifndef rtmSetErrorStatusPointer +#define rtmSetErrorStatusPointer(rtm, val) ((rtm)->errorStatus = (val)) +#endif +#endif // RTW_HEADER_motion_controller_single_private_h_ + +// +// File trailer for generated code. +// +// [EOF] +// diff --git a/emBODY/eBcode/arch-arm/mbd/amcbldc/motion-controller-single/motion_controller_single_types.h b/emBODY/eBcode/arch-arm/mbd/amcbldc/motion-controller-single/motion_controller_single_types.h new file mode 100644 index 000000000..cb4b22dcd --- /dev/null +++ b/emBODY/eBcode/arch-arm/mbd/amcbldc/motion-controller-single/motion_controller_single_types.h @@ -0,0 +1,366 @@ +// +// Non-Degree Granting Education License -- for use at non-degree +// granting, nonprofit, education, and research organizations only. Not +// for commercial or industrial use. +// +// File: motion_controller_single_types.h +// +// Code generated for Simulink model 'motion_controller_single'. +// +// Model version : 1.137 +// Simulink Coder version : 23.2 (R2023b) 01-Aug-2023 +// C/C++ source code generated on : Wed Feb 28 11:29:36 2024 +// +// Target selection: ert.tlc +// Embedded hardware selection: ARM Compatible->ARM Cortex-M +// Code generation objectives: Unspecified +// Validation result: Not run +// +#ifndef RTW_HEADER_motion_controller_single_types_h_ +#define RTW_HEADER_motion_controller_single_types_h_ +#include "rtwtypes.h" + +// Includes for objects with custom storage classes +#include "mc_supervisor_defines.h" + +// +// Registered constraints for dimension variants + +#if MAX_EVENTS_PER_TICK <= 0 +# error "The preprocessor definition 'MAX_EVENTS_PER_TICK' must be greater than '0'" +#endif + +#if MAX_EVENTS_PER_TICK >= 5 +# error "The preprocessor definition 'MAX_EVENTS_PER_TICK' must be less than '5'" +#endif + +#ifndef DEFINED_TYPEDEF_FOR_DriverSensors_ +#define DEFINED_TYPEDEF_FOR_DriverSensors_ + +struct DriverSensors +{ + real32_T Vcc; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_MotorSensors_ +#define DEFINED_TYPEDEF_FOR_MotorSensors_ + +struct MotorSensors +{ + real32_T Iabc[3]; + + // electrical angle = angle * pole_pairs + real32_T angle; + real32_T temperature; + real32_T voltage; + real32_T current; + uint8_T hallABC; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_SensorsData_ +#define DEFINED_TYPEDEF_FOR_SensorsData_ + +struct SensorsData +{ + // position encoders + real32_T jointpositions; + DriverSensors driversensors; + MotorSensors motorsensors; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_Targets_ +#define DEFINED_TYPEDEF_FOR_Targets_ + +struct Targets +{ + real32_T jointpositions; + real32_T jointvelocities; + real32_T motorcurrent; + real32_T motorvoltage; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_ControlModes_ +#define DEFINED_TYPEDEF_FOR_ControlModes_ + +typedef enum { + ControlModes_NotConfigured = 0, // Default value + ControlModes_Idle, + ControlModes_Position, + ControlModes_PositionDirect, + ControlModes_Current, + ControlModes_Velocity, + ControlModes_Voltage, + ControlModes_HwFaultCM +} ControlModes; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_PID_ +#define DEFINED_TYPEDEF_FOR_PID_ + +struct PID +{ + ControlModes type; + real32_T OutMax; + real32_T OutMin; + real32_T P; + real32_T I; + real32_T D; + real32_T N; + real32_T I0; + real32_T D0; + uint8_T shift_factor; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_SupervisorEvents_ +#define DEFINED_TYPEDEF_FOR_SupervisorEvents_ + +typedef enum { + SupervisorEvents_None = 0, // Default value + SupervisorEvents_SetLimit, + SupervisorEvents_SetControlMode, + SupervisorEvents_SetMotorConfig, + SupervisorEvents_SetPid, + SupervisorEvents_SetTarget +} SupervisorEvents; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_SupervisorInputLimits_ +#define DEFINED_TYPEDEF_FOR_SupervisorInputLimits_ + +struct SupervisorInputLimits +{ + real32_T overload; + real32_T peak; + real32_T nominal; + ControlModes type; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_MotorConfiguration_ +#define DEFINED_TYPEDEF_FOR_MotorConfiguration_ + +struct MotorConfiguration +{ + boolean_T has_hall_sens; + boolean_T has_quadrature_encoder; + boolean_T has_speed_quadrature_encoder; + boolean_T has_torque_sens; + boolean_T use_index; + boolean_T enable_verbosity; + int16_T hall_sensors_offset; + int16_T rotor_encoder_resolution; + int16_T rotor_index_offset; + uint8_T encoder_tolerance; + uint8_T pole_pairs; + real32_T Kbemf; + real32_T Rphase; + real32_T Imin; + real32_T Imax; + real32_T Vcc; + real32_T Vmax; + real32_T resistance; + real32_T inductance; + real32_T thermal_resistance; + real32_T thermal_time_constant; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_SupervisorReceivedEvents_ +#define DEFINED_TYPEDEF_FOR_SupervisorReceivedEvents_ + +struct SupervisorReceivedEvents +{ + Targets targets_content; + PID pid_content; + SupervisorEvents event_type; + ControlModes control_mode_content; + SupervisorInputLimits limits_content; + MotorConfiguration motor_config_content; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_ExternalFlags_ +#define DEFINED_TYPEDEF_FOR_ExternalFlags_ + +struct ExternalFlags +{ + // External Fault Button (1 == pressed) + boolean_T fault_button; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_EstimationVelocityModes_ +#define DEFINED_TYPEDEF_FOR_EstimationVelocityModes_ + +typedef enum { + EstimationVelocityModes_Disabled = 0,// Default value + EstimationVelocityModes_MovingAverage, + EstimationVelocityModes_LeastSquares +} EstimationVelocityModes; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_EstimationConfiguration_ +#define DEFINED_TYPEDEF_FOR_EstimationConfiguration_ + +struct EstimationConfiguration +{ + real32_T environment_temperature; + real32_T current_rms_lambda; + EstimationVelocityModes velocity_est_mode; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_GlobalConfiguration_ +#define DEFINED_TYPEDEF_FOR_GlobalConfiguration_ + +struct GlobalConfiguration +{ + EstimationConfiguration estimation; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_FOCOutputs_ +#define DEFINED_TYPEDEF_FOR_FOCOutputs_ + +struct FOCOutputs +{ + // control effort (quadrature) + real32_T Vq; + + // control effort (3-phases) + real32_T Vabc[3]; + + // quadrature current + real32_T Iq_fbk; + + // direct current + real32_T Id_fbk; + + // RMS of Iq + real32_T Iq_rms; + + // RMS of Id + real32_T Id_rms; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_EstimatedData_ +#define DEFINED_TYPEDEF_FOR_EstimatedData_ + +struct EstimatedData +{ + // velocity + real32_T jointvelocities; + + // filtered motor current + real32_T Iq_filtered; + + // motor temperature + real32_T motor_temperature; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_Thresholds_ +#define DEFINED_TYPEDEF_FOR_Thresholds_ + +struct Thresholds +{ + // Can be only non-negative + real32_T jntVelMax; + + // Current that can be kept for an indefinite period of time w/o damaging the motor + // Expressed in [A] as all the internal computations are done this way + // Can be only non-negative + real32_T motorNominalCurrents; + + // Current that can be applied for a short period of time + // Expressed in [A] as all the internal computations are done this way + // Can be only non-negative + real32_T motorPeakCurrents; + + // Currents over this threshold can instantaneously damages the motor + // Expressed in [A] as all the internal computations are done this way + // Can be only non-negative + real32_T motorOverloadCurrents; + + // Expressed in ticks + // Max value is 32000 + // Can be only non-negative + uint32_T motorPwmLimit; + + // The critical temperature of the motor that triggers i2t current protection. + real32_T motorCriticalTemperature; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_PIDsConfiguration_ +#define DEFINED_TYPEDEF_FOR_PIDsConfiguration_ + +struct PIDsConfiguration +{ + PID currentPID; + PID velocityPID; + PID positionPID; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_ActuatorConfiguration_ +#define DEFINED_TYPEDEF_FOR_ActuatorConfiguration_ + +struct ActuatorConfiguration +{ + Thresholds thresholds; + PIDsConfiguration pids; + MotorConfiguration motor; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_Flags_ +#define DEFINED_TYPEDEF_FOR_Flags_ + +struct Flags +{ + boolean_T enable_sending_msg_status; + + // control mode + ControlModes control_mode; + boolean_T enable_thermal_protection; +}; + +#endif + +// Forward declaration for rtModel +typedef struct tag_RTM_motion_controller_single_T + RT_MODEL_motion_controller_single_T; + +#endif // RTW_HEADER_motion_controller_single_types_h_ + +// +// File trailer for generated code. +// +// [EOF] +// diff --git a/emBODY/eBcode/arch-arm/mbd/amcbldc/motion-controller/motion_controller.cpp b/emBODY/eBcode/arch-arm/mbd/amcbldc/motion-controller/motion_controller.cpp new file mode 100644 index 000000000..c009aa09b --- /dev/null +++ b/emBODY/eBcode/arch-arm/mbd/amcbldc/motion-controller/motion_controller.cpp @@ -0,0 +1,349 @@ +// +// Non-Degree Granting Education License -- for use at non-degree +// granting, nonprofit, education, and research organizations only. Not +// for commercial or industrial use. +// +// File: motion_controller.cpp +// +// Code generated for Simulink model 'motion_controller'. +// +// Model version : 2.214 +// Simulink Coder version : 23.2 (R2023b) 01-Aug-2023 +// C/C++ source code generated on : Wed Feb 28 11:29:27 2024 +// +// Target selection: ert.tlc +// Embedded hardware selection: ARM Compatible->ARM Cortex-M +// Code generation objectives: Unspecified +// Validation result: Not run +// +#include "motion_controller.h" +#include "motion_controller_types.h" +#include "rtw_mutex.h" +#include "rtwtypes.h" +#include "mc_supervisor_defines.h" +#include "control_foc.h" +#include "estimation_velocity.h" +#include "filter_current.h" +#include "supervisor.h" +#include "control_outer.h" + +// System initialize for referenced model: 'motion_controller' +void motion_controller_Init(Flags *rty_Flags, ActuatorConfiguration + *rty_ActuatorsConfiguration, B_motion_controller_c_T *localB, + DW_motion_controller_f_T *localDW) +{ + // Start for RateTransition: '/Rate Transition' + rtw_mutex_init(); + + // Start for RateTransition: '/Rate Transition2' + rtw_mutex_init(); + + // Start for RateTransition: '/Rate Transition3' + rtw_mutex_init(); + + // SystemInitialize for ModelReference generated from: '/Control outer' + control_outer_Init(&(localDW->Controlouter_InstanceData.rtdw)); + + // SystemInitialize for ModelReference: '/Current Filter' + filter_current_Init(&(localDW->CurrentFilter_InstanceData.rtdw)); + + // SystemInitialize for ModelReference: '/Velocity Estimator' + estimation_velocity_Init(&(localDW->VelocityEstimator_InstanceData.rtdw)); + + // SystemInitialize for ModelReference generated from: '/FOC' + control_foc_Init(&(localDW->FOC_InstanceData.rtdw)); + + // SystemInitialize for ModelReference: '/Motor Supervisor' + supervisor_Init(&localB->targets, rty_ActuatorsConfiguration, rty_Flags, + &(localDW->MotorSupervisor_InstanceData.rtdw)); +} + +// Enable for referenced model: 'motion_controller' +void motion_controller_Enable(DW_motion_controller_f_T *localDW) +{ + // Enable for ModelReference generated from: '/Control outer' + control_outer_Enable(&(localDW->Controlouter_InstanceData.rtdw)); +} + +// Disable for referenced model: 'motion_controller' +void motion_controller_Disable(DW_motion_controller_f_T *localDW) +{ + // Disable for ModelReference generated from: '/Control outer' + control_outer_Disable(&(localDW->Controlouter_InstanceData.rtb), + &(localDW->Controlouter_InstanceData.rtdw)); +} + +// Output and update for referenced model: 'motion_controller' +void motion_controllerTID0(void) +{ +} + +// Output and update for referenced model: 'motion_controller' +void mc_step_foc(const SensorsData *rtu_SensorData, FOCOutputs *rty_FOCOutputs, + B_motion_controller_c_T *localB, DW_motion_controller_f_T + *localDW) +{ + int8_T wrBufIdx; + + // RateTransition: '/Rate Transition' + rtw_mutex_lock(); + localDW->RateTransition_RDBuf = localDW->RateTransition_LstBufWR; + rtw_mutex_unlock(); + + // RateTransition: '/Rate Transition' + localB->RateTransition = localDW->RateTransition_Buf + [localDW->RateTransition_RDBuf]; + + // ModelReference generated from: '/FOC' + control_foc(rtu_SensorData, + &localB->RateTransition.actuator_configuration.pids.currentPID.P, + &localB->RateTransition.actuator_configuration.pids.currentPID.I, + &localB->RateTransition.actuator_configuration.pids.currentPID.D, + &localB->RateTransition.actuator_configuration.pids.currentPID.N, + &localB->RateTransition.actuator_configuration.motor.Kbemf, + &localB->RateTransition.actuator_configuration.motor.Rphase, + &localB->RateTransition.actuator_configuration.motor.Vmax, + &localB->RateTransition.estimated_data.jointvelocities, + &localB->RateTransition.targets.motorvoltage, + &localB->RateTransition.control_outer_outputs.cur_en, + &localB->RateTransition.control_outer_outputs.out_en, + &localB->RateTransition.control_outer_outputs.pid_reset, + &localB->RateTransition.control_outer_outputs.motorcurrent, + &localB->RateTransition.control_outer_outputs.current_limiter, + rty_FOCOutputs, &(localDW->FOC_InstanceData.rtb), + &(localDW->FOC_InstanceData.rtdw), + &(localDW->FOC_InstanceData.rtzce)); + + // RateTransition: '/Rate Transition2' + rtw_mutex_lock(); + wrBufIdx = static_cast(localDW->RateTransition2_LstBufWR + 1); + if (wrBufIdx == 3) { + wrBufIdx = 0; + } + + if (wrBufIdx == localDW->RateTransition2_RDBuf) { + wrBufIdx = static_cast(wrBufIdx + 1); + if (wrBufIdx == 3) { + wrBufIdx = 0; + } + } + + rtw_mutex_unlock(); + switch (wrBufIdx) { + case 0: + localDW->RateTransition2_Buf0 = *rty_FOCOutputs; + break; + + case 1: + localDW->RateTransition2_Buf1 = *rty_FOCOutputs; + break; + + case 2: + localDW->RateTransition2_Buf2 = *rty_FOCOutputs; + break; + } + + localDW->RateTransition2_LstBufWR = wrBufIdx; + + // End of RateTransition: '/Rate Transition2' + + // RateTransition: '/Rate Transition3' + rtw_mutex_lock(); + wrBufIdx = static_cast(localDW->RateTransition3_LstBufWR + 1); + if (wrBufIdx == 3) { + wrBufIdx = 0; + } + + if (wrBufIdx == localDW->RateTransition3_RDBuf) { + wrBufIdx = static_cast(wrBufIdx + 1); + if (wrBufIdx == 3) { + wrBufIdx = 0; + } + } + + rtw_mutex_unlock(); + switch (wrBufIdx) { + case 0: + localDW->RateTransition3_Buf0 = *rtu_SensorData; + break; + + case 1: + localDW->RateTransition3_Buf1 = *rtu_SensorData; + break; + + case 2: + localDW->RateTransition3_Buf2 = *rtu_SensorData; + break; + } + + localDW->RateTransition3_LstBufWR = wrBufIdx; + + // End of RateTransition: '/Rate Transition3' +} + +// Output and update for referenced model: 'motion_controller' +void mc_step_1ms(const ExternalFlags *rtu_ExternalFlags, const + GlobalConfiguration *rtu_globalConfig, const + SupervisorReceivedEvents rtu_Events[MAX_EVENTS_PER_TICK], const + ActuatorConfiguration *rtu_InitConf, EstimatedData + *rty_EstimatedData, Flags *rty_Flags, ActuatorConfiguration + *rty_ActuatorsConfiguration, B_motion_controller_c_T *localB, + DW_motion_controller_f_T *localDW) +{ + // local block i/o variables + real32_T rtb_velocity; + real32_T rtb_current; + ActuatorConfiguration rtb_BusCreator_actuator_configuration; + ControlOuterOutputs rtb_Controlouter; + int8_T wrBufIdx; + + // RateTransition: '/Rate Transition3' + rtw_mutex_lock(); + localDW->RateTransition3_RDBuf = localDW->RateTransition3_LstBufWR; + rtw_mutex_unlock(); + switch (localDW->RateTransition3_RDBuf) { + case 0: + // RateTransition: '/Rate Transition3' + localB->RateTransition3 = localDW->RateTransition3_Buf0; + break; + + case 1: + // RateTransition: '/Rate Transition3' + localB->RateTransition3 = localDW->RateTransition3_Buf1; + break; + + case 2: + // RateTransition: '/Rate Transition3' + localB->RateTransition3 = localDW->RateTransition3_Buf2; + break; + } + + // End of RateTransition: '/Rate Transition3' + + // ModelReference: '/Velocity Estimator' + estimation_velocity(&localB->RateTransition3, + &rtu_globalConfig->estimation.velocity_est_mode, + &rtb_velocity, + &(localDW->VelocityEstimator_InstanceData.rtdw)); + + // RateTransition: '/Rate Transition2' + rtw_mutex_lock(); + localDW->RateTransition2_RDBuf = localDW->RateTransition2_LstBufWR; + rtw_mutex_unlock(); + switch (localDW->RateTransition2_RDBuf) { + case 0: + // RateTransition: '/Rate Transition2' + localB->RateTransition2 = localDW->RateTransition2_Buf0; + break; + + case 1: + // RateTransition: '/Rate Transition2' + localB->RateTransition2 = localDW->RateTransition2_Buf1; + break; + + case 2: + // RateTransition: '/Rate Transition2' + localB->RateTransition2 = localDW->RateTransition2_Buf2; + break; + } + + // End of RateTransition: '/Rate Transition2' + + // ModelReference: '/Current Filter' + filter_current(&localB->RateTransition2, &rtb_current, + &(localDW->CurrentFilter_InstanceData.rtdw)); + + // BusCreator generated from: '/Motor Supervisor' + localB->BusConversion_InsertedFor_MotorSupervisor_at_inport_1_BusCre.jointvelocities + = rtb_velocity; + localB->BusConversion_InsertedFor_MotorSupervisor_at_inport_1_BusCre.Iq_filtered + = rtb_current; + localB->BusConversion_InsertedFor_MotorSupervisor_at_inport_1_BusCre.motor_temperature + = 0.0F; + + // ModelReference: '/Motor Supervisor' + supervisor(rtu_ExternalFlags, + &localB->BusConversion_InsertedFor_MotorSupervisor_at_inport_1_BusCre, + &localB->RateTransition2, &localB->RateTransition3, &rtu_Events[0], + rtu_InitConf, &localB->targets, rty_ActuatorsConfiguration, + rty_Flags, &(localDW->MotorSupervisor_InstanceData.rtdw)); + + // ModelReference generated from: '/Control outer' + control_outer(rty_Flags, rty_ActuatorsConfiguration, &rtb_velocity, + &rtb_current, &localB->RateTransition3.jointpositions, + &localB->targets.jointpositions, + &localB->targets.jointvelocities, &localB->targets.motorcurrent, + &rtb_Controlouter, &(localDW->Controlouter_InstanceData.rtb), + &(localDW->Controlouter_InstanceData.rtdw), + &(localDW->Controlouter_InstanceData.rtzce)); + + // BusCreator: '/Bus Creator' + rtb_BusCreator_actuator_configuration = *rty_ActuatorsConfiguration; + + // RateTransition: '/Rate Transition' incorporates: + // BusCreator: '/Bus Creator' + // + rtw_mutex_lock(); + wrBufIdx = static_cast(localDW->RateTransition_LstBufWR + 1); + if (wrBufIdx == 3) { + wrBufIdx = 0; + } + + if (wrBufIdx == localDW->RateTransition_RDBuf) { + wrBufIdx = static_cast(wrBufIdx + 1); + if (wrBufIdx == 3) { + wrBufIdx = 0; + } + } + + rtw_mutex_unlock(); + localDW->RateTransition_Buf[wrBufIdx].global_configuration = *rtu_globalConfig; + localDW->RateTransition_Buf[wrBufIdx].actuator_configuration = + rtb_BusCreator_actuator_configuration; + localDW->RateTransition_Buf[wrBufIdx].estimated_data.jointvelocities = + rtb_velocity; + localDW->RateTransition_Buf[wrBufIdx].estimated_data.Iq_filtered = rtb_current; + localDW->RateTransition_Buf[wrBufIdx].estimated_data.motor_temperature = 0.0F; + localDW->RateTransition_Buf[wrBufIdx].targets = localB->targets; + localDW->RateTransition_Buf[wrBufIdx].control_outer_outputs = rtb_Controlouter; + localDW->RateTransition_LstBufWR = wrBufIdx; + + // End of RateTransition: '/Rate Transition' + + // BusCreator generated from: '/EstimatedData' + rty_EstimatedData->jointvelocities = rtb_velocity; + rty_EstimatedData->Iq_filtered = rtb_current; + rty_EstimatedData->motor_temperature = 0.0F; +} + +// Termination for referenced model: 'motion_controller' +void motion_controller_Term(DW_motion_controller_f_T *localDW) +{ + // Terminate for RateTransition: '/Rate Transition' + rtw_mutex_destroy(); + + // Terminate for RateTransition: '/Rate Transition2' + rtw_mutex_destroy(); + + // Terminate for RateTransition: '/Rate Transition3' + rtw_mutex_destroy(); + + // Terminate for ModelReference: '/Current Filter' + filter_current_Term(&(localDW->CurrentFilter_InstanceData.rtdw)); +} + +// Model initialize function +void motion_controller_initialize(DW_motion_controller_f_T *localDW) +{ + // Model Initialize function for ModelReference Block: '/Control outer' + control_outer_initialize(&(localDW->Controlouter_InstanceData.rtzce)); + + // Model Initialize function for ModelReference Block: '/FOC' + control_foc_initialize(&(localDW->FOC_InstanceData.rtzce)); +} + +// +// File trailer for generated code. +// +// [EOF] +// diff --git a/emBODY/eBcode/arch-arm/mbd/amcbldc/motion-controller/motion_controller.h b/emBODY/eBcode/arch-arm/mbd/amcbldc/motion-controller/motion_controller.h new file mode 100644 index 000000000..c56768c5f --- /dev/null +++ b/emBODY/eBcode/arch-arm/mbd/amcbldc/motion-controller/motion_controller.h @@ -0,0 +1,120 @@ +// +// Non-Degree Granting Education License -- for use at non-degree +// granting, nonprofit, education, and research organizations only. Not +// for commercial or industrial use. +// +// File: motion_controller.h +// +// Code generated for Simulink model 'motion_controller'. +// +// Model version : 2.214 +// Simulink Coder version : 23.2 (R2023b) 01-Aug-2023 +// C/C++ source code generated on : Wed Feb 28 11:29:27 2024 +// +// Target selection: ert.tlc +// Embedded hardware selection: ARM Compatible->ARM Cortex-M +// Code generation objectives: Unspecified +// Validation result: Not run +// +#ifndef RTW_HEADER_motion_controller_h_ +#define RTW_HEADER_motion_controller_h_ +#include "rtwtypes.h" +#include "motion_controller_types.h" +#include "control_foc.h" +#include "estimation_velocity.h" +#include "filter_current.h" +#include "supervisor.h" +#include "control_outer.h" +#include "mc_supervisor_defines.h" +#include "zero_crossing_types.h" + +// Block signals for model 'motion_controller' +struct B_motion_controller_c_T { + FOCSlowInputs RateTransition; // '/Rate Transition' + SensorsData RateTransition3; // '/Rate Transition3' + FOCOutputs RateTransition2; // '/Rate Transition2' + Targets targets; // '/Motor Supervisor' + EstimatedData BusConversion_InsertedFor_MotorSupervisor_at_inport_1_BusCre; +}; + +// Block states (default storage) for model 'motion_controller' +struct DW_motion_controller_f_T { + FOCSlowInputs RateTransition_Buf[3]; // '/Rate Transition' + SensorsData RateTransition3_Buf0; // '/Rate Transition3' + SensorsData RateTransition3_Buf1; // '/Rate Transition3' + SensorsData RateTransition3_Buf2; // '/Rate Transition3' + FOCOutputs RateTransition2_Buf0; // '/Rate Transition2' + FOCOutputs RateTransition2_Buf1; // '/Rate Transition2' + FOCOutputs RateTransition2_Buf2; // '/Rate Transition2' + void* RateTransition_SEMAPHORE; // '/Rate Transition' + void* RateTransition2_SEMAPHORE; // '/Rate Transition2' + void* RateTransition3_SEMAPHORE; // '/Rate Transition3' + int8_T RateTransition_LstBufWR; // '/Rate Transition' + int8_T RateTransition_RDBuf; // '/Rate Transition' + int8_T RateTransition2_LstBufWR; // '/Rate Transition2' + int8_T RateTransition2_RDBuf; // '/Rate Transition2' + int8_T RateTransition3_LstBufWR; // '/Rate Transition3' + int8_T RateTransition3_RDBuf; // '/Rate Transition3' + MdlrefDW_control_foc_T FOC_InstanceData;// '/FOC' + MdlrefDW_estimation_velocity_T VelocityEstimator_InstanceData;// '/Velocity Estimator' + MdlrefDW_filter_current_T CurrentFilter_InstanceData;// '/Current Filter' + MdlrefDW_supervisor_T MotorSupervisor_InstanceData;// '/Motor Supervisor' + MdlrefDW_control_outer_T Controlouter_InstanceData;// '/Control outer' +}; + +struct MdlrefDW_motion_controller_T { + B_motion_controller_c_T rtb; + DW_motion_controller_f_T rtdw; +}; + +// Model reference registration function +extern void motion_controller_initialize(DW_motion_controller_f_T *localDW); +extern void motion_controller_Init(Flags *rty_Flags, ActuatorConfiguration + *rty_ActuatorsConfiguration, B_motion_controller_c_T *localB, + DW_motion_controller_f_T *localDW); +extern void motion_controller_Enable(DW_motion_controller_f_T *localDW); +extern void motion_controller_Disable(DW_motion_controller_f_T *localDW); +extern void motion_controllerTID0(void); +extern void mc_step_foc(const SensorsData *rtu_SensorData, FOCOutputs + *rty_FOCOutputs, B_motion_controller_c_T *localB, DW_motion_controller_f_T + *localDW); +extern void mc_step_1ms(const ExternalFlags *rtu_ExternalFlags, const + GlobalConfiguration *rtu_globalConfig, const SupervisorReceivedEvents + rtu_Events[MAX_EVENTS_PER_TICK], const ActuatorConfiguration *rtu_InitConf, + EstimatedData *rty_EstimatedData, Flags *rty_Flags, ActuatorConfiguration + *rty_ActuatorsConfiguration, B_motion_controller_c_T *localB, + DW_motion_controller_f_T *localDW); +extern void motion_controller_Term(DW_motion_controller_f_T *localDW); + +//- +// These blocks were eliminated from the model due to optimizations: +// +// Block '/Rate Transition2' : Eliminated since input and output rates are identical + + +//- +// The generated code includes comments that allow you to trace directly +// back to the appropriate location in the model. The basic format +// is /block_name, where system is the system number (uniquely +// assigned by Simulink) and block_name is the name of the block. +// +// Use the MATLAB hilite_system command to trace the generated code back +// to the model. For example, +// +// hilite_system('') - opens system 3 +// hilite_system('/Kp') - opens and selects block Kp which resides in S3 +// +// Here is the system hierarchy for this model +// +// '' : 'motion_controller' +// '' : 'motion_controller/Estimation' +// '' : 'motion_controller/Estimation/Thermal model' +// '' : 'motion_controller/Estimation/Thermal model/Thermal model OFF' + +#endif // RTW_HEADER_motion_controller_h_ + +// +// File trailer for generated code. +// +// [EOF] +// diff --git a/emBODY/eBcode/arch-arm/mbd/amcbldc/motion-controller/motion_controller_private.h b/emBODY/eBcode/arch-arm/mbd/amcbldc/motion-controller/motion_controller_private.h new file mode 100644 index 000000000..aeeb579f9 --- /dev/null +++ b/emBODY/eBcode/arch-arm/mbd/amcbldc/motion-controller/motion_controller_private.h @@ -0,0 +1,30 @@ +// +// Non-Degree Granting Education License -- for use at non-degree +// granting, nonprofit, education, and research organizations only. Not +// for commercial or industrial use. +// +// File: motion_controller_private.h +// +// Code generated for Simulink model 'motion_controller'. +// +// Model version : 2.214 +// Simulink Coder version : 23.2 (R2023b) 01-Aug-2023 +// C/C++ source code generated on : Wed Feb 28 11:29:27 2024 +// +// Target selection: ert.tlc +// Embedded hardware selection: ARM Compatible->ARM Cortex-M +// Code generation objectives: Unspecified +// Validation result: Not run +// +#ifndef RTW_HEADER_motion_controller_private_h_ +#define RTW_HEADER_motion_controller_private_h_ +#include "rtwtypes.h" +#include "zero_crossing_types.h" +#include "motion_controller_types.h" +#endif // RTW_HEADER_motion_controller_private_h_ + +// +// File trailer for generated code. +// +// [EOF] +// diff --git a/emBODY/eBcode/arch-arm/mbd/amcbldc/motion-controller/motion_controller_types.h b/emBODY/eBcode/arch-arm/mbd/amcbldc/motion-controller/motion_controller_types.h new file mode 100644 index 000000000..c43406f7a --- /dev/null +++ b/emBODY/eBcode/arch-arm/mbd/amcbldc/motion-controller/motion_controller_types.h @@ -0,0 +1,390 @@ +// +// Non-Degree Granting Education License -- for use at non-degree +// granting, nonprofit, education, and research organizations only. Not +// for commercial or industrial use. +// +// File: motion_controller_types.h +// +// Code generated for Simulink model 'motion_controller'. +// +// Model version : 2.214 +// Simulink Coder version : 23.2 (R2023b) 01-Aug-2023 +// C/C++ source code generated on : Wed Feb 28 11:29:27 2024 +// +// Target selection: ert.tlc +// Embedded hardware selection: ARM Compatible->ARM Cortex-M +// Code generation objectives: Unspecified +// Validation result: Not run +// +#ifndef RTW_HEADER_motion_controller_types_h_ +#define RTW_HEADER_motion_controller_types_h_ +#include "rtwtypes.h" + +// Includes for objects with custom storage classes +#include "mc_supervisor_defines.h" + +// +// Registered constraints for dimension variants + +#if MAX_EVENTS_PER_TICK <= 0 +# error "The preprocessor definition 'MAX_EVENTS_PER_TICK' must be greater than '0'" +#endif + +#if MAX_EVENTS_PER_TICK >= 5 +# error "The preprocessor definition 'MAX_EVENTS_PER_TICK' must be less than '5'" +#endif + +#ifndef DEFINED_TYPEDEF_FOR_Thresholds_ +#define DEFINED_TYPEDEF_FOR_Thresholds_ + +struct Thresholds +{ + // Can be only non-negative + real32_T jntVelMax; + + // Current that can be kept for an indefinite period of time w/o damaging the motor + // Expressed in [A] as all the internal computations are done this way + // Can be only non-negative + real32_T motorNominalCurrents; + + // Current that can be applied for a short period of time + // Expressed in [A] as all the internal computations are done this way + // Can be only non-negative + real32_T motorPeakCurrents; + + // Currents over this threshold can instantaneously damages the motor + // Expressed in [A] as all the internal computations are done this way + // Can be only non-negative + real32_T motorOverloadCurrents; + + // Expressed in ticks + // Max value is 32000 + // Can be only non-negative + uint32_T motorPwmLimit; + + // The critical temperature of the motor that triggers i2t current protection. + real32_T motorCriticalTemperature; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_ControlModes_ +#define DEFINED_TYPEDEF_FOR_ControlModes_ + +typedef enum { + ControlModes_NotConfigured = 0, // Default value + ControlModes_Idle, + ControlModes_Position, + ControlModes_PositionDirect, + ControlModes_Current, + ControlModes_Velocity, + ControlModes_Voltage, + ControlModes_HwFaultCM +} ControlModes; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_PID_ +#define DEFINED_TYPEDEF_FOR_PID_ + +struct PID +{ + ControlModes type; + real32_T OutMax; + real32_T OutMin; + real32_T P; + real32_T I; + real32_T D; + real32_T N; + real32_T I0; + real32_T D0; + uint8_T shift_factor; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_PIDsConfiguration_ +#define DEFINED_TYPEDEF_FOR_PIDsConfiguration_ + +struct PIDsConfiguration +{ + PID currentPID; + PID velocityPID; + PID positionPID; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_MotorConfiguration_ +#define DEFINED_TYPEDEF_FOR_MotorConfiguration_ + +struct MotorConfiguration +{ + boolean_T has_hall_sens; + boolean_T has_quadrature_encoder; + boolean_T has_speed_quadrature_encoder; + boolean_T has_torque_sens; + boolean_T use_index; + boolean_T enable_verbosity; + int16_T hall_sensors_offset; + int16_T rotor_encoder_resolution; + int16_T rotor_index_offset; + uint8_T encoder_tolerance; + uint8_T pole_pairs; + real32_T Kbemf; + real32_T Rphase; + real32_T Imin; + real32_T Imax; + real32_T Vcc; + real32_T Vmax; + real32_T resistance; + real32_T inductance; + real32_T thermal_resistance; + real32_T thermal_time_constant; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_ActuatorConfiguration_ +#define DEFINED_TYPEDEF_FOR_ActuatorConfiguration_ + +struct ActuatorConfiguration +{ + Thresholds thresholds; + PIDsConfiguration pids; + MotorConfiguration motor; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_EstimationVelocityModes_ +#define DEFINED_TYPEDEF_FOR_EstimationVelocityModes_ + +typedef enum { + EstimationVelocityModes_Disabled = 0,// Default value + EstimationVelocityModes_MovingAverage, + EstimationVelocityModes_LeastSquares +} EstimationVelocityModes; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_EstimationConfiguration_ +#define DEFINED_TYPEDEF_FOR_EstimationConfiguration_ + +struct EstimationConfiguration +{ + real32_T environment_temperature; + real32_T current_rms_lambda; + EstimationVelocityModes velocity_est_mode; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_GlobalConfiguration_ +#define DEFINED_TYPEDEF_FOR_GlobalConfiguration_ + +struct GlobalConfiguration +{ + EstimationConfiguration estimation; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_EstimatedData_ +#define DEFINED_TYPEDEF_FOR_EstimatedData_ + +struct EstimatedData +{ + // velocity + real32_T jointvelocities; + + // filtered motor current + real32_T Iq_filtered; + + // motor temperature + real32_T motor_temperature; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_DriverSensors_ +#define DEFINED_TYPEDEF_FOR_DriverSensors_ + +struct DriverSensors +{ + real32_T Vcc; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_MotorSensors_ +#define DEFINED_TYPEDEF_FOR_MotorSensors_ + +struct MotorSensors +{ + real32_T Iabc[3]; + + // electrical angle = angle * pole_pairs + real32_T angle; + real32_T temperature; + real32_T voltage; + real32_T current; + uint8_T hallABC; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_SensorsData_ +#define DEFINED_TYPEDEF_FOR_SensorsData_ + +struct SensorsData +{ + // position encoders + real32_T jointpositions; + DriverSensors driversensors; + MotorSensors motorsensors; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_ExternalFlags_ +#define DEFINED_TYPEDEF_FOR_ExternalFlags_ + +struct ExternalFlags +{ + // External Fault Button (1 == pressed) + boolean_T fault_button; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_Targets_ +#define DEFINED_TYPEDEF_FOR_Targets_ + +struct Targets +{ + real32_T jointpositions; + real32_T jointvelocities; + real32_T motorcurrent; + real32_T motorvoltage; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_SupervisorEvents_ +#define DEFINED_TYPEDEF_FOR_SupervisorEvents_ + +typedef enum { + SupervisorEvents_None = 0, // Default value + SupervisorEvents_SetLimit, + SupervisorEvents_SetControlMode, + SupervisorEvents_SetMotorConfig, + SupervisorEvents_SetPid, + SupervisorEvents_SetTarget +} SupervisorEvents; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_SupervisorInputLimits_ +#define DEFINED_TYPEDEF_FOR_SupervisorInputLimits_ + +struct SupervisorInputLimits +{ + real32_T overload; + real32_T peak; + real32_T nominal; + ControlModes type; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_SupervisorReceivedEvents_ +#define DEFINED_TYPEDEF_FOR_SupervisorReceivedEvents_ + +struct SupervisorReceivedEvents +{ + Targets targets_content; + PID pid_content; + SupervisorEvents event_type; + ControlModes control_mode_content; + SupervisorInputLimits limits_content; + MotorConfiguration motor_config_content; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_ControlOuterOutputs_ +#define DEFINED_TYPEDEF_FOR_ControlOuterOutputs_ + +struct ControlOuterOutputs +{ + boolean_T vel_en; + boolean_T cur_en; + boolean_T out_en; + boolean_T pid_reset; + real32_T motorcurrent; + real32_T current_limiter; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_FOCSlowInputs_ +#define DEFINED_TYPEDEF_FOR_FOCSlowInputs_ + +struct FOCSlowInputs +{ + GlobalConfiguration global_configuration; + ActuatorConfiguration actuator_configuration; + EstimatedData estimated_data; + Targets targets; + ControlOuterOutputs control_outer_outputs; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_Flags_ +#define DEFINED_TYPEDEF_FOR_Flags_ + +struct Flags +{ + boolean_T enable_sending_msg_status; + + // control mode + ControlModes control_mode; + boolean_T enable_thermal_protection; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_FOCOutputs_ +#define DEFINED_TYPEDEF_FOR_FOCOutputs_ + +struct FOCOutputs +{ + // control effort (quadrature) + real32_T Vq; + + // control effort (3-phases) + real32_T Vabc[3]; + + // quadrature current + real32_T Iq_fbk; + + // direct current + real32_T Id_fbk; + + // RMS of Iq + real32_T Iq_rms; + + // RMS of Id + real32_T Id_rms; +}; + +#endif +#endif // RTW_HEADER_motion_controller_types_h_ + +// +// File trailer for generated code. +// +// [EOF] +// diff --git a/emBODY/eBcode/arch-arm/mbd/amcbldc/sharedutils/const_params.cpp b/emBODY/eBcode/arch-arm/mbd/amcbldc/sharedutils/const_params.cpp index 8a167d7d5..ceb143f91 100644 --- a/emBODY/eBcode/arch-arm/mbd/amcbldc/sharedutils/const_params.cpp +++ b/emBODY/eBcode/arch-arm/mbd/amcbldc/sharedutils/const_params.cpp @@ -7,9 +7,9 @@ // // Code generation for model "estimation_velocity". // -// Model version : 6.4 +// Model version : 6.19 // Simulink Coder version : 23.2 (R2023b) 01-Aug-2023 -// C++ source code generated on : Mon Jan 15 18:18:33 2024 +// C++ source code generated on : Tue Feb 27 16:24:51 2024 #include "rtwtypes.h" diff --git a/emBODY/eBcode/arch-arm/mbd/amcbldc/sharedutils/mc_supervisor_defines.h b/emBODY/eBcode/arch-arm/mbd/amcbldc/sharedutils/mc_supervisor_defines.h new file mode 100644 index 000000000..66f1597a5 --- /dev/null +++ b/emBODY/eBcode/arch-arm/mbd/amcbldc/sharedutils/mc_supervisor_defines.h @@ -0,0 +1,28 @@ +// +// Non-Degree Granting Education License -- for use at non-degree +// granting, nonprofit, education, and research organizations only. Not +// for commercial or industrial use. +// +// File: mc_supervisor_defines.h +// +// Code generated for Simulink model 'can_decoder'. +// +// Model version : 6.40 +// Simulink Coder version : 23.2 (R2023b) 01-Aug-2023 +// C/C++ source code generated on : Tue Feb 27 16:23:49 2024 +// + +#ifndef RTW_HEADER_mc_supervisor_defines_h_ +#define RTW_HEADER_mc_supervisor_defines_h_ +#include "rtwtypes.h" + +// Exported data define +// Definition for custom storage class: Define +#define MAX_EVENTS_PER_TICK 4 +#endif // RTW_HEADER_mc_supervisor_defines_h_ + +// +// File trailer for generated code. +// +// [EOF] +// diff --git a/emBODY/eBcode/arch-arm/mbd/amcbldc/sharedutils/rt_defines.h b/emBODY/eBcode/arch-arm/mbd/amcbldc/sharedutils/rt_defines.h new file mode 100644 index 000000000..398b2c36c --- /dev/null +++ b/emBODY/eBcode/arch-arm/mbd/amcbldc/sharedutils/rt_defines.h @@ -0,0 +1,52 @@ +// +// Non-Degree Granting Education License -- for use at non-degree +// granting, nonprofit, education, and research organizations only. Not +// for commercial or industrial use. +// +// File: rt_defines.h +// +// Code generated for Simulink model 'AMC_BLDC'. +// +// Model version : 7.53 +// Simulink Coder version : 23.2 (R2023b) 01-Aug-2023 +// C/C++ source code generated on : Tue Feb 27 16:29:04 2024 +// +#ifndef RTW_HEADER_rt_defines_h_ +#define RTW_HEADER_rt_defines_h_ + +//===========* +// Constants * +// =========== +#define RT_PI 3.14159265358979323846 +#define RT_PIF 3.1415927F +#define RT_LN_10 2.30258509299404568402 +#define RT_LN_10F 2.3025851F +#define RT_LOG10E 0.43429448190325182765 +#define RT_LOG10EF 0.43429449F +#define RT_E 2.7182818284590452354 +#define RT_EF 2.7182817F + +// +// UNUSED_PARAMETER(x) +// Used to specify that a function parameter (argument) is required but not +// accessed by the function body. + +#ifndef UNUSED_PARAMETER +#if defined(__LCC__) +#define UNUSED_PARAMETER(x) // do nothing +#else + +// +// This is the semi-ANSI standard way of indicating that an +// unused function parameter is required. + +#define UNUSED_PARAMETER(x) (void) (x) +#endif +#endif +#endif // RTW_HEADER_rt_defines_h_ + +// +// File trailer for generated code. +// +// [EOF] +// diff --git a/emBODY/eBcode/arch-arm/mbd/amcbldc/sharedutils/rt_hypotf.cpp b/emBODY/eBcode/arch-arm/mbd/amcbldc/sharedutils/rt_hypotf.cpp new file mode 100644 index 000000000..5acb74be5 --- /dev/null +++ b/emBODY/eBcode/arch-arm/mbd/amcbldc/sharedutils/rt_hypotf.cpp @@ -0,0 +1,46 @@ +// +// Non-Degree Granting Education License -- for use at non-degree +// granting, nonprofit, education, and research organizations only. Not +// for commercial or industrial use. +// +// File: rt_hypotf.cpp +// +// Code generated for Simulink model 'estimation_velocity'. +// +// Model version : 6.19 +// Simulink Coder version : 23.2 (R2023b) 01-Aug-2023 +// C/C++ source code generated on : Tue Feb 27 16:24:51 2024 +// +#include "rtwtypes.h" +#include "rt_hypotf.h" +#include +#include "mw_cmsis.h" + +real32_T rt_hypotf(real32_T u0, real32_T u1) +{ + real32_T a; + real32_T b; + real32_T tmp; + real32_T y; + a = std::abs(u0); + b = std::abs(u1); + if (a < b) { + a /= b; + mw_arm_sqrt_f32(a * a + 1.0F, &tmp); + y = tmp * b; + } else if (a > b) { + b /= a; + mw_arm_sqrt_f32(b * b + 1.0F, &tmp); + y = tmp * a; + } else { + y = a * 1.41421354F; + } + + return y; +} + +// +// File trailer for generated code. +// +// [EOF] +// diff --git a/emBODY/eBcode/arch-arm/mbd/amcbldc/sharedutils/rt_hypotf.h b/emBODY/eBcode/arch-arm/mbd/amcbldc/sharedutils/rt_hypotf.h new file mode 100644 index 000000000..f75b1a3de --- /dev/null +++ b/emBODY/eBcode/arch-arm/mbd/amcbldc/sharedutils/rt_hypotf.h @@ -0,0 +1,26 @@ +// +// Non-Degree Granting Education License -- for use at non-degree +// granting, nonprofit, education, and research organizations only. Not +// for commercial or industrial use. +// +// File: rt_hypotf.h +// +// Code generated for Simulink model 'estimation_velocity'. +// +// Model version : 6.19 +// Simulink Coder version : 23.2 (R2023b) 01-Aug-2023 +// C/C++ source code generated on : Tue Feb 27 16:24:51 2024 +// +#ifndef RTW_HEADER_rt_hypotf_h_ +#define RTW_HEADER_rt_hypotf_h_ +#include "rtwtypes.h" + +extern real32_T rt_hypotf(real32_T u0, real32_T u1); + +#endif // RTW_HEADER_rt_hypotf_h_ + +// +// File trailer for generated code. +// +// [EOF] +// diff --git a/emBODY/eBcode/arch-arm/mbd/amcbldc/sharedutils/rtw_defines.h b/emBODY/eBcode/arch-arm/mbd/amcbldc/sharedutils/rtw_defines.h index b0484f4e5..1fa9c2471 100644 --- a/emBODY/eBcode/arch-arm/mbd/amcbldc/sharedutils/rtw_defines.h +++ b/emBODY/eBcode/arch-arm/mbd/amcbldc/sharedutils/rtw_defines.h @@ -5,11 +5,11 @@ // // File: rtw_defines.h // -// Code generated for Simulink model 'SupervisorFSM_RX'. +// Code generated for Simulink model 'can_decoder'. // -// Model version : 7.5 +// Model version : 6.40 // Simulink Coder version : 23.2 (R2023b) 01-Aug-2023 -// C/C++ source code generated on : Mon Jan 15 18:17:08 2024 +// C/C++ source code generated on : Tue Feb 27 16:23:49 2024 // #ifndef RTW_HEADER_rtw_defines_h_ diff --git a/emBODY/eBcode/arch-arm/mbd/amcbldc/sharedutils/rtwtypes.h b/emBODY/eBcode/arch-arm/mbd/amcbldc/sharedutils/rtwtypes.h index 51f5e0c47..d8c86222e 100644 --- a/emBODY/eBcode/arch-arm/mbd/amcbldc/sharedutils/rtwtypes.h +++ b/emBODY/eBcode/arch-arm/mbd/amcbldc/sharedutils/rtwtypes.h @@ -5,11 +5,11 @@ // // File: rtwtypes.h // -// Code generated for Simulink model 'SupervisorFSM_RX'. +// Code generated for Simulink model 'SupervisorFSM_TX'. // -// Model version : 7.5 +// Model version : 7.10 // Simulink Coder version : 23.2 (R2023b) 01-Aug-2023 -// C/C++ source code generated on : Mon Jan 15 18:17:08 2024 +// C/C++ source code generated on : Tue Feb 27 16:23:34 2024 // #ifndef RTWTYPES_H #define RTWTYPES_H diff --git a/emBODY/eBcode/arch-arm/mbd/amcbldc/sharedutils/zero_crossing_types.h b/emBODY/eBcode/arch-arm/mbd/amcbldc/sharedutils/zero_crossing_types.h index 06e5c9eb2..9e0ddd955 100644 --- a/emBODY/eBcode/arch-arm/mbd/amcbldc/sharedutils/zero_crossing_types.h +++ b/emBODY/eBcode/arch-arm/mbd/amcbldc/sharedutils/zero_crossing_types.h @@ -7,9 +7,9 @@ // // Code generated for Simulink model 'control_foc'. // -// Model version : 6.16 +// Model version : 6.68 // Simulink Coder version : 23.2 (R2023b) 01-Aug-2023 -// C/C++ source code generated on : Mon Jan 15 18:18:06 2024 +// C/C++ source code generated on : Tue Feb 27 16:24:10 2024 // #ifndef ZERO_CROSSING_TYPES_H #define ZERO_CROSSING_TYPES_H diff --git a/emBODY/eBcode/arch-arm/mbd/amcbldc/supervisor-rx/SupervisorFSM_RX.cpp b/emBODY/eBcode/arch-arm/mbd/amcbldc/supervisor-rx/SupervisorFSM_RX.cpp deleted file mode 100644 index 3e41eb6ad..000000000 --- a/emBODY/eBcode/arch-arm/mbd/amcbldc/supervisor-rx/SupervisorFSM_RX.cpp +++ /dev/null @@ -1,1436 +0,0 @@ -// -// Non-Degree Granting Education License -- for use at non-degree -// granting, nonprofit, education, and research organizations only. Not -// for commercial or industrial use. -// -// File: SupervisorFSM_RX.cpp -// -// Code generated for Simulink model 'SupervisorFSM_RX'. -// -// Model version : 7.5 -// Simulink Coder version : 23.2 (R2023b) 01-Aug-2023 -// C/C++ source code generated on : Tue Feb 13 11:54:08 2024 -// -// Target selection: ert.tlc -// Embedded hardware selection: ARM Compatible->ARM Cortex-M -// Code generation objectives: Unspecified -// Validation result: Not run -// -#include "SupervisorFSM_RX.h" -#include "rtwtypes.h" -#include "SupervisorFSM_RX_types.h" -#include "SupervisorFSM_RX_private.h" -#include -#include "rt_roundd_snf.h" -#include "rt_roundf_snf.h" -#include "rtw_defines.h" - -// Named constants for Chart: '/ControlMode_SM_motor0' -const uint8_T SupervisorFSM_RX_IN_Current = 1U; -const uint8_T SupervisorFSM_RX_IN_HWFault = 2U; -const uint8_T SupervisorFSM_RX_IN_Idle = 3U; -const uint8_T SupervisorFSM_RX_IN_NotConfigured = 4U; -const uint8_T SupervisorFSM_RX_IN_Position = 5U; -const uint8_T SupervisorFSM_RX_IN_Velocity = 6U; -const uint8_T SupervisorFSM_RX_IN_Voltage = 7U; - -// Named constants for Chart: '/SupervisorRX State Handler' -const uint8_T SupervisorFSM_RX_IN_ButtonPressed = 1U; -const uint8_T SupervisorFSM_RX_IN_Configured = 1U; -const uint8_T SupervisorFSM_RX_IN_Fault = 2U; -const uint8_T SupervisorFSM_RX_IN_LimitNonConfigured = 1U; -const uint8_T SupervisorFSM_RX_IN_NoFault = 2U; -const uint8_T SupervisorFSM_RX_IN_NotConfigured_j = 3U; -const uint8_T SupervisorFSM_RX_IN_OverCurrentFault = 3U; -const uint8_T SupervisorFSM_RX_IN_state1 = 1U; -MdlrefDW_SupervisorFSM_RX_T SupervisorFSM_RX_MdlrefDW; - -// Block signals (default storage) -B_SupervisorFSM_RX_c_T SupervisorFSM_RX_B; - -// Block states (default storage) -DW_SupervisorFSM_RX_f_T SupervisorFSM_RX_DW; - -// Forward declaration for local functions -static boolean_T SupervisorFSM_RX_CheckSetPointVoltage(void); -static boolean_T SupervisorFSM_RX_CheckSetPointCurrent(void); -static boolean_T SupervisorFSM_RX_CheckSetPointVelocity(void); -static boolean_T SupervisorFSM_RX_CheckSetPointPosition(void); - -// Forward declaration for local functions -static boolean_T SupervisorFSM_RX_IsBoardReady(void); -static boolean_T SupervisorFSM_RX_isConfigurationSet(void); -static boolean_T SupervisorFSM_RX_IsNewCtrl_Velocity(void); -static boolean_T SupervisorFSM_RX_IsNewCtrl_Voltage(void); -static boolean_T SupervisorFSM_RX_IsNewCtrl_Current(void); -static boolean_T SupervisorFSM_RX_IsNewCtrl_Position(void); -static boolean_T SupervisorFSM_RX_IsNewCtrl_Idle(void); -static void SupervisorFSM_RX_Voltage(void); - -// Forward declaration for local functions -static ControlModes SupervisorFSM_RX_convert(MCControlModes mccontrolmode); -static void SupervisorFSM_RX_formatMotorConfig(const BUS_MESSAGES_RX *Selector2); -static void SupervisorFSM_RX_hardwareConfigMotor(void); - -// Forward declaration for local functions -static boolean_T SupervisorFSM_RX_isBoardConfigured(void); - -// Declare global variables for system: model 'SupervisorFSM_RX' -const SensorsData *SupervisorFSM_RX_rtu_SensorsData;// '/SensorsData' -const ControlOutputs *SupervisorFSM_RX_rtu_ControlOutputs;// '/ControlOutputs' -const BUS_MESSAGES_RX_MULTIPLE *SupervisorFSM_RX_rtu_MessagesRx;// '/MessagesRx' -const BUS_STATUS_RX_MULTIPLE *SupervisorFSM_RX_rtu_StatusRx;// '/StatusRx' -const BUS_CAN_RX_ERRORS_MULTIPLE *SupervisorFSM_RX_rtu_ErrorsRx;// '/ErrorsRx' - -// Function for Chart: '/Chart1' -static boolean_T SupervisorFSM_RX_CheckSetPointVoltage(void) -{ - return SupervisorFSM_RX_B.controlModeDefined == ControlModes_Voltage; -} - -// Function for Chart: '/Chart1' -static boolean_T SupervisorFSM_RX_CheckSetPointCurrent(void) -{ - return SupervisorFSM_RX_B.controlModeDefined == ControlModes_Current; -} - -// Function for Chart: '/Chart1' -static boolean_T SupervisorFSM_RX_CheckSetPointVelocity(void) -{ - return SupervisorFSM_RX_B.controlModeDefined == ControlModes_Velocity; -} - -// Function for Chart: '/Chart1' -static boolean_T SupervisorFSM_RX_CheckSetPointPosition(void) -{ - return SupervisorFSM_RX_B.controlModeDefined == ControlModes_Position; -} - -// Output and update for function-call system: '/SetpointHandler' -void SupervisorFSM_RX_SetpointHandler(void) -{ - real32_T newSetpoint; - - // Chart: '/Chart1' - if (SupervisorFSM_RX_DW.is_active_c4_SupervisorFSM_RX == 0U) { - SupervisorFSM_RX_DW.is_active_c4_SupervisorFSM_RX = 1U; - if (SupervisorFSM_RX_B.receivedNewSetpoint) { - if (SupervisorFSM_RX_CheckSetPointVoltage()) { - newSetpoint = SupervisorFSM_RX_B.newSetpoint.voltage; - } else if (SupervisorFSM_RX_CheckSetPointCurrent()) { - newSetpoint = SupervisorFSM_RX_B.newSetpoint.current; - } else if (SupervisorFSM_RX_CheckSetPointVelocity()) { - newSetpoint = SupervisorFSM_RX_B.newSetpoint.velocity; - } else { - newSetpoint = 0.0F; - } - } else { - newSetpoint = SupervisorFSM_RX_B.newSetpoint_g; - } - - SupervisorFSM_RX_B.targets.motorvoltage.voltage = 0.0F; - SupervisorFSM_RX_B.targets.motorcurrent.current = 0.0F; - SupervisorFSM_RX_B.targets.jointvelocities.velocity = 0.0F; - SupervisorFSM_RX_B.targets.jointpositions.position = 0.0F; - if (SupervisorFSM_RX_CheckSetPointCurrent()) { - SupervisorFSM_RX_B.targets.motorcurrent.current = newSetpoint; - SupervisorFSM_RX_B.hasSetpointChanged = true; - } else if (SupervisorFSM_RX_CheckSetPointVoltage()) { - SupervisorFSM_RX_B.targets.motorvoltage.voltage = newSetpoint; - SupervisorFSM_RX_B.hasSetpointChanged = true; - } else if (SupervisorFSM_RX_CheckSetPointVelocity()) { - SupervisorFSM_RX_B.targets.jointvelocities.velocity = newSetpoint; - SupervisorFSM_RX_B.hasSetpointChanged = true; - } else if (SupervisorFSM_RX_CheckSetPointPosition()) { - SupervisorFSM_RX_B.targets.jointpositions.position = newSetpoint; - SupervisorFSM_RX_B.hasSetpointChanged = true; - } - } else { - if (SupervisorFSM_RX_B.receivedNewSetpoint) { - if (SupervisorFSM_RX_CheckSetPointVoltage()) { - newSetpoint = SupervisorFSM_RX_B.newSetpoint.voltage; - } else if (SupervisorFSM_RX_CheckSetPointCurrent()) { - newSetpoint = SupervisorFSM_RX_B.newSetpoint.current; - } else if (SupervisorFSM_RX_CheckSetPointVelocity()) { - newSetpoint = SupervisorFSM_RX_B.newSetpoint.velocity; - } else { - newSetpoint = 0.0F; - } - } else { - newSetpoint = SupervisorFSM_RX_B.newSetpoint_g; - } - - SupervisorFSM_RX_B.targets.motorvoltage.voltage = 0.0F; - SupervisorFSM_RX_B.targets.motorcurrent.current = 0.0F; - SupervisorFSM_RX_B.targets.jointvelocities.velocity = 0.0F; - SupervisorFSM_RX_B.targets.jointpositions.position = 0.0F; - if (SupervisorFSM_RX_CheckSetPointCurrent()) { - SupervisorFSM_RX_B.targets.motorcurrent.current = newSetpoint; - SupervisorFSM_RX_B.hasSetpointChanged = true; - } else if (SupervisorFSM_RX_CheckSetPointVoltage()) { - SupervisorFSM_RX_B.targets.motorvoltage.voltage = newSetpoint; - SupervisorFSM_RX_B.hasSetpointChanged = true; - } else if (SupervisorFSM_RX_CheckSetPointVelocity()) { - SupervisorFSM_RX_B.targets.jointvelocities.velocity = newSetpoint; - SupervisorFSM_RX_B.hasSetpointChanged = true; - } else if (SupervisorFSM_RX_CheckSetPointPosition()) { - SupervisorFSM_RX_B.targets.jointpositions.position = newSetpoint; - SupervisorFSM_RX_B.hasSetpointChanged = true; - } - } - - // End of Chart: '/Chart1' -} - -// Function for Chart: '/ControlMode_SM_motor0' -static boolean_T SupervisorFSM_RX_IsBoardReady(void) -{ - return SupervisorFSM_RX_B.BoardSt == BoardState_Configured; -} - -// Function for Chart: '/ControlMode_SM_motor0' -static boolean_T SupervisorFSM_RX_isConfigurationSet(void) -{ - return SupervisorFSM_RX_B.areLimitsSet; -} - -// Function for Chart: '/ControlMode_SM_motor0' -static boolean_T SupervisorFSM_RX_IsNewCtrl_Velocity(void) -{ - return SupervisorFSM_RX_B.requiredControlMode == ControlModes_Velocity; -} - -// Function for Chart: '/ControlMode_SM_motor0' -static boolean_T SupervisorFSM_RX_IsNewCtrl_Voltage(void) -{ - return SupervisorFSM_RX_B.requiredControlMode == ControlModes_Voltage; -} - -// Function for Chart: '/ControlMode_SM_motor0' -static boolean_T SupervisorFSM_RX_IsNewCtrl_Current(void) -{ - return SupervisorFSM_RX_B.requiredControlMode == ControlModes_Current; -} - -// Function for Chart: '/ControlMode_SM_motor0' -static boolean_T SupervisorFSM_RX_IsNewCtrl_Position(void) -{ - return SupervisorFSM_RX_B.requiredControlMode == ControlModes_Position; -} - -// Function for Chart: '/ControlMode_SM_motor0' -static boolean_T SupervisorFSM_RX_IsNewCtrl_Idle(void) -{ - return SupervisorFSM_RX_B.requiredControlMode == ControlModes_Idle; -} - -// Function for Chart: '/ControlMode_SM_motor0' -static void SupervisorFSM_RX_Voltage(void) -{ - boolean_T guard1; - boolean_T guard2; - boolean_T guard3; - SupervisorFSM_RX_B.controlModeDefined = ControlModes_Voltage; - guard1 = false; - guard2 = false; - guard3 = false; - if (SupervisorFSM_RX_B.isInOverCurrent || - SupervisorFSM_RX_B.isFaultButtonPressed) { - if (SupervisorFSM_RX_B.isInOverCurrent) { - SupervisorFSM_RX_DW.is_c12_SupervisorFSM_RX = SupervisorFSM_RX_IN_HWFault; - SupervisorFSM_RX_B.controlModeDefined = ControlModes_HwFaultCM; - rtw_disableMotor(); - - // Outputs for Function Call SubSystem: '/SetpointHandler' - // this updates the targets value - SupervisorFSM_RX_SetpointHandler(); - - // End of Outputs for SubSystem: '/SetpointHandler' - } else if (SupervisorFSM_RX_B.isFaultButtonPressed) { - guard1 = true; - } else { - guard3 = true; - } - } else { - guard3 = true; - } - - if (guard3) { - if (SupervisorFSM_RX_IsNewCtrl_Velocity()) { - if ((!SupervisorFSM_RX_IsNewCtrl_Voltage()) && - SupervisorFSM_RX_IsNewCtrl_Velocity()) { - SupervisorFSM_RX_DW.is_c12_SupervisorFSM_RX = - SupervisorFSM_RX_IN_Velocity; - SupervisorFSM_RX_B.controlModeDefined = ControlModes_Velocity; - SupervisorFSM_RX_B.newSetpoint_g = 0.0F; - - // Outputs for Function Call SubSystem: '/SetpointHandler' - SupervisorFSM_RX_SetpointHandler(); - - // End of Outputs for SubSystem: '/SetpointHandler' - } else { - guard2 = true; - } - } else if (!SupervisorFSM_RX_IsNewCtrl_Voltage()) { - if (SupervisorFSM_RX_IsNewCtrl_Current()) { - // Chart: '/ControlMode_SM_motor0' - SupervisorFSM_RX_B.newSetpoint_g = - SupervisorFSM_RX_rtu_ControlOutputs->Iq_fbk.current; - SupervisorFSM_RX_DW.is_c12_SupervisorFSM_RX = - SupervisorFSM_RX_IN_Current; - SupervisorFSM_RX_B.controlModeDefined = ControlModes_Current; - - // Outputs for Function Call SubSystem: '/SetpointHandler' - SupervisorFSM_RX_SetpointHandler(); - - // End of Outputs for SubSystem: '/SetpointHandler' - } else if (SupervisorFSM_RX_IsNewCtrl_Position()) { - SupervisorFSM_RX_DW.is_c12_SupervisorFSM_RX = - SupervisorFSM_RX_IN_Position; - SupervisorFSM_RX_B.controlModeDefined = ControlModes_Position; - - // Chart: '/ControlMode_SM_motor0' - SupervisorFSM_RX_B.newSetpoint_g = - SupervisorFSM_RX_rtu_SensorsData->jointpositions.position; - - // Outputs for Function Call SubSystem: '/SetpointHandler' - SupervisorFSM_RX_SetpointHandler(); - - // End of Outputs for SubSystem: '/SetpointHandler' - } else if (SupervisorFSM_RX_IsNewCtrl_Idle()) { - guard1 = true; - } else { - guard2 = true; - } - } else { - guard2 = true; - } - } - - if (guard2) { - SupervisorFSM_RX_DW.is_c12_SupervisorFSM_RX = SupervisorFSM_RX_IN_Voltage; - SupervisorFSM_RX_B.controlModeDefined = ControlModes_Voltage; - - // Outputs for Function Call SubSystem: '/SetpointHandler' - SupervisorFSM_RX_SetpointHandler(); - - // End of Outputs for SubSystem: '/SetpointHandler' - } - - if (guard1) { - SupervisorFSM_RX_DW.is_c12_SupervisorFSM_RX = SupervisorFSM_RX_IN_Idle; - SupervisorFSM_RX_B.controlModeDefined = ControlModes_Idle; - rtw_disableMotor(); - - // Outputs for Function Call SubSystem: '/SetpointHandler' - // this updates the targets value - SupervisorFSM_RX_SetpointHandler(); - - // End of Outputs for SubSystem: '/SetpointHandler' - } -} - -// Output and update for function-call system: '/Control Mode Handler Motor 0' -void SupervisorFSM_RX_ControlModeHandlerMotor0(void) -{ - boolean_T guard1; - boolean_T guard10; - boolean_T guard11; - boolean_T guard2; - boolean_T guard3; - boolean_T guard4; - boolean_T guard5; - boolean_T guard6; - boolean_T guard7; - boolean_T guard8; - boolean_T guard9; - boolean_T tmp; - - // Chart: '/ControlMode_SM_motor0' - if (SupervisorFSM_RX_DW.is_active_c12_SupervisorFSM_RX == 0U) { - SupervisorFSM_RX_DW.is_active_c12_SupervisorFSM_RX = 1U; - if (SupervisorFSM_RX_IsBoardReady() && SupervisorFSM_RX_isConfigurationSet()) - { - SupervisorFSM_RX_DW.is_c12_SupervisorFSM_RX = SupervisorFSM_RX_IN_Idle; - SupervisorFSM_RX_B.controlModeDefined = ControlModes_Idle; - rtw_disableMotor(); - - // Outputs for Function Call SubSystem: '/SetpointHandler' - // this updates the targets value - SupervisorFSM_RX_SetpointHandler(); - - // End of Outputs for SubSystem: '/SetpointHandler' - } else { - SupervisorFSM_RX_DW.is_c12_SupervisorFSM_RX = - SupervisorFSM_RX_IN_NotConfigured; - SupervisorFSM_RX_B.controlModeDefined = ControlModes_NotConfigured; - rtw_disableMotor(); - } - } else { - guard1 = false; - guard2 = false; - guard3 = false; - guard4 = false; - guard5 = false; - guard6 = false; - guard7 = false; - guard8 = false; - guard9 = false; - guard10 = false; - guard11 = false; - switch (SupervisorFSM_RX_DW.is_c12_SupervisorFSM_RX) { - case SupervisorFSM_RX_IN_Current: - SupervisorFSM_RX_B.controlModeDefined = ControlModes_Current; - if (SupervisorFSM_RX_B.isInOverCurrent || - SupervisorFSM_RX_B.isFaultButtonPressed) { - if (SupervisorFSM_RX_B.isInOverCurrent) { - SupervisorFSM_RX_DW.is_c12_SupervisorFSM_RX = - SupervisorFSM_RX_IN_HWFault; - SupervisorFSM_RX_B.controlModeDefined = ControlModes_HwFaultCM; - rtw_disableMotor(); - - // Outputs for Function Call SubSystem: '/SetpointHandler' - // this updates the targets value - SupervisorFSM_RX_SetpointHandler(); - - // End of Outputs for SubSystem: '/SetpointHandler' - } else if (SupervisorFSM_RX_B.isFaultButtonPressed) { - guard1 = true; - } else { - guard7 = true; - } - } else { - guard7 = true; - } - break; - - case SupervisorFSM_RX_IN_HWFault: - SupervisorFSM_RX_B.controlModeDefined = ControlModes_HwFaultCM; - tmp = !SupervisorFSM_RX_B.isInOverCurrent; - if (tmp && SupervisorFSM_RX_IsNewCtrl_Idle() && - (!SupervisorFSM_RX_isConfigurationSet())) { - SupervisorFSM_RX_DW.is_c12_SupervisorFSM_RX = - SupervisorFSM_RX_IN_NotConfigured; - SupervisorFSM_RX_B.controlModeDefined = ControlModes_NotConfigured; - rtw_disableMotor(); - } else if (tmp && SupervisorFSM_RX_IsNewCtrl_Idle()) { - SupervisorFSM_RX_DW.is_c12_SupervisorFSM_RX = SupervisorFSM_RX_IN_Idle; - SupervisorFSM_RX_B.controlModeDefined = ControlModes_Idle; - rtw_disableMotor(); - - // Outputs for Function Call SubSystem: '/SetpointHandler' - // this updates the targets value - SupervisorFSM_RX_SetpointHandler(); - - // End of Outputs for SubSystem: '/SetpointHandler' - } - break; - - case SupervisorFSM_RX_IN_Idle: - SupervisorFSM_RX_B.controlModeDefined = ControlModes_Idle; - if (SupervisorFSM_RX_B.isInOverCurrent || - SupervisorFSM_RX_B.isFaultButtonPressed) { - if (SupervisorFSM_RX_B.isInOverCurrent) { - SupervisorFSM_RX_DW.is_c12_SupervisorFSM_RX = - SupervisorFSM_RX_IN_HWFault; - SupervisorFSM_RX_B.controlModeDefined = ControlModes_HwFaultCM; - rtw_disableMotor(); - - // Outputs for Function Call SubSystem: '/SetpointHandler' - // this updates the targets value - SupervisorFSM_RX_SetpointHandler(); - - // End of Outputs for SubSystem: '/SetpointHandler' - } else if (SupervisorFSM_RX_B.isFaultButtonPressed) { - guard3 = true; - } else { - guard8 = true; - } - } else { - guard8 = true; - } - break; - - case SupervisorFSM_RX_IN_NotConfigured: - SupervisorFSM_RX_B.controlModeDefined = ControlModes_NotConfigured; - if (SupervisorFSM_RX_IsBoardReady() && SupervisorFSM_RX_isConfigurationSet - ()) { - SupervisorFSM_RX_DW.is_c12_SupervisorFSM_RX = SupervisorFSM_RX_IN_Idle; - SupervisorFSM_RX_B.controlModeDefined = ControlModes_Idle; - rtw_disableMotor(); - - // Outputs for Function Call SubSystem: '/SetpointHandler' - // this updates the targets value - SupervisorFSM_RX_SetpointHandler(); - - // End of Outputs for SubSystem: '/SetpointHandler' - } else if (SupervisorFSM_RX_B.isInOverCurrent) { - SupervisorFSM_RX_DW.is_c12_SupervisorFSM_RX = - SupervisorFSM_RX_IN_HWFault; - SupervisorFSM_RX_B.controlModeDefined = ControlModes_HwFaultCM; - rtw_disableMotor(); - - // Outputs for Function Call SubSystem: '/SetpointHandler' - // this updates the targets value - SupervisorFSM_RX_SetpointHandler(); - - // End of Outputs for SubSystem: '/SetpointHandler' - } - break; - - case SupervisorFSM_RX_IN_Position: - SupervisorFSM_RX_B.controlModeDefined = ControlModes_Position; - if (SupervisorFSM_RX_B.isInOverCurrent || - SupervisorFSM_RX_B.isFaultButtonPressed) { - if (SupervisorFSM_RX_B.isInOverCurrent) { - SupervisorFSM_RX_DW.is_c12_SupervisorFSM_RX = - SupervisorFSM_RX_IN_HWFault; - SupervisorFSM_RX_B.controlModeDefined = ControlModes_HwFaultCM; - rtw_disableMotor(); - - // Outputs for Function Call SubSystem: '/SetpointHandler' - // this updates the targets value - SupervisorFSM_RX_SetpointHandler(); - - // End of Outputs for SubSystem: '/SetpointHandler' - } else if (SupervisorFSM_RX_B.isFaultButtonPressed) { - guard4 = true; - } else { - guard11 = true; - } - } else { - guard11 = true; - } - break; - - case SupervisorFSM_RX_IN_Velocity: - SupervisorFSM_RX_B.controlModeDefined = ControlModes_Velocity; - if (SupervisorFSM_RX_B.isInOverCurrent || - SupervisorFSM_RX_B.isFaultButtonPressed) { - if (SupervisorFSM_RX_B.isInOverCurrent) { - SupervisorFSM_RX_DW.is_c12_SupervisorFSM_RX = - SupervisorFSM_RX_IN_HWFault; - SupervisorFSM_RX_B.controlModeDefined = ControlModes_HwFaultCM; - rtw_disableMotor(); - - // Outputs for Function Call SubSystem: '/SetpointHandler' - // this updates the targets value - SupervisorFSM_RX_SetpointHandler(); - - // End of Outputs for SubSystem: '/SetpointHandler' - } else if (SupervisorFSM_RX_B.isFaultButtonPressed) { - guard5 = true; - } else { - guard10 = true; - } - } else { - guard10 = true; - } - break; - - default: - // case IN_Voltage: - SupervisorFSM_RX_Voltage(); - break; - } - - if (guard11) { - if ((!SupervisorFSM_RX_IsNewCtrl_Idle()) && - (!SupervisorFSM_RX_IsNewCtrl_Position())) { - if (SupervisorFSM_RX_IsNewCtrl_Current()) { - SupervisorFSM_RX_B.newSetpoint_g = - SupervisorFSM_RX_rtu_ControlOutputs->Iq_fbk.current; - SupervisorFSM_RX_DW.is_c12_SupervisorFSM_RX = - SupervisorFSM_RX_IN_Current; - SupervisorFSM_RX_B.controlModeDefined = ControlModes_Current; - - // Outputs for Function Call SubSystem: '/SetpointHandler' - SupervisorFSM_RX_SetpointHandler(); - - // End of Outputs for SubSystem: '/SetpointHandler' - } else if (SupervisorFSM_RX_IsNewCtrl_Voltage()) { - SupervisorFSM_RX_B.newSetpoint_g = - SupervisorFSM_RX_rtu_ControlOutputs->Vq; - SupervisorFSM_RX_DW.is_c12_SupervisorFSM_RX = - SupervisorFSM_RX_IN_Voltage; - SupervisorFSM_RX_B.controlModeDefined = ControlModes_Voltage; - - // Outputs for Function Call SubSystem: '/SetpointHandler' - SupervisorFSM_RX_SetpointHandler(); - - // End of Outputs for SubSystem: '/SetpointHandler' - } else if (SupervisorFSM_RX_IsNewCtrl_Velocity()) { - SupervisorFSM_RX_DW.is_c12_SupervisorFSM_RX = - SupervisorFSM_RX_IN_Velocity; - SupervisorFSM_RX_B.controlModeDefined = ControlModes_Velocity; - SupervisorFSM_RX_B.newSetpoint_g = 0.0F; - - // Outputs for Function Call SubSystem: '/SetpointHandler' - SupervisorFSM_RX_SetpointHandler(); - - // End of Outputs for SubSystem: '/SetpointHandler' - } else { - guard9 = true; - } - } else { - guard9 = true; - } - } - - if (guard10) { - if (!SupervisorFSM_RX_IsNewCtrl_Velocity()) { - if (SupervisorFSM_RX_IsNewCtrl_Voltage()) { - SupervisorFSM_RX_B.newSetpoint_g = - SupervisorFSM_RX_rtu_ControlOutputs->Vq; - SupervisorFSM_RX_DW.is_c12_SupervisorFSM_RX = - SupervisorFSM_RX_IN_Voltage; - SupervisorFSM_RX_B.controlModeDefined = ControlModes_Voltage; - - // Outputs for Function Call SubSystem: '/SetpointHandler' - SupervisorFSM_RX_SetpointHandler(); - - // End of Outputs for SubSystem: '/SetpointHandler' - } else if (SupervisorFSM_RX_IsNewCtrl_Current()) { - SupervisorFSM_RX_B.newSetpoint_g = - SupervisorFSM_RX_rtu_ControlOutputs->Iq_fbk.current; - SupervisorFSM_RX_DW.is_c12_SupervisorFSM_RX = - SupervisorFSM_RX_IN_Current; - SupervisorFSM_RX_B.controlModeDefined = ControlModes_Current; - - // Outputs for Function Call SubSystem: '/SetpointHandler' - SupervisorFSM_RX_SetpointHandler(); - - // End of Outputs for SubSystem: '/SetpointHandler' - } else if (SupervisorFSM_RX_IsNewCtrl_Position()) { - SupervisorFSM_RX_DW.is_c12_SupervisorFSM_RX = - SupervisorFSM_RX_IN_Position; - SupervisorFSM_RX_B.controlModeDefined = ControlModes_Position; - SupervisorFSM_RX_B.newSetpoint_g = - SupervisorFSM_RX_rtu_SensorsData->jointpositions.position; - - // Outputs for Function Call SubSystem: '/SetpointHandler' - SupervisorFSM_RX_SetpointHandler(); - - // End of Outputs for SubSystem: '/SetpointHandler' - } else if (SupervisorFSM_RX_IsNewCtrl_Idle()) { - guard5 = true; - } else { - guard6 = true; - } - } else { - guard6 = true; - } - } - - if (guard9) { - if (SupervisorFSM_RX_IsNewCtrl_Idle()) { - guard4 = true; - } - } - - if (guard8) { - if (!SupervisorFSM_RX_IsNewCtrl_Idle()) { - rtw_enableMotor(); - if (SupervisorFSM_RX_IsNewCtrl_Position()) { - SupervisorFSM_RX_DW.is_c12_SupervisorFSM_RX = - SupervisorFSM_RX_IN_Position; - SupervisorFSM_RX_B.controlModeDefined = ControlModes_Position; - SupervisorFSM_RX_B.newSetpoint_g = - SupervisorFSM_RX_rtu_SensorsData->jointpositions.position; - - // Outputs for Function Call SubSystem: '/SetpointHandler' - SupervisorFSM_RX_SetpointHandler(); - - // End of Outputs for SubSystem: '/SetpointHandler' - } else if (SupervisorFSM_RX_IsNewCtrl_Current()) { - SupervisorFSM_RX_B.newSetpoint_g = 0.0F; - SupervisorFSM_RX_DW.is_c12_SupervisorFSM_RX = - SupervisorFSM_RX_IN_Current; - SupervisorFSM_RX_B.controlModeDefined = ControlModes_Current; - - // Outputs for Function Call SubSystem: '/SetpointHandler' - SupervisorFSM_RX_SetpointHandler(); - - // End of Outputs for SubSystem: '/SetpointHandler' - } else if (SupervisorFSM_RX_IsNewCtrl_Voltage()) { - SupervisorFSM_RX_B.newSetpoint_g = 0.0F; - SupervisorFSM_RX_DW.is_c12_SupervisorFSM_RX = - SupervisorFSM_RX_IN_Voltage; - SupervisorFSM_RX_B.controlModeDefined = ControlModes_Voltage; - - // Outputs for Function Call SubSystem: '/SetpointHandler' - SupervisorFSM_RX_SetpointHandler(); - - // End of Outputs for SubSystem: '/SetpointHandler' - } else if (SupervisorFSM_RX_IsNewCtrl_Velocity()) { - SupervisorFSM_RX_DW.is_c12_SupervisorFSM_RX = - SupervisorFSM_RX_IN_Velocity; - SupervisorFSM_RX_B.controlModeDefined = ControlModes_Velocity; - SupervisorFSM_RX_B.newSetpoint_g = 0.0F; - - // Outputs for Function Call SubSystem: '/SetpointHandler' - SupervisorFSM_RX_SetpointHandler(); - - // End of Outputs for SubSystem: '/SetpointHandler' - } else { - guard3 = true; - } - } else { - guard3 = true; - } - } - - if (guard7) { - if ((!SupervisorFSM_RX_IsNewCtrl_Idle()) && - (!SupervisorFSM_RX_IsNewCtrl_Position())) { - if (!SupervisorFSM_RX_IsNewCtrl_Current()) { - if (SupervisorFSM_RX_IsNewCtrl_Voltage()) { - SupervisorFSM_RX_B.newSetpoint_g = - SupervisorFSM_RX_rtu_ControlOutputs->Vq; - SupervisorFSM_RX_DW.is_c12_SupervisorFSM_RX = - SupervisorFSM_RX_IN_Voltage; - SupervisorFSM_RX_B.controlModeDefined = ControlModes_Voltage; - - // Outputs for Function Call SubSystem: '/SetpointHandler' - SupervisorFSM_RX_SetpointHandler(); - - // End of Outputs for SubSystem: '/SetpointHandler' - } else if (SupervisorFSM_RX_IsNewCtrl_Velocity()) { - SupervisorFSM_RX_DW.is_c12_SupervisorFSM_RX = - SupervisorFSM_RX_IN_Velocity; - SupervisorFSM_RX_B.controlModeDefined = ControlModes_Velocity; - SupervisorFSM_RX_B.newSetpoint_g = 0.0F; - - // Outputs for Function Call SubSystem: '/SetpointHandler' - SupervisorFSM_RX_SetpointHandler(); - - // End of Outputs for SubSystem: '/SetpointHandler' - } else { - guard2 = true; - } - } else { - guard2 = true; - } - } else if (!SupervisorFSM_RX_IsNewCtrl_Current()) { - if (SupervisorFSM_RX_IsNewCtrl_Position()) { - SupervisorFSM_RX_DW.is_c12_SupervisorFSM_RX = - SupervisorFSM_RX_IN_Position; - SupervisorFSM_RX_B.controlModeDefined = ControlModes_Position; - SupervisorFSM_RX_B.newSetpoint_g = - SupervisorFSM_RX_rtu_SensorsData->jointpositions.position; - - // Outputs for Function Call SubSystem: '/SetpointHandler' - SupervisorFSM_RX_SetpointHandler(); - - // End of Outputs for SubSystem: '/SetpointHandler' - } else if (SupervisorFSM_RX_IsNewCtrl_Idle()) { - guard1 = true; - } else { - guard2 = true; - } - } else { - guard2 = true; - } - } - - if (guard6) { - SupervisorFSM_RX_DW.is_c12_SupervisorFSM_RX = SupervisorFSM_RX_IN_Velocity; - SupervisorFSM_RX_B.controlModeDefined = ControlModes_Velocity; - SupervisorFSM_RX_B.newSetpoint_g = 0.0F; - - // Outputs for Function Call SubSystem: '/SetpointHandler' - SupervisorFSM_RX_SetpointHandler(); - - // End of Outputs for SubSystem: '/SetpointHandler' - } - - if (guard5) { - SupervisorFSM_RX_DW.is_c12_SupervisorFSM_RX = SupervisorFSM_RX_IN_Idle; - SupervisorFSM_RX_B.controlModeDefined = ControlModes_Idle; - rtw_disableMotor(); - - // Outputs for Function Call SubSystem: '/SetpointHandler' - // this updates the targets value - SupervisorFSM_RX_SetpointHandler(); - - // End of Outputs for SubSystem: '/SetpointHandler' - } - - if (guard4) { - SupervisorFSM_RX_DW.is_c12_SupervisorFSM_RX = SupervisorFSM_RX_IN_Idle; - SupervisorFSM_RX_B.controlModeDefined = ControlModes_Idle; - rtw_disableMotor(); - - // Outputs for Function Call SubSystem: '/SetpointHandler' - // this updates the targets value - SupervisorFSM_RX_SetpointHandler(); - - // End of Outputs for SubSystem: '/SetpointHandler' - } - - if (guard3) { - SupervisorFSM_RX_DW.is_c12_SupervisorFSM_RX = SupervisorFSM_RX_IN_Idle; - SupervisorFSM_RX_B.controlModeDefined = ControlModes_Idle; - rtw_disableMotor(); - - // Outputs for Function Call SubSystem: '/SetpointHandler' - // this updates the targets value - SupervisorFSM_RX_SetpointHandler(); - - // End of Outputs for SubSystem: '/SetpointHandler' - } - - if (guard2) { - SupervisorFSM_RX_DW.is_c12_SupervisorFSM_RX = SupervisorFSM_RX_IN_Current; - SupervisorFSM_RX_B.controlModeDefined = ControlModes_Current; - - // Outputs for Function Call SubSystem: '/SetpointHandler' - SupervisorFSM_RX_SetpointHandler(); - - // End of Outputs for SubSystem: '/SetpointHandler' - } - - if (guard1) { - SupervisorFSM_RX_DW.is_c12_SupervisorFSM_RX = SupervisorFSM_RX_IN_Idle; - SupervisorFSM_RX_B.controlModeDefined = ControlModes_Idle; - rtw_disableMotor(); - - // Outputs for Function Call SubSystem: '/SetpointHandler' - // this updates the targets value - SupervisorFSM_RX_SetpointHandler(); - - // End of Outputs for SubSystem: '/SetpointHandler' - } - } - - // End of Chart: '/ControlMode_SM_motor0' -} - -// Output and update for function-call system: '/Limits Handler' -void SupervisorFSM_RX_LimitsHandler(void) -{ - // Chart: '/Chart' - if (SupervisorFSM_RX_DW.is_active_c14_SupervisorFSM_RX == 0U) { - SupervisorFSM_RX_DW.is_active_c14_SupervisorFSM_RX = 1U; - SupervisorFSM_RX_B.thresholds = InitConfParams.thresholds; - if (SupervisorFSM_RX_B.newLimit.type == ControlModes_Current) { - SupervisorFSM_RX_B.thresholds.motorNominalCurrents = std::abs - (SupervisorFSM_RX_B.newLimit.nominal); - SupervisorFSM_RX_B.thresholds.motorPeakCurrents = std::abs - (SupervisorFSM_RX_B.newLimit.peak); - SupervisorFSM_RX_B.thresholds.motorOverloadCurrents = std::abs - (SupervisorFSM_RX_B.newLimit.overload); - if (!SupervisorFSM_RX_B.areLimitsSet) { - SupervisorFSM_RX_B.areLimitsSet = true; - - // Outputs for Function Call SubSystem: '/Control Mode Handler Motor 0' - SupervisorFSM_RX_ControlModeHandlerMotor0(); - - // End of Outputs for SubSystem: '/Control Mode Handler Motor 0' - } - } - } else if (SupervisorFSM_RX_B.newLimit.type == ControlModes_Current) { - SupervisorFSM_RX_B.thresholds.motorNominalCurrents = std::abs - (SupervisorFSM_RX_B.newLimit.nominal); - SupervisorFSM_RX_B.thresholds.motorPeakCurrents = std::abs - (SupervisorFSM_RX_B.newLimit.peak); - SupervisorFSM_RX_B.thresholds.motorOverloadCurrents = std::abs - (SupervisorFSM_RX_B.newLimit.overload); - if (!SupervisorFSM_RX_B.areLimitsSet) { - SupervisorFSM_RX_B.areLimitsSet = true; - - // Outputs for Function Call SubSystem: '/Control Mode Handler Motor 0' - SupervisorFSM_RX_ControlModeHandlerMotor0(); - - // End of Outputs for SubSystem: '/Control Mode Handler Motor 0' - } - } - - // End of Chart: '/Chart' -} - -// Output and update for function-call system: '/PID Handler' -void SupervisorFSM_RX_PIDHandler(void) -{ - // Chart: '/Chart' - if (SupervisorFSM_RX_DW.is_active_c3_SupervisorFSM_RX == 0U) { - SupervisorFSM_RX_DW.is_active_c3_SupervisorFSM_RX = 1U; - SupervisorFSM_RX_B.PositionPID = InitConfParams.PosLoopPID; - SupervisorFSM_RX_B.CurrentPID = InitConfParams.CurLoopPID; - SupervisorFSM_RX_B.VelocityPID = InitConfParams.VelLoopPID; - switch (SupervisorFSM_RX_B.newPIDType) { - case ControlModes_Current: - SupervisorFSM_RX_B.CurrentPID.P = SupervisorFSM_RX_B.newPID.Kp; - SupervisorFSM_RX_B.CurrentPID.I = SupervisorFSM_RX_B.newPID.Ki; - SupervisorFSM_RX_B.CurrentPID.D = SupervisorFSM_RX_B.newPID.Kd; - SupervisorFSM_RX_B.CurrentPID.shift_factor = SupervisorFSM_RX_B.newPID.Ks; - break; - - case ControlModes_Position: - SupervisorFSM_RX_B.PositionPID.P = SupervisorFSM_RX_B.newPID.Kp; - SupervisorFSM_RX_B.PositionPID.I = SupervisorFSM_RX_B.newPID.Ki; - SupervisorFSM_RX_B.PositionPID.D = SupervisorFSM_RX_B.newPID.Kd; - SupervisorFSM_RX_B.PositionPID.shift_factor = SupervisorFSM_RX_B.newPID.Ks; - break; - - case ControlModes_Voltage: - SupervisorFSM_RX_B.OpenLoopPID.P = SupervisorFSM_RX_B.newPID.Kp; - SupervisorFSM_RX_B.OpenLoopPID.I = SupervisorFSM_RX_B.newPID.Ki; - SupervisorFSM_RX_B.OpenLoopPID.D = SupervisorFSM_RX_B.newPID.Kd; - SupervisorFSM_RX_B.OpenLoopPID.shift_factor = SupervisorFSM_RX_B.newPID.Ks; - break; - - case ControlModes_Velocity: - SupervisorFSM_RX_B.VelocityPID.P = SupervisorFSM_RX_B.newPID.Kp; - SupervisorFSM_RX_B.VelocityPID.I = SupervisorFSM_RX_B.newPID.Ki; - SupervisorFSM_RX_B.VelocityPID.D = SupervisorFSM_RX_B.newPID.Kd; - SupervisorFSM_RX_B.VelocityPID.shift_factor = SupervisorFSM_RX_B.newPID.Ks; - break; - } - } else { - switch (SupervisorFSM_RX_B.newPIDType) { - case ControlModes_Current: - SupervisorFSM_RX_B.CurrentPID.P = SupervisorFSM_RX_B.newPID.Kp; - SupervisorFSM_RX_B.CurrentPID.I = SupervisorFSM_RX_B.newPID.Ki; - SupervisorFSM_RX_B.CurrentPID.D = SupervisorFSM_RX_B.newPID.Kd; - SupervisorFSM_RX_B.CurrentPID.shift_factor = SupervisorFSM_RX_B.newPID.Ks; - break; - - case ControlModes_Position: - SupervisorFSM_RX_B.PositionPID.P = SupervisorFSM_RX_B.newPID.Kp; - SupervisorFSM_RX_B.PositionPID.I = SupervisorFSM_RX_B.newPID.Ki; - SupervisorFSM_RX_B.PositionPID.D = SupervisorFSM_RX_B.newPID.Kd; - SupervisorFSM_RX_B.PositionPID.shift_factor = SupervisorFSM_RX_B.newPID.Ks; - break; - - case ControlModes_Voltage: - SupervisorFSM_RX_B.OpenLoopPID.P = SupervisorFSM_RX_B.newPID.Kp; - SupervisorFSM_RX_B.OpenLoopPID.I = SupervisorFSM_RX_B.newPID.Ki; - SupervisorFSM_RX_B.OpenLoopPID.D = SupervisorFSM_RX_B.newPID.Kd; - SupervisorFSM_RX_B.OpenLoopPID.shift_factor = SupervisorFSM_RX_B.newPID.Ks; - break; - - case ControlModes_Velocity: - SupervisorFSM_RX_B.VelocityPID.P = SupervisorFSM_RX_B.newPID.Kp; - SupervisorFSM_RX_B.VelocityPID.I = SupervisorFSM_RX_B.newPID.Ki; - SupervisorFSM_RX_B.VelocityPID.D = SupervisorFSM_RX_B.newPID.Kd; - SupervisorFSM_RX_B.VelocityPID.shift_factor = SupervisorFSM_RX_B.newPID.Ks; - break; - } - } - - // End of Chart: '/Chart' -} - -// Function for Chart: '/CAN Event Dispatcher' -static ControlModes SupervisorFSM_RX_convert(MCControlModes mccontrolmode) -{ - ControlModes controlmode; - switch (mccontrolmode) { - case MCControlModes_Idle: - controlmode = ControlModes_Idle; - break; - - case MCControlModes_Current: - controlmode = ControlModes_Current; - break; - - case MCControlModes_SpeedVoltage: - controlmode = ControlModes_Velocity; - break; - - case MCControlModes_OpenLoop: - controlmode = ControlModes_Voltage; - break; - - default: - controlmode = ControlModes_Idle; - break; - } - - return controlmode; -} - -// Function for Chart: '/CAN Event Dispatcher' -static void SupervisorFSM_RX_formatMotorConfig(const BUS_MESSAGES_RX *Selector2) -{ - SupervisorFSM_RX_B.motorConfig.use_index = Selector2->motor_config.use_index; - SupervisorFSM_RX_B.motorConfig.pole_pairs = static_cast(rt_roundd_snf - (static_cast(Selector2->motor_config.number_poles) / 2.0)); - SupervisorFSM_RX_B.motorConfig.encoder_tolerance = - Selector2->motor_config.encoder_tolerance; - SupervisorFSM_RX_B.motorConfig.rotor_encoder_resolution = - Selector2->motor_config.rotor_encoder_resolution; - SupervisorFSM_RX_B.motorConfig.rotor_index_offset = - Selector2->motor_config.rotor_index_offset; - SupervisorFSM_RX_B.motorConfig.has_quadrature_encoder = - Selector2->motor_config.has_quadrature_encoder; - SupervisorFSM_RX_B.motorConfig.has_hall_sens = - Selector2->motor_config.has_hall_sens; - SupervisorFSM_RX_B.motorConfig.has_speed_quadrature_encoder = - Selector2->motor_config.has_speed_quadrature_encoder; - SupervisorFSM_RX_B.motorConfig.has_torque_sens = - Selector2->motor_config.has_torque_sens; - SupervisorFSM_RX_B.motorConfig.enable_verbosity = - Selector2->motor_config.enable_verbosity; -} - -// Function for Chart: '/CAN Event Dispatcher' -static void SupervisorFSM_RX_hardwareConfigMotor(void) -{ - real32_T tmp; - uint16_T tmp_0; - tmp = rt_roundf_snf(SupervisorFSM_RX_B.motorConfig.hall_sens_offset * 65536.0F - / 360.0F); - if (tmp < 65536.0F) { - if (tmp >= 0.0F) { - tmp_0 = static_cast(tmp); - } else { - tmp_0 = 0U; - } - } else { - tmp_0 = MAX_uint16_T; - } - - rtw_configMotor(static_cast - (SupervisorFSM_RX_B.motorConfig.has_quadrature_encoder), - SupervisorFSM_RX_B.motorConfig.rotor_encoder_resolution, - SupervisorFSM_RX_B.motorConfig.pole_pairs, static_cast - (SupervisorFSM_RX_B.motorConfig.has_hall_sens), 1U, tmp_0); -} - -// Output and update for function-call system: '/CAN Message Handler' -void SupervisorFSM_RX_CANMessageHandler(void) -{ - BUS_MESSAGES_RX Selector2; - - // Selector: '/Selector2' - Selector2 = SupervisorFSM_RX_rtu_MessagesRx-> - messages[SupervisorFSM_RX_B.messageIndex - 1]; - - // Chart: '/CAN Event Dispatcher' incorporates: - // Selector: '/Selector' - // Selector: '/Selector1' - // Selector: '/Selector2' - - if (SupervisorFSM_RX_DW.is_active_c11_SupervisorFSM_RX == 0U) { - SupervisorFSM_RX_DW.is_active_c11_SupervisorFSM_RX = 1U; - SupervisorFSM_RX_B.motorConfig = InitConfParams.motorconfig; - SupervisorFSM_RX_B.estimationConfig = InitConfParams.estimationconfig; - if (!SupervisorFSM_RX_rtu_ErrorsRx->errors[SupervisorFSM_RX_B.messageIndex - - 1].event) { - if (SupervisorFSM_RX_rtu_StatusRx->status[SupervisorFSM_RX_B.messageIndex - - 1].control_mode) { - SupervisorFSM_RX_B.requiredControlMode = SupervisorFSM_RX_convert - (SupervisorFSM_RX_rtu_MessagesRx-> - messages[SupervisorFSM_RX_B.messageIndex - 1].control_mode.mode); - SupervisorFSM_RX_B.receivedNewSetpoint = false; - - // Outputs for Function Call SubSystem: '/Control Mode Handler Motor 0' - SupervisorFSM_RX_ControlModeHandlerMotor0(); - - // End of Outputs for SubSystem: '/Control Mode Handler Motor 0' - } else if (SupervisorFSM_RX_rtu_StatusRx-> - status[SupervisorFSM_RX_B.messageIndex - 1].current_limit) { - SupervisorFSM_RX_B.newLimit.type = ControlModes_Current; - SupervisorFSM_RX_B.newLimit.peak = - SupervisorFSM_RX_rtu_MessagesRx-> - messages[SupervisorFSM_RX_B.messageIndex - 1].current_limit.peak; - SupervisorFSM_RX_B.newLimit.nominal = - SupervisorFSM_RX_rtu_MessagesRx-> - messages[SupervisorFSM_RX_B.messageIndex - 1].current_limit.nominal; - SupervisorFSM_RX_B.newLimit.overload = - SupervisorFSM_RX_rtu_MessagesRx-> - messages[SupervisorFSM_RX_B.messageIndex - 1].current_limit.overload; - - // Outputs for Function Call SubSystem: '/Limits Handler' - SupervisorFSM_RX_LimitsHandler(); - - // End of Outputs for SubSystem: '/Limits Handler' - } else if (SupervisorFSM_RX_rtu_StatusRx-> - status[SupervisorFSM_RX_B.messageIndex - 1].current_pid) { - SupervisorFSM_RX_B.newPIDType = ControlModes_Current; - SupervisorFSM_RX_B.newPID = SupervisorFSM_RX_rtu_MessagesRx-> - messages[SupervisorFSM_RX_B.messageIndex - 1].pid; - - // Outputs for Function Call SubSystem: '/PID Handler' - SupervisorFSM_RX_PIDHandler(); - - // End of Outputs for SubSystem: '/PID Handler' - } else if (SupervisorFSM_RX_rtu_StatusRx-> - status[SupervisorFSM_RX_B.messageIndex - 1].velocity_pid) { - SupervisorFSM_RX_B.newPIDType = ControlModes_Velocity; - SupervisorFSM_RX_B.newPID = SupervisorFSM_RX_rtu_MessagesRx-> - messages[SupervisorFSM_RX_B.messageIndex - 1].pid; - - // Outputs for Function Call SubSystem: '/PID Handler' - SupervisorFSM_RX_PIDHandler(); - - // End of Outputs for SubSystem: '/PID Handler' - } else if (SupervisorFSM_RX_rtu_StatusRx-> - status[SupervisorFSM_RX_B.messageIndex - 1].motor_config) { - SupervisorFSM_RX_formatMotorConfig(&Selector2); - SupervisorFSM_RX_hardwareConfigMotor(); - } else if (SupervisorFSM_RX_rtu_StatusRx-> - status[SupervisorFSM_RX_B.messageIndex - 1].desired_targets) { - SupervisorFSM_RX_B.newSetpoint = - SupervisorFSM_RX_rtu_MessagesRx-> - messages[SupervisorFSM_RX_B.messageIndex - 1].desired_targets; - SupervisorFSM_RX_B.enableSendingMsgStatus = true; - SupervisorFSM_RX_B.receivedNewSetpoint = true; - - // Outputs for Function Call SubSystem: '/SetpointHandler' - SupervisorFSM_RX_SetpointHandler(); - - // End of Outputs for SubSystem: '/SetpointHandler' - } - } - } else { - SupervisorFSM_RX_B.enableSendingMsgStatus = - ((SupervisorFSM_RX_B.messageIndex != 1) && - SupervisorFSM_RX_B.enableSendingMsgStatus); - if (!SupervisorFSM_RX_rtu_ErrorsRx->errors[SupervisorFSM_RX_B.messageIndex - - 1].event) { - if (SupervisorFSM_RX_rtu_StatusRx->status[SupervisorFSM_RX_B.messageIndex - - 1].control_mode) { - SupervisorFSM_RX_B.requiredControlMode = SupervisorFSM_RX_convert - (SupervisorFSM_RX_rtu_MessagesRx-> - messages[SupervisorFSM_RX_B.messageIndex - 1].control_mode.mode); - SupervisorFSM_RX_B.receivedNewSetpoint = false; - - // Outputs for Function Call SubSystem: '/Control Mode Handler Motor 0' - SupervisorFSM_RX_ControlModeHandlerMotor0(); - - // End of Outputs for SubSystem: '/Control Mode Handler Motor 0' - } else if (SupervisorFSM_RX_rtu_StatusRx-> - status[SupervisorFSM_RX_B.messageIndex - 1].current_limit) { - SupervisorFSM_RX_B.newLimit.type = ControlModes_Current; - SupervisorFSM_RX_B.newLimit.peak = - SupervisorFSM_RX_rtu_MessagesRx-> - messages[SupervisorFSM_RX_B.messageIndex - 1].current_limit.peak; - SupervisorFSM_RX_B.newLimit.nominal = - SupervisorFSM_RX_rtu_MessagesRx-> - messages[SupervisorFSM_RX_B.messageIndex - 1].current_limit.nominal; - SupervisorFSM_RX_B.newLimit.overload = - SupervisorFSM_RX_rtu_MessagesRx-> - messages[SupervisorFSM_RX_B.messageIndex - 1].current_limit.overload; - - // Outputs for Function Call SubSystem: '/Limits Handler' - SupervisorFSM_RX_LimitsHandler(); - - // End of Outputs for SubSystem: '/Limits Handler' - } else if (SupervisorFSM_RX_rtu_StatusRx-> - status[SupervisorFSM_RX_B.messageIndex - 1].current_pid) { - SupervisorFSM_RX_B.newPIDType = ControlModes_Current; - SupervisorFSM_RX_B.newPID = SupervisorFSM_RX_rtu_MessagesRx-> - messages[SupervisorFSM_RX_B.messageIndex - 1].pid; - - // Outputs for Function Call SubSystem: '/PID Handler' - SupervisorFSM_RX_PIDHandler(); - - // End of Outputs for SubSystem: '/PID Handler' - } else if (SupervisorFSM_RX_rtu_StatusRx-> - status[SupervisorFSM_RX_B.messageIndex - 1].velocity_pid) { - SupervisorFSM_RX_B.newPIDType = ControlModes_Velocity; - SupervisorFSM_RX_B.newPID = SupervisorFSM_RX_rtu_MessagesRx-> - messages[SupervisorFSM_RX_B.messageIndex - 1].pid; - - // Outputs for Function Call SubSystem: '/PID Handler' - SupervisorFSM_RX_PIDHandler(); - - // End of Outputs for SubSystem: '/PID Handler' - } else if (SupervisorFSM_RX_rtu_StatusRx-> - status[SupervisorFSM_RX_B.messageIndex - 1].motor_config) { - SupervisorFSM_RX_formatMotorConfig(&Selector2); - SupervisorFSM_RX_hardwareConfigMotor(); - } else if (SupervisorFSM_RX_rtu_StatusRx-> - status[SupervisorFSM_RX_B.messageIndex - 1].desired_targets) { - SupervisorFSM_RX_B.newSetpoint = - SupervisorFSM_RX_rtu_MessagesRx-> - messages[SupervisorFSM_RX_B.messageIndex - 1].desired_targets; - SupervisorFSM_RX_B.enableSendingMsgStatus = true; - SupervisorFSM_RX_B.receivedNewSetpoint = true; - - // Outputs for Function Call SubSystem: '/SetpointHandler' - SupervisorFSM_RX_SetpointHandler(); - - // End of Outputs for SubSystem: '/SetpointHandler' - } - } - } - - // End of Chart: '/CAN Event Dispatcher' -} - -// Function for Chart: '/SupervisorRX State Handler' -static boolean_T SupervisorFSM_RX_isBoardConfigured(void) -{ - return true; -} - -// System initialize for referenced model: 'SupervisorFSM_RX' -void SupervisorFSM_RX_Init(Flags *rty_Flags, ConfigurationParameters - *rty_ConfigurationParameters) -{ - // SystemInitialize for BusCreator: '/Bus Creator' - rty_Flags->control_mode = SupervisorFSM_RX_B.controlModeDefined; - rty_Flags->enable_sending_msg_status = - SupervisorFSM_RX_B.enableSendingMsgStatus; - rty_Flags->fault_button = false; - rty_Flags->enable_thermal_protection = SupervisorFSM_RX_ConstB.Constant5; - - // SystemInitialize for BusCreator: '/Bus Creator1' incorporates: - // Constant: '/Constant4' - - rty_ConfigurationParameters->motorconfig = SupervisorFSM_RX_B.motorConfig; - rty_ConfigurationParameters->estimationconfig = - SupervisorFSM_RX_B.estimationConfig; - rty_ConfigurationParameters->CurLoopPID = SupervisorFSM_RX_B.CurrentPID; - rty_ConfigurationParameters->PosLoopPID = SupervisorFSM_RX_B.PositionPID; - rty_ConfigurationParameters->VelLoopPID = SupervisorFSM_RX_B.VelocityPID; - rty_ConfigurationParameters->DirLoopPID = SupervisorFSM_RX_B.OpenLoopPID; - rty_ConfigurationParameters->thresholds = SupervisorFSM_RX_B.thresholds; - rty_ConfigurationParameters->environment_temperature = - InitConfParams.environment_temperature; -} - -// Output and update for referenced model: 'SupervisorFSM_RX' -void SupervisorFSM_RX(const SensorsData *rtu_SensorsData, const ExternalFlags - *rtu_ExternalFlags, const ControlOutputs - *rtu_ControlOutputs, const BUS_MESSAGES_RX_MULTIPLE - *rtu_MessagesRx, const EstimatedData *rtu_EstimatedData, - const BUS_STATUS_RX_MULTIPLE *rtu_StatusRx, const - BUS_CAN_RX_ERRORS_MULTIPLE *rtu_ErrorsRx, Flags *rty_Flags, - Targets *rty_Targets, ConfigurationParameters - *rty_ConfigurationParameters) -{ - real32_T rtb_UnitDelay_thresholds_motorOverloadCurrents; - SupervisorFSM_RX_rtu_SensorsData = rtu_SensorsData; - SupervisorFSM_RX_rtu_ControlOutputs = rtu_ControlOutputs; - SupervisorFSM_RX_rtu_MessagesRx = rtu_MessagesRx; - SupervisorFSM_RX_rtu_StatusRx = rtu_StatusRx; - SupervisorFSM_RX_rtu_ErrorsRx = rtu_ErrorsRx; - - // UnitDelay: '/Unit Delay' - rtb_UnitDelay_thresholds_motorOverloadCurrents = - SupervisorFSM_RX_DW.UnitDelay_DSTATE.thresholds.motorOverloadCurrents; - - // Chart: '/SupervisorRX State Handler' incorporates: - // UnitDelay: '/Unit Delay' - - SupervisorFSM_RX_DW.ExternalFlags_fault_button_prev = - SupervisorFSM_RX_DW.ExternalFlags_fault_button_start; - SupervisorFSM_RX_DW.ExternalFlags_fault_button_start = - rtu_ExternalFlags->fault_button; - if (SupervisorFSM_RX_DW.is_active_c2_SupervisorFSM_RX == 0U) { - SupervisorFSM_RX_DW.ExternalFlags_fault_button_prev = - rtu_ExternalFlags->fault_button; - SupervisorFSM_RX_DW.ExternalFlags_fault_button_start = - rtu_ExternalFlags->fault_button; - SupervisorFSM_RX_DW.is_active_c2_SupervisorFSM_RX = 1U; - SupervisorFSM_RX_DW.is_active_STATE_HANDLER = 1U; - SupervisorFSM_RX_DW.is_STATE_HANDLER = SupervisorFSM_RX_IN_NotConfigured_j; - SupervisorFSM_RX_B.BoardSt = BoardState_NotConfigured; - SupervisorFSM_RX_DW.is_active_FAULT_HANDLER = 1U; - SupervisorFSM_RX_DW.is_active_FaultButtonPressed = 1U; - SupervisorFSM_RX_DW.is_FaultButtonPressed = SupervisorFSM_RX_IN_NoFault; - SupervisorFSM_RX_B.isFaultButtonPressed = false; - SupervisorFSM_RX_DW.is_active_OverCurrent = 1U; - SupervisorFSM_RX_DW.is_OverCurrent = SupervisorFSM_RX_IN_LimitNonConfigured; - SupervisorFSM_RX_DW.is_active_CAN_MESSAGES_FOR_LOOP = 1U; - - // Outputs for Function Call SubSystem: '/PID Handler' - SupervisorFSM_RX_PIDHandler(); - - // End of Outputs for SubSystem: '/PID Handler' - - // Outputs for Function Call SubSystem: '/Limits Handler' - SupervisorFSM_RX_LimitsHandler(); - - // End of Outputs for SubSystem: '/Limits Handler' - SupervisorFSM_RX_B.messageIndex = 1; - while (SupervisorFSM_RX_B.messageIndex <= CAN_MAX_NUM_PACKETS) { - // Outputs for Function Call SubSystem: '/CAN Message Handler' - SupervisorFSM_RX_CANMessageHandler(); - - // End of Outputs for SubSystem: '/CAN Message Handler' - SupervisorFSM_RX_B.messageIndex++; - } - - SupervisorFSM_RX_DW.is_CAN_MESSAGES_FOR_LOOP = SupervisorFSM_RX_IN_state1; - } else { - if (SupervisorFSM_RX_DW.is_active_STATE_HANDLER != 0U) { - switch (SupervisorFSM_RX_DW.is_STATE_HANDLER) { - case SupervisorFSM_RX_IN_Configured: - SupervisorFSM_RX_B.BoardSt = BoardState_Configured; - break; - - case SupervisorFSM_RX_IN_Fault: - SupervisorFSM_RX_B.BoardSt = BoardState_Fault; - break; - - case SupervisorFSM_RX_IN_NotConfigured_j: - SupervisorFSM_RX_B.BoardSt = BoardState_NotConfigured; - if (SupervisorFSM_RX_isBoardConfigured()) { - SupervisorFSM_RX_DW.is_STATE_HANDLER = SupervisorFSM_RX_IN_Configured; - SupervisorFSM_RX_B.BoardSt = BoardState_Configured; - } - break; - } - } - - if (SupervisorFSM_RX_DW.is_active_FAULT_HANDLER != 0U) { - if (SupervisorFSM_RX_DW.is_active_FaultButtonPressed != 0U) { - switch (SupervisorFSM_RX_DW.is_FaultButtonPressed) { - case SupervisorFSM_RX_IN_ButtonPressed: - SupervisorFSM_RX_B.isFaultButtonPressed = true; - if ((SupervisorFSM_RX_DW.ExternalFlags_fault_button_prev != - SupervisorFSM_RX_DW.ExternalFlags_fault_button_start) && - (!SupervisorFSM_RX_DW.ExternalFlags_fault_button_start)) { - SupervisorFSM_RX_DW.is_FaultButtonPressed = - SupervisorFSM_RX_IN_NoFault; - SupervisorFSM_RX_B.isFaultButtonPressed = false; - } - break; - - case SupervisorFSM_RX_IN_NoFault: - SupervisorFSM_RX_B.isFaultButtonPressed = false; - if ((SupervisorFSM_RX_DW.ExternalFlags_fault_button_prev != - SupervisorFSM_RX_DW.ExternalFlags_fault_button_start) && - SupervisorFSM_RX_DW.ExternalFlags_fault_button_start) { - SupervisorFSM_RX_DW.is_FaultButtonPressed = - SupervisorFSM_RX_IN_ButtonPressed; - SupervisorFSM_RX_B.isFaultButtonPressed = true; - - // Outputs for Function Call SubSystem: '/Control Mode Handler Motor 0' - SupervisorFSM_RX_ControlModeHandlerMotor0(); - - // End of Outputs for SubSystem: '/Control Mode Handler Motor 0' - } - break; - } - } - - if (SupervisorFSM_RX_DW.is_active_OverCurrent != 0U) { - switch (SupervisorFSM_RX_DW.is_OverCurrent) { - case SupervisorFSM_RX_IN_LimitNonConfigured: - if (SupervisorFSM_RX_B.areLimitsSet) { - SupervisorFSM_RX_DW.is_OverCurrent = SupervisorFSM_RX_IN_NoFault; - SupervisorFSM_RX_B.isInOverCurrent = false; - - // MotorFaultFlags.overCurrent=0; - } - break; - - case SupervisorFSM_RX_IN_NoFault: - SupervisorFSM_RX_B.isInOverCurrent = false; - if (std::abs(rtu_EstimatedData->Iq_filtered.current) >= - rtb_UnitDelay_thresholds_motorOverloadCurrents) { - SupervisorFSM_RX_DW.is_OverCurrent = - SupervisorFSM_RX_IN_OverCurrentFault; - - // MotorFaultFlags.overCurrent=1; - SupervisorFSM_RX_B.isInOverCurrent = true; - - // Outputs for Function Call SubSystem: '/Control Mode Handler Motor 0' - SupervisorFSM_RX_ControlModeHandlerMotor0(); - - // End of Outputs for SubSystem: '/Control Mode Handler Motor 0' - } - break; - - case SupervisorFSM_RX_IN_OverCurrentFault: - SupervisorFSM_RX_B.isInOverCurrent = true; - if (std::abs(rtu_EstimatedData->Iq_filtered.current) < - rtb_UnitDelay_thresholds_motorOverloadCurrents) { - SupervisorFSM_RX_DW.is_OverCurrent = SupervisorFSM_RX_IN_NoFault; - SupervisorFSM_RX_B.isInOverCurrent = false; - - // MotorFaultFlags.overCurrent=0; - } - break; - } - } - } - - if ((SupervisorFSM_RX_DW.is_active_CAN_MESSAGES_FOR_LOOP != 0U) && - (SupervisorFSM_RX_DW.is_CAN_MESSAGES_FOR_LOOP == - SupervisorFSM_RX_IN_state1)) { - SupervisorFSM_RX_B.messageIndex = 1; - while (SupervisorFSM_RX_B.messageIndex <= CAN_MAX_NUM_PACKETS) { - // Outputs for Function Call SubSystem: '/CAN Message Handler' - SupervisorFSM_RX_CANMessageHandler(); - - // End of Outputs for SubSystem: '/CAN Message Handler' - SupervisorFSM_RX_B.messageIndex++; - } - - SupervisorFSM_RX_DW.is_CAN_MESSAGES_FOR_LOOP = SupervisorFSM_RX_IN_state1; - } - } - - // End of Chart: '/SupervisorRX State Handler' - - // BusCreator: '/Bus Creator' - rty_Flags->control_mode = SupervisorFSM_RX_B.controlModeDefined; - rty_Flags->enable_sending_msg_status = - SupervisorFSM_RX_B.enableSendingMsgStatus; - rty_Flags->fault_button = SupervisorFSM_RX_B.isFaultButtonPressed; - rty_Flags->enable_thermal_protection = SupervisorFSM_RX_ConstB.Constant5; - - // BusCreator: '/Bus Creator1' incorporates: - // Constant: '/Constant4' - - rty_ConfigurationParameters->motorconfig = SupervisorFSM_RX_B.motorConfig; - rty_ConfigurationParameters->estimationconfig = - SupervisorFSM_RX_B.estimationConfig; - rty_ConfigurationParameters->CurLoopPID = SupervisorFSM_RX_B.CurrentPID; - rty_ConfigurationParameters->PosLoopPID = SupervisorFSM_RX_B.PositionPID; - rty_ConfigurationParameters->VelLoopPID = SupervisorFSM_RX_B.VelocityPID; - rty_ConfigurationParameters->DirLoopPID = SupervisorFSM_RX_B.OpenLoopPID; - rty_ConfigurationParameters->thresholds = SupervisorFSM_RX_B.thresholds; - rty_ConfigurationParameters->environment_temperature = - InitConfParams.environment_temperature; - - // Switch: '/Switch2' incorporates: - // BusCreator: '/Bus Creator2' - // BusCreator: '/Bus Creator3' - // BusCreator: '/Bus Creator4' - // BusCreator: '/Bus Creator5' - // Constant: '/Constant' - // Constant: '/Constant1' - // Constant: '/Constant2' - // Constant: '/Constant3' - - if (SupervisorFSM_RX_B.hasSetpointChanged) { - *rty_Targets = SupervisorFSM_RX_B.targets; - } else { - rty_Targets->jointpositions.position = 0.0F; - rty_Targets->jointvelocities.velocity = 0.0F; - rty_Targets->motorcurrent.current = 0.0F; - rty_Targets->motorvoltage.voltage = 0.0F; - } - - // End of Switch: '/Switch2' - - // Update for UnitDelay: '/Unit Delay' - SupervisorFSM_RX_DW.UnitDelay_DSTATE = *rty_ConfigurationParameters; -} - -// Model initialize function -void SupervisorFSM_RX_initialize(const char_T **rt_errorStatus) -{ - RT_MODEL_SupervisorFSM_RX_T *const SupervisorFSM_RX_M = - &(SupervisorFSM_RX_MdlrefDW.rtm); - - // Registration code - - // initialize error status - rtmSetErrorStatusPointer(SupervisorFSM_RX_M, rt_errorStatus); -} - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/emBODY/eBcode/arch-arm/mbd/amcbldc/supervisor-rx/SupervisorFSM_RX.h b/emBODY/eBcode/arch-arm/mbd/amcbldc/supervisor-rx/SupervisorFSM_RX.h deleted file mode 100644 index 4dc9e32f4..000000000 --- a/emBODY/eBcode/arch-arm/mbd/amcbldc/supervisor-rx/SupervisorFSM_RX.h +++ /dev/null @@ -1,91 +0,0 @@ -// -// Non-Degree Granting Education License -- for use at non-degree -// granting, nonprofit, education, and research organizations only. Not -// for commercial or industrial use. -// -// File: SupervisorFSM_RX.h -// -// Code generated for Simulink model 'SupervisorFSM_RX'. -// -// Model version : 7.5 -// Simulink Coder version : 23.2 (R2023b) 01-Aug-2023 -// C/C++ source code generated on : Tue Feb 13 11:54:08 2024 -// -// Target selection: ert.tlc -// Embedded hardware selection: ARM Compatible->ARM Cortex-M -// Code generation objectives: Unspecified -// Validation result: Not run -// -#ifndef RTW_HEADER_SupervisorFSM_RX_h_ -#define RTW_HEADER_SupervisorFSM_RX_h_ -#include "rtwtypes.h" -#include "SupervisorFSM_RX_types.h" - -// Includes for objects with custom storage classes -#include "rtw_defines.h" - -// user code (top of header file) -#include "rtw_enable_disable_motors.h" -#include "rtw_motor_config.h" - -// -// Exported Global Parameters -// -// Note: Exported global parameters are tunable parameters with an exported -// global storage class designation. Code generation will declare the memory for -// these parameters and exports their symbols. -// - -extern ConfigurationParameters InitConfParams;// Variable: InitConfParams - // Referenced by: - // '/Constant4' - // '/CAN Event Dispatcher' - // '/Chart' - // '/Chart' - -extern void SupervisorFSM_RX_Init(Flags *rty_Flags, ConfigurationParameters - *rty_ConfigurationParameters); -extern void SupervisorFSM_RX(const SensorsData *rtu_SensorsData, const - ExternalFlags *rtu_ExternalFlags, const ControlOutputs *rtu_ControlOutputs, - const BUS_MESSAGES_RX_MULTIPLE *rtu_MessagesRx, const EstimatedData - *rtu_EstimatedData, const BUS_STATUS_RX_MULTIPLE *rtu_StatusRx, const - BUS_CAN_RX_ERRORS_MULTIPLE *rtu_ErrorsRx, Flags *rty_Flags, Targets - *rty_Targets, ConfigurationParameters *rty_ConfigurationParameters); - -// Model reference registration function -extern void SupervisorFSM_RX_initialize(const char_T **rt_errorStatus); - -//- -// The generated code includes comments that allow you to trace directly -// back to the appropriate location in the model. The basic format -// is /block_name, where system is the system number (uniquely -// assigned by Simulink) and block_name is the name of the block. -// -// Use the MATLAB hilite_system command to trace the generated code back -// to the model. For example, -// -// hilite_system('') - opens system 3 -// hilite_system('/Kp') - opens and selects block Kp which resides in S3 -// -// Here is the system hierarchy for this model -// -// '' : 'SupervisorFSM_RX' -// '' : 'SupervisorFSM_RX/CAN Message Handler' -// '' : 'SupervisorFSM_RX/Control Mode Handler Motor 0' -// '' : 'SupervisorFSM_RX/Limits Handler' -// '' : 'SupervisorFSM_RX/PID Handler' -// '' : 'SupervisorFSM_RX/SetpointHandler' -// '' : 'SupervisorFSM_RX/SupervisorRX State Handler' -// '' : 'SupervisorFSM_RX/CAN Message Handler/CAN Event Dispatcher' -// '' : 'SupervisorFSM_RX/Control Mode Handler Motor 0/ControlMode_SM_motor0' -// '' : 'SupervisorFSM_RX/Limits Handler/Chart' -// '' : 'SupervisorFSM_RX/PID Handler/Chart' -// '' : 'SupervisorFSM_RX/SetpointHandler/Chart1' - -#endif // RTW_HEADER_SupervisorFSM_RX_h_ - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/emBODY/eBcode/arch-arm/mbd/amcbldc/supervisor-rx/SupervisorFSM_RX_private.h b/emBODY/eBcode/arch-arm/mbd/amcbldc/supervisor-rx/SupervisorFSM_RX_private.h deleted file mode 100644 index 9da06bc40..000000000 --- a/emBODY/eBcode/arch-arm/mbd/amcbldc/supervisor-rx/SupervisorFSM_RX_private.h +++ /dev/null @@ -1,134 +0,0 @@ -// -// Non-Degree Granting Education License -- for use at non-degree -// granting, nonprofit, education, and research organizations only. Not -// for commercial or industrial use. -// -// File: SupervisorFSM_RX_private.h -// -// Code generated for Simulink model 'SupervisorFSM_RX'. -// -// Model version : 7.5 -// Simulink Coder version : 23.2 (R2023b) 01-Aug-2023 -// C/C++ source code generated on : Tue Feb 13 11:54:08 2024 -// -// Target selection: ert.tlc -// Embedded hardware selection: ARM Compatible->ARM Cortex-M -// Code generation objectives: Unspecified -// Validation result: Not run -// -#ifndef RTW_HEADER_SupervisorFSM_RX_private_h_ -#define RTW_HEADER_SupervisorFSM_RX_private_h_ -#include "rtwtypes.h" -#include "SupervisorFSM_RX_types.h" - -// Block signals for model 'SupervisorFSM_RX' -struct B_SupervisorFSM_RX_c_T { - Thresholds thresholds; // '/Chart' - MotorConfig motorConfig; // '/CAN Event Dispatcher' - PIDConfig CurrentPID; // '/Chart' - PIDConfig VelocityPID; // '/Chart' - PIDConfig PositionPID; // '/Chart' - PIDConfig OpenLoopPID; // '/Chart' - Targets targets; // '/Chart1' - BUS_MSG_PID newPID; // '/CAN Event Dispatcher' - SV_Limits newLimit; // '/CAN Event Dispatcher' - BUS_MSG_DESIRED_TARGETS newSetpoint; // '/CAN Event Dispatcher' - EstimationConfig estimationConfig; // '/CAN Event Dispatcher' - real32_T newSetpoint_g; // '/ControlMode_SM_motor0' - int32_T messageIndex; // '/SupervisorRX State Handler' - ControlModes requiredControlMode; // '/CAN Event Dispatcher' - ControlModes newPIDType; // '/CAN Event Dispatcher' - ControlModes controlModeDefined; // '/ControlMode_SM_motor0' - BoardState BoardSt; // '/SupervisorRX State Handler' - boolean_T isInOverCurrent; // '/SupervisorRX State Handler' - boolean_T isFaultButtonPressed; // '/SupervisorRX State Handler' - boolean_T enableSendingMsgStatus; // '/CAN Event Dispatcher' - boolean_T receivedNewSetpoint; // '/CAN Event Dispatcher' - boolean_T areLimitsSet; // '/Chart' - boolean_T hasSetpointChanged; // '/Chart1' -}; - -// Block states (default storage) for model 'SupervisorFSM_RX' -struct DW_SupervisorFSM_RX_f_T { - ConfigurationParameters UnitDelay_DSTATE;// '/Unit Delay' - uint8_T is_active_c2_SupervisorFSM_RX;// '/SupervisorRX State Handler' - uint8_T is_active_STATE_HANDLER; // '/SupervisorRX State Handler' - uint8_T is_STATE_HANDLER; // '/SupervisorRX State Handler' - uint8_T is_active_FAULT_HANDLER; // '/SupervisorRX State Handler' - uint8_T is_active_FaultButtonPressed;// '/SupervisorRX State Handler' - uint8_T is_FaultButtonPressed; // '/SupervisorRX State Handler' - uint8_T is_active_OverCurrent; // '/SupervisorRX State Handler' - uint8_T is_OverCurrent; // '/SupervisorRX State Handler' - uint8_T is_active_CAN_MESSAGES_FOR_LOOP;// '/SupervisorRX State Handler' - uint8_T is_CAN_MESSAGES_FOR_LOOP; // '/SupervisorRX State Handler' - uint8_T is_active_c11_SupervisorFSM_RX;// '/CAN Event Dispatcher' - uint8_T is_active_c3_SupervisorFSM_RX;// '/Chart' - uint8_T is_active_c14_SupervisorFSM_RX;// '/Chart' - uint8_T is_active_c12_SupervisorFSM_RX;// '/ControlMode_SM_motor0' - uint8_T is_c12_SupervisorFSM_RX; // '/ControlMode_SM_motor0' - uint8_T is_active_c4_SupervisorFSM_RX;// '/Chart1' - boolean_T ExternalFlags_fault_button_prev;// '/SupervisorRX State Handler' - boolean_T ExternalFlags_fault_button_start;// '/SupervisorRX State Handler' -}; - -// Invariant block signals for model 'SupervisorFSM_RX' -struct ConstB_SupervisorFSM_RX_h_T { - boolean_T Constant5; // '/Constant5' -}; - -// Real-time Model Data Structure -struct tag_RTM_SupervisorFSM_RX_T { - const char_T **errorStatus; -}; - -// Macros for accessing real-time model data structure -#ifndef rtmGetErrorStatus -#define rtmGetErrorStatus(rtm) (*((rtm)->errorStatus)) -#endif - -#ifndef rtmSetErrorStatus -#define rtmSetErrorStatus(rtm, val) (*((rtm)->errorStatus) = (val)) -#endif - -#ifndef rtmGetErrorStatusPointer -#define rtmGetErrorStatusPointer(rtm) (rtm)->errorStatus -#endif - -#ifndef rtmSetErrorStatusPointer -#define rtmSetErrorStatusPointer(rtm, val) ((rtm)->errorStatus = (val)) -#endif - -struct MdlrefDW_SupervisorFSM_RX_T { - RT_MODEL_SupervisorFSM_RX_T rtm; -}; - -extern void SupervisorFSM_RX_SetpointHandler(void); -extern void SupervisorFSM_RX_ControlModeHandlerMotor0(void); -extern void SupervisorFSM_RX_LimitsHandler(void); -extern void SupervisorFSM_RX_PIDHandler(void); -extern void SupervisorFSM_RX_CANMessageHandler(void); - -// Invariant block signals (default storage) -extern const ConstB_SupervisorFSM_RX_h_T SupervisorFSM_RX_ConstB; -extern MdlrefDW_SupervisorFSM_RX_T SupervisorFSM_RX_MdlrefDW; - -// Block signals (default storage) -extern B_SupervisorFSM_RX_c_T SupervisorFSM_RX_B; - -// Block states (default storage) -extern DW_SupervisorFSM_RX_f_T SupervisorFSM_RX_DW; - -// Declare global variables for system: model 'SupervisorFSM_RX' -extern const SensorsData *SupervisorFSM_RX_rtu_SensorsData;// '/SensorsData' -extern const ControlOutputs *SupervisorFSM_RX_rtu_ControlOutputs;// '/ControlOutputs' -extern const BUS_MESSAGES_RX_MULTIPLE *SupervisorFSM_RX_rtu_MessagesRx;// '/MessagesRx' -extern const BUS_STATUS_RX_MULTIPLE *SupervisorFSM_RX_rtu_StatusRx;// '/StatusRx' -extern const BUS_CAN_RX_ERRORS_MULTIPLE *SupervisorFSM_RX_rtu_ErrorsRx;// '/ErrorsRx' - -#endif // RTW_HEADER_SupervisorFSM_RX_private_h_ - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/emBODY/eBcode/arch-arm/mbd/amcbldc/supervisor-rx/SupervisorFSM_RX_types.h b/emBODY/eBcode/arch-arm/mbd/amcbldc/supervisor-rx/SupervisorFSM_RX_types.h deleted file mode 100644 index 74ad19b19..000000000 --- a/emBODY/eBcode/arch-arm/mbd/amcbldc/supervisor-rx/SupervisorFSM_RX_types.h +++ /dev/null @@ -1,605 +0,0 @@ -// -// Non-Degree Granting Education License -- for use at non-degree -// granting, nonprofit, education, and research organizations only. Not -// for commercial or industrial use. -// -// File: SupervisorFSM_RX_types.h -// -// Code generated for Simulink model 'SupervisorFSM_RX'. -// -// Model version : 7.5 -// Simulink Coder version : 23.2 (R2023b) 01-Aug-2023 -// C/C++ source code generated on : Tue Feb 13 11:54:08 2024 -// -// Target selection: ert.tlc -// Embedded hardware selection: ARM Compatible->ARM Cortex-M -// Code generation objectives: Unspecified -// Validation result: Not run -// -#ifndef RTW_HEADER_SupervisorFSM_RX_types_h_ -#define RTW_HEADER_SupervisorFSM_RX_types_h_ -#include "rtwtypes.h" -#ifndef DEFINED_TYPEDEF_FOR_JointPositions_ -#define DEFINED_TYPEDEF_FOR_JointPositions_ - -struct JointPositions -{ - // joint positions - real32_T position; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_MotorSensors_ -#define DEFINED_TYPEDEF_FOR_MotorSensors_ - -struct MotorSensors -{ - real32_T Iabc[3]; - - // electrical angle = angle * pole_pairs - real32_T angle; - real32_T temperature; - real32_T voltage; - real32_T current; - uint8_T hallABC; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_SupplyVoltage_ -#define DEFINED_TYPEDEF_FOR_SupplyVoltage_ - -struct SupplyVoltage -{ - real32_T voltage; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_SensorsData_ -#define DEFINED_TYPEDEF_FOR_SensorsData_ - -struct SensorsData -{ - // position encoders - JointPositions jointpositions; - - // motor probes - MotorSensors motorsensors; - - // supply probes - SupplyVoltage supplyvoltagesensors; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_ExternalFlags_ -#define DEFINED_TYPEDEF_FOR_ExternalFlags_ - -struct ExternalFlags -{ - // External Fault Button (1 == pressed) - boolean_T fault_button; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_MotorCurrent_ -#define DEFINED_TYPEDEF_FOR_MotorCurrent_ - -struct MotorCurrent -{ - // motor current - real32_T current; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_ControlOutputs_ -#define DEFINED_TYPEDEF_FOR_ControlOutputs_ - -struct ControlOutputs -{ - // control effort (quadrature) - real32_T Vq; - - // control effort (3-phases) - real32_T Vabc[3]; - - // quadrature current - MotorCurrent Iq_fbk; - - // direct current - MotorCurrent Id_fbk; - - // RMS of Iq - MotorCurrent Iq_rms; - - // RMS of Id - MotorCurrent Id_rms; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_MCControlModes_ -#define DEFINED_TYPEDEF_FOR_MCControlModes_ - -typedef enum { - MCControlModes_Idle = 0, // Default value - MCControlModes_OpenLoop = 80, - MCControlModes_SpeedVoltage = 10, - MCControlModes_SpeedCurrent = 11, - MCControlModes_Current = 6, - MCControlModes_NotConfigured = 176, - MCControlModes_HWFault = 160 -} MCControlModes; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_BUS_MSG_CONTROL_MODE_ -#define DEFINED_TYPEDEF_FOR_BUS_MSG_CONTROL_MODE_ - -// Fields of a CONTROL_MODE message. -struct BUS_MSG_CONTROL_MODE -{ - // Motor selector. - boolean_T motor; - - // Control mode. - MCControlModes mode; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_BUS_MSG_CURRENT_LIMIT_ -#define DEFINED_TYPEDEF_FOR_BUS_MSG_CURRENT_LIMIT_ - -// Fields of a CURRENT_LIMIT message. -struct BUS_MSG_CURRENT_LIMIT -{ - // Motor selector. - boolean_T motor; - - // Nominal current in A. - real32_T nominal; - - // Peak current in A. - real32_T peak; - - // Overload current in A. - real32_T overload; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_BUS_MSG_DESIRED_TARGETS_ -#define DEFINED_TYPEDEF_FOR_BUS_MSG_DESIRED_TARGETS_ - -// Fields of a DESIRED_TARGETS message. -struct BUS_MSG_DESIRED_TARGETS -{ - // Target current in A. - real32_T current; - - // Target voltage in %. - real32_T voltage; - - // Target veocity in deg/s. - real32_T velocity; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_BUS_MSG_PID_ -#define DEFINED_TYPEDEF_FOR_BUS_MSG_PID_ - -// Fields of a CURRENT_PID message. -struct BUS_MSG_PID -{ - // Motor selector. - boolean_T motor; - - // Proportional gain. - real32_T Kp; - - // Integral gain. - real32_T Ki; - - // Derivative gain. - real32_T Kd; - - // Shift factor. - uint8_T Ks; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_BUS_MSG_MOTOR_CONFIG_ -#define DEFINED_TYPEDEF_FOR_BUS_MSG_MOTOR_CONFIG_ - -struct BUS_MSG_MOTOR_CONFIG -{ - boolean_T has_hall_sens; - boolean_T has_quadrature_encoder; - boolean_T has_speed_quadrature_encoder; - boolean_T has_torque_sens; - boolean_T use_index; - boolean_T enable_verbosity; - - // Number of polese of the motor. - uint8_T number_poles; - - // Encoder tolerance. - uint8_T encoder_tolerance; - - // Resolution of rotor encoder. - int16_T rotor_encoder_resolution; - - // Offset of the rotor encoder. - int16_T rotor_index_offset; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_BUS_MESSAGES_RX_ -#define DEFINED_TYPEDEF_FOR_BUS_MESSAGES_RX_ - -// Aggregate of all CAN received messages. -struct BUS_MESSAGES_RX -{ - BUS_MSG_CONTROL_MODE control_mode; - BUS_MSG_CURRENT_LIMIT current_limit; - BUS_MSG_DESIRED_TARGETS desired_targets; - BUS_MSG_PID pid; - BUS_MSG_MOTOR_CONFIG motor_config; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_BUS_MESSAGES_RX_MULTIPLE_ -#define DEFINED_TYPEDEF_FOR_BUS_MESSAGES_RX_MULTIPLE_ - -struct BUS_MESSAGES_RX_MULTIPLE -{ - BUS_MESSAGES_RX messages[4]; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_JointVelocities_ -#define DEFINED_TYPEDEF_FOR_JointVelocities_ - -struct JointVelocities -{ - // joint velocities - real32_T velocity; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_MotorTemperature_ -#define DEFINED_TYPEDEF_FOR_MotorTemperature_ - -struct MotorTemperature -{ - // motor temperature - real32_T temperature; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_EstimatedData_ -#define DEFINED_TYPEDEF_FOR_EstimatedData_ - -struct EstimatedData -{ - // velocity - JointVelocities jointvelocities; - - // filtered motor current - MotorCurrent Iq_filtered; - - // motor temperature - MotorTemperature motor_temperature; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_BUS_STATUS_RX_ -#define DEFINED_TYPEDEF_FOR_BUS_STATUS_RX_ - -// Aggregate of all events specifying types of received messages. -struct BUS_STATUS_RX -{ - boolean_T control_mode; - boolean_T current_limit; - boolean_T desired_targets; - boolean_T current_pid; - boolean_T velocity_pid; - boolean_T motor_config; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_BUS_STATUS_RX_MULTIPLE_ -#define DEFINED_TYPEDEF_FOR_BUS_STATUS_RX_MULTIPLE_ - -struct BUS_STATUS_RX_MULTIPLE -{ - BUS_STATUS_RX status[4]; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_CANErrorTypes_ -#define DEFINED_TYPEDEF_FOR_CANErrorTypes_ - -typedef enum { - CANErrorTypes_No_Error = 0, // Default value - CANErrorTypes_Packet_Not4Us, - CANErrorTypes_Packet_Unrecognized, - CANErrorTypes_Packet_Malformed, - CANErrorTypes_Packet_MultiFunctionsDetected, - CANErrorTypes_Mode_Unrecognized -} CANErrorTypes; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_BUS_CAN_RX_ERRORS_ -#define DEFINED_TYPEDEF_FOR_BUS_CAN_RX_ERRORS_ - -// Specifies the CAN error types. -struct BUS_CAN_RX_ERRORS -{ - // True if an error has been detected. - boolean_T event; - CANErrorTypes type; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_BUS_CAN_RX_ERRORS_MULTIPLE_ -#define DEFINED_TYPEDEF_FOR_BUS_CAN_RX_ERRORS_MULTIPLE_ - -struct BUS_CAN_RX_ERRORS_MULTIPLE -{ - BUS_CAN_RX_ERRORS errors[4]; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_ControlModes_ -#define DEFINED_TYPEDEF_FOR_ControlModes_ - -typedef enum { - ControlModes_NotConfigured = 0, // Default value - ControlModes_Idle, - ControlModes_Position, - ControlModes_PositionDirect, - ControlModes_Current, - ControlModes_Velocity, - ControlModes_Voltage, - ControlModes_Torque, - ControlModes_HwFaultCM -} ControlModes; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_Flags_ -#define DEFINED_TYPEDEF_FOR_Flags_ - -struct Flags -{ - // control mode - ControlModes control_mode; - boolean_T enable_sending_msg_status; - boolean_T fault_button; - boolean_T enable_thermal_protection; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_MotorConfig_ -#define DEFINED_TYPEDEF_FOR_MotorConfig_ - -struct MotorConfig -{ - // Angular offset in degrees between the stator windings and the hall sensors. - real32_T hall_sens_offset; - boolean_T has_hall_sens; - boolean_T has_quadrature_encoder; - boolean_T has_speed_quadrature_encoder; - boolean_T has_torque_sens; - boolean_T use_index; - boolean_T enable_verbosity; - int16_T rotor_encoder_resolution; - int16_T rotor_index_offset; - uint8_T encoder_tolerance; - uint8_T pole_pairs; - real32_T Kbemf; - real32_T Rphase; - real32_T Imin; - real32_T Imax; - real32_T Vmax; - real32_T resistance; - real32_T inductance; - real32_T thermal_resistance; - real32_T thermal_time_constant; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_EstimationVelocityModes_ -#define DEFINED_TYPEDEF_FOR_EstimationVelocityModes_ - -typedef enum { - EstimationVelocityModes_Disabled = 0,// Default value - EstimationVelocityModes_MovingAverage, - EstimationVelocityModes_LeastSquares -} EstimationVelocityModes; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_EstimationConfig_ -#define DEFINED_TYPEDEF_FOR_EstimationConfig_ - -struct EstimationConfig -{ - EstimationVelocityModes velocity_mode; - - // Forgetting factor in [0, 1] for exponential weighting-based estimation of RMS current value - real32_T current_rms_lambda; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_PIDConfig_ -#define DEFINED_TYPEDEF_FOR_PIDConfig_ - -struct PIDConfig -{ - real32_T OutMax; - real32_T OutMin; - real32_T P; - real32_T I; - real32_T D; - real32_T N; - real32_T I0; - real32_T D0; - uint8_T shift_factor; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_Thresholds_ -#define DEFINED_TYPEDEF_FOR_Thresholds_ - -struct Thresholds -{ - // It shall be greater than hardwareJntPosMin - real32_T jntPosMin; - - // It shall be smaller than hardwareJntPosMax - real32_T jntPosMax; - - // Imposed by hardware constraint - real32_T hardwareJntPosMin; - - // Imposed by hardware constraint - real32_T hardwareJntPosMax; - - // If robotMin == rotorMax == 0, there's no check - real32_T rotorPosMin; - - // If robotMin == rotorMax == 0, there's no check - real32_T rotorPosMax; - - // Can be only non-negative - real32_T jntVelMax; - - // Timeout on reception of velocity setpoint - // Can be only non-negative - uint32_T velocityTimeout; - - // Current that can be kept for an indefinite period of time w/o damaging the motor - // Expressed in [A] as all the internal computations are done this way - // Can be only non-negative - real32_T motorNominalCurrents; - - // Current that can be applied for a short period of time - // Expressed in [A] as all the internal computations are done this way - // Can be only non-negative - real32_T motorPeakCurrents; - - // Currents over this threshold can instantaneously damages the motor - // Expressed in [A] as all the internal computations are done this way - // Can be only non-negative - real32_T motorOverloadCurrents; - - // Expressed in ticks - // Max value is 32000 - // Can be only non-negative - uint32_T motorPwmLimit; - - // The critical temperature of the motor that triggers i2t current protection. - real32_T motorCriticalTemperature; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_ConfigurationParameters_ -#define DEFINED_TYPEDEF_FOR_ConfigurationParameters_ - -struct ConfigurationParameters -{ - MotorConfig motorconfig; - EstimationConfig estimationconfig; - PIDConfig CurLoopPID; - PIDConfig PosLoopPID; - PIDConfig VelLoopPID; - PIDConfig DirLoopPID; - Thresholds thresholds; - real32_T environment_temperature; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_MotorVoltage_ -#define DEFINED_TYPEDEF_FOR_MotorVoltage_ - -struct MotorVoltage -{ - // motor voltage - real32_T voltage; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_Targets_ -#define DEFINED_TYPEDEF_FOR_Targets_ - -struct Targets -{ - JointPositions jointpositions; - JointVelocities jointvelocities; - MotorCurrent motorcurrent; - MotorVoltage motorvoltage; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_SV_Limits_ -#define DEFINED_TYPEDEF_FOR_SV_Limits_ - -struct SV_Limits -{ - real32_T overload; - real32_T peak; - real32_T nominal; - ControlModes type; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_BoardState_ -#define DEFINED_TYPEDEF_FOR_BoardState_ - -typedef enum { - BoardState_NotConfigured = 0, // Default value - BoardState_Configured, - BoardState_Fault -} BoardState; - -#endif - -// Forward declaration for rtModel -typedef struct tag_RTM_SupervisorFSM_RX_T RT_MODEL_SupervisorFSM_RX_T; - -#endif // RTW_HEADER_SupervisorFSM_RX_types_h_ - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/emBODY/eBcode/arch-arm/mbd/amcbldc/supervisor-tx/SupervisorFSM_TX.cpp b/emBODY/eBcode/arch-arm/mbd/amcbldc/supervisor-tx/SupervisorFSM_TX.cpp index 808768a11..5f11bd907 100644 --- a/emBODY/eBcode/arch-arm/mbd/amcbldc/supervisor-tx/SupervisorFSM_TX.cpp +++ b/emBODY/eBcode/arch-arm/mbd/amcbldc/supervisor-tx/SupervisorFSM_TX.cpp @@ -7,9 +7,9 @@ // // Code generated for Simulink model 'SupervisorFSM_TX'. // -// Model version : 7.3 +// Model version : 7.11 // Simulink Coder version : 23.2 (R2023b) 01-Aug-2023 -// C/C++ source code generated on : Tue Feb 13 11:54:17 2024 +// C/C++ source code generated on : Wed Feb 28 11:27:46 2024 // // Target selection: ert.tlc // Embedded hardware selection: ARM Compatible->ARM Cortex-M @@ -18,16 +18,9 @@ // #include "SupervisorFSM_TX.h" #include "SupervisorFSM_TX_types.h" +#include "rtwtypes.h" #include "SupervisorFSM_TX_private.h" -MdlrefDW_SupervisorFSM_TX_T SupervisorFSM_TX_MdlrefDW; - -// Block signals (default storage) -B_SupervisorFSM_TX_c_T SupervisorFSM_TX_B; - -// Block states (default storage) -DW_SupervisorFSM_TX_f_T SupervisorFSM_TX_DW; - // Forward declaration for local functions static MCControlModes SupervisorFSM_TX_convert(ControlModes controlmode); @@ -114,32 +107,36 @@ void SupervisorFSM_TX_Init(BUS_MESSAGES_TX *rty_MessagesTx) // Output and update for referenced model: 'SupervisorFSM_TX' void SupervisorFSM_TX(const SensorsData *rtu_SensorsData, const EstimatedData *rtu_EstimatedData, const Flags *rtu_Flags, const - ControlOutputs *rtu_ControlOutputs, BUS_MESSAGES_TX - *rty_MessagesTx, BUS_STATUS_TX *rty_StatusTx) + FOCOutputs *rtu_ControlOutputs, const boolean_T + *rtu_ExternalFlags_fault_button, BUS_MESSAGES_TX + *rty_MessagesTx, BUS_STATUS_TX *rty_StatusTx, + B_SupervisorFSM_TX_c_T *localB, DW_SupervisorFSM_TX_f_T + *localDW) { // Chart: '/SupervisorFSM_TX' - if (SupervisorFSM_TX_DW.is_active_c3_SupervisorFSM_TX == 0U) { - SupervisorFSM_TX_DW.is_active_c3_SupervisorFSM_TX = 1U; + if (localDW->is_active_c3_SupervisorFSM_TX == 0U) { + localDW->is_active_c3_SupervisorFSM_TX = 1U; } else if (rtu_Flags->enable_sending_msg_status) { - rty_MessagesTx->foc.current = rtu_EstimatedData->Iq_filtered.current; - rty_MessagesTx->foc.velocity = rtu_EstimatedData->jointvelocities.velocity; - rty_MessagesTx->foc.position = rtu_SensorsData->jointpositions.position; - SupervisorFSM_TX_DW.ev_focEventCounter++; + rty_MessagesTx->foc.current = rtu_EstimatedData->Iq_filtered; + rty_MessagesTx->foc.velocity = rtu_EstimatedData->jointvelocities; + rty_MessagesTx->foc.position = rtu_SensorsData->jointpositions; + localDW->ev_focEventCounter++; rty_MessagesTx->status.control_mode = SupervisorFSM_TX_convert (rtu_Flags->control_mode); rty_MessagesTx->status.pwm_fbk = rtu_ControlOutputs->Vq; - rty_MessagesTx->status.flags.ExternalFaultAsserted = rtu_Flags->fault_button; - SupervisorFSM_TX_DW.ev_statusEventCounter++; + rty_MessagesTx->status.flags.ExternalFaultAsserted = + *rtu_ExternalFlags_fault_button; + localDW->ev_statusEventCounter++; } - if (SupervisorFSM_TX_DW.ev_focEventCounter > 0U) { - SupervisorFSM_TX_B.ev_foc = !SupervisorFSM_TX_B.ev_foc; - SupervisorFSM_TX_DW.ev_focEventCounter--; + if (localDW->ev_focEventCounter > 0U) { + localB->ev_foc = !localB->ev_foc; + localDW->ev_focEventCounter--; } - if (SupervisorFSM_TX_DW.ev_statusEventCounter > 0U) { - SupervisorFSM_TX_B.ev_status = !SupervisorFSM_TX_B.ev_status; - SupervisorFSM_TX_DW.ev_statusEventCounter--; + if (localDW->ev_statusEventCounter > 0U) { + localB->ev_status = !localB->ev_status; + localDW->ev_statusEventCounter--; } // End of Chart: '/SupervisorFSM_TX' @@ -151,8 +148,7 @@ void SupervisorFSM_TX(const SensorsData *rtu_SensorsData, const EstimatedData // // Store in Global RAM - rty_StatusTx->foc = (SupervisorFSM_TX_B.ev_foc != - SupervisorFSM_TX_DW.DelayInput1_DSTATE); + rty_StatusTx->foc = (localB->ev_foc != localDW->DelayInput1_DSTATE); // RelationalOperator: '/FixPt Relational Operator' incorporates: // UnitDelay: '/Delay Input1' @@ -161,8 +157,7 @@ void SupervisorFSM_TX(const SensorsData *rtu_SensorsData, const EstimatedData // // Store in Global RAM - rty_StatusTx->status = (SupervisorFSM_TX_B.ev_status != - SupervisorFSM_TX_DW.DelayInput1_DSTATE_d); + rty_StatusTx->status = (localB->ev_status != localDW->DelayInput1_DSTATE_d); // Update for UnitDelay: '/Delay Input1' // @@ -170,7 +165,7 @@ void SupervisorFSM_TX(const SensorsData *rtu_SensorsData, const EstimatedData // // Store in Global RAM - SupervisorFSM_TX_DW.DelayInput1_DSTATE = SupervisorFSM_TX_B.ev_foc; + localDW->DelayInput1_DSTATE = localB->ev_foc; // Update for UnitDelay: '/Delay Input1' // @@ -178,15 +173,13 @@ void SupervisorFSM_TX(const SensorsData *rtu_SensorsData, const EstimatedData // // Store in Global RAM - SupervisorFSM_TX_DW.DelayInput1_DSTATE_d = SupervisorFSM_TX_B.ev_status; + localDW->DelayInput1_DSTATE_d = localB->ev_status; } // Model initialize function -void SupervisorFSM_TX_initialize(const char_T **rt_errorStatus) +void SupervisorFSM_TX_initialize(const char_T **rt_errorStatus, + RT_MODEL_SupervisorFSM_TX_T *const SupervisorFSM_TX_M) { - RT_MODEL_SupervisorFSM_TX_T *const SupervisorFSM_TX_M = - &(SupervisorFSM_TX_MdlrefDW.rtm); - // Registration code // initialize error status diff --git a/emBODY/eBcode/arch-arm/mbd/amcbldc/supervisor-tx/SupervisorFSM_TX.h b/emBODY/eBcode/arch-arm/mbd/amcbldc/supervisor-tx/SupervisorFSM_TX.h index 7dd6de719..b696b0f42 100644 --- a/emBODY/eBcode/arch-arm/mbd/amcbldc/supervisor-tx/SupervisorFSM_TX.h +++ b/emBODY/eBcode/arch-arm/mbd/amcbldc/supervisor-tx/SupervisorFSM_TX.h @@ -7,9 +7,9 @@ // // Code generated for Simulink model 'SupervisorFSM_TX'. // -// Model version : 7.3 +// Model version : 7.11 // Simulink Coder version : 23.2 (R2023b) 01-Aug-2023 -// C/C++ source code generated on : Tue Feb 13 11:54:17 2024 +// C/C++ source code generated on : Wed Feb 28 11:27:46 2024 // // Target selection: ert.tlc // Embedded hardware selection: ARM Compatible->ARM Cortex-M @@ -21,14 +21,41 @@ #include "rtwtypes.h" #include "SupervisorFSM_TX_types.h" -extern void SupervisorFSM_TX_Init(BUS_MESSAGES_TX *rty_MessagesTx); -extern void SupervisorFSM_TX(const SensorsData *rtu_SensorsData, const - EstimatedData *rtu_EstimatedData, const Flags *rtu_Flags, const ControlOutputs - *rtu_ControlOutputs, BUS_MESSAGES_TX *rty_MessagesTx, BUS_STATUS_TX - *rty_StatusTx); +// Block signals for model 'SupervisorFSM_TX' +struct B_SupervisorFSM_TX_c_T { + boolean_T ev_foc; // '/SupervisorFSM_TX' + boolean_T ev_status; // '/SupervisorFSM_TX' +}; + +// Block states (default storage) for model 'SupervisorFSM_TX' +struct DW_SupervisorFSM_TX_f_T { + uint32_T ev_focEventCounter; // '/SupervisorFSM_TX' + uint32_T ev_statusEventCounter; // '/SupervisorFSM_TX' + boolean_T DelayInput1_DSTATE; // '/Delay Input1' + boolean_T DelayInput1_DSTATE_d; // '/Delay Input1' + uint8_T is_active_c3_SupervisorFSM_TX;// '/SupervisorFSM_TX' +}; + +// Real-time Model Data Structure +struct tag_RTM_SupervisorFSM_TX_T { + const char_T **errorStatus; +}; + +struct MdlrefDW_SupervisorFSM_TX_T { + B_SupervisorFSM_TX_c_T rtb; + DW_SupervisorFSM_TX_f_T rtdw; + RT_MODEL_SupervisorFSM_TX_T rtm; +}; // Model reference registration function -extern void SupervisorFSM_TX_initialize(const char_T **rt_errorStatus); +extern void SupervisorFSM_TX_initialize(const char_T **rt_errorStatus, + RT_MODEL_SupervisorFSM_TX_T *const SupervisorFSM_TX_M); +extern void SupervisorFSM_TX_Init(BUS_MESSAGES_TX *rty_MessagesTx); +extern void SupervisorFSM_TX(const SensorsData *rtu_SensorsData, const + EstimatedData *rtu_EstimatedData, const Flags *rtu_Flags, const FOCOutputs + *rtu_ControlOutputs, const boolean_T *rtu_ExternalFlags_fault_button, + BUS_MESSAGES_TX *rty_MessagesTx, BUS_STATUS_TX *rty_StatusTx, + B_SupervisorFSM_TX_c_T *localB, DW_SupervisorFSM_TX_f_T *localDW); //- // The generated code includes comments that allow you to trace directly diff --git a/emBODY/eBcode/arch-arm/mbd/amcbldc/supervisor-tx/SupervisorFSM_TX_private.h b/emBODY/eBcode/arch-arm/mbd/amcbldc/supervisor-tx/SupervisorFSM_TX_private.h index e92134be3..ae8c2c887 100644 --- a/emBODY/eBcode/arch-arm/mbd/amcbldc/supervisor-tx/SupervisorFSM_TX_private.h +++ b/emBODY/eBcode/arch-arm/mbd/amcbldc/supervisor-tx/SupervisorFSM_TX_private.h @@ -7,9 +7,9 @@ // // Code generated for Simulink model 'SupervisorFSM_TX'. // -// Model version : 7.3 +// Model version : 7.11 // Simulink Coder version : 23.2 (R2023b) 01-Aug-2023 -// C/C++ source code generated on : Tue Feb 13 11:54:17 2024 +// C/C++ source code generated on : Wed Feb 28 11:27:46 2024 // // Target selection: ert.tlc // Embedded hardware selection: ARM Compatible->ARM Cortex-M @@ -21,26 +21,6 @@ #include "rtwtypes.h" #include "SupervisorFSM_TX_types.h" -// Block signals for model 'SupervisorFSM_TX' -struct B_SupervisorFSM_TX_c_T { - boolean_T ev_foc; // '/SupervisorFSM_TX' - boolean_T ev_status; // '/SupervisorFSM_TX' -}; - -// Block states (default storage) for model 'SupervisorFSM_TX' -struct DW_SupervisorFSM_TX_f_T { - uint32_T ev_focEventCounter; // '/SupervisorFSM_TX' - uint32_T ev_statusEventCounter; // '/SupervisorFSM_TX' - boolean_T DelayInput1_DSTATE; // '/Delay Input1' - boolean_T DelayInput1_DSTATE_d; // '/Delay Input1' - uint8_T is_active_c3_SupervisorFSM_TX;// '/SupervisorFSM_TX' -}; - -// Real-time Model Data Structure -struct tag_RTM_SupervisorFSM_TX_T { - const char_T **errorStatus; -}; - // Macros for accessing real-time model data structure #ifndef rtmGetErrorStatus #define rtmGetErrorStatus(rtm) (*((rtm)->errorStatus)) @@ -57,19 +37,6 @@ struct tag_RTM_SupervisorFSM_TX_T { #ifndef rtmSetErrorStatusPointer #define rtmSetErrorStatusPointer(rtm, val) ((rtm)->errorStatus = (val)) #endif - -struct MdlrefDW_SupervisorFSM_TX_T { - RT_MODEL_SupervisorFSM_TX_T rtm; -}; - -extern MdlrefDW_SupervisorFSM_TX_T SupervisorFSM_TX_MdlrefDW; - -// Block signals (default storage) -extern B_SupervisorFSM_TX_c_T SupervisorFSM_TX_B; - -// Block states (default storage) -extern DW_SupervisorFSM_TX_f_T SupervisorFSM_TX_DW; - #endif // RTW_HEADER_SupervisorFSM_TX_private_h_ // diff --git a/emBODY/eBcode/arch-arm/mbd/amcbldc/supervisor-tx/SupervisorFSM_TX_types.h b/emBODY/eBcode/arch-arm/mbd/amcbldc/supervisor-tx/SupervisorFSM_TX_types.h index 158d1c82e..d4d67377a 100644 --- a/emBODY/eBcode/arch-arm/mbd/amcbldc/supervisor-tx/SupervisorFSM_TX_types.h +++ b/emBODY/eBcode/arch-arm/mbd/amcbldc/supervisor-tx/SupervisorFSM_TX_types.h @@ -7,9 +7,9 @@ // // Code generated for Simulink model 'SupervisorFSM_TX'. // -// Model version : 7.3 +// Model version : 7.11 // Simulink Coder version : 23.2 (R2023b) 01-Aug-2023 -// C/C++ source code generated on : Tue Feb 13 11:54:17 2024 +// C/C++ source code generated on : Wed Feb 28 11:27:46 2024 // // Target selection: ert.tlc // Embedded hardware selection: ARM Compatible->ARM Cortex-M @@ -19,13 +19,12 @@ #ifndef RTW_HEADER_SupervisorFSM_TX_types_h_ #define RTW_HEADER_SupervisorFSM_TX_types_h_ #include "rtwtypes.h" -#ifndef DEFINED_TYPEDEF_FOR_JointPositions_ -#define DEFINED_TYPEDEF_FOR_JointPositions_ +#ifndef DEFINED_TYPEDEF_FOR_DriverSensors_ +#define DEFINED_TYPEDEF_FOR_DriverSensors_ -struct JointPositions +struct DriverSensors { - // joint positions - real32_T position; + real32_T Vcc; }; #endif @@ -47,62 +46,15 @@ struct MotorSensors #endif -#ifndef DEFINED_TYPEDEF_FOR_SupplyVoltage_ -#define DEFINED_TYPEDEF_FOR_SupplyVoltage_ - -struct SupplyVoltage -{ - real32_T voltage; -}; - -#endif - #ifndef DEFINED_TYPEDEF_FOR_SensorsData_ #define DEFINED_TYPEDEF_FOR_SensorsData_ struct SensorsData { // position encoders - JointPositions jointpositions; - - // motor probes + real32_T jointpositions; + DriverSensors driversensors; MotorSensors motorsensors; - - // supply probes - SupplyVoltage supplyvoltagesensors; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_JointVelocities_ -#define DEFINED_TYPEDEF_FOR_JointVelocities_ - -struct JointVelocities -{ - // joint velocities - real32_T velocity; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_MotorCurrent_ -#define DEFINED_TYPEDEF_FOR_MotorCurrent_ - -struct MotorCurrent -{ - // motor current - real32_T current; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_MotorTemperature_ -#define DEFINED_TYPEDEF_FOR_MotorTemperature_ - -struct MotorTemperature -{ - // motor temperature - real32_T temperature; }; #endif @@ -113,13 +65,13 @@ struct MotorTemperature struct EstimatedData { // velocity - JointVelocities jointvelocities; + real32_T jointvelocities; // filtered motor current - MotorCurrent Iq_filtered; + real32_T Iq_filtered; // motor temperature - MotorTemperature motor_temperature; + real32_T motor_temperature; }; #endif @@ -135,7 +87,6 @@ typedef enum { ControlModes_Current, ControlModes_Velocity, ControlModes_Voltage, - ControlModes_Torque, ControlModes_HwFaultCM } ControlModes; @@ -146,19 +97,19 @@ typedef enum { struct Flags { + boolean_T enable_sending_msg_status; + // control mode ControlModes control_mode; - boolean_T enable_sending_msg_status; - boolean_T fault_button; boolean_T enable_thermal_protection; }; #endif -#ifndef DEFINED_TYPEDEF_FOR_ControlOutputs_ -#define DEFINED_TYPEDEF_FOR_ControlOutputs_ +#ifndef DEFINED_TYPEDEF_FOR_FOCOutputs_ +#define DEFINED_TYPEDEF_FOR_FOCOutputs_ -struct ControlOutputs +struct FOCOutputs { // control effort (quadrature) real32_T Vq; @@ -167,79 +118,60 @@ struct ControlOutputs real32_T Vabc[3]; // quadrature current - MotorCurrent Iq_fbk; + real32_T Iq_fbk; // direct current - MotorCurrent Id_fbk; + real32_T Id_fbk; // RMS of Iq - MotorCurrent Iq_rms; + real32_T Iq_rms; // RMS of Id - MotorCurrent Id_rms; + real32_T Id_rms; }; #endif -#ifndef DEFINED_TYPEDEF_FOR_MotorConfig_ -#define DEFINED_TYPEDEF_FOR_MotorConfig_ +#ifndef DEFINED_TYPEDEF_FOR_Thresholds_ +#define DEFINED_TYPEDEF_FOR_Thresholds_ -struct MotorConfig +struct Thresholds { - // Angular offset in degrees between the stator windings and the hall sensors. - real32_T hall_sens_offset; - boolean_T has_hall_sens; - boolean_T has_quadrature_encoder; - boolean_T has_speed_quadrature_encoder; - boolean_T has_torque_sens; - boolean_T use_index; - boolean_T enable_verbosity; - int16_T rotor_encoder_resolution; - int16_T rotor_index_offset; - uint8_T encoder_tolerance; - uint8_T pole_pairs; - real32_T Kbemf; - real32_T Rphase; - real32_T Imin; - real32_T Imax; - real32_T Vmax; - real32_T resistance; - real32_T inductance; - real32_T thermal_resistance; - real32_T thermal_time_constant; -}; - -#endif - -#ifndef DEFINED_TYPEDEF_FOR_EstimationVelocityModes_ -#define DEFINED_TYPEDEF_FOR_EstimationVelocityModes_ + // Can be only non-negative + real32_T jntVelMax; -typedef enum { - EstimationVelocityModes_Disabled = 0,// Default value - EstimationVelocityModes_MovingAverage, - EstimationVelocityModes_LeastSquares -} EstimationVelocityModes; + // Current that can be kept for an indefinite period of time w/o damaging the motor + // Expressed in [A] as all the internal computations are done this way + // Can be only non-negative + real32_T motorNominalCurrents; -#endif + // Current that can be applied for a short period of time + // Expressed in [A] as all the internal computations are done this way + // Can be only non-negative + real32_T motorPeakCurrents; -#ifndef DEFINED_TYPEDEF_FOR_EstimationConfig_ -#define DEFINED_TYPEDEF_FOR_EstimationConfig_ + // Currents over this threshold can instantaneously damages the motor + // Expressed in [A] as all the internal computations are done this way + // Can be only non-negative + real32_T motorOverloadCurrents; -struct EstimationConfig -{ - EstimationVelocityModes velocity_mode; + // Expressed in ticks + // Max value is 32000 + // Can be only non-negative + uint32_T motorPwmLimit; - // Forgetting factor in [0, 1] for exponential weighting-based estimation of RMS current value - real32_T current_rms_lambda; + // The critical temperature of the motor that triggers i2t current protection. + real32_T motorCriticalTemperature; }; #endif -#ifndef DEFINED_TYPEDEF_FOR_PIDConfig_ -#define DEFINED_TYPEDEF_FOR_PIDConfig_ +#ifndef DEFINED_TYPEDEF_FOR_PID_ +#define DEFINED_TYPEDEF_FOR_PID_ -struct PIDConfig +struct PID { + ControlModes type; real32_T OutMax; real32_T OutMin; real32_T P; @@ -253,75 +185,56 @@ struct PIDConfig #endif -#ifndef DEFINED_TYPEDEF_FOR_Thresholds_ -#define DEFINED_TYPEDEF_FOR_Thresholds_ +#ifndef DEFINED_TYPEDEF_FOR_PIDsConfiguration_ +#define DEFINED_TYPEDEF_FOR_PIDsConfiguration_ -struct Thresholds +struct PIDsConfiguration { - // It shall be greater than hardwareJntPosMin - real32_T jntPosMin; - - // It shall be smaller than hardwareJntPosMax - real32_T jntPosMax; - - // Imposed by hardware constraint - real32_T hardwareJntPosMin; - - // Imposed by hardware constraint - real32_T hardwareJntPosMax; - - // If robotMin == rotorMax == 0, there's no check - real32_T rotorPosMin; - - // If robotMin == rotorMax == 0, there's no check - real32_T rotorPosMax; - - // Can be only non-negative - real32_T jntVelMax; - - // Timeout on reception of velocity setpoint - // Can be only non-negative - uint32_T velocityTimeout; - - // Current that can be kept for an indefinite period of time w/o damaging the motor - // Expressed in [A] as all the internal computations are done this way - // Can be only non-negative - real32_T motorNominalCurrents; - - // Current that can be applied for a short period of time - // Expressed in [A] as all the internal computations are done this way - // Can be only non-negative - real32_T motorPeakCurrents; + PID currentPID; + PID velocityPID; + PID positionPID; +}; - // Currents over this threshold can instantaneously damages the motor - // Expressed in [A] as all the internal computations are done this way - // Can be only non-negative - real32_T motorOverloadCurrents; +#endif - // Expressed in ticks - // Max value is 32000 - // Can be only non-negative - uint32_T motorPwmLimit; +#ifndef DEFINED_TYPEDEF_FOR_MotorConfiguration_ +#define DEFINED_TYPEDEF_FOR_MotorConfiguration_ - // The critical temperature of the motor that triggers i2t current protection. - real32_T motorCriticalTemperature; +struct MotorConfiguration +{ + boolean_T has_hall_sens; + boolean_T has_quadrature_encoder; + boolean_T has_speed_quadrature_encoder; + boolean_T has_torque_sens; + boolean_T use_index; + boolean_T enable_verbosity; + int16_T hall_sensors_offset; + int16_T rotor_encoder_resolution; + int16_T rotor_index_offset; + uint8_T encoder_tolerance; + uint8_T pole_pairs; + real32_T Kbemf; + real32_T Rphase; + real32_T Imin; + real32_T Imax; + real32_T Vcc; + real32_T Vmax; + real32_T resistance; + real32_T inductance; + real32_T thermal_resistance; + real32_T thermal_time_constant; }; #endif -#ifndef DEFINED_TYPEDEF_FOR_ConfigurationParameters_ -#define DEFINED_TYPEDEF_FOR_ConfigurationParameters_ +#ifndef DEFINED_TYPEDEF_FOR_ActuatorConfiguration_ +#define DEFINED_TYPEDEF_FOR_ActuatorConfiguration_ -struct ConfigurationParameters +struct ActuatorConfiguration { - MotorConfig motorconfig; - EstimationConfig estimationconfig; - PIDConfig CurLoopPID; - PIDConfig PosLoopPID; - PIDConfig VelLoopPID; - PIDConfig DirLoopPID; Thresholds thresholds; - real32_T environment_temperature; + PIDsConfiguration pids; + MotorConfiguration motor; }; #endif diff --git a/emBODY/eBcode/arch-arm/mbd/amcbldc/supervisor/supervisor.cpp b/emBODY/eBcode/arch-arm/mbd/amcbldc/supervisor/supervisor.cpp new file mode 100644 index 000000000..66f75c6a2 --- /dev/null +++ b/emBODY/eBcode/arch-arm/mbd/amcbldc/supervisor/supervisor.cpp @@ -0,0 +1,1073 @@ +// +// Non-Degree Granting Education License -- for use at non-degree +// granting, nonprofit, education, and research organizations only. Not +// for commercial or industrial use. +// +// File: supervisor.cpp +// +// Code generated for Simulink model 'supervisor'. +// +// Model version : 1.280 +// Simulink Coder version : 23.2 (R2023b) 01-Aug-2023 +// C/C++ source code generated on : Wed Feb 28 11:29:17 2024 +// +// Target selection: ert.tlc +// Embedded hardware selection: ARM Compatible->ARM Cortex-M +// Code generation objectives: Unspecified +// Validation result: Not run +// +#include "supervisor.h" +#include "supervisor_types.h" +#include "rtwtypes.h" +#include +#include "mc_supervisor_defines.h" + +// Named constants for Chart: '/Supervisor' +const int32_T supervisor_CALL_EVENT = -1; +const uint8_T supervisor_IN_ButtonPressed = 1U; +const uint8_T supervisor_IN_Current = 1U; +const uint8_T supervisor_IN_Dummy = 1U; +const uint8_T supervisor_IN_HWFault = 2U; +const uint8_T supervisor_IN_Home = 1U; +const uint8_T supervisor_IN_Idle = 3U; +const uint8_T supervisor_IN_LimitNonConfigured = 1U; +const uint8_T supervisor_IN_NoFault = 2U; +const uint8_T supervisor_IN_NotConfigured = 4U; +const uint8_T supervisor_IN_OverCurrentFault = 3U; +const uint8_T supervisor_IN_Position = 5U; +const uint8_T supervisor_IN_Velocity = 6U; +const uint8_T supervisor_IN_Voltage = 7U; +const int32_T supervisor_event_ControlModeSetpointChange = 0; +const int32_T supervisor_event_DispatcherSetpointChange = 1; +const int32_T supervisor_event_SetCtrlMode = 2; +const int32_T supervisor_event_initialControlModeTrigger = 3; + +// Forward declaration for local functions +static void supervisor_ResetTargets(Targets *rty_targets); +static void supervisor_TargetsManager(Targets *rty_targets, DW_supervisor_f_T + *localDW); +static void supervisor_Idle(const SensorsData *rtu_SensorsData, Targets + *rty_targets, Flags *rty_Flags, DW_supervisor_f_T *localDW); +static void supervisor_Position(const FOCOutputs *rtu_ControlOutputs, Targets + *rty_targets, Flags *rty_Flags, DW_supervisor_f_T *localDW); +static void supervisor_Velocity(const FOCOutputs *rtu_ControlOutputs, const + SensorsData *rtu_SensorsData, Targets *rty_targets, Flags *rty_Flags, + DW_supervisor_f_T *localDW); +static boolean_T supervisor_isConfigurationSet(DW_supervisor_f_T *localDW); +static void supervisor_ControlModeHandler(const FOCOutputs *rtu_ControlOutputs, + const SensorsData *rtu_SensorsData, const ActuatorConfiguration + *rtu_MotionControlInitConf, Targets *rty_targets, ActuatorConfiguration + *rty_ConfigurationParameters, Flags *rty_Flags, DW_supervisor_f_T *localDW); +static void supervisor_SetLimits(real32_T ev_limits_content_overload, real32_T + ev_limits_content_peak, real32_T ev_limits_content_nominal, const FOCOutputs + *rtu_ControlOutputs, const SensorsData *rtu_SensorsData, const + ActuatorConfiguration *rtu_MotionControlInitConf, Targets *rty_targets, + ActuatorConfiguration *rty_ConfigurationParameters, Flags *rty_Flags, + DW_supervisor_f_T *localDW); +static void supervisor_SetPid(ControlModes ev_pid_content_type, real32_T + ev_pid_content_P, real32_T ev_pid_content_I, real32_T ev_pid_content_D, + uint8_T ev_pid_content_shift_factor, ActuatorConfiguration + *rty_ConfigurationParameters); +static void supervisor_SetTarget(real32_T ev_targets_content_jointvelocities, + real32_T ev_targets_content_motorcurrent, real32_T + ev_targets_content_motorvoltage, Targets *rty_targets, DW_supervisor_f_T + *localDW); + +// Function for Chart: '/Supervisor' +static void supervisor_ResetTargets(Targets *rty_targets) +{ + rty_targets->motorvoltage = 0.0F; + rty_targets->motorcurrent = 0.0F; + rty_targets->jointvelocities = 0.0F; + rty_targets->jointpositions = 0.0F; +} + +// Function for Chart: '/Supervisor' +static void supervisor_TargetsManager(Targets *rty_targets, DW_supervisor_f_T + *localDW) +{ + if ((localDW->is_TargetsManager == supervisor_IN_Home) && ((localDW->sfEvent == + supervisor_event_ControlModeSetpointChange) || (localDW->sfEvent == + supervisor_event_DispatcherSetpointChange))) { + if (localDW->is_ControlModeHandler == supervisor_IN_Current) { + rty_targets->motorcurrent = static_cast(localDW->newSetpoint); + } else if (localDW->is_ControlModeHandler == supervisor_IN_Voltage) { + rty_targets->motorvoltage = static_cast(localDW->newSetpoint); + } else if (localDW->is_ControlModeHandler == supervisor_IN_Idle) { + supervisor_ResetTargets(rty_targets); + } else if (localDW->is_ControlModeHandler == supervisor_IN_Velocity) { + rty_targets->jointvelocities = static_cast(localDW->newSetpoint); + } else if (localDW->is_ControlModeHandler == supervisor_IN_Position) { + rty_targets->jointpositions = static_cast(localDW->newSetpoint); + } else { + // Default Transition + } + } +} + +// Function for Chart: '/Supervisor' +static void supervisor_Idle(const SensorsData *rtu_SensorsData, Targets + *rty_targets, Flags *rty_Flags, DW_supervisor_f_T *localDW) +{ + int32_T f_previousEvent; + if ((localDW->isInFault != 0.0) || (localDW->isFaultButtonPressed != 0.0)) { + if (localDW->isInFault != 0.0) { + localDW->is_ControlModeHandler = supervisor_IN_HWFault; + rty_Flags->control_mode = ControlModes_HwFaultCM; + rtw_disableMotor(); + + // this updates the targets value + f_previousEvent = localDW->sfEvent; + localDW->sfEvent = supervisor_event_ControlModeSetpointChange; + if (localDW->is_active_TargetsManager != 0U) { + supervisor_TargetsManager(rty_targets, localDW); + } + + localDW->sfEvent = f_previousEvent; + } else if (localDW->isFaultButtonPressed != 0.0) { + localDW->is_ControlModeHandler = supervisor_IN_Idle; + rty_Flags->control_mode = ControlModes_Idle; + rtw_disableMotor(); + rtw_enableMotor(); + + // this updates the targets value + f_previousEvent = localDW->sfEvent; + localDW->sfEvent = supervisor_event_ControlModeSetpointChange; + if (localDW->is_active_TargetsManager != 0U) { + supervisor_TargetsManager(rty_targets, localDW); + } + + localDW->sfEvent = f_previousEvent; + } + } else if ((localDW->sfEvent == supervisor_event_SetCtrlMode) && + (static_cast(localDW->requiredControlMode) == 2)) { + localDW->is_ControlModeHandler = supervisor_IN_Position; + rty_Flags->control_mode = ControlModes_Position; + localDW->newSetpoint = rtu_SensorsData->jointpositions; + f_previousEvent = localDW->sfEvent; + localDW->sfEvent = supervisor_event_ControlModeSetpointChange; + if (localDW->is_active_TargetsManager != 0U) { + supervisor_TargetsManager(rty_targets, localDW); + } + + localDW->sfEvent = f_previousEvent; + } else if ((localDW->sfEvent == supervisor_event_SetCtrlMode) && + (static_cast(localDW->requiredControlMode) == 4)) { + localDW->newSetpoint = 0.0; + localDW->is_ControlModeHandler = supervisor_IN_Current; + rty_Flags->control_mode = ControlModes_Current; + f_previousEvent = localDW->sfEvent; + localDW->sfEvent = supervisor_event_ControlModeSetpointChange; + if (localDW->is_active_TargetsManager != 0U) { + supervisor_TargetsManager(rty_targets, localDW); + } + + localDW->sfEvent = f_previousEvent; + } else if ((localDW->sfEvent == supervisor_event_SetCtrlMode) && + (static_cast(localDW->requiredControlMode) == 6)) { + localDW->newSetpoint = 0.0; + localDW->is_ControlModeHandler = supervisor_IN_Voltage; + rty_Flags->control_mode = ControlModes_Voltage; + f_previousEvent = localDW->sfEvent; + localDW->sfEvent = supervisor_event_ControlModeSetpointChange; + if (localDW->is_active_TargetsManager != 0U) { + supervisor_TargetsManager(rty_targets, localDW); + } + + localDW->sfEvent = f_previousEvent; + } else if ((localDW->sfEvent == supervisor_event_SetCtrlMode) && + (static_cast(localDW->requiredControlMode) == 5)) { + localDW->is_ControlModeHandler = supervisor_IN_Velocity; + rty_Flags->control_mode = ControlModes_Velocity; + localDW->newSetpoint = 0.0; + f_previousEvent = localDW->sfEvent; + localDW->sfEvent = supervisor_event_ControlModeSetpointChange; + if (localDW->is_active_TargetsManager != 0U) { + supervisor_TargetsManager(rty_targets, localDW); + } + + localDW->sfEvent = f_previousEvent; + } +} + +// Function for Chart: '/Supervisor' +static void supervisor_Position(const FOCOutputs *rtu_ControlOutputs, Targets + *rty_targets, Flags *rty_Flags, DW_supervisor_f_T *localDW) +{ + int32_T e_previousEvent; + boolean_T guard1; + boolean_T guard2; + guard1 = false; + guard2 = false; + if ((localDW->isInFault != 0.0) || (localDW->isFaultButtonPressed != 0.0)) { + if (localDW->isInFault != 0.0) { + localDW->is_ControlModeHandler = supervisor_IN_HWFault; + rty_Flags->control_mode = ControlModes_HwFaultCM; + rtw_disableMotor(); + + // this updates the targets value + e_previousEvent = localDW->sfEvent; + localDW->sfEvent = supervisor_event_ControlModeSetpointChange; + if (localDW->is_active_TargetsManager != 0U) { + supervisor_TargetsManager(rty_targets, localDW); + } + + localDW->sfEvent = e_previousEvent; + } else if (localDW->isFaultButtonPressed != 0.0) { + guard1 = true; + } + } else if ((localDW->sfEvent == supervisor_event_SetCtrlMode) && + (static_cast(localDW->requiredControlMode) != 1)) { + if ((localDW->sfEvent == supervisor_event_SetCtrlMode) && + (static_cast(localDW->requiredControlMode) == 4)) { + localDW->newSetpoint = rtu_ControlOutputs->Iq_fbk; + localDW->is_ControlModeHandler = supervisor_IN_Current; + rty_Flags->control_mode = ControlModes_Current; + e_previousEvent = localDW->sfEvent; + localDW->sfEvent = supervisor_event_ControlModeSetpointChange; + if (localDW->is_active_TargetsManager != 0U) { + supervisor_TargetsManager(rty_targets, localDW); + } + + localDW->sfEvent = e_previousEvent; + } else if ((localDW->sfEvent == supervisor_event_SetCtrlMode) && ( + static_cast(localDW->requiredControlMode) == 6)) { + localDW->newSetpoint = rtu_ControlOutputs->Vq; + localDW->is_ControlModeHandler = supervisor_IN_Voltage; + rty_Flags->control_mode = ControlModes_Voltage; + e_previousEvent = localDW->sfEvent; + localDW->sfEvent = supervisor_event_ControlModeSetpointChange; + if (localDW->is_active_TargetsManager != 0U) { + supervisor_TargetsManager(rty_targets, localDW); + } + + localDW->sfEvent = e_previousEvent; + } else if ((localDW->sfEvent == supervisor_event_SetCtrlMode) && ( + static_cast(localDW->requiredControlMode) == 5)) { + localDW->is_ControlModeHandler = supervisor_IN_Velocity; + rty_Flags->control_mode = ControlModes_Velocity; + localDW->newSetpoint = 0.0; + e_previousEvent = localDW->sfEvent; + localDW->sfEvent = supervisor_event_ControlModeSetpointChange; + if (localDW->is_active_TargetsManager != 0U) { + supervisor_TargetsManager(rty_targets, localDW); + } + + localDW->sfEvent = e_previousEvent; + } else { + guard2 = true; + } + } else { + guard2 = true; + } + + if (guard2) { + if ((localDW->sfEvent == supervisor_event_SetCtrlMode) && + (static_cast(localDW->requiredControlMode) == 1)) { + guard1 = true; + } + } + + if (guard1) { + localDW->is_ControlModeHandler = supervisor_IN_Idle; + rty_Flags->control_mode = ControlModes_Idle; + rtw_disableMotor(); + rtw_enableMotor(); + + // this updates the targets value + e_previousEvent = localDW->sfEvent; + localDW->sfEvent = supervisor_event_ControlModeSetpointChange; + if (localDW->is_active_TargetsManager != 0U) { + supervisor_TargetsManager(rty_targets, localDW); + } + + localDW->sfEvent = e_previousEvent; + } +} + +// Function for Chart: '/Supervisor' +static void supervisor_Velocity(const FOCOutputs *rtu_ControlOutputs, const + SensorsData *rtu_SensorsData, Targets *rty_targets, Flags *rty_Flags, + DW_supervisor_f_T *localDW) +{ + int32_T e_previousEvent; + boolean_T guard1; + guard1 = false; + if ((localDW->isInFault != 0.0) || (localDW->isFaultButtonPressed != 0.0)) { + if (localDW->isInFault != 0.0) { + localDW->is_ControlModeHandler = supervisor_IN_HWFault; + rty_Flags->control_mode = ControlModes_HwFaultCM; + rtw_disableMotor(); + + // this updates the targets value + e_previousEvent = localDW->sfEvent; + localDW->sfEvent = supervisor_event_ControlModeSetpointChange; + if (localDW->is_active_TargetsManager != 0U) { + supervisor_TargetsManager(rty_targets, localDW); + } + + localDW->sfEvent = e_previousEvent; + } else if (localDW->isFaultButtonPressed != 0.0) { + guard1 = true; + } + } else if ((localDW->sfEvent == supervisor_event_SetCtrlMode) && + (static_cast(localDW->requiredControlMode) != 5)) { + if ((localDW->sfEvent == supervisor_event_SetCtrlMode) && + (static_cast(localDW->requiredControlMode) == 6)) { + localDW->newSetpoint = rtu_ControlOutputs->Vq; + localDW->is_ControlModeHandler = supervisor_IN_Voltage; + rty_Flags->control_mode = ControlModes_Voltage; + e_previousEvent = localDW->sfEvent; + localDW->sfEvent = supervisor_event_ControlModeSetpointChange; + if (localDW->is_active_TargetsManager != 0U) { + supervisor_TargetsManager(rty_targets, localDW); + } + + localDW->sfEvent = e_previousEvent; + } else if ((localDW->sfEvent == supervisor_event_SetCtrlMode) && ( + static_cast(localDW->requiredControlMode) == 4)) { + localDW->newSetpoint = rtu_ControlOutputs->Iq_fbk; + localDW->is_ControlModeHandler = supervisor_IN_Current; + rty_Flags->control_mode = ControlModes_Current; + e_previousEvent = localDW->sfEvent; + localDW->sfEvent = supervisor_event_ControlModeSetpointChange; + if (localDW->is_active_TargetsManager != 0U) { + supervisor_TargetsManager(rty_targets, localDW); + } + + localDW->sfEvent = e_previousEvent; + } else if ((localDW->sfEvent == supervisor_event_SetCtrlMode) && ( + static_cast(localDW->requiredControlMode) == 2)) { + localDW->is_ControlModeHandler = supervisor_IN_Position; + rty_Flags->control_mode = ControlModes_Position; + localDW->newSetpoint = rtu_SensorsData->jointpositions; + e_previousEvent = localDW->sfEvent; + localDW->sfEvent = supervisor_event_ControlModeSetpointChange; + if (localDW->is_active_TargetsManager != 0U) { + supervisor_TargetsManager(rty_targets, localDW); + } + + localDW->sfEvent = e_previousEvent; + } else if ((localDW->sfEvent == supervisor_event_SetCtrlMode) && ( + static_cast(localDW->requiredControlMode) == 1)) { + guard1 = true; + } + } + + if (guard1) { + localDW->is_ControlModeHandler = supervisor_IN_Idle; + rty_Flags->control_mode = ControlModes_Idle; + rtw_disableMotor(); + rtw_enableMotor(); + + // this updates the targets value + e_previousEvent = localDW->sfEvent; + localDW->sfEvent = supervisor_event_ControlModeSetpointChange; + if (localDW->is_active_TargetsManager != 0U) { + supervisor_TargetsManager(rty_targets, localDW); + } + + localDW->sfEvent = e_previousEvent; + } +} + +// Function for Chart: '/Supervisor' +static boolean_T supervisor_isConfigurationSet(DW_supervisor_f_T *localDW) +{ + return localDW->areLimitsSet; +} + +// Function for Chart: '/Supervisor' +static void supervisor_ControlModeHandler(const FOCOutputs *rtu_ControlOutputs, + const SensorsData *rtu_SensorsData, const ActuatorConfiguration + *rtu_MotionControlInitConf, Targets *rty_targets, ActuatorConfiguration + *rty_ConfigurationParameters, Flags *rty_Flags, DW_supervisor_f_T *localDW) +{ + int32_T i_previousEvent; + boolean_T guard1; + boolean_T guard2; + boolean_T guard3; + guard1 = false; + guard2 = false; + guard3 = false; + switch (localDW->is_ControlModeHandler) { + case supervisor_IN_Current: + if ((localDW->isInFault != 0.0) || (localDW->isFaultButtonPressed != 0.0)) { + if (localDW->isInFault != 0.0) { + localDW->is_ControlModeHandler = supervisor_IN_HWFault; + + // Chart: '/Supervisor' + rty_Flags->control_mode = ControlModes_HwFaultCM; + rtw_disableMotor(); + + // this updates the targets value + i_previousEvent = localDW->sfEvent; + localDW->sfEvent = supervisor_event_ControlModeSetpointChange; + if (localDW->is_active_TargetsManager != 0U) { + // Chart: '/Supervisor' + supervisor_TargetsManager(rty_targets, localDW); + } + + localDW->sfEvent = i_previousEvent; + } else if (localDW->isFaultButtonPressed != 0.0) { + guard1 = true; + } + } else if ((localDW->sfEvent == supervisor_event_SetCtrlMode) && (( + static_cast(localDW->requiredControlMode) != 1) && ( + static_cast(localDW->requiredControlMode) != 2))) { + if ((localDW->sfEvent == supervisor_event_SetCtrlMode) && + (static_cast(localDW->requiredControlMode) == 6)) { + // Chart: '/Supervisor' + localDW->newSetpoint = rtu_ControlOutputs->Vq; + localDW->is_ControlModeHandler = supervisor_IN_Voltage; + + // Chart: '/Supervisor' + rty_Flags->control_mode = ControlModes_Voltage; + i_previousEvent = localDW->sfEvent; + localDW->sfEvent = supervisor_event_ControlModeSetpointChange; + if (localDW->is_active_TargetsManager != 0U) { + // Chart: '/Supervisor' + supervisor_TargetsManager(rty_targets, localDW); + } + + localDW->sfEvent = i_previousEvent; + } else if ((localDW->sfEvent == supervisor_event_SetCtrlMode) && ( + static_cast(localDW->requiredControlMode) == 5)) { + localDW->is_ControlModeHandler = supervisor_IN_Velocity; + + // Chart: '/Supervisor' + rty_Flags->control_mode = ControlModes_Velocity; + localDW->newSetpoint = 0.0; + i_previousEvent = localDW->sfEvent; + localDW->sfEvent = supervisor_event_ControlModeSetpointChange; + if (localDW->is_active_TargetsManager != 0U) { + // Chart: '/Supervisor' + supervisor_TargetsManager(rty_targets, localDW); + } + + localDW->sfEvent = i_previousEvent; + } else { + guard3 = true; + } + } else { + guard3 = true; + } + break; + + case supervisor_IN_HWFault: + i_previousEvent = (static_cast(localDW->requiredControlMode) == 1); + if ((localDW->isInFault == 0.0) && (i_previousEvent != 0) && + (!supervisor_isConfigurationSet(localDW))) { + localDW->is_ControlModeHandler = supervisor_IN_NotConfigured; + + // Chart: '/Supervisor' + *rty_ConfigurationParameters = *rtu_MotionControlInitConf; + rty_Flags->control_mode = ControlModes_NotConfigured; + rtw_disableMotor(); + } else if ((localDW->sfEvent == supervisor_event_SetCtrlMode) && + ((localDW->isInFault == 0.0) && (static_cast + (localDW->requiredControlMode) == 1))) { + localDW->is_ControlModeHandler = supervisor_IN_Idle; + + // Chart: '/Supervisor' + rty_Flags->control_mode = ControlModes_Idle; + rtw_disableMotor(); + rtw_enableMotor(); + + // this updates the targets value + i_previousEvent = localDW->sfEvent; + localDW->sfEvent = supervisor_event_ControlModeSetpointChange; + if (localDW->is_active_TargetsManager != 0U) { + // Chart: '/Supervisor' + supervisor_TargetsManager(rty_targets, localDW); + } + + localDW->sfEvent = i_previousEvent; + } + break; + + case supervisor_IN_Idle: + // Chart: '/Supervisor' + supervisor_Idle(rtu_SensorsData, rty_targets, rty_Flags, localDW); + break; + + case supervisor_IN_NotConfigured: + if (localDW->sfEvent == supervisor_event_initialControlModeTrigger) { + localDW->requiredControlMode = ControlModes_Idle; + localDW->is_ControlModeHandler = supervisor_IN_Idle; + + // Chart: '/Supervisor' + rty_Flags->control_mode = ControlModes_Idle; + rtw_disableMotor(); + rtw_enableMotor(); + + // this updates the targets value + i_previousEvent = localDW->sfEvent; + localDW->sfEvent = supervisor_event_ControlModeSetpointChange; + if (localDW->is_active_TargetsManager != 0U) { + // Chart: '/Supervisor' + supervisor_TargetsManager(rty_targets, localDW); + } + + localDW->sfEvent = i_previousEvent; + } else if (localDW->isInFault != 0.0) { + localDW->is_ControlModeHandler = supervisor_IN_HWFault; + + // Chart: '/Supervisor' + rty_Flags->control_mode = ControlModes_HwFaultCM; + rtw_disableMotor(); + + // this updates the targets value + i_previousEvent = localDW->sfEvent; + localDW->sfEvent = supervisor_event_ControlModeSetpointChange; + if (localDW->is_active_TargetsManager != 0U) { + // Chart: '/Supervisor' + supervisor_TargetsManager(rty_targets, localDW); + } + + localDW->sfEvent = i_previousEvent; + } + break; + + case supervisor_IN_Position: + // Chart: '/Supervisor' + supervisor_Position(rtu_ControlOutputs, rty_targets, rty_Flags, localDW); + break; + + case supervisor_IN_Velocity: + // Chart: '/Supervisor' + supervisor_Velocity(rtu_ControlOutputs, rtu_SensorsData, rty_targets, + rty_Flags, localDW); + break; + + case supervisor_IN_Voltage: + if ((localDW->isInFault != 0.0) || (localDW->isFaultButtonPressed != 0.0)) { + if (localDW->isInFault != 0.0) { + localDW->is_ControlModeHandler = supervisor_IN_HWFault; + + // Chart: '/Supervisor' + rty_Flags->control_mode = ControlModes_HwFaultCM; + rtw_disableMotor(); + + // this updates the targets value + i_previousEvent = localDW->sfEvent; + localDW->sfEvent = supervisor_event_ControlModeSetpointChange; + if (localDW->is_active_TargetsManager != 0U) { + // Chart: '/Supervisor' + supervisor_TargetsManager(rty_targets, localDW); + } + + localDW->sfEvent = i_previousEvent; + } else if (localDW->isFaultButtonPressed != 0.0) { + guard2 = true; + } + } else if ((localDW->sfEvent == supervisor_event_SetCtrlMode) && ( + static_cast(localDW->requiredControlMode) == 4)) { + // Chart: '/Supervisor' + localDW->newSetpoint = rtu_ControlOutputs->Iq_fbk; + localDW->is_ControlModeHandler = supervisor_IN_Current; + + // Chart: '/Supervisor' + rty_Flags->control_mode = ControlModes_Current; + i_previousEvent = localDW->sfEvent; + localDW->sfEvent = supervisor_event_ControlModeSetpointChange; + if (localDW->is_active_TargetsManager != 0U) { + // Chart: '/Supervisor' + supervisor_TargetsManager(rty_targets, localDW); + } + + localDW->sfEvent = i_previousEvent; + } else if ((localDW->sfEvent == supervisor_event_SetCtrlMode) && ( + static_cast(localDW->requiredControlMode) == 2)) { + localDW->is_ControlModeHandler = supervisor_IN_Position; + + // Chart: '/Supervisor' + rty_Flags->control_mode = ControlModes_Position; + localDW->newSetpoint = rtu_SensorsData->jointpositions; + i_previousEvent = localDW->sfEvent; + localDW->sfEvent = supervisor_event_ControlModeSetpointChange; + if (localDW->is_active_TargetsManager != 0U) { + // Chart: '/Supervisor' + supervisor_TargetsManager(rty_targets, localDW); + } + + localDW->sfEvent = i_previousEvent; + } else if ((localDW->sfEvent == supervisor_event_SetCtrlMode) && ( + static_cast(localDW->requiredControlMode) == 1)) { + guard2 = true; + } else if ((localDW->sfEvent == supervisor_event_SetCtrlMode) && ( + static_cast(localDW->requiredControlMode) == 5)) { + localDW->is_ControlModeHandler = supervisor_IN_Velocity; + + // Chart: '/Supervisor' + rty_Flags->control_mode = ControlModes_Velocity; + localDW->newSetpoint = 0.0; + i_previousEvent = localDW->sfEvent; + localDW->sfEvent = supervisor_event_ControlModeSetpointChange; + if (localDW->is_active_TargetsManager != 0U) { + // Chart: '/Supervisor' + supervisor_TargetsManager(rty_targets, localDW); + } + + localDW->sfEvent = i_previousEvent; + } + break; + } + + if (guard3) { + if ((localDW->sfEvent == supervisor_event_SetCtrlMode) && + (static_cast(localDW->requiredControlMode) == 2)) { + localDW->is_ControlModeHandler = supervisor_IN_Position; + + // Chart: '/Supervisor' + rty_Flags->control_mode = ControlModes_Position; + localDW->newSetpoint = rtu_SensorsData->jointpositions; + i_previousEvent = localDW->sfEvent; + localDW->sfEvent = supervisor_event_ControlModeSetpointChange; + if (localDW->is_active_TargetsManager != 0U) { + // Chart: '/Supervisor' + supervisor_TargetsManager(rty_targets, localDW); + } + + localDW->sfEvent = i_previousEvent; + } else if ((localDW->sfEvent == supervisor_event_SetCtrlMode) && ( + static_cast(localDW->requiredControlMode) == 1)) { + guard1 = true; + } + } + + if (guard2) { + localDW->is_ControlModeHandler = supervisor_IN_Idle; + + // Chart: '/Supervisor' + rty_Flags->control_mode = ControlModes_Idle; + rtw_disableMotor(); + rtw_enableMotor(); + + // this updates the targets value + i_previousEvent = localDW->sfEvent; + localDW->sfEvent = supervisor_event_ControlModeSetpointChange; + if (localDW->is_active_TargetsManager != 0U) { + // Chart: '/Supervisor' + supervisor_TargetsManager(rty_targets, localDW); + } + + localDW->sfEvent = i_previousEvent; + } + + if (guard1) { + localDW->is_ControlModeHandler = supervisor_IN_Idle; + + // Chart: '/Supervisor' + rty_Flags->control_mode = ControlModes_Idle; + rtw_disableMotor(); + rtw_enableMotor(); + + // this updates the targets value + i_previousEvent = localDW->sfEvent; + localDW->sfEvent = supervisor_event_ControlModeSetpointChange; + if (localDW->is_active_TargetsManager != 0U) { + // Chart: '/Supervisor' + supervisor_TargetsManager(rty_targets, localDW); + } + + localDW->sfEvent = i_previousEvent; + } +} + +// Function for Chart: '/Supervisor' +static void supervisor_SetLimits(real32_T ev_limits_content_overload, real32_T + ev_limits_content_peak, real32_T ev_limits_content_nominal, const FOCOutputs + *rtu_ControlOutputs, const SensorsData *rtu_SensorsData, const + ActuatorConfiguration *rtu_MotionControlInitConf, Targets *rty_targets, + ActuatorConfiguration *rty_ConfigurationParameters, Flags *rty_Flags, + DW_supervisor_f_T *localDW) +{ + int32_T b_previousEvent; + + // Chart: '/Supervisor' + rty_ConfigurationParameters->thresholds.motorNominalCurrents = std::abs + (ev_limits_content_nominal); + rty_ConfigurationParameters->thresholds.motorPeakCurrents = std::abs + (ev_limits_content_peak); + rty_ConfigurationParameters->thresholds.motorOverloadCurrents = std::abs + (ev_limits_content_overload); + if (!localDW->areLimitsSet) { + localDW->areLimitsSet = true; + b_previousEvent = localDW->sfEvent; + localDW->sfEvent = supervisor_event_initialControlModeTrigger; + if (localDW->is_active_ControlModeHandler != 0U) { + // Chart: '/Supervisor' + supervisor_ControlModeHandler(rtu_ControlOutputs, rtu_SensorsData, + rtu_MotionControlInitConf, rty_targets, rty_ConfigurationParameters, + rty_Flags, localDW); + } + + localDW->sfEvent = b_previousEvent; + } +} + +// Function for Chart: '/Supervisor' +static void supervisor_SetPid(ControlModes ev_pid_content_type, real32_T + ev_pid_content_P, real32_T ev_pid_content_I, real32_T ev_pid_content_D, + uint8_T ev_pid_content_shift_factor, ActuatorConfiguration + *rty_ConfigurationParameters) +{ + switch (ev_pid_content_type) { + case ControlModes_Current: + // Chart: '/Supervisor' + rty_ConfigurationParameters->pids.currentPID.P = ev_pid_content_P; + rty_ConfigurationParameters->pids.currentPID.I = ev_pid_content_I; + rty_ConfigurationParameters->pids.currentPID.D = ev_pid_content_D; + rty_ConfigurationParameters->pids.currentPID.shift_factor = + ev_pid_content_shift_factor; + rty_ConfigurationParameters->pids.currentPID.type = ControlModes_Current; + break; + + case ControlModes_Position: + // Chart: '/Supervisor' + rty_ConfigurationParameters->pids.positionPID.P = 9.0F; + rty_ConfigurationParameters->pids.positionPID.I = 15.0F; + rty_ConfigurationParameters->pids.positionPID.D = 7.0F; + rty_ConfigurationParameters->pids.positionPID.shift_factor = + ev_pid_content_shift_factor; + rty_ConfigurationParameters->pids.positionPID.type = ControlModes_Position; + break; + + default: + if ((ev_pid_content_type != ControlModes_Voltage) && (ev_pid_content_type == + ControlModes_Velocity)) { + // Chart: '/Supervisor' + rty_ConfigurationParameters->pids.velocityPID.P = ev_pid_content_P; + rty_ConfigurationParameters->pids.velocityPID.I = ev_pid_content_I; + rty_ConfigurationParameters->pids.velocityPID.D = ev_pid_content_D; + rty_ConfigurationParameters->pids.velocityPID.shift_factor = + ev_pid_content_shift_factor; + } + break; + } +} + +// Function for Chart: '/Supervisor' +static void supervisor_SetTarget(real32_T ev_targets_content_jointvelocities, + real32_T ev_targets_content_motorcurrent, real32_T + ev_targets_content_motorvoltage, Targets *rty_targets, DW_supervisor_f_T + *localDW) +{ + int32_T b_previousEvent; + if (localDW->is_ControlModeHandler == supervisor_IN_Voltage) { + localDW->newSetpoint = ev_targets_content_motorvoltage; + } else if (localDW->is_ControlModeHandler == supervisor_IN_Current) { + localDW->newSetpoint = ev_targets_content_motorcurrent; + } else if (localDW->is_ControlModeHandler == supervisor_IN_Velocity) { + localDW->newSetpoint = ev_targets_content_jointvelocities; + } else { + localDW->newSetpoint = 0.0; + } + + b_previousEvent = localDW->sfEvent; + localDW->sfEvent = supervisor_event_DispatcherSetpointChange; + if (localDW->is_active_TargetsManager != 0U) { + // Chart: '/Supervisor' + supervisor_TargetsManager(rty_targets, localDW); + } + + localDW->sfEvent = b_previousEvent; +} + +// System initialize for referenced model: 'supervisor' +void supervisor_Init(Targets *rty_targets, ActuatorConfiguration + *rty_ConfigurationParameters, Flags *rty_Flags, + DW_supervisor_f_T *localDW) +{ + // SystemInitialize for Chart: '/Supervisor' + localDW->sfEvent = supervisor_CALL_EVENT; + rty_targets->jointpositions = 0.0F; + rty_targets->jointvelocities = 0.0F; + rty_targets->motorcurrent = 0.0F; + rty_targets->motorvoltage = 0.0F; + rty_ConfigurationParameters->thresholds.jntVelMax = 0.0F; + rty_ConfigurationParameters->thresholds.motorNominalCurrents = 0.0F; + rty_ConfigurationParameters->thresholds.motorPeakCurrents = 0.0F; + rty_ConfigurationParameters->thresholds.motorOverloadCurrents = 0.0F; + rty_ConfigurationParameters->thresholds.motorPwmLimit = 0U; + rty_ConfigurationParameters->thresholds.motorCriticalTemperature = 0.0F; + rty_ConfigurationParameters->pids.currentPID.type = ControlModes_NotConfigured; + rty_ConfigurationParameters->pids.currentPID.OutMax = 0.0F; + rty_ConfigurationParameters->pids.currentPID.OutMin = 0.0F; + rty_ConfigurationParameters->pids.currentPID.P = 0.0F; + rty_ConfigurationParameters->pids.currentPID.I = 0.0F; + rty_ConfigurationParameters->pids.currentPID.D = 0.0F; + rty_ConfigurationParameters->pids.currentPID.N = 0.0F; + rty_ConfigurationParameters->pids.currentPID.I0 = 0.0F; + rty_ConfigurationParameters->pids.currentPID.D0 = 0.0F; + rty_ConfigurationParameters->pids.currentPID.shift_factor = 0U; + rty_ConfigurationParameters->pids.velocityPID.type = + ControlModes_NotConfigured; + rty_ConfigurationParameters->pids.velocityPID.OutMax = 0.0F; + rty_ConfigurationParameters->pids.velocityPID.OutMin = 0.0F; + rty_ConfigurationParameters->pids.velocityPID.P = 0.0F; + rty_ConfigurationParameters->pids.velocityPID.I = 0.0F; + rty_ConfigurationParameters->pids.velocityPID.D = 0.0F; + rty_ConfigurationParameters->pids.velocityPID.N = 0.0F; + rty_ConfigurationParameters->pids.velocityPID.I0 = 0.0F; + rty_ConfigurationParameters->pids.velocityPID.D0 = 0.0F; + rty_ConfigurationParameters->pids.velocityPID.shift_factor = 0U; + rty_ConfigurationParameters->pids.positionPID.type = + ControlModes_NotConfigured; + rty_ConfigurationParameters->pids.positionPID.OutMax = 0.0F; + rty_ConfigurationParameters->pids.positionPID.OutMin = 0.0F; + rty_ConfigurationParameters->pids.positionPID.P = 0.0F; + rty_ConfigurationParameters->pids.positionPID.I = 0.0F; + rty_ConfigurationParameters->pids.positionPID.D = 0.0F; + rty_ConfigurationParameters->pids.positionPID.N = 0.0F; + rty_ConfigurationParameters->pids.positionPID.I0 = 0.0F; + rty_ConfigurationParameters->pids.positionPID.D0 = 0.0F; + rty_ConfigurationParameters->pids.positionPID.shift_factor = 0U; + rty_ConfigurationParameters->motor.has_hall_sens = false; + rty_ConfigurationParameters->motor.has_quadrature_encoder = false; + rty_ConfigurationParameters->motor.has_speed_quadrature_encoder = false; + rty_ConfigurationParameters->motor.has_torque_sens = false; + rty_ConfigurationParameters->motor.use_index = false; + rty_ConfigurationParameters->motor.enable_verbosity = false; + rty_ConfigurationParameters->motor.hall_sensors_offset = 0; + rty_ConfigurationParameters->motor.rotor_encoder_resolution = 0; + rty_ConfigurationParameters->motor.rotor_index_offset = 0; + rty_ConfigurationParameters->motor.encoder_tolerance = 0U; + rty_ConfigurationParameters->motor.pole_pairs = 0U; + rty_ConfigurationParameters->motor.Kbemf = 0.0F; + rty_ConfigurationParameters->motor.Rphase = 0.0F; + rty_ConfigurationParameters->motor.Imin = 0.0F; + rty_ConfigurationParameters->motor.Imax = 0.0F; + rty_ConfigurationParameters->motor.Vcc = 0.0F; + rty_ConfigurationParameters->motor.Vmax = 0.0F; + rty_ConfigurationParameters->motor.resistance = 0.0F; + rty_ConfigurationParameters->motor.inductance = 0.0F; + rty_ConfigurationParameters->motor.thermal_resistance = 0.0F; + rty_ConfigurationParameters->motor.thermal_time_constant = 0.0F; + rty_Flags->enable_sending_msg_status = false; + rty_Flags->control_mode = ControlModes_NotConfigured; + rty_Flags->enable_thermal_protection = false; +} + +// Output and update for referenced model: 'supervisor' +void supervisor(const ExternalFlags *rtu_ExternalFlags, const EstimatedData + *rtu_EstimatedData, const FOCOutputs *rtu_ControlOutputs, const + SensorsData *rtu_SensorsData, const SupervisorReceivedEvents + rtu_ReceivedEvents[MAX_EVENTS_PER_TICK], const + ActuatorConfiguration *rtu_MotionControlInitConf, Targets + *rty_targets, ActuatorConfiguration *rty_ConfigurationParameters, + Flags *rty_Flags, DW_supervisor_f_T *localDW) +{ + int32_T b_previousEvent; + int32_T ei; + + // Chart: '/Supervisor' + localDW->sfEvent = supervisor_CALL_EVENT; + localDW->ExternalFlags_fault_button_prev = + localDW->ExternalFlags_fault_button_start; + localDW->ExternalFlags_fault_button_start = rtu_ExternalFlags->fault_button; + if (localDW->is_active_c3_supervisor == 0U) { + *rty_ConfigurationParameters = *rtu_MotionControlInitConf; + localDW->ExternalFlags_fault_button_prev = rtu_ExternalFlags->fault_button; + localDW->ExternalFlags_fault_button_start = rtu_ExternalFlags->fault_button; + localDW->is_active_c3_supervisor = 1U; + localDW->is_active_FaultsManager = 1U; + localDW->is_active_HWFaults = 1U; + localDW->is_HWFaults = supervisor_IN_LimitNonConfigured; + localDW->is_active_FaultButton = 1U; + localDW->is_FaultButton = supervisor_IN_NoFault; + localDW->isFaultButtonPressed = 0.0; + localDW->is_active_ControlModeHandler = 1U; + localDW->is_ControlModeHandler = supervisor_IN_NotConfigured; + rty_Flags->control_mode = ControlModes_NotConfigured; + rtw_disableMotor(); + localDW->is_active_InputsDispatcher = 1U; + + // Consume N events per tick + for (ei = 0; ei < MAX_EVENTS_PER_TICK; ei++) { + switch (rtu_ReceivedEvents[ei].event_type) { + case SupervisorEvents_SetLimit: + supervisor_SetLimits(rtu_ReceivedEvents[ei].limits_content.overload, + rtu_ReceivedEvents[ei].limits_content.peak, + rtu_ReceivedEvents[ei].limits_content.nominal, + rtu_ControlOutputs, rtu_SensorsData, + rtu_MotionControlInitConf, rty_targets, + rty_ConfigurationParameters, rty_Flags, localDW); + break; + + case SupervisorEvents_SetPid: + supervisor_SetPid(rtu_ReceivedEvents[ei].pid_content.type, + rtu_ReceivedEvents[ei].pid_content.P, + rtu_ReceivedEvents[ei].pid_content.I, + rtu_ReceivedEvents[ei].pid_content.D, + rtu_ReceivedEvents[ei].pid_content.shift_factor, + rty_ConfigurationParameters); + break; + + case SupervisorEvents_SetMotorConfig: + rty_ConfigurationParameters->motor = rtu_ReceivedEvents[ei]. + motor_config_content; + break; + + case SupervisorEvents_SetTarget: + supervisor_SetTarget(rtu_ReceivedEvents[ei]. + targets_content.jointvelocities, + rtu_ReceivedEvents[ei].targets_content.motorcurrent, + rtu_ReceivedEvents[ei].targets_content.motorvoltage, + rty_targets, localDW); + break; + + case SupervisorEvents_SetControlMode: + localDW->requiredControlMode = rtu_ReceivedEvents[ei]. + control_mode_content; + b_previousEvent = localDW->sfEvent; + localDW->sfEvent = supervisor_event_SetCtrlMode; + if (localDW->is_active_ControlModeHandler != 0U) { + supervisor_ControlModeHandler(rtu_ControlOutputs, rtu_SensorsData, + rtu_MotionControlInitConf, rty_targets, rty_ConfigurationParameters, + rty_Flags, localDW); + } + + localDW->sfEvent = b_previousEvent; + break; + } + } + + localDW->is_InputsDispatcher = supervisor_IN_Dummy; + localDW->is_active_TargetsManager = 1U; + supervisor_ResetTargets(rty_targets); + localDW->is_TargetsManager = supervisor_IN_Home; + } else { + if (localDW->is_active_FaultsManager != 0U) { + if (localDW->is_active_HWFaults != 0U) { + switch (localDW->is_HWFaults) { + case supervisor_IN_LimitNonConfigured: + if (localDW->areLimitsSet) { + localDW->is_HWFaults = supervisor_IN_NoFault; + localDW->isInFault = 0.0; + + // MotorFaultFlags.overCurrent=0; + } + break; + + case supervisor_IN_NoFault: + if (std::abs(rtu_EstimatedData->Iq_filtered) >= + rty_ConfigurationParameters->thresholds.motorOverloadCurrents) { + localDW->is_HWFaults = supervisor_IN_OverCurrentFault; + + // MotorFaultFlags.overCurrent=1; + localDW->isInFault = 1.0; + } + break; + + case supervisor_IN_OverCurrentFault: + if (std::abs(rtu_EstimatedData->Iq_filtered) < + rty_ConfigurationParameters->thresholds.motorOverloadCurrents) { + localDW->is_HWFaults = supervisor_IN_NoFault; + localDW->isInFault = 0.0; + + // MotorFaultFlags.overCurrent=0; + } + break; + } + } + + if (localDW->is_active_FaultButton != 0U) { + switch (localDW->is_FaultButton) { + case supervisor_IN_ButtonPressed: + if ((localDW->ExternalFlags_fault_button_prev != + localDW->ExternalFlags_fault_button_start) && + (!localDW->ExternalFlags_fault_button_start)) { + localDW->is_FaultButton = supervisor_IN_NoFault; + localDW->isFaultButtonPressed = 0.0; + } + break; + + case supervisor_IN_NoFault: + if ((localDW->ExternalFlags_fault_button_prev != + localDW->ExternalFlags_fault_button_start) && + localDW->ExternalFlags_fault_button_start) { + localDW->is_FaultButton = supervisor_IN_ButtonPressed; + localDW->isFaultButtonPressed = 1.0; + } + break; + } + } + } + + if (localDW->is_active_ControlModeHandler != 0U) { + supervisor_ControlModeHandler(rtu_ControlOutputs, rtu_SensorsData, + rtu_MotionControlInitConf, rty_targets, rty_ConfigurationParameters, + rty_Flags, localDW); + } + + if ((localDW->is_active_InputsDispatcher != 0U) && + (localDW->is_InputsDispatcher == supervisor_IN_Dummy)) { + for (ei = 0; ei < MAX_EVENTS_PER_TICK; ei++) { + switch (rtu_ReceivedEvents[ei].event_type) { + case SupervisorEvents_SetLimit: + supervisor_SetLimits(rtu_ReceivedEvents[ei].limits_content.overload, + rtu_ReceivedEvents[ei].limits_content.peak, + rtu_ReceivedEvents[ei].limits_content.nominal, + rtu_ControlOutputs, rtu_SensorsData, + rtu_MotionControlInitConf, rty_targets, + rty_ConfigurationParameters, rty_Flags, localDW); + break; + + case SupervisorEvents_SetPid: + supervisor_SetPid(rtu_ReceivedEvents[ei].pid_content.type, + rtu_ReceivedEvents[ei].pid_content.P, + rtu_ReceivedEvents[ei].pid_content.I, + rtu_ReceivedEvents[ei].pid_content.D, + rtu_ReceivedEvents[ei].pid_content.shift_factor, + rty_ConfigurationParameters); + break; + + case SupervisorEvents_SetMotorConfig: + rty_ConfigurationParameters->motor = rtu_ReceivedEvents[ei]. + motor_config_content; + break; + + case SupervisorEvents_SetTarget: + supervisor_SetTarget(rtu_ReceivedEvents[ei]. + targets_content.jointvelocities, + rtu_ReceivedEvents[ei]. + targets_content.motorcurrent, + rtu_ReceivedEvents[ei]. + targets_content.motorvoltage, rty_targets, + localDW); + break; + + case SupervisorEvents_SetControlMode: + localDW->requiredControlMode = rtu_ReceivedEvents[ei]. + control_mode_content; + b_previousEvent = localDW->sfEvent; + localDW->sfEvent = supervisor_event_SetCtrlMode; + if (localDW->is_active_ControlModeHandler != 0U) { + supervisor_ControlModeHandler(rtu_ControlOutputs, rtu_SensorsData, + rtu_MotionControlInitConf, rty_targets, + rty_ConfigurationParameters, rty_Flags, localDW); + } + + localDW->sfEvent = b_previousEvent; + break; + } + } + + localDW->is_InputsDispatcher = supervisor_IN_Dummy; + } + + if (localDW->is_active_TargetsManager != 0U) { + supervisor_TargetsManager(rty_targets, localDW); + } + } + + // End of Chart: '/Supervisor' +} + +// +// File trailer for generated code. +// +// [EOF] +// diff --git a/emBODY/eBcode/arch-arm/mbd/amcbldc/supervisor/supervisor.h b/emBODY/eBcode/arch-arm/mbd/amcbldc/supervisor/supervisor.h new file mode 100644 index 000000000..8f59cd8d2 --- /dev/null +++ b/emBODY/eBcode/arch-arm/mbd/amcbldc/supervisor/supervisor.h @@ -0,0 +1,89 @@ +// +// Non-Degree Granting Education License -- for use at non-degree +// granting, nonprofit, education, and research organizations only. Not +// for commercial or industrial use. +// +// File: supervisor.h +// +// Code generated for Simulink model 'supervisor'. +// +// Model version : 1.280 +// Simulink Coder version : 23.2 (R2023b) 01-Aug-2023 +// C/C++ source code generated on : Wed Feb 28 11:29:17 2024 +// +// Target selection: ert.tlc +// Embedded hardware selection: ARM Compatible->ARM Cortex-M +// Code generation objectives: Unspecified +// Validation result: Not run +// +#ifndef RTW_HEADER_supervisor_h_ +#define RTW_HEADER_supervisor_h_ +#include "rtwtypes.h" +#include "supervisor_types.h" +#include "mc_supervisor_defines.h" + +// user code (top of header file) +#include "rtw_enable_disable_motors.h" +#include "rtw_motor_config.h" + +// Block states (default storage) for model 'supervisor' +struct DW_supervisor_f_T { + real_T isFaultButtonPressed; // '/Supervisor' + real_T isInFault; // '/Supervisor' + real_T newSetpoint; // '/Supervisor' + int32_T sfEvent; // '/Supervisor' + ControlModes requiredControlMode; // '/Supervisor' + uint8_T is_active_c3_supervisor; // '/Supervisor' + uint8_T is_active_FaultsManager; // '/Supervisor' + uint8_T is_active_HWFaults; // '/Supervisor' + uint8_T is_HWFaults; // '/Supervisor' + uint8_T is_active_FaultButton; // '/Supervisor' + uint8_T is_FaultButton; // '/Supervisor' + uint8_T is_active_ControlModeHandler;// '/Supervisor' + uint8_T is_ControlModeHandler; // '/Supervisor' + uint8_T is_active_InputsDispatcher; // '/Supervisor' + uint8_T is_InputsDispatcher; // '/Supervisor' + uint8_T is_active_TargetsManager; // '/Supervisor' + uint8_T is_TargetsManager; // '/Supervisor' + boolean_T areLimitsSet; // '/Supervisor' + boolean_T ExternalFlags_fault_button_prev;// '/Supervisor' + boolean_T ExternalFlags_fault_button_start;// '/Supervisor' +}; + +struct MdlrefDW_supervisor_T { + DW_supervisor_f_T rtdw; +}; + +extern void supervisor_Init(Targets *rty_targets, ActuatorConfiguration + *rty_ConfigurationParameters, Flags *rty_Flags, DW_supervisor_f_T *localDW); +extern void supervisor(const ExternalFlags *rtu_ExternalFlags, const + EstimatedData *rtu_EstimatedData, const FOCOutputs *rtu_ControlOutputs, const + SensorsData *rtu_SensorsData, const SupervisorReceivedEvents + rtu_ReceivedEvents[MAX_EVENTS_PER_TICK], const ActuatorConfiguration + *rtu_MotionControlInitConf, Targets *rty_targets, ActuatorConfiguration + *rty_ConfigurationParameters, Flags *rty_Flags, DW_supervisor_f_T *localDW); + +//- +// The generated code includes comments that allow you to trace directly +// back to the appropriate location in the model. The basic format +// is /block_name, where system is the system number (uniquely +// assigned by Simulink) and block_name is the name of the block. +// +// Use the MATLAB hilite_system command to trace the generated code back +// to the model. For example, +// +// hilite_system('') - opens system 3 +// hilite_system('/Kp') - opens and selects block Kp which resides in S3 +// +// Here is the system hierarchy for this model +// +// '' : 'supervisor' +// '' : 'supervisor/Supervisor' + +#endif // RTW_HEADER_supervisor_h_ + +// +// File trailer for generated code. +// +// [EOF] +// diff --git a/emBODY/eBcode/arch-arm/mbd/amcbldc/supervisor-rx/SupervisorFSM_RX_data.cpp b/emBODY/eBcode/arch-arm/mbd/amcbldc/supervisor/supervisor_private.h similarity index 54% rename from emBODY/eBcode/arch-arm/mbd/amcbldc/supervisor-rx/SupervisorFSM_RX_data.cpp rename to emBODY/eBcode/arch-arm/mbd/amcbldc/supervisor/supervisor_private.h index 8323cc8d6..e96cbd195 100644 --- a/emBODY/eBcode/arch-arm/mbd/amcbldc/supervisor-rx/SupervisorFSM_RX_data.cpp +++ b/emBODY/eBcode/arch-arm/mbd/amcbldc/supervisor/supervisor_private.h @@ -3,26 +3,24 @@ // granting, nonprofit, education, and research organizations only. Not // for commercial or industrial use. // -// File: SupervisorFSM_RX_data.cpp +// File: supervisor_private.h // -// Code generated for Simulink model 'SupervisorFSM_RX'. +// Code generated for Simulink model 'supervisor'. // -// Model version : 7.5 +// Model version : 1.280 // Simulink Coder version : 23.2 (R2023b) 01-Aug-2023 -// C/C++ source code generated on : Tue Feb 13 11:54:08 2024 +// C/C++ source code generated on : Wed Feb 28 11:29:17 2024 // // Target selection: ert.tlc // Embedded hardware selection: ARM Compatible->ARM Cortex-M // Code generation objectives: Unspecified // Validation result: Not run // -#include "SupervisorFSM_RX_private.h" - -// Invariant block signals (default storage) -const ConstB_SupervisorFSM_RX_h_T SupervisorFSM_RX_ConstB = { - 0 - // '/Constant5' -}; +#ifndef RTW_HEADER_supervisor_private_h_ +#define RTW_HEADER_supervisor_private_h_ +#include "rtwtypes.h" +#include "supervisor_types.h" +#endif // RTW_HEADER_supervisor_private_h_ // // File trailer for generated code. diff --git a/emBODY/eBcode/arch-arm/mbd/amcbldc/supervisor/supervisor_types.h b/emBODY/eBcode/arch-arm/mbd/amcbldc/supervisor/supervisor_types.h new file mode 100644 index 000000000..9de8f994a --- /dev/null +++ b/emBODY/eBcode/arch-arm/mbd/amcbldc/supervisor/supervisor_types.h @@ -0,0 +1,328 @@ +// +// Non-Degree Granting Education License -- for use at non-degree +// granting, nonprofit, education, and research organizations only. Not +// for commercial or industrial use. +// +// File: supervisor_types.h +// +// Code generated for Simulink model 'supervisor'. +// +// Model version : 1.280 +// Simulink Coder version : 23.2 (R2023b) 01-Aug-2023 +// C/C++ source code generated on : Wed Feb 28 11:29:17 2024 +// +// Target selection: ert.tlc +// Embedded hardware selection: ARM Compatible->ARM Cortex-M +// Code generation objectives: Unspecified +// Validation result: Not run +// +#ifndef RTW_HEADER_supervisor_types_h_ +#define RTW_HEADER_supervisor_types_h_ +#include "rtwtypes.h" + +// Includes for objects with custom storage classes +#include "mc_supervisor_defines.h" + +// +// Registered constraints for dimension variants + +#if MAX_EVENTS_PER_TICK <= 0 +# error "The preprocessor definition 'MAX_EVENTS_PER_TICK' must be greater than '0'" +#endif + +#if MAX_EVENTS_PER_TICK >= 5 +# error "The preprocessor definition 'MAX_EVENTS_PER_TICK' must be less than '5'" +#endif + +#ifndef DEFINED_TYPEDEF_FOR_ExternalFlags_ +#define DEFINED_TYPEDEF_FOR_ExternalFlags_ + +struct ExternalFlags +{ + // External Fault Button (1 == pressed) + boolean_T fault_button; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_EstimatedData_ +#define DEFINED_TYPEDEF_FOR_EstimatedData_ + +struct EstimatedData +{ + // velocity + real32_T jointvelocities; + + // filtered motor current + real32_T Iq_filtered; + + // motor temperature + real32_T motor_temperature; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_FOCOutputs_ +#define DEFINED_TYPEDEF_FOR_FOCOutputs_ + +struct FOCOutputs +{ + // control effort (quadrature) + real32_T Vq; + + // control effort (3-phases) + real32_T Vabc[3]; + + // quadrature current + real32_T Iq_fbk; + + // direct current + real32_T Id_fbk; + + // RMS of Iq + real32_T Iq_rms; + + // RMS of Id + real32_T Id_rms; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_DriverSensors_ +#define DEFINED_TYPEDEF_FOR_DriverSensors_ + +struct DriverSensors +{ + real32_T Vcc; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_MotorSensors_ +#define DEFINED_TYPEDEF_FOR_MotorSensors_ + +struct MotorSensors +{ + real32_T Iabc[3]; + + // electrical angle = angle * pole_pairs + real32_T angle; + real32_T temperature; + real32_T voltage; + real32_T current; + uint8_T hallABC; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_SensorsData_ +#define DEFINED_TYPEDEF_FOR_SensorsData_ + +struct SensorsData +{ + // position encoders + real32_T jointpositions; + DriverSensors driversensors; + MotorSensors motorsensors; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_Targets_ +#define DEFINED_TYPEDEF_FOR_Targets_ + +struct Targets +{ + real32_T jointpositions; + real32_T jointvelocities; + real32_T motorcurrent; + real32_T motorvoltage; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_ControlModes_ +#define DEFINED_TYPEDEF_FOR_ControlModes_ + +typedef enum { + ControlModes_NotConfigured = 0, // Default value + ControlModes_Idle, + ControlModes_Position, + ControlModes_PositionDirect, + ControlModes_Current, + ControlModes_Velocity, + ControlModes_Voltage, + ControlModes_HwFaultCM +} ControlModes; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_PID_ +#define DEFINED_TYPEDEF_FOR_PID_ + +struct PID +{ + ControlModes type; + real32_T OutMax; + real32_T OutMin; + real32_T P; + real32_T I; + real32_T D; + real32_T N; + real32_T I0; + real32_T D0; + uint8_T shift_factor; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_SupervisorEvents_ +#define DEFINED_TYPEDEF_FOR_SupervisorEvents_ + +typedef enum { + SupervisorEvents_None = 0, // Default value + SupervisorEvents_SetLimit, + SupervisorEvents_SetControlMode, + SupervisorEvents_SetMotorConfig, + SupervisorEvents_SetPid, + SupervisorEvents_SetTarget +} SupervisorEvents; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_SupervisorInputLimits_ +#define DEFINED_TYPEDEF_FOR_SupervisorInputLimits_ + +struct SupervisorInputLimits +{ + real32_T overload; + real32_T peak; + real32_T nominal; + ControlModes type; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_MotorConfiguration_ +#define DEFINED_TYPEDEF_FOR_MotorConfiguration_ + +struct MotorConfiguration +{ + boolean_T has_hall_sens; + boolean_T has_quadrature_encoder; + boolean_T has_speed_quadrature_encoder; + boolean_T has_torque_sens; + boolean_T use_index; + boolean_T enable_verbosity; + int16_T hall_sensors_offset; + int16_T rotor_encoder_resolution; + int16_T rotor_index_offset; + uint8_T encoder_tolerance; + uint8_T pole_pairs; + real32_T Kbemf; + real32_T Rphase; + real32_T Imin; + real32_T Imax; + real32_T Vcc; + real32_T Vmax; + real32_T resistance; + real32_T inductance; + real32_T thermal_resistance; + real32_T thermal_time_constant; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_SupervisorReceivedEvents_ +#define DEFINED_TYPEDEF_FOR_SupervisorReceivedEvents_ + +struct SupervisorReceivedEvents +{ + Targets targets_content; + PID pid_content; + SupervisorEvents event_type; + ControlModes control_mode_content; + SupervisorInputLimits limits_content; + MotorConfiguration motor_config_content; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_Thresholds_ +#define DEFINED_TYPEDEF_FOR_Thresholds_ + +struct Thresholds +{ + // Can be only non-negative + real32_T jntVelMax; + + // Current that can be kept for an indefinite period of time w/o damaging the motor + // Expressed in [A] as all the internal computations are done this way + // Can be only non-negative + real32_T motorNominalCurrents; + + // Current that can be applied for a short period of time + // Expressed in [A] as all the internal computations are done this way + // Can be only non-negative + real32_T motorPeakCurrents; + + // Currents over this threshold can instantaneously damages the motor + // Expressed in [A] as all the internal computations are done this way + // Can be only non-negative + real32_T motorOverloadCurrents; + + // Expressed in ticks + // Max value is 32000 + // Can be only non-negative + uint32_T motorPwmLimit; + + // The critical temperature of the motor that triggers i2t current protection. + real32_T motorCriticalTemperature; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_PIDsConfiguration_ +#define DEFINED_TYPEDEF_FOR_PIDsConfiguration_ + +struct PIDsConfiguration +{ + PID currentPID; + PID velocityPID; + PID positionPID; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_ActuatorConfiguration_ +#define DEFINED_TYPEDEF_FOR_ActuatorConfiguration_ + +struct ActuatorConfiguration +{ + Thresholds thresholds; + PIDsConfiguration pids; + MotorConfiguration motor; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_Flags_ +#define DEFINED_TYPEDEF_FOR_Flags_ + +struct Flags +{ + boolean_T enable_sending_msg_status; + + // control mode + ControlModes control_mode; + boolean_T enable_thermal_protection; +}; + +#endif +#endif // RTW_HEADER_supervisor_types_h_ + +// +// File trailer for generated code. +// +// [EOF] +//