diff --git a/emBODY/eBcode/arch-arm/board/amc2c/application/proj/amc2c-appl-can.uvoptx b/emBODY/eBcode/arch-arm/board/amc2c/application/proj/amc2c-appl-can.uvoptx index 864f58607f..085cfd003c 100644 --- a/emBODY/eBcode/arch-arm/board/amc2c/application/proj/amc2c-appl-can.uvoptx +++ b/emBODY/eBcode/arch-arm/board/amc2c/application/proj/amc2c-appl-can.uvoptx @@ -259,244 +259,6 @@ - - appl-can-ulpro - 0x4 - ARM-ADS - - 12000000 - - 1 - 1 - 0 - 1 - 0 - - - 1 - 65535 - 0 - 0 - 0 - - - 79 - 66 - 8 - .\lst\ - - - 1 - 1 - 1 - 0 - 1 - 1 - 0 - 1 - 0 - 0 - 0 - 0 - - - 1 - 1 - 1 - 1 - 1 - 1 - 1 - 0 - 0 - - - 1 - 0 - 1 - - 18 - - 0 - 1 - 1 - 1 - 1 - 1 - 1 - 1 - 1 - 1 - 1 - 1 - 1 - 1 - 0 - 1 - 1 - 1 - 1 - 0 - 0 - 1 - 0 - 0 - 6 - - - - - - - - - - - STLink\ST-LINKIII-KEIL_SWO.dll - - - - 0 - ULP2CM3 - -UP0948199 -O206 -S8 -C0 -P00000003 -N00("ARM CoreSight SW-DP") -D00(6BA02477) -L00(0) -TO65554 -TC200000000 -TT10000000 -TP18 -TDX0 -TDD0 -TDS8000 -TDT0 -TDC1F -TIE80000001 -TIP9 -FO7 -FD10000000 -FC8000 -FN1 -FF0STM32H7x_2048.FLM -FS08000000 -FL0200000 -FP0($$Device:STM32H745IIKx$CMSIS\Flash\STM32H7x_2048.FLM) - - - 0 - UL2CM3 - UL2CM3(-S0 -C0 -P0 ) -FN1 -FC8000 -FD10000000 -FF0STM32H7x_2048 -FL0200000 -FS08000000 -FP0($$Device:STM32H745IIKx$CMSIS\Flash\STM32H7x_2048.FLM) - - - 0 - ST-LINKIII-KEIL_SWO - -U005700373137510539383538 -O462 -SF24000 -C0 -A3 -I0 -HNlocalhost -HP7184 -P1 -N00("ARM CoreSight SW-DP") -D00(6BA02477) -L00(0) -TO19 -TC200000000 -TT400000000 -TP21 -TDS8021 -TDT0 -TDC1F -TIE80000000 -TIP9 -FO7 -FD10000000 -FC8000 -FN1 -FF0STM32H7x_2048.FLM -FS08000000 -FL0200000 -FP0($$Device:STM32H745IIKx$CMSIS\Flash\STM32H7x_2048.FLM) - - - 0 - ARMRTXEVENTFLAGS - -L200 -Z14 -C0 -M1 -T1 - - - 0 - DLGTARM - (1010=-1,-1,-1,-1,0)(1007=-1,-1,-1,-1,0)(1008=-1,-1,-1,-1,0)(1009=-1,-1,-1,-1,0)(1012=-1,-1,-1,-1,0) - - - 0 - ARMDBGFLAGS - - - - 0 - DLGUARM - (105=-1,-1,-1,-1,0) - - - - - - 0 - 1 - signalled - - - 1 - 1 - TXreceivedback - - - 2 - 1 - tde.str - - - 3 - 1 - tout,0x0A - - - 4 - 1 - replyinfo - - - 5 - 1 - dd - - - - - 0 - 2 - osRtxInfo - - - - - 1 - 0 - 0x5C004000 - 0 - - - - 0 - - - 0 - 1 - 0 - 0 - 0 - 0 - 0 - 1 - 0 - 0 - 0 - 0 - 0 - 0 - 0 - 0 - 0 - 0 - 0 - 1 - 1 - 0 - 0 - 0 - - - - 0 - 0 - 0 - - - - - - - - - - - OS Support\Event Viewer - 35905 - - - - 0 - 0 - 0 - 2 - 10000000 - - - - appl-can-ulpro 0x4 @@ -596,7 +358,7 @@ 0 ULP2CM3 - -UP0948193 -O462 -S12 -C0 -P00000003 -N00("ARM CoreSight SW-DP") -D00(6BA02477) -L00(0) -TO32787 -TC200000000 -TT400000000 -TP18 -TDX0 -TDD0 -TDS8001 -TDT0 -TDC1F -TIE80000001 -TIP9 -FO7 -FD10000000 -FC8000 -FN1 -FF0STM32H7x_2048.FLM -FS08000000 -FL0200000 -FP0($$Device:STM32H745IIKx$CMSIS\Flash\STM32H7x_2048.FLM) + -UP1123199 -O462 -S12 -C0 -P00000003 -N00("ARM CoreSight SW-DP") -D00(6BA02477) -L00(0) -TO32819 -TC200000000 -TT400000000 -TP18 -TDX0 -TDD0 -TDS8001 -TDT0 -TDC1F -TIE80000001 -TIP9 -FO7 -FD10000000 -FC8000 -FN1 -FF0STM32H7x_2048.FLM -FS08000000 -FL0200000 -FP0($$Device:STM32H745IIKx$CMSIS\Flash\STM32H7x_2048.FLM) 0 @@ -606,12 +368,12 @@ 0 ST-LINKIII-KEIL_SWO - -U005700373137510539383538 -O206 -SF10000 -C0 -A3 -I2 -HNlocalhost -HP7184 -P1 -N00("ARM CoreSight SW-DP") -D00(6BA02477) -L00(0) -TO19 -TC200000000 -TT64000000 -TP21 -TDS8005 -TDT0 -TDC1F -TIE80000003 -TIP9 -FO7 -FD10000000 -FC8000 -FN1 -FF0STM32H7x_2048.FLM -FS08000000 -FL0200000 -FP0($$Device:STM32H745IIKx$CMSIS\Flash\STM32H7x_2048.FLM) + -U005700373137510539383538 -O462 -SF24000 -C0 -A3 -I0 -HNlocalhost -HP7184 -P1 -N00("ARM CoreSight SW-DP") -D00(6BA02477) -L00(0) -TO19 -TC200000000 -TT400000000 -TP21 -TDS8021 -TDT0 -TDC1F -TIE80000000 -TIP9 -FO7 -FD10000000 -FC8000 -FN1 -FF0STM32H7x_2048.FLM -FS08000000 -FL0200000 -FP0($$Device:STM32H745IIKx$CMSIS\Flash\STM32H7x_2048.FLM) 0 ARMRTXEVENTFLAGS - -L200 -Z23 -C0 -M1 -T1 + -L200 -Z11 -C0 -M1 -T1 0 @@ -634,22 +396,12 @@ 0 1 - signalled + PPP 1 1 - TXreceivedback - - - 2 - 1 - tde.str - - - 3 - 1 - tout,0x0A + activated @@ -709,12 +461,6 @@ - - - OS Support\Event Viewer - 35905 - - 0 0 @@ -732,10 +478,10 @@ 12000000 - 1 + 0 1 - 0 - 1 + 1 + 0 0 @@ -777,14 +523,14 @@ 0 - 1 + 0 0 0 18 - 0 - 1 + 1 + 0 1 1 1 @@ -793,7 +539,7 @@ 1 1 1 - 1 + 0 1 1 1 @@ -807,91 +553,32 @@ 1 0 0 - 6 + -1 - - - - - STLink\ST-LINKIII-KEIL_SWO.dll - - - - 0 - ULP2CM3 - -UP0948199 -O206 -S8 -C0 -P00000003 -N00("ARM CoreSight SW-DP") -D00(6BA02477) -L00(0) -TO65555 -TC200000000 -TT10000000 -TP18 -TDX0 -TDD0 -TDS8000 -TDT0 -TDC1F -TIE80000001 -TIP9 -FO7 -FD10000000 -FC8000 -FN1 -FF0STM32H7x_2048.FLM -FS08000000 -FL0200000 -FP0($$Device:STM32H745IIKx$CMSIS\Flash\STM32H7x_2048.FLM) - - - 0 - UL2CM3 - UL2CM3(-S0 -C0 -P0 ) -FN1 -FC8000 -FD10000000 -FF0STM32H7x_2048 -FL0200000 -FS08000000 -FP0($$Device:STM32H745IIKx$CMSIS\Flash\STM32H7x_2048.FLM) - - - 0 - ST-LINKIII-KEIL_SWO - -U002E00303137510A39383538 -O206 -SF10000 -C0 -A3 -I0 -HNlocalhost -HP7184 -P1 -N00("ARM CoreSight SW-DP (ARM Core") -D00(6BA02477) -L00(0) -TO131091 -TC200000000 -TT10000000 -TP21 -TDS8010 -TDT0 -TDC1F -TIE80000001 -TIP9 -FO7 -FD10000000 -FC8000 -FN1 -FF0STM32H7x_2048.FLM -FS08000000 -FL0200000 -FP0($$Device:STM32H745IIKx$CMSIS\Flash\STM32H7x_2048.FLM) - - - 0 - ARMRTXEVENTFLAGS - -L200 -Z20 -C0 -M1 -T1 - - - 0 - DLGTARM - (1010=-1,-1,-1,-1,0)(1007=-1,-1,-1,-1,0)(1008=-1,-1,-1,-1,0)(1009=-1,-1,-1,-1,0)(1012=-1,-1,-1,-1,0) - - - 0 - ARMDBGFLAGS - - - - 0 - DLGUARM - (105=-1,-1,-1,-1,0) - - + + + + + + - - - 0 - 1 - abs_pos,0x0A - - - - - 0 - 2 - osRtxInfo - - - - - 1 - 0 - 0x20003134 - 0 - - 0 0 - 1 - 1 + 0 + 0 0 0 0 0 - 1 + 0 0 0 0 @@ -903,7 +590,7 @@ 0 0 0 - 1 + 0 0 0 0 @@ -922,13 +609,6 @@ - - 1 - 0 - 0 - 2 - 10000000 - @@ -954,7 +634,7 @@ embot::app::board::amc2c - 0 + 1 0 0 0 @@ -1010,7 +690,7 @@ theMBD - 1 + 0 0 0 0 @@ -1490,7 +1170,7 @@ embot::hw::bsp - 0 + 1 0 0 0 @@ -1581,7 +1261,7 @@ - embot::os + motorhal 0 0 0 @@ -1593,8 +1273,8 @@ 0 0 0 - ..\..\..\..\embot\os\embot_os.cpp - embot_os.cpp + ..\..\bsp\motorhal\motorhal.cpp + motorhal.cpp 0 0 @@ -1605,8 +1285,8 @@ 0 0 0 - ..\..\..\..\embot\os\embot_os_Action.cpp - embot_os_Action.cpp + ..\..\bsp\motorhal\motorhal_config.c + motorhal_config.c 0 0 @@ -1617,8 +1297,8 @@ 0 0 0 - ..\..\..\..\embot\os\embot_os_theCallbackManager.cpp - embot_os_theCallbackManager.cpp + ..\..\bsp\motorhal\adcm.c + adcm.c 0 0 @@ -1629,8 +1309,8 @@ 0 0 0 - ..\..\..\..\embot\os\embot_os_theScheduler.cpp - embot_os_theScheduler.cpp + ..\..\bsp\motorhal\enc.c + enc.c 0 0 @@ -1641,8 +1321,8 @@ 0 0 0 - ..\..\..\..\embot\os\embot_os_theTimerManager.cpp - embot_os_theTimerManager.cpp + ..\..\bsp\motorhal\hall.c + hall.c 0 0 @@ -1653,8 +1333,8 @@ 0 0 0 - ..\..\..\..\embot\os\embot_os_Thread.cpp - embot_os_Thread.cpp + ..\..\bsp\motorhal\pwm.c + pwm.c 0 0 @@ -1665,31 +1345,31 @@ 0 0 0 - ..\..\..\..\embot\os\embot_os_Timer.cpp - embot_os_Timer.cpp + ..\..\bsp\motorhal\motorhal_faults.c + motorhal_faults.c 0 0 + + + + embot::os + 0 + 0 + 0 + 0 - 13 + 14 54 8 0 0 0 - ..\..\..\..\embot\os\embot_os_rtos.cpp - embot_os_rtos.cpp + ..\..\..\..\embot\os\embot_os.cpp + embot_os.cpp 0 0 - - - - embot::prot::can - 0 - 0 - 0 - 0 14 55 @@ -1697,8 +1377,8 @@ 0 0 0 - ..\..\..\..\embot\prot\can\embot_prot_can.cpp - embot_prot_can.cpp + ..\..\..\..\embot\os\embot_os_Action.cpp + embot_os_Action.cpp 0 0 @@ -1709,8 +1389,8 @@ 0 0 0 - ..\..\..\..\embot\prot\can\embot_prot_can_analog_periodic.cpp - embot_prot_can_analog_periodic.cpp + ..\..\..\..\embot\os\embot_os_theCallbackManager.cpp + embot_os_theCallbackManager.cpp 0 0 @@ -1721,8 +1401,8 @@ 0 0 0 - ..\..\..\..\embot\prot\can\embot_prot_can_analog_polling.cpp - embot_prot_can_analog_polling.cpp + ..\..\..\..\embot\os\embot_os_theScheduler.cpp + embot_os_theScheduler.cpp 0 0 @@ -1733,8 +1413,8 @@ 0 0 0 - ..\..\..\..\embot\prot\can\embot_prot_can_bootloader.cpp - embot_prot_can_bootloader.cpp + ..\..\..\..\embot\os\embot_os_theTimerManager.cpp + embot_os_theTimerManager.cpp 0 0 @@ -1745,8 +1425,8 @@ 0 0 0 - ..\..\..\..\embot\prot\can\embot_prot_can_inertial_periodic.cpp - embot_prot_can_inertial_periodic.cpp + ..\..\..\..\embot\os\embot_os_Thread.cpp + embot_os_Thread.cpp 0 0 @@ -1757,8 +1437,8 @@ 0 0 0 - ..\..\..\..\embot\prot\can\embot_prot_can_motor_periodic.cpp - embot_prot_can_motor_periodic.cpp + ..\..\..\..\embot\os\embot_os_Timer.cpp + embot_os_Timer.cpp 0 0 @@ -1769,120 +1449,132 @@ 0 0 0 - ..\..\..\..\embot\prot\can\embot_prot_can_motor_polling.cpp - embot_prot_can_motor_polling.cpp + ..\..\..\..\embot\os\embot_os_rtos.cpp + embot_os_rtos.cpp 0 0 + + + + embot::prot::can + 0 + 0 + 0 + 0 - 14 + 15 62 8 0 0 0 - ..\..\..\..\embot\prot\can\embot_prot_can_skin_periodic.cpp - embot_prot_can_skin_periodic.cpp + ..\..\..\..\embot\prot\can\embot_prot_can.cpp + embot_prot_can.cpp 0 0 - - - - others-cmsis-math - 0 - 0 - 0 - 0 15 63 - 4 + 8 0 0 0 - ..\..\..\..\libs\lowlevel\cmsis\dsp\v160\Lib\ARM\arm_cortexM4lf_math.lib - arm_cortexM4lf_math.lib + ..\..\..\..\embot\prot\can\embot_prot_can_analog_periodic.cpp + embot_prot_can_analog_periodic.cpp 0 0 - - - - others::motorhal - 0 - 0 - 0 - 0 - 16 + 15 64 8 0 0 0 - ..\..\bsp\motorhal\motorhal.cpp - motorhal.cpp + ..\..\..\..\embot\prot\can\embot_prot_can_analog_polling.cpp + embot_prot_can_analog_polling.cpp 0 0 - 16 + 15 65 8 0 0 0 - ..\..\bsp\motorhal\motorhal_config.c - motorhal_config.c + ..\..\..\..\embot\prot\can\embot_prot_can_bootloader.cpp + embot_prot_can_bootloader.cpp 0 0 - 16 + 15 66 8 0 0 0 - ..\..\bsp\motorhal\adcm.c - adcm.c + ..\..\..\..\embot\prot\can\embot_prot_can_inertial_periodic.cpp + embot_prot_can_inertial_periodic.cpp 0 0 - 16 + 15 67 8 0 0 0 - ..\..\bsp\motorhal\enc.c - enc.c + ..\..\..\..\embot\prot\can\embot_prot_can_motor_periodic.cpp + embot_prot_can_motor_periodic.cpp 0 0 - 16 + 15 68 8 0 0 0 - ..\..\bsp\motorhal\hall.c - hall.c + ..\..\..\..\embot\prot\can\embot_prot_can_motor_polling.cpp + embot_prot_can_motor_polling.cpp 0 0 - 16 + 15 69 8 0 0 0 - ..\..\bsp\motorhal\pwm.c - pwm.c + ..\..\..\..\embot\prot\can\embot_prot_can_skin_periodic.cpp + embot_prot_can_skin_periodic.cpp + 0 + 0 + + + + + others-cmsis-math + 0 + 0 + 0 + 0 + + 16 + 70 + 4 + 0 + 0 + 0 + ..\..\..\..\libs\lowlevel\cmsis\dsp\v160\Lib\ARM\arm_cortexM4lf_math.lib + arm_cortexM4lf_math.lib 0 0 @@ -1896,120 +1588,120 @@ 0 17 - 70 + 71 8 0 0 0 - ..\..\..\amcbldc\application\src\model-based-design\sharedutils\const_params.cpp + ..\src\model-based-design\sharedutils\const_params.cpp const_params.cpp 0 0 17 - 71 + 72 8 0 0 0 - ..\..\..\amcbldc\application\src\model-based-design\sharedutils\rt_hypotf_snf.cpp + ..\src\model-based-design\sharedutils\rt_hypotf_snf.cpp rt_hypotf_snf.cpp 0 0 17 - 72 + 73 8 0 0 0 - ..\..\..\amcbldc\application\src\model-based-design\sharedutils\rt_nonfinite.cpp + ..\src\model-based-design\sharedutils\rt_nonfinite.cpp rt_nonfinite.cpp 0 0 17 - 73 + 74 8 0 0 0 - ..\..\..\amcbldc\application\src\model-based-design\sharedutils\rt_roundd_snf.cpp + ..\src\model-based-design\sharedutils\rt_roundd_snf.cpp rt_roundd_snf.cpp 0 0 17 - 74 + 75 8 0 0 0 - ..\..\..\amcbldc\application\src\model-based-design\sharedutils\rtGetInf.cpp + ..\src\model-based-design\sharedutils\rtGetInf.cpp rtGetInf.cpp 0 0 17 - 75 + 76 8 0 0 0 - ..\..\..\amcbldc\application\src\model-based-design\sharedutils\rtGetNaN.cpp + ..\src\model-based-design\sharedutils\rtGetNaN.cpp rtGetNaN.cpp 0 0 17 - 76 + 77 8 0 0 0 - ..\..\..\amcbldc\application\src\model-based-design\sharedutils\uMultiWord2Double.cpp + ..\src\model-based-design\sharedutils\uMultiWord2Double.cpp uMultiWord2Double.cpp 0 0 17 - 77 + 78 8 0 0 0 - ..\..\..\amcbldc\application\src\model-based-design\sharedutils\uMultiWordShl.cpp + ..\src\model-based-design\sharedutils\uMultiWordShl.cpp uMultiWordShl.cpp 0 0 17 - 78 + 79 8 0 0 0 - ..\..\..\amcbldc\application\src\model-based-design\sharedutils\rtw_enable_disable_motors.c + ..\src\model-based-design\sharedutils\rtw_enable_disable_motors.c rtw_enable_disable_motors.c 0 0 17 - 79 + 80 8 0 0 0 - ..\..\..\amcbldc\application\src\model-based-design\sharedutils\rtw_motor_config.c + ..\src\model-based-design\sharedutils\rtw_motor_config.c rtw_motor_config.c 0 0 @@ -2024,12 +1716,12 @@ 0 18 - 80 + 81 8 0 0 0 - ..\..\..\amcbldc\application\src\model-based-design\can-encoder\can_encoder.cpp + ..\src\model-based-design\can-encoder\can_encoder.cpp can_encoder.cpp 0 0 @@ -2044,12 +1736,12 @@ 0 19 - 81 + 82 8 0 0 0 - ..\..\..\amcbldc\application\src\model-based-design\can-decoder\can_decoder.cpp + ..\src\model-based-design\can-decoder\can_decoder.cpp can_decoder.cpp 0 0 @@ -2064,24 +1756,24 @@ 0 20 - 82 + 83 8 0 0 0 - ..\..\..\amcbldc\application\src\model-based-design\supervisor-rx\SupervisorFSM_RX.cpp + ..\src\model-based-design\supervisor-rx\SupervisorFSM_RX.cpp SupervisorFSM_RX.cpp 0 0 20 - 83 + 84 8 0 0 0 - ..\..\..\amcbldc\application\src\model-based-design\supervisor-rx\SupervisorFSM_RX_data.cpp + ..\src\model-based-design\supervisor-rx\SupervisorFSM_RX_data.cpp SupervisorFSM_RX_data.cpp 0 0 @@ -2096,12 +1788,12 @@ 0 21 - 84 + 85 8 0 0 0 - ..\..\..\amcbldc\application\src\model-based-design\supervisor-tx\SupervisorFSM_TX.cpp + ..\src\model-based-design\supervisor-tx\SupervisorFSM_TX.cpp SupervisorFSM_TX.cpp 0 0 @@ -2116,12 +1808,12 @@ 0 22 - 85 + 86 8 0 0 0 - ..\..\..\amcbldc\application\src\model-based-design\control-outer\control_outer.cpp + ..\src\model-based-design\control-outer\control_outer.cpp control_outer.cpp 0 0 @@ -2136,36 +1828,36 @@ 0 23 - 86 + 87 8 0 0 0 - ..\..\..\amcbldc\application\src\model-based-design\control-foc\control_foc.cpp + ..\src\model-based-design\control-foc\control_foc.cpp control_foc.cpp 0 0 23 - 87 + 88 8 0 0 0 - ..\..\..\amcbldc\application\src\model-based-design\control-foc\control_foc_data.cpp + ..\src\model-based-design\control-foc\control_foc_data.cpp control_foc_data.cpp 0 0 23 - 88 + 89 8 0 0 0 - ..\..\..\amcbldc\application\src\model-based-design\control-foc\FOCInnerLoop.cpp + ..\src\model-based-design\control-foc\FOCInnerLoop.cpp FOCInnerLoop.cpp 0 0 @@ -2180,12 +1872,12 @@ 0 24 - 89 + 90 8 0 0 0 - ..\..\..\amcbldc\application\src\model-based-design\estimator\estimation_velocity.cpp + ..\src\model-based-design\estimator\estimation_velocity.cpp estimation_velocity.cpp 0 0 @@ -2200,12 +1892,12 @@ 0 25 - 90 + 91 8 0 0 0 - ..\..\..\amcbldc\application\src\model-based-design\filter-current\filter_current.cpp + ..\src\model-based-design\filter-current\filter_current.cpp filter_current.cpp 0 0 @@ -2220,12 +1912,12 @@ 0 26 - 91 + 92 8 0 0 0 - ..\..\..\amcbldc\application\src\model-based-design\amc-bldc\AMC_BLDC.cpp + ..\src\model-based-design\amc-bldc\AMC_BLDC.cpp AMC_BLDC.cpp 0 0 @@ -2240,12 +1932,12 @@ 0 27 - 92 + 93 8 0 0 0 - ..\..\..\amcbldc\application\src\model-based-design\thermal-model\thermal_model.cpp + ..\src\model-based-design\thermal-model\thermal_model.cpp thermal_model.cpp 0 0 diff --git a/emBODY/eBcode/arch-arm/board/amc2c/application/proj/amc2c-appl-can.uvprojx b/emBODY/eBcode/arch-arm/board/amc2c/application/proj/amc2c-appl-can.uvprojx index d788552c2d..736b350e2d 100644 --- a/emBODY/eBcode/arch-arm/board/amc2c/application/proj/amc2c-appl-can.uvprojx +++ b/emBODY/eBcode/arch-arm/board/amc2c/application/proj/amc2c-appl-can.uvprojx @@ -340,7 +340,7 @@ -Wno-pragma-pack -Wno-deprecated-register -DEMBOT_USE_rtos_osal USE_STM32HAL STM32HAL_BOARD_AMC2C STM32HAL_DRIVER_V1A0 - ..\..\..\..\libs\lowlevel\stm32hal\api;..\..\..\..\libs\highlevel\abslayer\osal\api;..\..\..\..\..\..\..\..\icub-firmware-shared\embot\core;..\..\..\..\embot\hw;..\..\..\..\embot\os;..\..\..\..\embot\app;..\..\..\..\embot\app;..\..\..\..\libs\midware\eventviewer\api;..\..\..\..\libs\highlevel\services\embenv\api;..\..\..\..\embot\prot\can;..\..\..\..\..\..\..\..\icub-firmware-shared\can\canProtocolLib;..\..\..\..\..\..\..\..\icub-firmware-shared\embot\tools;..\..\bsp;..\..\..\..\embot\app\eth;..\..\..\..\embot\app\bldc;..\src\app-board-amc2c;..\..\..\amcbldc\application\src\model-based-design\sharedutils;..\..\..\amcbldc\application\src\model-based-design\can-decoder;..\..\..\amcbldc\application\src\model-based-design\can-encoder;..\..\..\amcbldc\application\src\model-based-design\supervisor-rx;..\..\..\amcbldc\application\src\model-based-design\supervisor-tx;..\..\..\amcbldc\application\src\model-based-design\control-outer;..\..\..\amcbldc\application\src\model-based-design\control-foc;..\..\..\amcbldc\application\src\model-based-design\estimator;..\..\..\amcbldc\application\src\model-based-design\amc-bldc;..\..\..\amcbldc\application\src\model-based-design\filter-current;..\..\..\amcbldc\application\src\model-based-design\thermal-model;..\..\..\..\libs\lowlevel\cmsis\dsp\v160\Include;..\..\bsp\motorhal + ..\..\..\..\libs\lowlevel\stm32hal\api;..\..\..\..\libs\highlevel\abslayer\osal\api;..\..\..\..\..\..\..\..\icub-firmware-shared\embot\core;..\..\..\..\embot\hw;..\..\..\..\embot\os;..\..\..\..\embot\app;..\..\..\..\embot\app;..\..\..\..\libs\midware\eventviewer\api;..\..\..\..\libs\highlevel\services\embenv\api;..\..\..\..\embot\prot\can;..\..\..\..\..\..\..\..\icub-firmware-shared\can\canProtocolLib;..\..\..\..\..\..\..\..\icub-firmware-shared\embot\tools;..\..\bsp;..\..\..\..\embot\app\eth;..\..\..\..\embot\app\bldc;..\src\app-board-amc2c;..\src\model-based-design\sharedutils;..\src\model-based-design\can-decoder;..\src\model-based-design\can-encoder;..\src\model-based-design\supervisor-rx;..\src\model-based-design\supervisor-tx;..\src\model-based-design\control-outer;..\src\model-based-design\control-foc;..\src\model-based-design\estimator;..\src\model-based-design\amc-bldc;..\src\model-based-design\filter-current;..\src\model-based-design\thermal-model;..\..\..\..\libs\lowlevel\cmsis\dsp\v160\Include;..\..\bsp\motorhal @@ -893,6 +893,46 @@ + + motorhal + + + motorhal.cpp + 8 + ..\..\bsp\motorhal\motorhal.cpp + + + motorhal_config.c + 8 + ..\..\bsp\motorhal\motorhal_config.c + + + adcm.c + 8 + ..\..\bsp\motorhal\adcm.c + + + enc.c + 8 + ..\..\bsp\motorhal\enc.c + + + hall.c + 8 + ..\..\bsp\motorhal\hall.c + + + pwm.c + 8 + ..\..\bsp\motorhal\pwm.c + + + motorhal_faults.c + 8 + ..\..\bsp\motorhal\motorhal_faults.c + + + embot::os @@ -993,93 +1033,58 @@ - - others::motorhal - - - motorhal.cpp - 8 - ..\..\bsp\motorhal\motorhal.cpp - - - motorhal_config.c - 8 - ..\..\bsp\motorhal\motorhal_config.c - - - adcm.c - 8 - ..\..\bsp\motorhal\adcm.c - - - enc.c - 8 - ..\..\bsp\motorhal\enc.c - - - hall.c - 8 - ..\..\bsp\motorhal\hall.c - - - pwm.c - 8 - ..\..\bsp\motorhal\pwm.c - - - mbd const_params.cpp 8 - ..\..\..\amcbldc\application\src\model-based-design\sharedutils\const_params.cpp + ..\src\model-based-design\sharedutils\const_params.cpp rt_hypotf_snf.cpp 8 - ..\..\..\amcbldc\application\src\model-based-design\sharedutils\rt_hypotf_snf.cpp + ..\src\model-based-design\sharedutils\rt_hypotf_snf.cpp rt_nonfinite.cpp 8 - ..\..\..\amcbldc\application\src\model-based-design\sharedutils\rt_nonfinite.cpp + ..\src\model-based-design\sharedutils\rt_nonfinite.cpp rt_roundd_snf.cpp 8 - ..\..\..\amcbldc\application\src\model-based-design\sharedutils\rt_roundd_snf.cpp + ..\src\model-based-design\sharedutils\rt_roundd_snf.cpp rtGetInf.cpp 8 - ..\..\..\amcbldc\application\src\model-based-design\sharedutils\rtGetInf.cpp + ..\src\model-based-design\sharedutils\rtGetInf.cpp rtGetNaN.cpp 8 - ..\..\..\amcbldc\application\src\model-based-design\sharedutils\rtGetNaN.cpp + ..\src\model-based-design\sharedutils\rtGetNaN.cpp uMultiWord2Double.cpp 8 - ..\..\..\amcbldc\application\src\model-based-design\sharedutils\uMultiWord2Double.cpp + ..\src\model-based-design\sharedutils\uMultiWord2Double.cpp uMultiWordShl.cpp 8 - ..\..\..\amcbldc\application\src\model-based-design\sharedutils\uMultiWordShl.cpp + ..\src\model-based-design\sharedutils\uMultiWordShl.cpp rtw_enable_disable_motors.c 8 - ..\..\..\amcbldc\application\src\model-based-design\sharedutils\rtw_enable_disable_motors.c + ..\src\model-based-design\sharedutils\rtw_enable_disable_motors.c rtw_motor_config.c 8 - ..\..\..\amcbldc\application\src\model-based-design\sharedutils\rtw_motor_config.c + ..\src\model-based-design\sharedutils\rtw_motor_config.c @@ -1089,7 +1094,7 @@ can_encoder.cpp 8 - ..\..\..\amcbldc\application\src\model-based-design\can-encoder\can_encoder.cpp + ..\src\model-based-design\can-encoder\can_encoder.cpp @@ -1099,7 +1104,7 @@ can_decoder.cpp 8 - ..\..\..\amcbldc\application\src\model-based-design\can-decoder\can_decoder.cpp + ..\src\model-based-design\can-decoder\can_decoder.cpp @@ -1109,12 +1114,12 @@ SupervisorFSM_RX.cpp 8 - ..\..\..\amcbldc\application\src\model-based-design\supervisor-rx\SupervisorFSM_RX.cpp + ..\src\model-based-design\supervisor-rx\SupervisorFSM_RX.cpp SupervisorFSM_RX_data.cpp 8 - ..\..\..\amcbldc\application\src\model-based-design\supervisor-rx\SupervisorFSM_RX_data.cpp + ..\src\model-based-design\supervisor-rx\SupervisorFSM_RX_data.cpp @@ -1124,7 +1129,7 @@ SupervisorFSM_TX.cpp 8 - ..\..\..\amcbldc\application\src\model-based-design\supervisor-tx\SupervisorFSM_TX.cpp + ..\src\model-based-design\supervisor-tx\SupervisorFSM_TX.cpp @@ -1134,7 +1139,7 @@ control_outer.cpp 8 - ..\..\..\amcbldc\application\src\model-based-design\control-outer\control_outer.cpp + ..\src\model-based-design\control-outer\control_outer.cpp @@ -1144,17 +1149,17 @@ control_foc.cpp 8 - ..\..\..\amcbldc\application\src\model-based-design\control-foc\control_foc.cpp + ..\src\model-based-design\control-foc\control_foc.cpp control_foc_data.cpp 8 - ..\..\..\amcbldc\application\src\model-based-design\control-foc\control_foc_data.cpp + ..\src\model-based-design\control-foc\control_foc_data.cpp FOCInnerLoop.cpp 8 - ..\..\..\amcbldc\application\src\model-based-design\control-foc\FOCInnerLoop.cpp + ..\src\model-based-design\control-foc\FOCInnerLoop.cpp @@ -1164,7 +1169,7 @@ estimation_velocity.cpp 8 - ..\..\..\amcbldc\application\src\model-based-design\estimator\estimation_velocity.cpp + ..\src\model-based-design\estimator\estimation_velocity.cpp @@ -1174,7 +1179,7 @@ filter_current.cpp 8 - ..\..\..\amcbldc\application\src\model-based-design\filter-current\filter_current.cpp + ..\src\model-based-design\filter-current\filter_current.cpp @@ -1184,7 +1189,7 @@ AMC_BLDC.cpp 8 - ..\..\..\amcbldc\application\src\model-based-design\amc-bldc\AMC_BLDC.cpp + ..\src\model-based-design\amc-bldc\AMC_BLDC.cpp @@ -1194,7 +1199,7 @@ thermal_model.cpp 8 - ..\..\..\amcbldc\application\src\model-based-design\thermal-model\thermal_model.cpp + ..\src\model-based-design\thermal-model\thermal_model.cpp @@ -1243,7 +1248,7 @@ 1 .\obj\ - amc2c + amc2c_can 1 0 1 @@ -1274,9 +1279,9 @@ 0 - 0 + 1 0 - + cmd.exe /C copy .\obj\amc2c_can.hex ..\bin\amc2c.can.hex 0 0 @@ -1508,7 +1513,7 @@ 1 - 5 + 6 0 0 1 @@ -1534,7 +1539,7 @@ -Wno-pragma-pack -Wno-deprecated-register -DEMBOT_USE_rtos_osal USE_STM32HAL STM32HAL_BOARD_AMC2C STM32HAL_DRIVER_V1A0 - ..\..\..\..\libs\lowlevel\stm32hal\api;..\..\..\..\libs\highlevel\abslayer\osal\api;..\..\..\..\..\..\..\..\icub-firmware-shared\embot\core;..\..\..\..\embot\hw;..\..\..\..\embot\os;..\..\..\..\embot\app;..\..\..\..\embot\app;..\..\..\..\libs\midware\eventviewer\api;..\..\..\..\libs\highlevel\services\embenv\api;..\..\..\..\embot\prot\can;..\..\..\..\..\..\..\..\icub-firmware-shared\can\canProtocolLib;..\..\..\..\..\..\..\..\icub-firmware-shared\embot\tools;..\..\bsp;..\..\..\..\embot\app\eth;..\..\..\..\embot\app\bldc;..\src\app-board-amc2c;..\..\..\amcbldc\application\src\model-based-design\sharedutils;..\..\..\amcbldc\application\src\model-based-design\can-decoder;..\..\..\amcbldc\application\src\model-based-design\can-encoder;..\..\..\amcbldc\application\src\model-based-design\supervisor-rx;..\..\..\amcbldc\application\src\model-based-design\supervisor-tx;..\..\..\amcbldc\application\src\model-based-design\control-outer;..\..\..\amcbldc\application\src\model-based-design\control-foc;..\..\..\amcbldc\application\src\model-based-design\estimator;..\..\..\amcbldc\application\src\model-based-design\amc-bldc;..\..\..\amcbldc\application\src\model-based-design\filter-current;..\..\..\amcbldc\application\src\model-based-design\thermal-model;..\..\..\..\libs\lowlevel\cmsis\dsp\v160\Include;..\..\bsp\motorhal + ..\..\..\..\libs\lowlevel\stm32hal\api;..\..\..\..\libs\highlevel\abslayer\osal\api;..\..\..\..\..\..\..\..\icub-firmware-shared\embot\core;..\..\..\..\embot\hw;..\..\..\..\embot\os;..\..\..\..\embot\app;..\..\..\..\embot\app;..\..\..\..\libs\midware\eventviewer\api;..\..\..\..\libs\highlevel\services\embenv\api;..\..\..\..\embot\prot\can;..\..\..\..\..\..\..\..\icub-firmware-shared\can\canProtocolLib;..\..\..\..\..\..\..\..\icub-firmware-shared\embot\tools;..\..\bsp;..\..\..\..\embot\app\eth;..\..\..\..\embot\app\bldc;..\src\app-board-amc2c;..\src\model-based-design\sharedutils;..\src\model-based-design\can-decoder;..\src\model-based-design\can-encoder;..\src\model-based-design\supervisor-rx;..\src\model-based-design\supervisor-tx;..\src\model-based-design\control-outer;..\src\model-based-design\control-foc;..\src\model-based-design\estimator;..\src\model-based-design\amc-bldc;..\src\model-based-design\filter-current;..\src\model-based-design\thermal-model;..\..\..\..\libs\lowlevel\cmsis\dsp\v160\Include;..\..\bsp\motorhal @@ -2087,6 +2092,46 @@ + + motorhal + + + motorhal.cpp + 8 + ..\..\bsp\motorhal\motorhal.cpp + + + motorhal_config.c + 8 + ..\..\bsp\motorhal\motorhal_config.c + + + adcm.c + 8 + ..\..\bsp\motorhal\adcm.c + + + enc.c + 8 + ..\..\bsp\motorhal\enc.c + + + hall.c + 8 + ..\..\bsp\motorhal\hall.c + + + pwm.c + 8 + ..\..\bsp\motorhal\pwm.c + + + motorhal_faults.c + 8 + ..\..\bsp\motorhal\motorhal_faults.c + + + embot::os @@ -2187,93 +2232,58 @@ - - others::motorhal - - - motorhal.cpp - 8 - ..\..\bsp\motorhal\motorhal.cpp - - - motorhal_config.c - 8 - ..\..\bsp\motorhal\motorhal_config.c - - - adcm.c - 8 - ..\..\bsp\motorhal\adcm.c - - - enc.c - 8 - ..\..\bsp\motorhal\enc.c - - - hall.c - 8 - ..\..\bsp\motorhal\hall.c - - - pwm.c - 8 - ..\..\bsp\motorhal\pwm.c - - - mbd const_params.cpp 8 - ..\..\..\amcbldc\application\src\model-based-design\sharedutils\const_params.cpp + ..\src\model-based-design\sharedutils\const_params.cpp rt_hypotf_snf.cpp 8 - ..\..\..\amcbldc\application\src\model-based-design\sharedutils\rt_hypotf_snf.cpp + ..\src\model-based-design\sharedutils\rt_hypotf_snf.cpp rt_nonfinite.cpp 8 - ..\..\..\amcbldc\application\src\model-based-design\sharedutils\rt_nonfinite.cpp + ..\src\model-based-design\sharedutils\rt_nonfinite.cpp rt_roundd_snf.cpp 8 - ..\..\..\amcbldc\application\src\model-based-design\sharedutils\rt_roundd_snf.cpp + ..\src\model-based-design\sharedutils\rt_roundd_snf.cpp rtGetInf.cpp 8 - ..\..\..\amcbldc\application\src\model-based-design\sharedutils\rtGetInf.cpp + ..\src\model-based-design\sharedutils\rtGetInf.cpp rtGetNaN.cpp 8 - ..\..\..\amcbldc\application\src\model-based-design\sharedutils\rtGetNaN.cpp + ..\src\model-based-design\sharedutils\rtGetNaN.cpp uMultiWord2Double.cpp 8 - ..\..\..\amcbldc\application\src\model-based-design\sharedutils\uMultiWord2Double.cpp + ..\src\model-based-design\sharedutils\uMultiWord2Double.cpp uMultiWordShl.cpp 8 - ..\..\..\amcbldc\application\src\model-based-design\sharedutils\uMultiWordShl.cpp + ..\src\model-based-design\sharedutils\uMultiWordShl.cpp rtw_enable_disable_motors.c 8 - ..\..\..\amcbldc\application\src\model-based-design\sharedutils\rtw_enable_disable_motors.c + ..\src\model-based-design\sharedutils\rtw_enable_disable_motors.c rtw_motor_config.c 8 - ..\..\..\amcbldc\application\src\model-based-design\sharedutils\rtw_motor_config.c + ..\src\model-based-design\sharedutils\rtw_motor_config.c @@ -2283,7 +2293,7 @@ can_encoder.cpp 8 - ..\..\..\amcbldc\application\src\model-based-design\can-encoder\can_encoder.cpp + ..\src\model-based-design\can-encoder\can_encoder.cpp @@ -2293,7 +2303,7 @@ can_decoder.cpp 8 - ..\..\..\amcbldc\application\src\model-based-design\can-decoder\can_decoder.cpp + ..\src\model-based-design\can-decoder\can_decoder.cpp @@ -2303,12 +2313,12 @@ SupervisorFSM_RX.cpp 8 - ..\..\..\amcbldc\application\src\model-based-design\supervisor-rx\SupervisorFSM_RX.cpp + ..\src\model-based-design\supervisor-rx\SupervisorFSM_RX.cpp SupervisorFSM_RX_data.cpp 8 - ..\..\..\amcbldc\application\src\model-based-design\supervisor-rx\SupervisorFSM_RX_data.cpp + ..\src\model-based-design\supervisor-rx\SupervisorFSM_RX_data.cpp @@ -2318,7 +2328,7 @@ SupervisorFSM_TX.cpp 8 - ..\..\..\amcbldc\application\src\model-based-design\supervisor-tx\SupervisorFSM_TX.cpp + ..\src\model-based-design\supervisor-tx\SupervisorFSM_TX.cpp @@ -2328,7 +2338,7 @@ control_outer.cpp 8 - ..\..\..\amcbldc\application\src\model-based-design\control-outer\control_outer.cpp + ..\src\model-based-design\control-outer\control_outer.cpp @@ -2338,17 +2348,17 @@ control_foc.cpp 8 - ..\..\..\amcbldc\application\src\model-based-design\control-foc\control_foc.cpp + ..\src\model-based-design\control-foc\control_foc.cpp control_foc_data.cpp 8 - ..\..\..\amcbldc\application\src\model-based-design\control-foc\control_foc_data.cpp + ..\src\model-based-design\control-foc\control_foc_data.cpp FOCInnerLoop.cpp 8 - ..\..\..\amcbldc\application\src\model-based-design\control-foc\FOCInnerLoop.cpp + ..\src\model-based-design\control-foc\FOCInnerLoop.cpp @@ -2358,7 +2368,7 @@ estimation_velocity.cpp 8 - ..\..\..\amcbldc\application\src\model-based-design\estimator\estimation_velocity.cpp + ..\src\model-based-design\estimator\estimation_velocity.cpp @@ -2368,7 +2378,7 @@ filter_current.cpp 8 - ..\..\..\amcbldc\application\src\model-based-design\filter-current\filter_current.cpp + ..\src\model-based-design\filter-current\filter_current.cpp @@ -2378,7 +2388,7 @@ AMC_BLDC.cpp 8 - ..\..\..\amcbldc\application\src\model-based-design\amc-bldc\AMC_BLDC.cpp + ..\src\model-based-design\amc-bldc\AMC_BLDC.cpp @@ -2388,7 +2398,7 @@ thermal_model.cpp 8 - ..\..\..\amcbldc\application\src\model-based-design\thermal-model\thermal_model.cpp + ..\src\model-based-design\thermal-model\thermal_model.cpp @@ -2728,7 +2738,7 @@ -Wno-pragma-pack -Wno-deprecated-register -DEMBOT_USE_rtos_osal USE_STM32HAL STM32HAL_BOARD_AMC2C STM32HAL_DRIVER_V1A0 MOTORHALCONFIG_enabletests - ..\..\..\..\libs\lowlevel\stm32hal\api;..\..\..\..\libs\highlevel\abslayer\osal\api;..\..\..\..\..\..\..\..\icub-firmware-shared\embot\core;..\..\..\..\embot\hw;..\..\..\..\embot\os;..\..\..\..\embot\app;..\..\..\..\embot\app;..\..\..\..\libs\midware\eventviewer\api;..\..\..\..\libs\highlevel\services\embenv\api;..\..\..\..\embot\prot\can;..\..\..\..\..\..\..\..\icub-firmware-shared\can\canProtocolLib;..\..\..\..\..\..\..\..\icub-firmware-shared\embot\tools;..\..\bsp;..\..\..\..\embot\app\eth;..\..\..\..\embot\app\bldc;..\src\app-board-amc2c;..\..\..\amcbldc\application\src\model-based-design\sharedutils;..\..\..\amcbldc\application\src\model-based-design\can-decoder;..\..\..\amcbldc\application\src\model-based-design\can-encoder;..\..\..\amcbldc\application\src\model-based-design\supervisor-rx;..\..\..\amcbldc\application\src\model-based-design\supervisor-tx;..\..\..\amcbldc\application\src\model-based-design\control-outer;..\..\..\amcbldc\application\src\model-based-design\control-foc;..\..\..\amcbldc\application\src\model-based-design\estimator;..\..\..\amcbldc\application\src\model-based-design\amc-bldc;..\..\..\amcbldc\application\src\model-based-design\filter-current;..\..\..\..\libs\lowlevel\cmsis\dsp\v160\Include;..\..\bsp\motorhal + ..\..\..\..\libs\lowlevel\stm32hal\api;..\..\..\..\libs\highlevel\abslayer\osal\api;..\..\..\..\..\..\..\..\icub-firmware-shared\embot\core;..\..\..\..\embot\hw;..\..\..\..\embot\os;..\..\..\..\embot\app;..\..\..\..\embot\app;..\..\..\..\libs\midware\eventviewer\api;..\..\..\..\libs\highlevel\services\embenv\api;..\..\..\..\embot\prot\can;..\..\..\..\..\..\..\..\icub-firmware-shared\can\canProtocolLib;..\..\..\..\..\..\..\..\icub-firmware-shared\embot\tools;..\..\bsp;..\..\..\..\embot\app\eth;..\..\..\..\embot\app\bldc;..\src\app-board-amc2c;..\src\model-based-design\sharedutils;..\src\model-based-design\can-decoder;..\src\model-based-design\can-encoder;..\src\model-based-design\supervisor-rx;..\src\model-based-design\supervisor-tx;..\src\model-based-design\control-outer;..\src\model-based-design\control-foc;..\src\model-based-design\estimator;..\src\model-based-design\amc-bldc;..\src\model-based-design\filter-current;..\..\..\..\libs\lowlevel\cmsis\dsp\v160\Include;..\..\bsp\motorhal @@ -3230,6 +3240,46 @@ + + motorhal + + + motorhal.cpp + 8 + ..\..\bsp\motorhal\motorhal.cpp + + + motorhal_config.c + 8 + ..\..\bsp\motorhal\motorhal_config.c + + + adcm.c + 8 + ..\..\bsp\motorhal\adcm.c + + + enc.c + 8 + ..\..\bsp\motorhal\enc.c + + + hall.c + 8 + ..\..\bsp\motorhal\hall.c + + + pwm.c + 8 + ..\..\bsp\motorhal\pwm.c + + + motorhal_faults.c + 8 + ..\..\bsp\motorhal\motorhal_faults.c + + + embot::os @@ -3330,93 +3380,58 @@ - - others::motorhal - - - motorhal.cpp - 8 - ..\..\bsp\motorhal\motorhal.cpp - - - motorhal_config.c - 8 - ..\..\bsp\motorhal\motorhal_config.c - - - adcm.c - 8 - ..\..\bsp\motorhal\adcm.c - - - enc.c - 8 - ..\..\bsp\motorhal\enc.c - - - hall.c - 8 - ..\..\bsp\motorhal\hall.c - - - pwm.c - 8 - ..\..\bsp\motorhal\pwm.c - - - mbd const_params.cpp 8 - ..\..\..\amcbldc\application\src\model-based-design\sharedutils\const_params.cpp + ..\src\model-based-design\sharedutils\const_params.cpp rt_hypotf_snf.cpp 8 - ..\..\..\amcbldc\application\src\model-based-design\sharedutils\rt_hypotf_snf.cpp + ..\src\model-based-design\sharedutils\rt_hypotf_snf.cpp rt_nonfinite.cpp 8 - ..\..\..\amcbldc\application\src\model-based-design\sharedutils\rt_nonfinite.cpp + ..\src\model-based-design\sharedutils\rt_nonfinite.cpp rt_roundd_snf.cpp 8 - ..\..\..\amcbldc\application\src\model-based-design\sharedutils\rt_roundd_snf.cpp + ..\src\model-based-design\sharedutils\rt_roundd_snf.cpp rtGetInf.cpp 8 - ..\..\..\amcbldc\application\src\model-based-design\sharedutils\rtGetInf.cpp + ..\src\model-based-design\sharedutils\rtGetInf.cpp rtGetNaN.cpp 8 - ..\..\..\amcbldc\application\src\model-based-design\sharedutils\rtGetNaN.cpp + ..\src\model-based-design\sharedutils\rtGetNaN.cpp uMultiWord2Double.cpp 8 - ..\..\..\amcbldc\application\src\model-based-design\sharedutils\uMultiWord2Double.cpp + ..\src\model-based-design\sharedutils\uMultiWord2Double.cpp uMultiWordShl.cpp 8 - ..\..\..\amcbldc\application\src\model-based-design\sharedutils\uMultiWordShl.cpp + ..\src\model-based-design\sharedutils\uMultiWordShl.cpp rtw_enable_disable_motors.c 8 - ..\..\..\amcbldc\application\src\model-based-design\sharedutils\rtw_enable_disable_motors.c + ..\src\model-based-design\sharedutils\rtw_enable_disable_motors.c rtw_motor_config.c 8 - ..\..\..\amcbldc\application\src\model-based-design\sharedutils\rtw_motor_config.c + ..\src\model-based-design\sharedutils\rtw_motor_config.c @@ -3426,7 +3441,7 @@ can_encoder.cpp 8 - ..\..\..\amcbldc\application\src\model-based-design\can-encoder\can_encoder.cpp + ..\src\model-based-design\can-encoder\can_encoder.cpp @@ -3436,7 +3451,7 @@ can_decoder.cpp 8 - ..\..\..\amcbldc\application\src\model-based-design\can-decoder\can_decoder.cpp + ..\src\model-based-design\can-decoder\can_decoder.cpp @@ -3446,12 +3461,12 @@ SupervisorFSM_RX.cpp 8 - ..\..\..\amcbldc\application\src\model-based-design\supervisor-rx\SupervisorFSM_RX.cpp + ..\src\model-based-design\supervisor-rx\SupervisorFSM_RX.cpp SupervisorFSM_RX_data.cpp 8 - ..\..\..\amcbldc\application\src\model-based-design\supervisor-rx\SupervisorFSM_RX_data.cpp + ..\src\model-based-design\supervisor-rx\SupervisorFSM_RX_data.cpp @@ -3461,7 +3476,7 @@ SupervisorFSM_TX.cpp 8 - ..\..\..\amcbldc\application\src\model-based-design\supervisor-tx\SupervisorFSM_TX.cpp + ..\src\model-based-design\supervisor-tx\SupervisorFSM_TX.cpp @@ -3471,7 +3486,7 @@ control_outer.cpp 8 - ..\..\..\amcbldc\application\src\model-based-design\control-outer\control_outer.cpp + ..\src\model-based-design\control-outer\control_outer.cpp @@ -3481,17 +3496,17 @@ control_foc.cpp 8 - ..\..\..\amcbldc\application\src\model-based-design\control-foc\control_foc.cpp + ..\src\model-based-design\control-foc\control_foc.cpp control_foc_data.cpp 8 - ..\..\..\amcbldc\application\src\model-based-design\control-foc\control_foc_data.cpp + ..\src\model-based-design\control-foc\control_foc_data.cpp FOCInnerLoop.cpp 8 - ..\..\..\amcbldc\application\src\model-based-design\control-foc\FOCInnerLoop.cpp + ..\src\model-based-design\control-foc\FOCInnerLoop.cpp @@ -3501,7 +3516,7 @@ estimation_velocity.cpp 8 - ..\..\..\amcbldc\application\src\model-based-design\estimator\estimation_velocity.cpp + ..\src\model-based-design\estimator\estimation_velocity.cpp @@ -3511,7 +3526,7 @@ filter_current.cpp 8 - ..\..\..\amcbldc\application\src\model-based-design\filter-current\filter_current.cpp + ..\src\model-based-design\filter-current\filter_current.cpp @@ -3521,7 +3536,7 @@ AMC_BLDC.cpp 8 - ..\..\..\amcbldc\application\src\model-based-design\amc-bldc\AMC_BLDC.cpp + ..\src\model-based-design\amc-bldc\AMC_BLDC.cpp @@ -3531,7 +3546,7 @@ thermal_model.cpp 8 - ..\..\..\amcbldc\application\src\model-based-design\thermal-model\thermal_model.cpp + ..\src\model-based-design\thermal-model\thermal_model.cpp diff --git a/emBODY/eBcode/arch-arm/board/amc2c/application/src/app-board-amc2c/embot_app_board_amc2c_info.cpp b/emBODY/eBcode/arch-arm/board/amc2c/application/src/app-board-amc2c/embot_app_board_amc2c_info.cpp index d991f7f39b..47f694e186 100644 --- a/emBODY/eBcode/arch-arm/board/amc2c/application/src/app-board-amc2c/embot_app_board_amc2c_info.cpp +++ b/emBODY/eBcode/arch-arm/board/amc2c/application/src/app-board-amc2c/embot_app_board_amc2c_info.cpp @@ -28,13 +28,13 @@ namespace embot::app::board::amc2c::info { constexpr embot::prot::can::applicationInfo applInfo { - embot::prot::can::versionOfAPPLICATION {100, 100, 0}, + embot::prot::can::versionOfAPPLICATION {2, 0, 4}, embot::prot::can::versionOfCANPROTOCOL {2, 0} }; constexpr embot::app::eth::Date date { - 2023, embot::app::eth::Month::Apr, embot::app::eth::Day::fourteen, 16, 54 + 2023, embot::app::eth::Month::Oct, embot::app::eth::Day::sixteen, 15, 51 }; constexpr embot::hw::FLASHpartitionID codePartition @@ -46,10 +46,19 @@ namespace embot::app::board::amc2c::info { { embot::hw::CAN::two }; + + constexpr uint8_t canaddress {3}; + + // marco.accame: i use the macro INFO32 just because ... i want to init eEmoduleExtendedInfo_t::userdefined with the same string + // and i dont know how to fit it inside otherwise + + // 0123456789abcde0123456789abcde + #define INFO32 "hi, i am an amc2c on CAN2:3" - static const char *info32 + constexpr const char *info32 { // 0123456789abcde0123456789abcde - "hi, i am an amc2c can" + //"hi, i am an amc2c CAN2:3" + INFO32 }; } // embot::app::board::amc2c::info { @@ -74,7 +83,7 @@ namespace embot::app::board::amc2c::info { if(!initted) { force_placement_of_moduleinfo(); - embot::app::board::amc2c::theCANagentCORE::getInstance().initialise({applInfo, canBus, info32}); + embot::app::board::amc2c::theCANagentCORE::getInstance().initialise({applInfo, canBus, canaddress, info32}); initted = true; } return &embot::app::board::amc2c::theCANagentCORE::getInstance(); @@ -139,22 +148,23 @@ constexpr eEmoduleExtendedInfo_t s_cm4app_info_extended __attribute__((section(E .size = 0, .addr = 0 }, - .communication = ee_commtype_none, + .communication = ee_commtype_can2, .name = "eOther01" }, .protocols = { .udpprotversion = { .major = 0, .minor = 0}, .can1protversion = { .major = 0, .minor = 0}, - .can2protversion = { .major = 0, .minor = 0}, + .can2protversion = { .major = 2, .minor = 0}, .gtwprotversion = { .major = 0, .minor = 0} }, .extra = {"EXT"} }, .compilationdatetime = __DATE__ " " __TIME__, - .userdefined = {0} + .userdefined = {INFO32} }; + #include eEmoduleExtendedInfo_t ss {}; diff --git a/emBODY/eBcode/arch-arm/board/amc2c/application/src/app-board-amc2c/embot_app_board_amc2c_theCANagentCORE.cpp b/emBODY/eBcode/arch-arm/board/amc2c/application/src/app-board-amc2c/embot_app_board_amc2c_theCANagentCORE.cpp index 2de3a5ce8d..d2636bc7cd 100644 --- a/emBODY/eBcode/arch-arm/board/amc2c/application/src/app-board-amc2c/embot_app_board_amc2c_theCANagentCORE.cpp +++ b/emBODY/eBcode/arch-arm/board/amc2c/application/src/app-board-amc2c/embot_app_board_amc2c_theCANagentCORE.cpp @@ -85,8 +85,10 @@ bool embot::app::board::amc2c::theCANagentCORE::Impl::initialise(const Config &c std::memmove(_storedinfo.info32, _config.boardinfo, sizeof(_storedinfo.info32)); + _storedinfo.canaddress = _config.canaddress; + _board = static_cast(_storedinfo.boardtype); -// _address = _storedinfo.canaddress; + _applicationinfo.version.major = _storedinfo.applicationVmajor; _applicationinfo.version.minor = _storedinfo.applicationVminor; _applicationinfo.version.build = _storedinfo.applicationVbuild; diff --git a/emBODY/eBcode/arch-arm/board/amc2c/application/src/app-board-amc2c/embot_app_board_amc2c_theCANagentCORE.h b/emBODY/eBcode/arch-arm/board/amc2c/application/src/app-board-amc2c/embot_app_board_amc2c_theCANagentCORE.h index d7696fa7aa..017c164148 100644 --- a/emBODY/eBcode/arch-arm/board/amc2c/application/src/app-board-amc2c/embot_app_board_amc2c_theCANagentCORE.h +++ b/emBODY/eBcode/arch-arm/board/amc2c/application/src/app-board-amc2c/embot_app_board_amc2c_theCANagentCORE.h @@ -28,10 +28,11 @@ namespace embot::app::board::amc2c { { embot::prot::can::applicationInfo applicationinfo {{0,0,1}, {2,0}}; embot::hw::CAN canbus {embot::hw::CAN::one}; + uint8_t canaddress {3}; const char *boardinfo {"hello, i am an amc2c"}; constexpr Config() = default; - constexpr Config(const embot::prot::can::applicationInfo& ai, embot::hw::CAN b, const char *bi) - : applicationinfo(ai), canbus(b), boardinfo(bi) {} + constexpr Config(const embot::prot::can::applicationInfo& ai, embot::hw::CAN b, uint8_t ca, const char *bi) + : applicationinfo(ai), canbus(b), canaddress(ca), boardinfo(bi) {} }; bool initialise(const Config &config); diff --git a/emBODY/eBcode/arch-arm/board/amc2c/application/src/app-board-amc2c/embot_app_board_amc2c_theMBD.cpp b/emBODY/eBcode/arch-arm/board/amc2c/application/src/app-board-amc2c/embot_app_board_amc2c_theMBD.cpp index d19ddc3316..cc4154355a 100644 --- a/emBODY/eBcode/arch-arm/board/amc2c/application/src/app-board-amc2c/embot_app_board_amc2c_theMBD.cpp +++ b/emBODY/eBcode/arch-arm/board/amc2c/application/src/app-board-amc2c/embot_app_board_amc2c_theMBD.cpp @@ -520,23 +520,17 @@ dbgFOCvalues dbgFOC {}; void embot::app::board::amc2c::theMBD::Impl::onCurrents_FOC_innerloop(void *owner, const embot::hw::motor::Currents * const currents) { - embot::hw::testpoint::on(tp2); - - //embot::hw::testpoint::on(tp2); Impl * impl = reinterpret_cast(owner); if((nullptr == impl) || (nullptr == currents)) { return; } - impl->measureFOC->start(); +// impl->measureFOC->start(); // 1. copy currents straight away, so that we can use them embot::hw::motor::Currents currs = *currents; -#if defined(TEST_DURATION_FOC) - embot::hw::sys::delay(25); -#else // remember to manage impl->EXTFAULTisPRESSED ............ @@ -568,78 +562,24 @@ void embot::app::board::amc2c::theMBD::Impl::onCurrents_FOC_innerloop(void *owne AMC_BLDC_U.SensorsData_p.motorsensors.Iabc[2] = 0.001f*currs.w; // ----------------------------------------------------------------------------- - // FOC Step Function (~26.6 KHz) + // FOC Step Function (~16.6 KHz) // ----------------------------------------------------------------------------- - AMC_BLDC_step_FOC(); + AMC_BLDC_step1(); // ----------------------------------------------------------------------------- AMC_BLDC_U.SensorsData_p.jointpositions.position = static_cast(position) * 0.0054931640625f; // iCubDegree -> deg - //constexpr float coneversion2pwmvalue (163.83F); - constexpr float coneversion2pwmvalue (12.19F); - - // Set the voltages - int32_T Vabc0 = static_cast(AMC_BLDC_Y.ControlOutputs_p.Vabc[0] * coneversion2pwmvalue); - int32_T Vabc1 = static_cast(AMC_BLDC_Y.ControlOutputs_p.Vabc[1] * coneversion2pwmvalue); - int32_T Vabc2 = static_cast(AMC_BLDC_Y.ControlOutputs_p.Vabc[2] * coneversion2pwmvalue); - -#if defined(TEST_FOC_logvalues) - dbgFOC.currents = currs; - dbgFOC.hall = hall; - dbgFOC.electricalangle = electricalAngle; - dbgFOC.position = position; - dbgFOC.jointangle = static_cast(position) * 0.0054931640625f; - dbgFOC.pwms = {Vabc0, Vabc1, Vabc2}; - -#endif - // TODO: Remove, used only for debug - // 0% PWM value on phase Vabc2 = 0,0,0 - // 10% PWM value on phase Vabc2 = 0,0,122 - // 20% PWM value on phase Vabc2 = 0,0,244 - // 30% PWM value on phase Vabc2 = 0,0,366 - - embot::hw::motor::setpwm(embot::hw::MOTOR::one, 0, 0, 122); - - -//#define DEBUG_PARAMS -#ifdef DEBUG_PARAMS - - static char msg2[64]; - static uint32_t counter; - if(counter % 10 == 0) + embot::hw::motor::PWMperc pwmperc { - sprintf(msg2, "%d,%d,%d,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f", \ - Vabc0, \ - Vabc1, \ - Vabc2, \ - AMC_BLDC_U.SensorsData_p.motorsensors.Iabc[0], \ - AMC_BLDC_U.SensorsData_p.motorsensors.Iabc[1], \ - AMC_BLDC_U.SensorsData_p.motorsensors.Iabc[2], \ - AMC_BLDC_U.SensorsData_p.motorsensors.angle, \ - AMC_BLDC_Y.EstimatedData_p.jointvelocities.velocity, \ - AMC_BLDC_B.Targets_n.motorcurrent.current, \ - AMC_BLDC_Y.ControlOutputs_p.Iq_fbk.current, \ - AMC_BLDC_Y.ControlOutputs_p.Vq); - -// sprintf(msg2, "%d,%d,%d,%.3f,%.3f,%.3f", \ -// Vabc0, \ -// Vabc1, \ -// Vabc2, \ -// AMC_BLDC_U.SensorsData_p.motorsensors.Iabc[0], \ -// AMC_BLDC_U.SensorsData_p.motorsensors.Iabc[1], \ -// AMC_BLDC_U.SensorsData_p.motorsensors.Iabc[2]); - embot::core::print(msg2); - counter = 0; - } - counter++; -#endif + AMC_BLDC_Y.ControlOutputs_p.Vabc[0], AMC_BLDC_Y.ControlOutputs_p.Vabc[1], AMC_BLDC_Y.ControlOutputs_p.Vabc[2] + }; + embot::hw::motor::setPWM(embot::hw::MOTOR::one, pwmperc); + -#endif // #if defined(TEST_DURATION_FOC) - impl->measureFOC->stop(); - embot::hw::testpoint::off(tp2); +// impl->measureFOC->stop(); } #ifdef PRINT_HISTO_DEBUG diff --git a/emBODY/eBcode/arch-arm/board/amc2c/application/src/model-based-design/amc-bldc/AMC_BLDC.cpp b/emBODY/eBcode/arch-arm/board/amc2c/application/src/model-based-design/amc-bldc/AMC_BLDC.cpp new file mode 100644 index 0000000000..aab5dce281 --- /dev/null +++ b/emBODY/eBcode/arch-arm/board/amc2c/application/src/model-based-design/amc-bldc/AMC_BLDC.cpp @@ -0,0 +1,922 @@ +// +// Non-Degree Granting Education License -- for use at non-degree +// granting, nonprofit, education, and research organizations only. Not +// for commercial or industrial use. +// +// File: AMC_BLDC.cpp +// +// Code generated for Simulink model 'AMC_BLDC'. +// +// Model version : 6.18 +// Simulink Coder version : 9.9 (R2023a) 19-Nov-2022 +// C/C++ source code generated on : Mon Oct 16 10:09:05 2023 +// +// Target selection: ert.tlc +// Embedded hardware selection: ARM Compatible->ARM Cortex-M +// Code generation objectives: Unspecified +// Validation result: Not run +// +#include "AMC_BLDC.h" +#include "rtw_mutex.h" +#include "rtwtypes.h" +#include "AMC_BLDC_types.h" +#include "thermal_model.h" +#include "estimation_velocity.h" +#include "filter_current.h" +#include "control_foc.h" +#include "control_outer.h" +#define can_decoder_MDLREF_HIDE_CHILD_ +#include "can_decoder.h" +#define can_encoder_MDLREF_HIDE_CHILD_ +#include "can_encoder.h" +#define SupervisorFSM_RX_MDLREF_HIDE_CHILD_ +#include "SupervisorFSM_RX.h" +#define SupervisorFSM_TX_MDLREF_HIDE_CHILD_ +#include "SupervisorFSM_TX.h" + +// Exported block parameters +ConfigurationParameters InitConfParams = { + { + true, + true, + false, + false, + true, + false, + 16000, + 0, + 0U, + 7U, + 0.0F, + 0.0F, + 0.0F, + 0.0F, + 44.0F, + 24.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' + + +real32_T CAN_ANGLE_DEG2ICUB = 182.044449F;// Variable: CAN_ANGLE_DEG2ICUB + // Referenced by: '/CAN_Encoder' + // 2^16/360 + +real32_T CAN_ANGLE_ICUB2DEG = 0.00549316406F;// Variable: CAN_ANGLE_ICUB2DEG + // Referenced by: '/CAN_Decoder' + // 360/2^16 + +uint8_T CAN_ID_AMC = 3U; // Variable: CAN_ID_AMC + // Referenced by: + // '/CAN_Decoder' + // '/CAN_Encoder' + // 4 bits defining the ID of the AMC_BLDC board. + + +// Block signals (default storage) +B_AMC_BLDC_T AMC_BLDC_B; + +// Block states (default storage) +DW_AMC_BLDC_T AMC_BLDC_DW; + +// External inputs (root inport signals with default storage) +ExtU_AMC_BLDC_T AMC_BLDC_U; + +// External outputs (root outports fed by signals with default storage) +ExtY_AMC_BLDC_T AMC_BLDC_Y; + +// Real-time model +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: [2.0E-5s, 0.0s] +{ + // (no output/update code required) +} + +// Model step function for TID1 +void AMC_BLDC_step1(void) // Sample time: [6.0E-5s, 0.0s] +{ + int8_T wrBufIdx; + + // 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 Element6' + // 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)); + + // RateTransition generated from: '/Adapter1' incorporates: + // Outport generated from: '/Out Bus Element' + + 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_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_LstB = + wrBufIdx; + + // End of RateTransition generated from: '/Adapter1' + + // RateTransition generated from: '/Adapter3' incorporates: + // Inport generated from: '/In Bus Element6' + + rtw_mutex_lock(); + wrBufIdx = static_cast + (AMC_BLDC_DW.RTBInsertedForAdapter_InsertedFor_Adapter3_at_outport_0_LstB + + 1); + if (wrBufIdx == 3) { + wrBufIdx = 0; + } + + if (wrBufIdx == + AMC_BLDC_DW.RTBInsertedForAdapter_InsertedFor_Adapter3_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_Adapter3_at_outport_0_Bu_e = + AMC_BLDC_U.SensorsData_p; + break; + + case 1: + AMC_BLDC_DW.RTBInsertedForAdapter_InsertedFor_Adapter3_at_outport_0_Bu_c = + AMC_BLDC_U.SensorsData_p; + break; + + case 2: + AMC_BLDC_DW.RTBInsertedForAdapter_InsertedFor_Adapter3_at_outport_0_Bu_j = + AMC_BLDC_U.SensorsData_p; + break; + } + + AMC_BLDC_DW.RTBInsertedForAdapter_InsertedFor_Adapter3_at_outport_0_LstB = + wrBufIdx; + + // End of RateTransition generated from: '/Adapter3' +} + +// 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_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_Bu_e; + 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_Bu_c; + 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_Bu_j; + break; + } + + // End of RateTransition generated from: '/Adapter3' + + // 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_RDBu = + AMC_BLDC_DW.RTBInsertedForAdapter_InsertedFor_Adapter1_at_outport_0_LstB; + rtw_mutex_unlock(); + switch + (AMC_BLDC_DW.RTBInsertedForAdapter_InsertedFor_Adapter1_at_outport_0_RDBu) { + 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; + + 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; + } + + // End of RateTransition generated from: '/Adapter1' + + // 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)); + + // 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(); + AMC_BLDC_Y.EstimatedData_p.motor_temperature = + AMC_BLDC_DW.RTBInsertedForAdapter_InsertedFor_Adapter1_at_outport_0_Buf[AMC_BLDC_DW.RTBInsertedForAdapter_InsertedFor_Adapter1_at_outport_0_RD_a]; + + // 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); + + // 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' + // 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); + + // 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); + + // ModelReference: '/OuterControl' 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; + + // RateTransition generated from: '/Adapter3' + rtw_mutex_lock(); + wrBufIdx = static_cast + (AMC_BLDC_DW.RTBInsertedForAdapter_InsertedFor_Adapter3_at_outport_0_Ls_g + + 1); + if (wrBufIdx == 3) { + wrBufIdx = 0; + } + + if (wrBufIdx == + AMC_BLDC_DW.RTBInsertedForAdapter_InsertedFor_Adapter3_at_outport_0_RD_p) + { + wrBufIdx = static_cast(wrBufIdx + 1); + if (wrBufIdx == 3) { + wrBufIdx = 0; + } + } + + rtw_mutex_unlock(); + switch (wrBufIdx) { + case 0: + AMC_BLDC_DW.RTBInsertedForAdapter_InsertedFor_Adapter3_at_outport_0_Buf0 = + AMC_BLDC_B.ZOHBlockInsertedForAdapter_InsertedFor_Adapter4_at_outport_0; + break; + + case 1: + AMC_BLDC_DW.RTBInsertedForAdapter_InsertedFor_Adapter3_at_outport_0_Buf1 = + AMC_BLDC_B.ZOHBlockInsertedForAdapter_InsertedFor_Adapter4_at_outport_0; + break; + + case 2: + AMC_BLDC_DW.RTBInsertedForAdapter_InsertedFor_Adapter3_at_outport_0_Buf2 = + AMC_BLDC_B.ZOHBlockInsertedForAdapter_InsertedFor_Adapter4_at_outport_0; + break; + } + + AMC_BLDC_DW.RTBInsertedForAdapter_InsertedFor_Adapter3_at_outport_0_Ls_g = + wrBufIdx; + + // End of RateTransition generated from: '/Adapter3' + + // RateTransition generated from: '/Adapter' + rtw_mutex_lock(); + wrBufIdx = static_cast + (AMC_BLDC_DW.RTBInsertedForAdapter_InsertedFor_Adapter_at_outport_0_LstBu + + 1); + if (wrBufIdx == 3) { + wrBufIdx = 0; + } + + if (wrBufIdx == + AMC_BLDC_DW.RTBInsertedForAdapter_InsertedFor_Adapter_at_outport_0_RDBuf) + { + wrBufIdx = static_cast(wrBufIdx + 1); + if (wrBufIdx == 3) { + wrBufIdx = 0; + } + } + + rtw_mutex_unlock(); + switch (wrBufIdx) { + case 0: + AMC_BLDC_DW.RTBInsertedForAdapter_InsertedFor_Adapter_at_outport_0_Buf0 = + AMC_BLDC_B.RTBInsertedForAdapter_InsertedFor_Adapter1_at_outport_0; + break; + + case 1: + AMC_BLDC_DW.RTBInsertedForAdapter_InsertedFor_Adapter_at_outport_0_Buf1 = + AMC_BLDC_B.RTBInsertedForAdapter_InsertedFor_Adapter1_at_outport_0; + break; + + case 2: + AMC_BLDC_DW.RTBInsertedForAdapter_InsertedFor_Adapter_at_outport_0_Buf2 = + AMC_BLDC_B.RTBInsertedForAdapter_InsertedFor_Adapter1_at_outport_0; + break; + } + + AMC_BLDC_DW.RTBInsertedForAdapter_InsertedFor_Adapter_at_outport_0_LstBu = + wrBufIdx; + + // End of RateTransition generated from: '/Adapter' + + // 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; +} + +// Model step function for TID3 +void AMC_BLDC_step_Time_10ms(void) // Sample time: [0.01s, 0.0s] +{ + // local block i/o variables + MotorTemperature rtb_Estimation_Temperature; + ConfigurationParameters + rtb_RTBInsertedForAdapter_InsertedFor_Adapter3_at_outport_0; + ControlOutputs rtb_RTBInsertedForAdapter_InsertedFor_Adapter_at_outport_0; + int8_T wrBufIdx; + + // RateTransition generated from: '/Adapter' + rtw_mutex_lock(); + AMC_BLDC_DW.RTBInsertedForAdapter_InsertedFor_Adapter_at_outport_0_RDBuf = + AMC_BLDC_DW.RTBInsertedForAdapter_InsertedFor_Adapter_at_outport_0_LstBu; + rtw_mutex_unlock(); + switch + (AMC_BLDC_DW.RTBInsertedForAdapter_InsertedFor_Adapter_at_outport_0_RDBuf) { + case 0: + rtb_RTBInsertedForAdapter_InsertedFor_Adapter_at_outport_0 = + AMC_BLDC_DW.RTBInsertedForAdapter_InsertedFor_Adapter_at_outport_0_Buf0; + break; + + case 1: + rtb_RTBInsertedForAdapter_InsertedFor_Adapter_at_outport_0 = + AMC_BLDC_DW.RTBInsertedForAdapter_InsertedFor_Adapter_at_outport_0_Buf1; + break; + + case 2: + rtb_RTBInsertedForAdapter_InsertedFor_Adapter_at_outport_0 = + AMC_BLDC_DW.RTBInsertedForAdapter_InsertedFor_Adapter_at_outport_0_Buf2; + break; + } + + // End of RateTransition generated from: '/Adapter' + + // RateTransition generated from: '/Adapter3' + rtw_mutex_lock(); + AMC_BLDC_DW.RTBInsertedForAdapter_InsertedFor_Adapter3_at_outport_0_RD_p = + AMC_BLDC_DW.RTBInsertedForAdapter_InsertedFor_Adapter3_at_outport_0_Ls_g; + rtw_mutex_unlock(); + switch + (AMC_BLDC_DW.RTBInsertedForAdapter_InsertedFor_Adapter3_at_outport_0_RD_p) { + case 0: + rtb_RTBInsertedForAdapter_InsertedFor_Adapter3_at_outport_0 = + AMC_BLDC_DW.RTBInsertedForAdapter_InsertedFor_Adapter3_at_outport_0_Buf0; + break; + + case 1: + rtb_RTBInsertedForAdapter_InsertedFor_Adapter3_at_outport_0 = + AMC_BLDC_DW.RTBInsertedForAdapter_InsertedFor_Adapter3_at_outport_0_Buf1; + break; + + case 2: + rtb_RTBInsertedForAdapter_InsertedFor_Adapter3_at_outport_0 = + AMC_BLDC_DW.RTBInsertedForAdapter_InsertedFor_Adapter3_at_outport_0_Buf2; + break; + } + + // End of RateTransition generated from: '/Adapter3' + + // ModelReference: '/Estimation_Temperature' + thermal_model(&rtb_RTBInsertedForAdapter_InsertedFor_Adapter_at_outport_0, + &rtb_RTBInsertedForAdapter_InsertedFor_Adapter3_at_outport_0, + &rtb_Estimation_Temperature, + &(AMC_BLDC_DW.Estimation_Temperature_InstanceData.rtdw)); + + // RateTransition generated from: '/Adapter1' + 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(); + AMC_BLDC_DW.RTBInsertedForAdapter_InsertedFor_Adapter1_at_outport_0_Buf[wrBufIdx] + = rtb_Estimation_Temperature; + AMC_BLDC_DW.RTBInsertedForAdapter_InsertedFor_Adapter1_at_outport_0_Ls_j = + wrBufIdx; + + // End of RateTransition generated from: '/Adapter1' +} + +// Model initialize function +void AMC_BLDC_initialize(void) +{ + // Model Initialize function for ModelReference Block: '/Estimation_Temperature' + thermal_model_initialize(rtmGetErrorStatusPointer(AMC_BLDC_M), + &(AMC_BLDC_DW.Estimation_Temperature_InstanceData.rtm), + &(AMC_BLDC_DW.Estimation_Temperature_InstanceData.rtdw)); + + // 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_Encoder' + can_encoder_initialize(rtmGetErrorStatusPointer(AMC_BLDC_M)); + + // 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: '/SupervisorFSM_RX' + SupervisorFSM_RX_initialize(rtmGetErrorStatusPointer(AMC_BLDC_M)); + + // Model Initialize function for ModelReference Block: '/SupervisorFSM_TX' + SupervisorFSM_TX_initialize(rtmGetErrorStatusPointer(AMC_BLDC_M)); + + // 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: '/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(); + + // Start for RateTransition generated from: '/Adapter1' + rtw_mutex_init(); + + // Start for RateTransition generated from: '/Adapter3' + rtw_mutex_init(); + + // Start for RateTransition generated from: '/Adapter' + rtw_mutex_init(); + + // SystemInitialize for ModelReference: '/Estimation_Temperature' + thermal_model_Init(&(AMC_BLDC_DW.Estimation_Temperature_InstanceData.rtdw)); + + // 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: '/SupervisorFSM_RX' incorporates: + // Inport generated from: '/In Bus Element' + // 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' + + SupervisorFSM_TX_Init(&AMC_BLDC_B.MessagesTx); + + // Enable for ModelReference: '/OuterControl' + control_outer_Enable(&(AMC_BLDC_DW.OuterControl_InstanceData.rtdw)); +} + +// Model terminate function +void AMC_BLDC_terminate(void) +{ + // Terminate for RateTransition generated from: '/Adapter2' + 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: '/Adapter2' + rtw_mutex_destroy(); + + // Terminate for RateTransition generated from: '/Adapter2' + rtw_mutex_destroy(); + + // Terminate for ModelReference: '/FOC' + control_foc_Term(&(AMC_BLDC_DW.FOC_InstanceData.rtdw)); + + // 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)); + + // Terminate for RateTransition generated from: '/Adapter1' + rtw_mutex_destroy(); + + // Terminate for RateTransition generated from: '/Adapter3' + rtw_mutex_destroy(); + + // Terminate for RateTransition generated from: '/Adapter' + rtw_mutex_destroy(); +} + +// +// File trailer for generated code. +// +// [EOF] +// diff --git a/emBODY/eBcode/arch-arm/board/amc2c/application/src/model-based-design/amc-bldc/AMC_BLDC.h b/emBODY/eBcode/arch-arm/board/amc2c/application/src/model-based-design/amc-bldc/AMC_BLDC.h new file mode 100644 index 0000000000..cb2de392c6 --- /dev/null +++ b/emBODY/eBcode/arch-arm/board/amc2c/application/src/model-based-design/amc-bldc/AMC_BLDC.h @@ -0,0 +1,295 @@ +// +// Non-Degree Granting Education License -- for use at non-degree +// granting, nonprofit, education, and research organizations only. Not +// for commercial or industrial use. +// +// File: AMC_BLDC.h +// +// Code generated for Simulink model 'AMC_BLDC'. +// +// Model version : 6.18 +// Simulink Coder version : 9.9 (R2023a) 19-Nov-2022 +// C/C++ source code generated on : Mon Oct 16 10:09:05 2023 +// +// Target selection: ert.tlc +// Embedded hardware selection: ARM Compatible->ARM Cortex-M +// Code generation objectives: Unspecified +// Validation result: Not run +// +#ifndef RTW_HEADER_AMC_BLDC_h_ +#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 "thermal_model.h" +#include +#include "zero_crossing_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) ((const char_T **)(&((rtm)->errorStatus))) +#endif + +#ifndef rtmStepTask +#define rtmStepTask(rtm, idx) ((rtm)->Timing.TaskCounters.TID[(idx)] == 0) +#endif + +#ifndef rtmTaskCounter +#define rtmTaskCounter(rtm, idx) ((rtm)->Timing.TaskCounters.TID[(idx)]) +#endif + +// 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' + ControlOutputs RTBInsertedForAdapter_InsertedFor_Adapter1_at_outport_0;// '/Adapter1' + SensorsData RTBInsertedForAdapter_InsertedFor_Adapter3_at_outport_0;// '/Adapter3' + BUS_STATUS_RX_MULTIPLE CAN_Decoder_o2;// '/CAN_Decoder' + BUS_CAN_RX_ERRORS_MULTIPLE CAN_Decoder_o3;// '/CAN_Decoder' +}; + +// 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 + ConfigurationParameters + RTBInsertedForAdapter_InsertedFor_Adapter3_at_outport_0_Buf0;// synthesized block + ConfigurationParameters + RTBInsertedForAdapter_InsertedFor_Adapter3_at_outport_0_Buf1;// synthesized block + ConfigurationParameters + 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 + ControlOutputs RTBInsertedForAdapter_InsertedFor_Adapter_at_outport_0_Buf0;// synthesized block + ControlOutputs RTBInsertedForAdapter_InsertedFor_Adapter_at_outport_0_Buf1;// synthesized block + ControlOutputs RTBInsertedForAdapter_InsertedFor_Adapter_at_outport_0_Buf2;// synthesized block + SensorsData RTBInsertedForAdapter_InsertedFor_Adapter3_at_outport_0_Bu_e;// synthesized block + SensorsData RTBInsertedForAdapter_InsertedFor_Adapter3_at_outport_0_Bu_c;// synthesized block + SensorsData RTBInsertedForAdapter_InsertedFor_Adapter3_at_outport_0_Bu_j;// 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_Buf[3];// 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_SEMA;// synthesized block + void* RTBInsertedForAdapter_InsertedFor_Adapter3_at_outport_0_SEMA;// synthesized block + void* RTBInsertedForAdapter_InsertedFor_Adapter1_at_outport_0_SE_l;// synthesized block + void* RTBInsertedForAdapter_InsertedFor_Adapter3_at_outport_0_SE_b;// synthesized block + void* RTBInsertedForAdapter_InsertedFor_Adapter_at_outport_0_SEMAP;// 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_LstB;// synthesized block + int8_T RTBInsertedForAdapter_InsertedFor_Adapter1_at_outport_0_RDBu;// synthesized block + int8_T RTBInsertedForAdapter_InsertedFor_Adapter3_at_outport_0_LstB;// synthesized block + int8_T RTBInsertedForAdapter_InsertedFor_Adapter3_at_outport_0_RDBu;// 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_Ls_g;// synthesized block + int8_T RTBInsertedForAdapter_InsertedFor_Adapter3_at_outport_0_RD_p;// synthesized block + int8_T RTBInsertedForAdapter_InsertedFor_Adapter_at_outport_0_LstBu;// synthesized block + int8_T RTBInsertedForAdapter_InsertedFor_Adapter_at_outport_0_RDBuf;// 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' + MdlrefDW_thermal_model_T Estimation_Temperature_InstanceData;// '/Estimation_Temperature' +}; + +// External inputs (root inport signals with default storage) +struct ExtU_AMC_BLDC_T { + SensorsData SensorsData_p; // '/SensorsData' + ExternalFlags ExternalFlags_p; // '/ExternalFlags' + BUS_CAN_MULTIPLE PacketsRx; // '/PacketsRx' +}; + +// External outputs (root outports fed by signals with default storage) +struct ExtY_AMC_BLDC_T { + ControlOutputs ControlOutputs_p; // '/ControlOutputs' + ConfigurationParameters ConfigurationParameters_p;// '/ConfigurationParameters' + Flags Flags_p; // '/Flags' + EstimatedData EstimatedData_p; // '/EstimatedData' + BUS_CAN_MULTIPLE PacketsTx; // '/PacketsTx' +}; + +// Real-time Model Data Structure +struct tag_RTM_AMC_BLDC_T { + const char_T *errorStatus; + + // + // Timing: + // The following substructure contains information regarding + // the timing information for the model. + + struct { + struct { + uint32_T TID[4]; + } TaskCounters; + } Timing; +}; + +// Block signals (default storage) +#ifdef __cplusplus + +extern "C" +{ + +#endif + + extern struct B_AMC_BLDC_T AMC_BLDC_B; + +#ifdef __cplusplus + +} + +#endif + +// Block states (default storage) +extern struct DW_AMC_BLDC_T AMC_BLDC_DW; + +#ifdef __cplusplus + +extern "C" +{ + +#endif + + // External inputs (root inport signals with default storage) + extern struct ExtU_AMC_BLDC_T AMC_BLDC_U; + + // External outputs (root outports fed by signals with default storage) + extern struct ExtY_AMC_BLDC_T AMC_BLDC_Y; + +#ifdef __cplusplus + +} + +#endif + +// +// 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: '/SupervisorFSM_RX' + +extern real32_T CAN_ANGLE_DEG2ICUB; // Variable: CAN_ANGLE_DEG2ICUB + // Referenced by: '/CAN_Encoder' + // 2^16/360 + +extern real32_T CAN_ANGLE_ICUB2DEG; // Variable: CAN_ANGLE_ICUB2DEG + // Referenced by: '/CAN_Decoder' + // 360/2^16 + +extern uint8_T CAN_ID_AMC; // Variable: CAN_ID_AMC + // Referenced by: + // '/CAN_Decoder' + // '/CAN_Encoder' + // 4 bits defining the ID of the AMC_BLDC board. + + +#ifdef __cplusplus + +extern "C" +{ + +#endif + + // Model entry point functions + extern void AMC_BLDC_initialize(void); + extern void AMC_BLDC_step0(void); + extern void AMC_BLDC_step1(void); + extern void AMC_BLDC_step_Time_1ms(void); + extern void AMC_BLDC_step_Time_10ms(void); + extern void AMC_BLDC_terminate(void); + +#ifdef __cplusplus + +} + +#endif + +// Real-time Model object +#ifdef __cplusplus + +extern "C" +{ + +#endif + + extern RT_MODEL_AMC_BLDC_T *const AMC_BLDC_M; + +#ifdef __cplusplus + +} + +#endif + +//- +// 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 +// +// '' : '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/Mux' + +#endif // RTW_HEADER_AMC_BLDC_h_ + +// +// File trailer for generated code. +// +// [EOF] +// diff --git a/emBODY/eBcode/arch-arm/board/amc2c/application/src/model-based-design/amc-bldc/AMC_BLDC_private.h b/emBODY/eBcode/arch-arm/board/amc2c/application/src/model-based-design/amc-bldc/AMC_BLDC_private.h new file mode 100644 index 0000000000..7254c864ea --- /dev/null +++ b/emBODY/eBcode/arch-arm/board/amc2c/application/src/model-based-design/amc-bldc/AMC_BLDC_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: AMC_BLDC_private.h +// +// Code generated for Simulink model 'AMC_BLDC'. +// +// Model version : 6.18 +// Simulink Coder version : 9.9 (R2023a) 19-Nov-2022 +// C/C++ source code generated on : Mon Oct 16 10:09:05 2023 +// +// Target selection: ert.tlc +// Embedded hardware selection: ARM Compatible->ARM Cortex-M +// Code generation objectives: Unspecified +// Validation result: Not run +// +#ifndef RTW_HEADER_AMC_BLDC_private_h_ +#define RTW_HEADER_AMC_BLDC_private_h_ +#include "rtwtypes.h" +#include "zero_crossing_types.h" +#include "AMC_BLDC_types.h" +#endif // RTW_HEADER_AMC_BLDC_private_h_ + +// +// File trailer for generated code. +// +// [EOF] +// diff --git a/emBODY/eBcode/arch-arm/board/amc2c/application/src/model-based-design/amc-bldc/AMC_BLDC_types.h b/emBODY/eBcode/arch-arm/board/amc2c/application/src/model-based-design/amc-bldc/AMC_BLDC_types.h new file mode 100644 index 0000000000..3fc4f2d93f --- /dev/null +++ b/emBODY/eBcode/arch-arm/board/amc2c/application/src/model-based-design/amc-bldc/AMC_BLDC_types.h @@ -0,0 +1,824 @@ +// +// Non-Degree Granting Education License -- for use at non-degree +// granting, nonprofit, education, and research organizations only. Not +// for commercial or industrial use. +// +// File: AMC_BLDC_types.h +// +// Code generated for Simulink model 'AMC_BLDC'. +// +// Model version : 6.18 +// Simulink Coder version : 9.9 (R2023a) 19-Nov-2022 +// C/C++ source code generated on : Mon Oct 16 10:09:05 2023 +// +// Target selection: ert.tlc +// Embedded hardware selection: ARM Compatible->ARM Cortex-M +// Code generation objectives: Unspecified +// Validation result: Not run +// +#ifndef RTW_HEADER_AMC_BLDC_types_h_ +#define RTW_HEADER_AMC_BLDC_types_h_ +#include "rtwtypes.h" + +// Includes for objects with custom storage classes +#include "rtw_defines.h" + +// +// Registered constraints for dimension variants + +#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'" +#endif + +#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_SensorsData_ +#define DEFINED_TYPEDEF_FOR_SensorsData_ + +struct SensorsData +{ + // position encoders + JointPositions jointpositions; + MotorSensors motorsensors; +}; + +#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 +{ + 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 Vcc; + 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_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 +{ + // 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_ + +struct ExternalFlags +{ + // External Fault Button (1 == pressed) + boolean_T fault_button; +}; + +#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[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_ + +struct BUS_FLAGS_TX +{ + boolean_T dirty; + boolean_T stuck; + boolean_T index_broken; + boolean_T phase_broken; + real32_T not_calibrated; + boolean_T ExternalFaultAsserted; + boolean_T UnderVoltageFailure; + boolean_T OverVoltageFailure; + boolean_T OverCurrentFailure; + boolean_T DHESInvalidValue; + boolean_T AS5045CSumError; + boolean_T DHESInvalidSequence; + boolean_T CANInvalidProtocol; + boolean_T CAN_BufferOverRun; + boolean_T SetpointExpired; + boolean_T CAN_TXIsPasv; + boolean_T CAN_RXIsPasv; + boolean_T CAN_IsWarnTX; + boolean_T CAN_IsWarnRX; + boolean_T OverHeating; + boolean_T ADCCalFailure; + boolean_T I2TFailure; + boolean_T EMUROMFault; + boolean_T EMUROMCRCFault; + boolean_T EncoderFault; + boolean_T FirmwareSPITimingError; + boolean_T AS5045CalcError; + boolean_T FirmwarePWMFatalError; + boolean_T CAN_TXWasPasv; + boolean_T CAN_RXWasPasv; + boolean_T CAN_RTRFlagActive; + boolean_T CAN_WasWarn; + boolean_T CAN_DLCError; + boolean_T SiliconRevisionFault; + boolean_T PositionLimitUpper; + boolean_T PositionLimitLower; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_BUS_MSG_STATUS_ +#define DEFINED_TYPEDEF_FOR_BUS_MSG_STATUS_ + +struct BUS_MSG_STATUS +{ + MCControlModes control_mode; + + // control effort (quadrature) + real32_T pwm_fbk; + BUS_FLAGS_TX flags; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_BUS_MESSAGES_TX_ +#define DEFINED_TYPEDEF_FOR_BUS_MESSAGES_TX_ + +// Aggregate of all CAN transmitted messages. +struct BUS_MESSAGES_TX +{ + BUS_MSG_FOC foc; + BUS_MSG_STATUS status; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_BUS_STATUS_TX_ +#define DEFINED_TYPEDEF_FOR_BUS_STATUS_TX_ + +// Aggregate of all events specifying types of transmitted messages. +struct BUS_STATUS_TX +{ + boolean_T foc; + boolean_T status; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_BUS_CAN_PACKET_ +#define DEFINED_TYPEDEF_FOR_BUS_CAN_PACKET_ + +// Fields of a transmitted CAN packet. +struct BUS_CAN_PACKET +{ + // ID of the CAN packet. + uint16_T ID; + + // PAYLOAD of the CAN packet. + uint8_T PAYLOAD[8]; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_BUS_CAN_ +#define DEFINED_TYPEDEF_FOR_BUS_CAN_ + +struct BUS_CAN +{ + // If true, the packet is available to be processed. + boolean_T available; + uint8_T length; + BUS_CAN_PACKET packet; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_BUS_CAN_MULTIPLE_ +#define DEFINED_TYPEDEF_FOR_BUS_CAN_MULTIPLE_ + +struct BUS_CAN_MULTIPLE +{ + BUS_CAN packets[CAN_MAX_NUM_PACKETS]; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_CANClassTypes_ +#define DEFINED_TYPEDEF_FOR_CANClassTypes_ + +typedef enum { + CANClassTypes_Motor_Control_Command = 0,// Default value + CANClassTypes_Motor_Control_Streaming = 1, + CANClassTypes_Analog_Sensors_Command = 2, + CANClassTypes_Skin_Sensor_Streaming = 4, + CANClassTypes_Inertial_Sensor_Streaming = 5, + CANClassTypes_Future_Use = 6, + CANClassTypes_Management_Bootloader = 7 +} CANClassTypes; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_BUS_CAN_ID_RX_ +#define DEFINED_TYPEDEF_FOR_BUS_CAN_ID_RX_ + +struct BUS_CAN_ID_RX +{ + // 3 bits defining the message class type. + CANClassTypes CLS; + + // 4 bits defining the source ID. + uint8_T SRC; + + // 4 bits definint the destination ID or the message sub-type. + uint8_T DST_TYP; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_BUS_CAN_CMD_ +#define DEFINED_TYPEDEF_FOR_BUS_CAN_CMD_ + +struct BUS_CAN_CMD +{ + // 1 bits for motor selector. + boolean_T M; + + // 7 bits defining the operational code of the command. + uint8_T OPC; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_BUS_CAN_PAYLOAD_RX_ +#define DEFINED_TYPEDEF_FOR_BUS_CAN_PAYLOAD_RX_ + +struct BUS_CAN_PAYLOAD_RX +{ + // Actual length of the total PAYLOAD field. + uint8_T LEN; + BUS_CAN_CMD CMD; + + // 8 bytes for the command argument in order to account also message of type streaming. + uint8_T ARG[8]; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_BUS_CAN_PACKET_RX_ +#define DEFINED_TYPEDEF_FOR_BUS_CAN_PACKET_RX_ + +// Fields of a received CAN packet. +struct BUS_CAN_PACKET_RX +{ + // ID of the CAN packet. + BUS_CAN_ID_RX ID; + + // PAYLOAD of the CAN packet. + BUS_CAN_PAYLOAD_RX PAYLOAD; +}; + +#endif + +// Forward declaration for rtModel +typedef struct tag_RTM_AMC_BLDC_T RT_MODEL_AMC_BLDC_T; + +#endif // RTW_HEADER_AMC_BLDC_types_h_ + +// +// File trailer for generated code. +// +// [EOF] +// diff --git a/emBODY/eBcode/arch-arm/board/amc2c/application/src/model-based-design/amc-bldc/ert_main.cpp b/emBODY/eBcode/arch-arm/board/amc2c/application/src/model-based-design/amc-bldc/ert_main.cpp new file mode 100644 index 0000000000..3b2a8a6add --- /dev/null +++ b/emBODY/eBcode/arch-arm/board/amc2c/application/src/model-based-design/amc-bldc/ert_main.cpp @@ -0,0 +1,293 @@ +// +// Non-Degree Granting Education License -- for use at non-degree +// granting, nonprofit, education, and research organizations only. Not +// for commercial or industrial use. +// +// File: ert_main.cpp +// +// Code generated for Simulink model 'AMC_BLDC'. +// +// Model version : 5.1 +// Simulink Coder version : 9.8 (R2022b) 13-May-2022 +// C/C++ source code generated on : Wed Sep 28 09:23:48 2022 +// +// Target selection: ert.tlc +// Embedded hardware selection: ARM Compatible->ARM Cortex-M +// Code generation objectives: Unspecified +// Validation result: Not run +// +#include +#include +#include +#include // This example main program uses printf/fflush +#include "AMC_BLDC.h" // Model header file +#define CHECK_STATUS(status,fcn) if (status != 0) {fprintf(stderr, "Call to %s returned error status (%d).\n", (fcn),(status)); perror((fcn)); fflush(stderr); exit(EXIT_FAILURE);} + +struct ThreadInfo { + HANDLE threadHandle; + unsigned threadID; + int_T index; + HANDLE startEvent; + HANDLE stopEvent; + LONG exitFlag; +}; + +ThreadInfo periodicThread[3]; +ThreadInfo periodicTriggerThread[1]; +int threadPriority[4] = { THREAD_PRIORITY_ABOVE_NORMAL, + THREAD_PRIORITY_NORMAL, + THREAD_PRIORITY_BELOW_NORMAL, + THREAD_PRIORITY_LOWEST }; + +HANDLE quitEvent = NULL; +BOOL OnCtrlHandler(DWORD ctrl) +{ + // Unused argument + (void)(ctrl); + SetEvent(quitEvent); + return true; +} + +// Model wrapper function +void AMC_BLDC_step(int_T tid) +{ + switch (tid) { + case 0 : + AMC_BLDC_step0(); + break; + + case 1 : + AMC_BLDC_step_FOC(); + break; + + case 2 : + AMC_BLDC_step_Time(); + break; + + default : + // do nothing + break; + } +} + +unsigned __stdcall baseRateTaskScheduler(void *arg) +{ + volatile boolean_T noErr; + DWORD waitResult; + HANDLE orTimer; + LARGE_INTEGER orDueTime; + int_T i; + + // Unused argument + (void)(arg); + int_T taskCounters[3] = { 0, 0, 0 }; + + int_T taskTicks[3] = { 1, 32, 875 }; + + int_T taskId[3] = { 0, 1, 2 }; + + noErr = (rtmGetErrorStatus(AMC_BLDC_M) == (NULL)); + orTimer = CreateWaitableTimer((NULL), false, (NULL)); + orDueTime.QuadPart = (LONGLONG)(1.1428571428571438E-6 * 1e7 * -1); + while (noErr) { + // Check for Ctrl+C event + waitResult = WaitForSingleObject(quitEvent, 0); + if ((waitResult == WAIT_OBJECT_0) || (waitResult == WAIT_FAILED)) { + // The quitEvent is set or the wait failed + noErr = false; + continue; + } + + CHECK_STATUS(SetWaitableTimer(orTimer, &orDueTime, 0, (NULL), (NULL), false) + == false, "SetWaitableTimer"); + for (i = 0; i < 3; i++) { + if (taskCounters[i] == 0) { + waitResult = WaitForSingleObject(periodicThread[taskId[i]].stopEvent, 0); + if (waitResult == WAIT_TIMEOUT) { + printf("Overrun - rate for periodic task %d too fast.\n", taskId[i]); + WaitForSingleObject(periodicThread[taskId[i]].stopEvent, INFINITE); + } + } + } + + noErr = (rtmGetErrorStatus(AMC_BLDC_M) == (NULL)); + if (noErr) { + for (i = 0; i <3; i++) { + if (taskCounters[i] == 0) { + SetEvent(periodicThread[taskId[i]].startEvent); + } + } + + for (i = 0; i <3; i++) { + taskCounters[i]++; + if (taskCounters[i] > (taskTicks[i]-1)) { + taskCounters[i] = 0; + } + } + } else { + for (i = 0; i < 3; i++) { + if (taskCounters[i] != 0) { + WaitForSingleObject(periodicThread[taskId[i]].stopEvent,INFINITE); + } + } + } + + if (WaitForSingleObject(orTimer, 0) == WAIT_OBJECT_0) { + printf("Overrun - periodic trigger 0 base rate too fast.\n"); + } + } // while + + for (i = 0; i < 3; i++) { + InterlockedIncrement(&periodicThread[taskId[i]].exitFlag); + SetEvent(periodicThread[taskId[i]].startEvent); + } + + _endthreadex(0); + return 0; +} + +unsigned __stdcall periodicTask(void *arg) +{ + DWORD waitResult; + ThreadInfo* info = (ThreadInfo*)arg; + volatile boolean_T noErr = true; + while (noErr) { + waitResult = WaitForSingleObject(info->startEvent,INFINITE); + if ((waitResult != WAIT_OBJECT_0) || info->exitFlag) { + // Wait failed or exitFlag is set + noErr = false; + continue; + } + + AMC_BLDC_step(info->index); + + // Get model outputs here + SetEvent(info->stopEvent); + } + + _endthreadex(0); + return 0; +} + +int main(int argc, char *argv[]) +{ + int i; + int priority[3]; + + // Unused arguments + (void)(argc); + (void)(argv); + priority[0] = threadPriority[0]; + priority[1] = threadPriority[1]; + priority[2] = threadPriority[2]; + CHECK_STATUS(SetConsoleCtrlHandler((PHANDLER_ROUTINE)OnCtrlHandler, true) == + false, "SetConsoleCtrlHandler"); + printf("**starting the model**\n"); + fflush(stdout); + + // Initialize model + AMC_BLDC_initialize(); + rtmSetErrorStatus(AMC_BLDC_M, 0); + + // Set the priority of the main thread + CHECK_STATUS(SetThreadPriority(GetCurrentThread(), + THREAD_PRIORITY_TIME_CRITICAL) == false, "SetThreadPriority"); + + // Create & initialize events used for thread synchronization + quitEvent = CreateEvent((NULL), true, false, (NULL)); + CHECK_STATUS(quitEvent == (NULL),"CreateEvent"); + + // Create periodic threads + for (i = 0; i < 3; i++) { + periodicThread[i].index = (int_T) i; + periodicThread[i].exitFlag = 0; + + // Create the events that will be used by the thread + periodicThread[i].startEvent = CreateEvent((NULL), false, false, (NULL)); + CHECK_STATUS(periodicThread[i].startEvent == (NULL), "CreateEvent"); + periodicThread[i].stopEvent = CreateEvent((NULL), false, true, (NULL)); + CHECK_STATUS(periodicThread[i].stopEvent == (NULL), "CreateStopEventEvent"); + + // Create the thread in suspended mode + periodicThread[i].threadHandle = (HANDLE)_beginthreadex((NULL), 0, + &periodicTask, &periodicThread[i], CREATE_SUSPENDED, &periodicThread[i]. + threadID); + CHECK_STATUS(periodicThread[i].threadID == 0,"_beginthreadex"); + + // Set the thread priority + CHECK_STATUS(SetThreadPriority(periodicThread[i].threadHandle, priority[i]) == + false, "SetThreadPriority"); + + // Start the thread + CHECK_STATUS(ResumeThread(periodicThread[i].threadHandle) == -1, + "ResumeThread"); + } + + // Create periodic trigger threads + { + periodicTriggerThread[0].index = 0; + periodicTriggerThread[0].exitFlag = 0; + periodicTriggerThread[0].startEvent = (NULL); + periodicTriggerThread[0].stopEvent = (NULL); + + // Create the thread in suspended mode + periodicTriggerThread[0].threadHandle = (HANDLE)_beginthreadex((NULL), 0, + &baseRateTaskScheduler, (NULL), CREATE_SUSPENDED, &periodicTriggerThread[0] + .threadID); + CHECK_STATUS(periodicTriggerThread[0].threadHandle == 0,"_beginthreadex"); + + // Set the periodic trigger thread priority + CHECK_STATUS(SetThreadPriority(periodicTriggerThread[0].threadHandle, + THREAD_PRIORITY_HIGHEST) == false, "SetThreadPriority"); + + // Start the periodic trigger thread + CHECK_STATUS(ResumeThread(periodicTriggerThread[0].threadHandle) == -1, + "ResumeThread"); + } + + // Wait for a stopping condition. + for (i = 0; i < 1; i++) { + WaitForSingleObject(periodicTriggerThread[i].threadHandle, INFINITE); + } + + // Clean up + for (i = 0; i< 1; i++) { + if (periodicTriggerThread[i].threadHandle != (NULL)) { + CloseHandle(periodicTriggerThread[i].threadHandle); + } + } + + if (quitEvent != (NULL)) { + CloseHandle(quitEvent); + } + + for (i = 0; i < 3; i++) { + if (periodicThread[i].startEvent != (NULL)) { + CloseHandle(periodicThread[i].startEvent); + } + + if (periodicThread[i].stopEvent != (NULL)) { + CloseHandle(periodicThread[i].stopEvent); + } + + if (periodicThread[i].threadHandle != (NULL)) { + CloseHandle(periodicThread[i].threadHandle); + } + } + + printf("**stopping the model**\n"); + fflush(stdout); + if (rtmGetErrorStatus(AMC_BLDC_M) != (NULL)) { + fprintf(stderr, "\n**%s**\n", rtmGetErrorStatus(AMC_BLDC_M)); + } + + // Terminate model + AMC_BLDC_terminate(); + return 0; +} + +// +// File trailer for generated code. +// +// [EOF] +// diff --git a/emBODY/eBcode/arch-arm/board/amc2c/application/src/model-based-design/can-decoder/can_decoder.cpp b/emBODY/eBcode/arch-arm/board/amc2c/application/src/model-based-design/can-decoder/can_decoder.cpp new file mode 100644 index 0000000000..cf5bcf5eaf --- /dev/null +++ b/emBODY/eBcode/arch-arm/board/amc2c/application/src/model-based-design/can-decoder/can_decoder.cpp @@ -0,0 +1,844 @@ +// +// Non-Degree Granting Education License -- for use at non-degree +// granting, nonprofit, education, and research organizations only. Not +// for commercial or industrial use. +// +// File: can_decoder.cpp +// +// Code generated for Simulink model 'can_decoder'. +// +// Model version : 5.0 +// Simulink Coder version : 9.9 (R2023a) 19-Nov-2022 +// C/C++ source code generated on : Mon Oct 16 10:07:44 2023 +// +// Target selection: ert.tlc +// Embedded hardware selection: ARM Compatible->ARM Cortex-M +// Code generation objectives: Unspecified +// Validation result: Not run +// +#include "can_decoder.h" +#include "rtwtypes.h" +#include "can_decoder_types.h" +#include +#include +#include "can_decoder_private.h" +#include "rtw_defines.h" + +// Named constants for Chart: '/Decoding Logic' +const int32_T can_decoder_CALL_EVENT = -1; +const uint8_T can_decoder_IN_Event_Error = 1U; +const uint8_T can_decoder_IN_Home = 1U; +const uint8_T can_decoder_IN_Home_k = 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); +static void can_decoder_ERROR_HANDLING(boolean_T rtu_pck_available, + B_DecodingLogic_can_decoder_T *localB, DW_DecodingLogic_can_decoder_T *localDW); +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 uint16_T can_decoder_merge_2bytes_unsigned(uint16_T bl, uint16_T bh); + +// Forward declaration for local functions +static CANClassTypes can_decoder_convert_to_enum_CANClassTypes(int32_T input); +static int32_T can_decoder_safe_cast_to_MCStreaming(int32_T input) +{ + int32_T output; + + // Initialize output value to default value for MCStreaming (Desired_Targets) + output = 15; + if ((input == 0) || (input == 15)) { + // Set output value to input value if it is a member of MCStreaming + output = input; + } + + return output; +} + +// Function for Chart: '/Decoding Logic' +static void can_decoder_ERROR_HANDLING(boolean_T rtu_pck_available, + B_DecodingLogic_can_decoder_T *localB, DW_DecodingLogic_can_decoder_T *localDW) +{ + boolean_T guard1; + guard1 = false; + switch (localDW->is_ERROR_HANDLING) { + case can_decoder_IN_Event_Error: + localDW->is_ERROR_HANDLING = can_decoder_IN_Home_k; + localDW->cmd_processed = 0U; + break; + + case can_decoder_IN_Home_k: + if (localDW->sfEvent == can_decoder_event_ev_error_pck_not4us) { + localB->error_type = CANErrorTypes_Packet_Not4Us; + localDW->ev_errorEventCounter++; + guard1 = true; + } else if (localDW->sfEvent == can_decoder_event_ev_error_pck_malformed) { + localB->error_type = CANErrorTypes_Packet_Malformed; + localDW->ev_errorEventCounter++; + guard1 = true; + } else if (localDW->sfEvent == can_decoder_event_ev_error_mode_unrecognized) + { + localB->error_type = CANErrorTypes_Mode_Unrecognized; + localDW->ev_errorEventCounter++; + guard1 = true; + } else { + if (!localDW->ev_async) { + if (rtu_pck_available && (localDW->cmd_processed == 0)) { + localB->error_type = CANErrorTypes_Packet_Unrecognized; + localDW->ev_errorEventCounter++; + } else if (localDW->cmd_processed > 1) { + localB->error_type = CANErrorTypes_Packet_MultiFunctionsDetected; + localDW->ev_errorEventCounter++; + } else { + localB->error_type = CANErrorTypes_No_Error; + } + } + + localDW->ev_async = false; + localDW->is_ERROR_HANDLING = can_decoder_IN_Home_k; + localDW->cmd_processed = 0U; + } + break; + } + + if (guard1) { + localDW->is_ERROR_HANDLING = can_decoder_IN_Event_Error; + localDW->ev_async = true; + } +} + +// Function for Chart: '/Decoding Logic' +static int16_T can_decoder_merge_2bytes_signed(uint16_T bl, uint16_T bh) +{ + int16_T sw; + uint16_T x; + x = static_cast(static_cast(bh << 8) | bl); + std::memcpy((void *)&sw, (void *)&x, (size_t)1 * sizeof(int16_T)); + return sw; +} + +// Function for Chart: '/Decoding Logic' +static boolean_T can_decoder_is_controlmode_recognized(int32_T mode) +{ + return (mode == static_cast(MCControlModes_Idle)) || (mode == + static_cast(MCControlModes_OpenLoop)) || (mode == + static_cast(MCControlModes_SpeedVoltage)) || (mode == + static_cast(MCControlModes_Current)); +} + +static int32_T can_decoder_safe_cast_to_MCControlModes(int32_T input) +{ + int32_T output; + + // Initialize output value to default value for MCControlModes (Idle) + output = 0; + if ((input == 0) || (input == 6) || ((input >= 10) && (input <= 11)) || (input + == 80) || (input == 160) || (input == 176)) { + // Set output value to input value if it is a member of MCControlModes + output = input; + } + + return output; +} + +// Function for Chart: '/Decoding Logic' +static uint16_T can_decoder_merge_2bytes_unsigned(uint16_T bl, uint16_T bh) +{ + return static_cast(static_cast(bh << 8) | bl); +} + +// System initialize for atomic system: '/Decoding Logic' +void can_decoder_DecodingLogic_Init(B_DecodingLogic_can_decoder_T *localB, + DW_DecodingLogic_can_decoder_T *localDW) +{ + localDW->sfEvent = can_decoder_CALL_EVENT; + localB->msg_set_control_mode.motor = false; + localB->msg_set_control_mode.mode = MCControlModes_Idle; + localB->msg_set_current_limit.motor = false; + localB->msg_set_current_limit.nominal = 0.0F; + localB->msg_set_current_limit.peak = 0.0F; + localB->msg_set_current_limit.overload = 0.0F; + localB->msg_desired_targets.current = 0.0F; + localB->msg_desired_targets.voltage = 0.0F; + localB->msg_desired_targets.velocity = 0.0F; + localB->msg_set_pid.motor = false; + localB->msg_set_pid.Kp = 0.0F; + localB->msg_set_pid.Ki = 0.0F; + localB->msg_set_pid.Kd = 0.0F; + localB->msg_set_pid.Ks = 0U; + localB->msg_set_motor_config.has_hall_sens = false; + localB->msg_set_motor_config.has_quadrature_encoder = false; + localB->msg_set_motor_config.has_speed_quadrature_encoder = false; + localB->msg_set_motor_config.has_torque_sens = false; + localB->msg_set_motor_config.use_index = false; + localB->msg_set_motor_config.enable_verbosity = false; + localB->msg_set_motor_config.number_poles = 0U; + localB->msg_set_motor_config.encoder_tolerance = 0U; + localB->msg_set_motor_config.rotor_encoder_resolution = 0; + localB->msg_set_motor_config.rotor_index_offset = 0; +} + +// 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 + *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) +{ + real32_T c; + int16_T tmp_merged; + uint8_T idx; + boolean_T guard1; + boolean_T tmp; + boolean_T tmp_0; + localDW->sfEvent = can_decoder_CALL_EVENT; + + // Chart: '/Decoding Logic' + // This state chart is responsible for decoding incoming CAN packets. + if (localDW->is_active_c3_can_decoder == 0U) { + localDW->is_active_c3_can_decoder = 1U; + localDW->is_active_SET_CONTROL_MODE = 1U; + localDW->is_SET_CONTROL_MODE = can_decoder_IN_Home; + localDW->is_active_DESIRED_TARGETS = 1U; + localDW->is_DESIRED_TARGETS = can_decoder_IN_Home; + localDW->is_active_SET_OPTIONS = 1U; + localDW->is_SET_OPTIONS = can_decoder_IN_Home; + localDW->is_active_SET_MOTOR_CONFIG = 1U; + localDW->is_SET_MOTOR_CONFIG = can_decoder_IN_Home; + localDW->is_active_ERROR_HANDLING = 1U; + localDW->ev_async = false; + localDW->is_ERROR_HANDLING = can_decoder_IN_Home_k; + localDW->cmd_processed = 0U; + } else { + tmp = !rtu_pck_available; + tmp_0 = (tmp || (rtu_pck_input->ID.CLS != + CANClassTypes_Motor_Control_Command)); + if ((localDW->is_active_SET_CONTROL_MODE != 0U) && + (localDW->is_SET_CONTROL_MODE == can_decoder_IN_Home) && (!tmp_0)) { + if (rtu_pck_input->ID.DST_TYP == rtu_CAN_ID_DST) { + if (rtu_pck_input->PAYLOAD.LEN >= 1) { + if (rtu_pck_input->PAYLOAD.CMD.OPC == static_cast + (MCOPC_Set_Control_Mode)) { + 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 + (can_decoder_safe_cast_to_MCControlModes + (rtu_pck_input->PAYLOAD.ARG[1])); + localDW->cmd_processed = static_cast + (localDW->cmd_processed + 1); + localDW->ev_set_control_modeEventCounter++; + } else { + localDW->sfEvent = can_decoder_event_ev_error_mode_unrecognized; + if (localDW->is_active_ERROR_HANDLING != 0U) { + can_decoder_ERROR_HANDLING(true, localB, localDW); + } + + localDW->sfEvent = -1; + localDW->is_SET_CONTROL_MODE = can_decoder_IN_Home; + } + } else { + localDW->sfEvent = can_decoder_event_ev_error_pck_malformed; + if (localDW->is_active_ERROR_HANDLING != 0U) { + can_decoder_ERROR_HANDLING(true, localB, localDW); + } + + localDW->sfEvent = -1; + localDW->is_SET_CONTROL_MODE = can_decoder_IN_Home; + } + } + } else { + localDW->sfEvent = can_decoder_event_ev_error_pck_malformed; + if (localDW->is_active_ERROR_HANDLING != 0U) { + can_decoder_ERROR_HANDLING(true, localB, localDW); + } + + localDW->sfEvent = -1; + localDW->is_SET_CONTROL_MODE = can_decoder_IN_Home; + } + } else { + localDW->sfEvent = can_decoder_event_ev_error_pck_not4us; + if (localDW->is_active_ERROR_HANDLING != 0U) { + can_decoder_ERROR_HANDLING(true, localB, localDW); + } + + localDW->sfEvent = -1; + localDW->is_SET_CONTROL_MODE = can_decoder_IN_Home; + } + } + + if ((localDW->is_active_DESIRED_TARGETS != 0U) && + (localDW->is_DESIRED_TARGETS == can_decoder_IN_Home) && ((!tmp) && + (rtu_pck_input->ID.CLS == CANClassTypes_Motor_Control_Streaming) && + (can_decoder_safe_cast_to_MCStreaming(rtu_pck_input->ID.DST_TYP) == + static_cast(MCStreaming_Desired_Targets)))) { + if ((rtu_pck_input->PAYLOAD.LEN == 8) && (rtu_CAN_ID_DST <= 4)) { + idx = static_cast((rtu_CAN_ID_DST - 1) << 1); + 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 + (tmp_merged); + localB->msg_desired_targets.voltage = static_cast + (static_cast(tmp_merged >> (rtu_CAN_VOLT_REF_SHIFT - + rtu_ConfigurationParameters->CurLoopPID.shift_factor))) / + rtu_CAN_VOLT_REF_GAIN; + localB->msg_desired_targets.velocity = 1000.0F * static_cast + (tmp_merged) * CAN_ANGLE_ICUB2DEG; + localDW->cmd_processed = static_cast(localDW->cmd_processed + + 1); + localDW->ev_desired_targetsEventCounter++; + } else { + localDW->sfEvent = can_decoder_event_ev_error_pck_malformed; + if (localDW->is_active_ERROR_HANDLING != 0U) { + can_decoder_ERROR_HANDLING(true, localB, localDW); + } + + localDW->sfEvent = -1; + localDW->is_DESIRED_TARGETS = can_decoder_IN_Home; + } + } + + if ((localDW->is_active_SET_OPTIONS != 0U) && (localDW->is_SET_OPTIONS == + can_decoder_IN_Home) && (!tmp_0)) { + if (rtu_pck_input->ID.DST_TYP == rtu_CAN_ID_DST) { + if (rtu_pck_input->PAYLOAD.LEN >= 1) { + guard1 = false; + 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; + localB->msg_set_current_limit.nominal = 0.001F * + static_cast(can_decoder_merge_2bytes_signed( + static_cast(rtu_pck_input->PAYLOAD.ARG[2]), + static_cast(rtu_pck_input->PAYLOAD.ARG[3]))); + localB->msg_set_current_limit.peak = 0.001F * static_cast + (can_decoder_merge_2bytes_unsigned(static_cast + (rtu_pck_input->PAYLOAD.ARG[4]), static_cast + (rtu_pck_input->PAYLOAD.ARG[5]))); + localB->msg_set_current_limit.overload = 0.001F * + static_cast(can_decoder_merge_2bytes_unsigned( + static_cast(rtu_pck_input->PAYLOAD.ARG[6]), + static_cast(rtu_pck_input->PAYLOAD.ARG[7]))); + localDW->cmd_processed = static_cast + (localDW->cmd_processed + 1); + localDW->ev_set_current_limitEventCounter++; + } else { + guard1 = true; + } + } else if ((rtu_pck_input->PAYLOAD.CMD.OPC == static_cast + (MCOPC_Set_Current_PID)) || + (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 + (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 + (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 + (can_decoder_merge_2bytes_signed(static_cast + (rtu_pck_input->PAYLOAD.ARG[5]), static_cast + (rtu_pck_input->PAYLOAD.ARG[6]))); + localDW->cmd_processed = static_cast + (localDW->cmd_processed + 1); + if (rtu_pck_input->PAYLOAD.CMD.OPC == static_cast + (MCOPC_Set_Current_PID)) { + localDW->ev_set_current_pidEventCounter++; + } 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++; + } + } else { + guard1 = true; + } + } + + if (guard1) { + localDW->sfEvent = can_decoder_event_ev_error_pck_malformed; + if (localDW->is_active_ERROR_HANDLING != 0U) { + can_decoder_ERROR_HANDLING(true, localB, localDW); + } + + localDW->sfEvent = -1; + localDW->is_SET_OPTIONS = can_decoder_IN_Home; + } + } else { + localDW->sfEvent = can_decoder_event_ev_error_pck_malformed; + if (localDW->is_active_ERROR_HANDLING != 0U) { + can_decoder_ERROR_HANDLING(true, localB, localDW); + } + + localDW->sfEvent = -1; + localDW->is_SET_OPTIONS = can_decoder_IN_Home; + } + } else { + localDW->sfEvent = can_decoder_event_ev_error_pck_not4us; + if (localDW->is_active_ERROR_HANDLING != 0U) { + can_decoder_ERROR_HANDLING(true, localB, localDW); + } + + localDW->sfEvent = -1; + localDW->is_SET_OPTIONS = can_decoder_IN_Home; + } + } + + if ((localDW->is_active_SET_MOTOR_CONFIG != 0U) && + (localDW->is_SET_MOTOR_CONFIG == can_decoder_IN_Home) && (!tmp_0)) { + if (rtu_pck_input->ID.DST_TYP == rtu_CAN_ID_DST) { + if (rtu_pck_input->PAYLOAD.LEN >= 1) { + if (rtu_pck_input->PAYLOAD.CMD.OPC == static_cast + (MCOPC_Set_Motor_Config)) { + if (rtu_pck_input->PAYLOAD.LEN == 8) { + localB->msg_set_motor_config.has_quadrature_encoder = + ((rtu_pck_input->PAYLOAD.ARG[1] & 1U) != 0U); + localB->msg_set_motor_config.has_hall_sens = + ((rtu_pck_input->PAYLOAD.ARG[1] & 2U) != 0U); + localB->msg_set_motor_config.has_torque_sens = + ((rtu_pck_input->PAYLOAD.ARG[1] & 4U) != 0U); + localB->msg_set_motor_config.use_index = + ((rtu_pck_input->PAYLOAD.ARG[1] & 8U) != 0U); + localB->msg_set_motor_config.has_speed_quadrature_encoder = + ((rtu_pck_input->PAYLOAD.ARG[1] & 16U) != 0U); + localB->msg_set_motor_config.enable_verbosity = + ((rtu_pck_input->PAYLOAD.ARG[1] & 32U) != 0U); + localB->msg_set_motor_config.rotor_encoder_resolution = + can_decoder_merge_2bytes_signed(static_cast + (rtu_pck_input->PAYLOAD.ARG[2]), static_cast + (rtu_pck_input->PAYLOAD.ARG[3])); + localB->msg_set_motor_config.rotor_index_offset = + 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 = + 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++; + } else { + localDW->sfEvent = can_decoder_event_ev_error_pck_malformed; + if (localDW->is_active_ERROR_HANDLING != 0U) { + can_decoder_ERROR_HANDLING(true, localB, localDW); + } + + localDW->sfEvent = -1; + localDW->is_SET_MOTOR_CONFIG = can_decoder_IN_Home; + } + } + } else { + localDW->sfEvent = can_decoder_event_ev_error_pck_malformed; + if (localDW->is_active_ERROR_HANDLING != 0U) { + can_decoder_ERROR_HANDLING(true, localB, localDW); + } + + localDW->sfEvent = -1; + localDW->is_SET_MOTOR_CONFIG = can_decoder_IN_Home; + } + } else { + localDW->sfEvent = can_decoder_event_ev_error_pck_not4us; + if (localDW->is_active_ERROR_HANDLING != 0U) { + can_decoder_ERROR_HANDLING(true, localB, localDW); + } + + localDW->sfEvent = -1; + localDW->is_SET_MOTOR_CONFIG = can_decoder_IN_Home; + } + } + + if (localDW->is_active_ERROR_HANDLING != 0U) { + can_decoder_ERROR_HANDLING(rtu_pck_available, localB, localDW); + } + } + + // End of Chart: '/Decoding Logic' + if (localDW->ev_errorEventCounter > 0U) { + 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' +static CANClassTypes can_decoder_convert_to_enum_CANClassTypes(int32_T input) +{ + CANClassTypes output; + + // Initialize output value to default value for CANClassTypes (Motor_Control_Command) + output = CANClassTypes_Motor_Control_Command; + if (((input >= 0) && (input <= 2)) || ((input >= 4) && (input <= 7))) { + // Set output value to input value if it is a member of CANClassTypes + output = static_cast(input); + } + + return output; +} + +// System initialize for referenced model: 'can_decoder' +void can_decoder_Init(void) +{ + int32_T ForEach_itr; + static const BUS_CAN_RX tmp = { false,// available + { { CANClassTypes_Motor_Control_Command,// CLS + 0U, // SRC + 0U // DST_TYP + }, // ID + + { 0U, // LEN + { false, // M + 0U // OPC + }, // CMD + + { 0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U }// ARG + } // PAYLOAD + } // packet + }; + + // SystemInitialize for Iterator SubSystem: '/Cycling Decoder' + for (ForEach_itr = 0; ForEach_itr < CAN_MAX_NUM_PACKETS; 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; + + // 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_B.CoreSubsys[ForEach_itr]. + sf_DecodingLogic, &can_decoder_DW.CoreSubsys[ForEach_itr].sf_DecodingLogic); + + // End of SystemInitialize for SubSystem: '/CAN_Decoder' + } + + // End of SystemInitialize for SubSystem: '/Cycling Decoder' +} + +// 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) +{ + int32_T ForEach_itr; + int32_T i; + uint16_T rtu_pck_rx_raw_0; + 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++) { + // 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 = + 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 = + can_decoder_convert_to_enum_CANClassTypes(static_cast( + static_cast(static_cast(rtu_pck_rx_raw_0 & 1792) >> 8))); + can_decoder_B.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 = + static_cast(rtu_pck_rx_raw_0 & 15); + x_idx_1 = rtu_pck_rx_raw->packets[ForEach_itr].length; + minval = 8U; + if (x_idx_1 < 8) { + minval = x_idx_1; + } + + can_decoder_B.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; + } + + 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 = + ((x_idx_1 & 128U) != 0U); + can_decoder_B.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] = + rtu_pck_rx_raw->packets[ForEach_itr].packet.PAYLOAD[i]; + } + + // End of MATLAB Function: '/RAW2STRUCT Decoding Logic' + // End of Outputs for SubSystem: '/CAN_RX_RAW2STRUCT' + + // Outputs for Atomic SubSystem: '/CAN_Decoder' + // Chart: '/Decoding Logic' incorporates: + // Constant: '/Constant' + // Constant: '/Constant1' + // Constant: '/Constant2' + + can_decoder_DecodingLogic(can_decoder_B.CoreSubsys[ForEach_itr]. + pck_rx_struct.available, &can_decoder_B.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); + + // RelationalOperator: '/FixPt Relational Operator' incorporates: + // UnitDelay: '/Delay Input1' + // + // Block description for '/Delay Input1': + // + // 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; + + // Update for UnitDelay: '/Delay Input1' + // + // Block description for '/Delay Input1': + // + // Store in Global RAM + + can_decoder_DW.CoreSubsys[ForEach_itr].DelayInput1_DSTATE_j = + can_decoder_B.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; + + // 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; + + // End of Outputs for SubSystem: '/CAN_Decoder' + } + + // End of Outputs for SubSystem: '/Cycling Decoder' +} + +// Model initialize function +void can_decoder_initialize(const char_T **rt_errorStatus) +{ + RT_MODEL_can_decoder_T *const can_decoder_M = &(can_decoder_MdlrefDW.rtm); + + // Registration code + + // initialize error status + rtmSetErrorStatusPointer(can_decoder_M, rt_errorStatus); +} + +// +// File trailer for generated code. +// +// [EOF] +// diff --git a/emBODY/eBcode/arch-arm/board/amc2c/application/src/model-based-design/can-decoder/can_decoder.h b/emBODY/eBcode/arch-arm/board/amc2c/application/src/model-based-design/can-decoder/can_decoder.h new file mode 100644 index 0000000000..a54dc490c2 --- /dev/null +++ b/emBODY/eBcode/arch-arm/board/amc2c/application/src/model-based-design/can-decoder/can_decoder.h @@ -0,0 +1,227 @@ +// +// Non-Degree Granting Education License -- for use at non-degree +// granting, nonprofit, education, and research organizations only. Not +// for commercial or industrial use. +// +// File: can_decoder.h +// +// Code generated for Simulink model 'can_decoder'. +// +// Model version : 5.0 +// Simulink Coder version : 9.9 (R2023a) 19-Nov-2022 +// C/C++ source code generated on : Mon Oct 16 10:07:44 2023 +// +// Target selection: ert.tlc +// Embedded hardware selection: ARM Compatible->ARM Cortex-M +// Code generation objectives: Unspecified +// Validation result: Not run +// +#ifndef RTW_HEADER_can_decoder_h_ +#define RTW_HEADER_can_decoder_h_ +#include "rtwtypes.h" +#include "can_decoder_types.h" +#include "rtw_defines.h" + +// Block signals for system '/Decoding Logic' +#ifndef can_decoder_MDLREF_HIDE_CHILD_ + +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' +}; + +#endif //can_decoder_MDLREF_HIDE_CHILD_ + +// Block states (default storage) for system '/Decoding Logic' +#ifndef can_decoder_MDLREF_HIDE_CHILD_ + +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' +}; + +#endif //can_decoder_MDLREF_HIDE_CHILD_ + +// Block signals for system '/Cycling Decoder' +#ifndef can_decoder_MDLREF_HIDE_CHILD_ + +struct B_CoreSubsys_can_decoder_T { + BUS_CAN_RX pck_rx_struct; // '/RAW2STRUCT Decoding Logic' + B_DecodingLogic_can_decoder_T sf_DecodingLogic;// '/Decoding Logic' +}; + +#endif //can_decoder_MDLREF_HIDE_CHILD_ + +// Block states (default storage) for system '/Cycling Decoder' +#ifndef can_decoder_MDLREF_HIDE_CHILD_ + +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' +}; + +#endif //can_decoder_MDLREF_HIDE_CHILD_ + +// Block signals for model 'can_decoder' +#ifndef can_decoder_MDLREF_HIDE_CHILD_ + +struct B_can_decoder_c_T { + B_CoreSubsys_can_decoder_T CoreSubsys[CAN_MAX_NUM_PACKETS];// '/Cycling Decoder' +}; + +#endif //can_decoder_MDLREF_HIDE_CHILD_ + +// Block states (default storage) for model 'can_decoder' +#ifndef can_decoder_MDLREF_HIDE_CHILD_ + +struct DW_can_decoder_f_T { + DW_CoreSubsys_can_decoder_T CoreSubsys[CAN_MAX_NUM_PACKETS];// '/Cycling Decoder' +}; + +#endif //can_decoder_MDLREF_HIDE_CHILD_ + +#ifndef can_decoder_MDLREF_HIDE_CHILD_ + +// Real-time Model Data Structure +struct tag_RTM_can_decoder_T { + const char_T **errorStatus; +}; + +#endif //can_decoder_MDLREF_HIDE_CHILD_ + +#ifndef can_decoder_MDLREF_HIDE_CHILD_ + +struct MdlrefDW_can_decoder_T { + RT_MODEL_can_decoder_T rtm; +}; + +#endif //can_decoder_MDLREF_HIDE_CHILD_ + +// +// 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 real32_T CAN_ANGLE_ICUB2DEG; // Variable: CAN_ANGLE_ICUB2DEG + // Referenced by: '/Decoding Logic' + // 360/2^16 + +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); + +#ifndef can_decoder_MDLREF_HIDE_CHILD_ + +extern void can_decoder_DecodingLogic_Init(B_DecodingLogic_can_decoder_T *localB, + 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); + +#endif //can_decoder_MDLREF_HIDE_CHILD_ + +#ifndef can_decoder_MDLREF_HIDE_CHILD_ + +extern MdlrefDW_can_decoder_T can_decoder_MdlrefDW; + +#endif //can_decoder_MDLREF_HIDE_CHILD_ + +#ifndef can_decoder_MDLREF_HIDE_CHILD_ + +// Block signals (default storage) +extern B_can_decoder_c_T can_decoder_B; + +#endif //can_decoder_MDLREF_HIDE_CHILD_ + +#ifndef can_decoder_MDLREF_HIDE_CHILD_ + +// Block states (default storage) +extern DW_can_decoder_f_T can_decoder_DW; + +#endif //can_decoder_MDLREF_HIDE_CHILD_ + +//- +// 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 +// +// '' : 'can_decoder' +// '' : 'can_decoder/Cycling Decoder' +// '' : 'can_decoder/Cycling Decoder/CAN_Decoder' +// '' : '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' + +#endif // RTW_HEADER_can_decoder_h_ + +// +// File trailer for generated code. +// +// [EOF] +// diff --git a/emBODY/eBcode/arch-arm/board/amc2c/application/src/model-based-design/can-decoder/can_decoder_private.h b/emBODY/eBcode/arch-arm/board/amc2c/application/src/model-based-design/can-decoder/can_decoder_private.h new file mode 100644 index 0000000000..4e5712a6b4 --- /dev/null +++ b/emBODY/eBcode/arch-arm/board/amc2c/application/src/model-based-design/can-decoder/can_decoder_private.h @@ -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: can_decoder_private.h +// +// Code generated for Simulink model 'can_decoder'. +// +// Model version : 5.0 +// Simulink Coder version : 9.9 (R2023a) 19-Nov-2022 +// C/C++ source code generated on : Mon Oct 16 10:07:44 2023 +// +// Target selection: ert.tlc +// Embedded hardware selection: ARM Compatible->ARM Cortex-M +// Code generation objectives: Unspecified +// Validation result: Not run +// +#ifndef RTW_HEADER_can_decoder_private_h_ +#define RTW_HEADER_can_decoder_private_h_ +#include "rtwtypes.h" +#include "can_decoder_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_can_decoder_private_h_ + +// +// File trailer for generated code. +// +// [EOF] +// diff --git a/emBODY/eBcode/arch-arm/board/amc2c/application/src/model-based-design/can-decoder/can_decoder_types.h b/emBODY/eBcode/arch-arm/board/amc2c/application/src/model-based-design/can-decoder/can_decoder_types.h new file mode 100644 index 0000000000..8b2ef44597 --- /dev/null +++ b/emBODY/eBcode/arch-arm/board/amc2c/application/src/model-based-design/can-decoder/can_decoder_types.h @@ -0,0 +1,556 @@ +// +// Non-Degree Granting Education License -- for use at non-degree +// granting, nonprofit, education, and research organizations only. Not +// for commercial or industrial use. +// +// File: can_decoder_types.h +// +// Code generated for Simulink model 'can_decoder'. +// +// Model version : 5.0 +// Simulink Coder version : 9.9 (R2023a) 19-Nov-2022 +// C/C++ source code generated on : Mon Oct 16 10:07:44 2023 +// +// Target selection: ert.tlc +// Embedded hardware selection: ARM Compatible->ARM Cortex-M +// Code generation objectives: Unspecified +// Validation result: Not run +// +#ifndef RTW_HEADER_can_decoder_types_h_ +#define RTW_HEADER_can_decoder_types_h_ +#include "rtwtypes.h" +#include "can_decoder_types.h" + +// Includes for objects with custom storage classes +#include "rtw_defines.h" + +// +// Constraints for division operations in dimension variants + +#if (1 == 0) || ((CAN_MAX_NUM_PACKETS % 1) != 0) +# error "The preprocessor definition '1' must not be equal to zero and the division of 'CAN_MAX_NUM_PACKETS' by '1' must not have a remainder." +#endif + +// +// Registered constraints for dimension variants + +#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'" +#endif + +#ifndef DEFINED_TYPEDEF_FOR_BUS_CAN_PACKET_ +#define DEFINED_TYPEDEF_FOR_BUS_CAN_PACKET_ + +// Fields of a transmitted CAN packet. +struct BUS_CAN_PACKET +{ + // ID of the CAN packet. + uint16_T ID; + + // PAYLOAD of the CAN packet. + uint8_T PAYLOAD[8]; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_BUS_CAN_ +#define DEFINED_TYPEDEF_FOR_BUS_CAN_ + +struct BUS_CAN +{ + // If true, the packet is available to be processed. + boolean_T available; + uint8_T length; + BUS_CAN_PACKET packet; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_BUS_CAN_MULTIPLE_ +#define DEFINED_TYPEDEF_FOR_BUS_CAN_MULTIPLE_ + +struct BUS_CAN_MULTIPLE +{ + BUS_CAN packets[CAN_MAX_NUM_PACKETS]; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_MotorConfig_ +#define DEFINED_TYPEDEF_FOR_MotorConfig_ + +struct MotorConfig +{ + 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 Vcc; + 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_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[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_CANClassTypes_ +#define DEFINED_TYPEDEF_FOR_CANClassTypes_ + +typedef enum { + CANClassTypes_Motor_Control_Command = 0,// Default value + CANClassTypes_Motor_Control_Streaming = 1, + CANClassTypes_Analog_Sensors_Command = 2, + CANClassTypes_Skin_Sensor_Streaming = 4, + CANClassTypes_Inertial_Sensor_Streaming = 5, + CANClassTypes_Future_Use = 6, + CANClassTypes_Management_Bootloader = 7 +} CANClassTypes; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_BUS_CAN_ID_RX_ +#define DEFINED_TYPEDEF_FOR_BUS_CAN_ID_RX_ + +struct BUS_CAN_ID_RX +{ + // 3 bits defining the message class type. + CANClassTypes CLS; + + // 4 bits defining the source ID. + uint8_T SRC; + + // 4 bits definint the destination ID or the message sub-type. + uint8_T DST_TYP; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_BUS_CAN_CMD_ +#define DEFINED_TYPEDEF_FOR_BUS_CAN_CMD_ + +struct BUS_CAN_CMD +{ + // 1 bits for motor selector. + boolean_T M; + + // 7 bits defining the operational code of the command. + uint8_T OPC; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_BUS_CAN_PAYLOAD_RX_ +#define DEFINED_TYPEDEF_FOR_BUS_CAN_PAYLOAD_RX_ + +struct BUS_CAN_PAYLOAD_RX +{ + // Actual length of the total PAYLOAD field. + uint8_T LEN; + BUS_CAN_CMD CMD; + + // 8 bytes for the command argument in order to account also message of type streaming. + uint8_T ARG[8]; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_BUS_CAN_PACKET_RX_ +#define DEFINED_TYPEDEF_FOR_BUS_CAN_PACKET_RX_ + +// Fields of a received CAN packet. +struct BUS_CAN_PACKET_RX +{ + // ID of the CAN packet. + BUS_CAN_ID_RX ID; + + // PAYLOAD of the CAN packet. + BUS_CAN_PAYLOAD_RX PAYLOAD; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_BUS_CAN_RX_ +#define DEFINED_TYPEDEF_FOR_BUS_CAN_RX_ + +// Specifies the CAN input. +struct BUS_CAN_RX +{ + // If true, the packet is available to be processed. + boolean_T available; + BUS_CAN_PACKET_RX packet; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_MCOPC_ +#define DEFINED_TYPEDEF_FOR_MCOPC_ + +typedef enum { + MCOPC_Set_Control_Mode = 9, // Default value + MCOPC_Set_Current_Limit = 72, + MCOPC_Set_Current_PID = 101, + MCOPC_Set_Velocity_PID = 105, + MCOPC_Set_Motor_Config = 119 +} MCOPC; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_MCStreaming_ +#define DEFINED_TYPEDEF_FOR_MCStreaming_ + +typedef enum { + MCStreaming_Desired_Targets = 15, // Default value + MCStreaming_FOC = 0 +} MCStreaming; + +#endif + +// Forward declaration for rtModel +typedef struct tag_RTM_can_decoder_T RT_MODEL_can_decoder_T; + +#endif // RTW_HEADER_can_decoder_types_h_ + +// +// File trailer for generated code. +// +// [EOF] +// diff --git a/emBODY/eBcode/arch-arm/board/amc2c/application/src/model-based-design/can-encoder/can_encoder.cpp b/emBODY/eBcode/arch-arm/board/amc2c/application/src/model-based-design/can-encoder/can_encoder.cpp new file mode 100644 index 0000000000..19ed43ac23 --- /dev/null +++ b/emBODY/eBcode/arch-arm/board/amc2c/application/src/model-based-design/can-encoder/can_encoder.cpp @@ -0,0 +1,229 @@ +// +// Non-Degree Granting Education License -- for use at non-degree +// granting, nonprofit, education, and research organizations only. Not +// for commercial or industrial use. +// +// File: can_encoder.cpp +// +// Code generated for Simulink model 'can_encoder'. +// +// Model version : 5.0 +// Simulink Coder version : 9.9 (R2023a) 19-Nov-2022 +// C/C++ source code generated on : Mon Oct 16 10:07:52 2023 +// +// Target selection: ert.tlc +// Embedded hardware selection: ARM Compatible->ARM Cortex-M +// Code generation objectives: Unspecified +// Validation result: Not run +// +#include "can_encoder.h" +#include "rtwtypes.h" +#include "can_encoder_types.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) +{ + *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); +} + +// 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) +{ + BUS_CAN rtb_BusCreator_n_0[4]; + BUS_CAN rtb_BusCreator_hs; + BUS_CAN rtb_BusCreator_m; + BUS_CAN rtb_BusCreator_n; + int32_T i; + real32_T tmp; + uint32_T qY; + int16_T rtb_DataTypeConversion1_0; + int16_T rtb_DataTypeConversion_k; + uint8_T b_tmp[4]; + uint8_T tmp2[2]; + + // Outputs for Atomic SubSystem: '/CAN_Encoder' + // MATLAB Function: '/format_status_pck' incorporates: + // BusCreator: '/Bus Creator' + + 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 = 0U; + } + + // DataTypeConversion: '/Data Type Conversion' incorporates: + // Constant: '/length1' + // Product: '/Product' + + tmp = rtu_messages_tx->status.pwm_fbk * 10.0F; + if (tmp < 32768.0F) { + if (tmp >= -32768.0F) { + rtb_DataTypeConversion_k = static_cast(tmp); + } else { + rtb_DataTypeConversion_k = MIN_int16_T; + } + } else { + rtb_DataTypeConversion_k = MAX_int16_T; + } + + // MATLAB Function: '/format_status_pck' incorporates: + // BusCreator: '/Bus Creator' + // DataTypeConversion: '/Data Type Conversion' + + rtb_DataTypeConversion_k = static_cast(rtb_DataTypeConversion_k << + static_cast(qY)); + std::memcpy((void *)&tmp2[0], (void *)&rtb_DataTypeConversion_k, (size_t)2 * + sizeof(uint8_T)); + rtb_BusCreator_m.packet.PAYLOAD[2] = tmp2[0]; + rtb_BusCreator_m.packet.PAYLOAD[3] = tmp2[1]; + rtb_BusCreator_m.packet.PAYLOAD[4] = + rtu_messages_tx->status.flags.ExternalFaultAsserted; + rtb_BusCreator_m.packet.PAYLOAD[5] = 0U; + rtb_BusCreator_m.packet.PAYLOAD[6] = 0U; + rtb_BusCreator_m.packet.PAYLOAD[7] = 0U; + + // DataTypeConversion: '/Data Type Conversion' incorporates: + // Gain: '/Gain2' + + tmp = 1000.0F * rtu_messages_tx->foc.current; + if (tmp < 32768.0F) { + if (tmp >= -32768.0F) { + rtb_DataTypeConversion_k = static_cast(tmp); + } else { + rtb_DataTypeConversion_k = MIN_int16_T; + } + } else { + rtb_DataTypeConversion_k = MAX_int16_T; + } + + // DataTypeConversion: '/Data Type Conversion1' incorporates: + // Gain: '/Gain1' + + tmp = CAN_ANGLE_DEG2ICUB / 1000.0F * rtu_messages_tx->foc.velocity; + if (tmp < 32768.0F) { + if (tmp >= -32768.0F) { + rtb_DataTypeConversion1_0 = static_cast(tmp); + } else { + rtb_DataTypeConversion1_0 = MIN_int16_T; + } + } else { + rtb_DataTypeConversion1_0 = MAX_int16_T; + } + + // DataTypeConversion: '/Data Type Conversion2' incorporates: + // Gain: '/Gain3' + + tmp = CAN_ANGLE_DEG2ICUB * rtu_messages_tx->foc.position; + if (tmp < 2.14748365E+9F) { + if (tmp >= -2.14748365E+9F) { + i = static_cast(tmp); + } else { + i = MIN_int32_T; + } + } else { + i = MAX_int32_T; + } + + // MATLAB Function: '/format_foc_pck' incorporates: + // BusCreator: '/Bus Creator' + // DataTypeConversion: '/Data Type Conversion' + // DataTypeConversion: '/Data Type Conversion1' + // DataTypeConversion: '/Data Type Conversion2' + + std::memcpy((void *)&tmp2[0], (void *)&rtb_DataTypeConversion_k, (size_t)2 * + sizeof(uint8_T)); + rtb_BusCreator_n.packet.PAYLOAD[0] = tmp2[0]; + rtb_BusCreator_n.packet.PAYLOAD[1] = tmp2[1]; + std::memcpy((void *)&tmp2[0], (void *)&rtb_DataTypeConversion1_0, (size_t)2 * + sizeof(uint8_T)); + rtb_BusCreator_n.packet.PAYLOAD[2] = tmp2[0]; + rtb_BusCreator_n.packet.PAYLOAD[3] = tmp2[1]; + std::memcpy((void *)&b_tmp[0], (void *)&i, (size_t)4 * sizeof(uint8_T)); + rtb_BusCreator_n.packet.PAYLOAD[4] = b_tmp[0]; + rtb_BusCreator_n.packet.PAYLOAD[5] = b_tmp[1]; + rtb_BusCreator_n.packet.PAYLOAD[6] = b_tmp[2]; + rtb_BusCreator_n.packet.PAYLOAD[7] = b_tmp[3]; + + // BusCreator: '/Bus Creator' incorporates: + // Constant: '/Constant' + // Constant: '/Motor Control Streaming' + // Constant: '/TYPESTATUS' + // 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; + + // BusCreator: '/Bus Creator' incorporates: + // Constant: '/Constant' + // Constant: '/Motor Control Streaming' + // Constant: '/TYPE2FOC' + // 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; + + // BusCreator: '/Bus Creator' incorporates: + // BusCreator: '/Bus Creator6' + // Constant: '/ID_raw' + // Constant: '/available' + // Constant: '/length' + + rtb_BusCreator_hs.available = false; + rtb_BusCreator_hs.length = 0U; + rtb_BusCreator_hs.packet.ID = 0U; + for (i = 0; i < 8; i++) { + rtb_BusCreator_hs.packet.PAYLOAD[i] = 0U; + } + + // End of BusCreator: '/Bus Creator' + + // Concatenate: '/Vector Concatenate' + rtb_BusCreator_n_0[0] = rtb_BusCreator_m; + rtb_BusCreator_n_0[1] = rtb_BusCreator_n; + rtb_BusCreator_n_0[2] = rtb_BusCreator_hs; + rtb_BusCreator_n_0[3] = rtb_BusCreator_hs; + for (i = 0; i < CAN_MAX_NUM_PACKETS; i++) { + rty_pck_tx->packets[i] = rtb_BusCreator_n_0[i]; + } + + // End of Concatenate: '/Vector Concatenate' + // End of Outputs for SubSystem: '/CAN_Encoder' +} + +// Model initialize function +void can_encoder_initialize(const char_T **rt_errorStatus) +{ + RT_MODEL_can_encoder_T *const can_encoder_M = &(can_encoder_MdlrefDW.rtm); + + // Registration code + + // initialize error status + rtmSetErrorStatusPointer(can_encoder_M, rt_errorStatus); +} + +// +// File trailer for generated code. +// +// [EOF] +// diff --git a/emBODY/eBcode/arch-arm/board/amc2c/application/src/model-based-design/can-encoder/can_encoder.h b/emBODY/eBcode/arch-arm/board/amc2c/application/src/model-based-design/can-encoder/can_encoder.h new file mode 100644 index 0000000000..0c0b813ef4 --- /dev/null +++ b/emBODY/eBcode/arch-arm/board/amc2c/application/src/model-based-design/can-encoder/can_encoder.h @@ -0,0 +1,112 @@ +// +// Non-Degree Granting Education License -- for use at non-degree +// granting, nonprofit, education, and research organizations only. Not +// for commercial or industrial use. +// +// File: can_encoder.h +// +// Code generated for Simulink model 'can_encoder'. +// +// Model version : 5.0 +// Simulink Coder version : 9.9 (R2023a) 19-Nov-2022 +// C/C++ source code generated on : Mon Oct 16 10:07:52 2023 +// +// Target selection: ert.tlc +// Embedded hardware selection: ARM Compatible->ARM Cortex-M +// Code generation objectives: Unspecified +// Validation result: Not run +// +#ifndef RTW_HEADER_can_encoder_h_ +#define RTW_HEADER_can_encoder_h_ +#include "rtwtypes.h" +#include "can_encoder_types.h" +#ifndef can_encoder_MDLREF_HIDE_CHILD_ + +// Real-time Model Data Structure +struct tag_RTM_can_encoder_T { + const char_T **errorStatus; +}; + +#endif //can_encoder_MDLREF_HIDE_CHILD_ + +#ifndef can_encoder_MDLREF_HIDE_CHILD_ + +struct MdlrefDW_can_encoder_T { + RT_MODEL_can_encoder_T rtm; +}; + +#endif //can_encoder_MDLREF_HIDE_CHILD_ + +// +// 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 real32_T CAN_ANGLE_DEG2ICUB; // Variable: CAN_ANGLE_DEG2ICUB + // Referenced by: + // '/Gain1' + // '/Gain3' + // 2^16/360 + +extern uint8_T CAN_ID_AMC; // Variable: CAN_ID_AMC + // Referenced by: + // '/Constant' + // '/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); + +#ifndef can_encoder_MDLREF_HIDE_CHILD_ + +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); + +#endif //can_encoder_MDLREF_HIDE_CHILD_ + +#ifndef can_encoder_MDLREF_HIDE_CHILD_ + +extern MdlrefDW_can_encoder_T can_encoder_MdlrefDW; + +#endif //can_encoder_MDLREF_HIDE_CHILD_ + +//- +// 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 +// +// '' : 'can_encoder' +// '' : 'can_encoder/CAN_Encoder' +// '' : 'can_encoder/CAN_Encoder/FOC' +// '' : 'can_encoder/CAN_Encoder/NotUsed' +// '' : 'can_encoder/CAN_Encoder/STATUS' +// '' : 'can_encoder/CAN_Encoder/FOC/format_can_id' +// '' : 'can_encoder/CAN_Encoder/FOC/format_foc_pck' +// '' : 'can_encoder/CAN_Encoder/FOC/format_can_id/format_can_id' +// '' : 'can_encoder/CAN_Encoder/STATUS/format_can_id' +// '' : 'can_encoder/CAN_Encoder/STATUS/format_status_pck' +// '' : 'can_encoder/CAN_Encoder/STATUS/format_can_id/format_can_id' + +#endif // RTW_HEADER_can_encoder_h_ + +// +// File trailer for generated code. +// +// [EOF] +// diff --git a/emBODY/eBcode/arch-arm/board/amc2c/application/src/model-based-design/can-encoder/can_encoder_private.h b/emBODY/eBcode/arch-arm/board/amc2c/application/src/model-based-design/can-encoder/can_encoder_private.h new file mode 100644 index 0000000000..3802e38a51 --- /dev/null +++ b/emBODY/eBcode/arch-arm/board/amc2c/application/src/model-based-design/can-encoder/can_encoder_private.h @@ -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: can_encoder_private.h +// +// Code generated for Simulink model 'can_encoder'. +// +// Model version : 5.0 +// Simulink Coder version : 9.9 (R2023a) 19-Nov-2022 +// C/C++ source code generated on : Mon Oct 16 10:07:52 2023 +// +// Target selection: ert.tlc +// Embedded hardware selection: ARM Compatible->ARM Cortex-M +// Code generation objectives: Unspecified +// Validation result: Not run +// +#ifndef RTW_HEADER_can_encoder_private_h_ +#define RTW_HEADER_can_encoder_private_h_ +#include "rtwtypes.h" +#include "can_encoder_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_can_encoder_private_h_ + +// +// File trailer for generated code. +// +// [EOF] +// diff --git a/emBODY/eBcode/arch-arm/board/amc2c/application/src/model-based-design/can-encoder/can_encoder_types.h b/emBODY/eBcode/arch-arm/board/amc2c/application/src/model-based-design/can-encoder/can_encoder_types.h new file mode 100644 index 0000000000..2490ad98e1 --- /dev/null +++ b/emBODY/eBcode/arch-arm/board/amc2c/application/src/model-based-design/can-encoder/can_encoder_types.h @@ -0,0 +1,344 @@ +// +// Non-Degree Granting Education License -- for use at non-degree +// granting, nonprofit, education, and research organizations only. Not +// for commercial or industrial use. +// +// File: can_encoder_types.h +// +// Code generated for Simulink model 'can_encoder'. +// +// Model version : 5.0 +// Simulink Coder version : 9.9 (R2023a) 19-Nov-2022 +// C/C++ source code generated on : Mon Oct 16 10:07:52 2023 +// +// Target selection: ert.tlc +// Embedded hardware selection: ARM Compatible->ARM Cortex-M +// Code generation objectives: Unspecified +// Validation result: Not run +// +#ifndef RTW_HEADER_can_encoder_types_h_ +#define RTW_HEADER_can_encoder_types_h_ +#include "rtwtypes.h" +#include "can_encoder_types.h" + +// Includes for objects with custom storage classes +#include "rtw_defines.h" + +// +// Registered constraints for dimension variants + +// Constraint 'CAN_MAX_NUM_PACKETS == 4' registered by: +// '/Vector Concatenate' + +#if CAN_MAX_NUM_PACKETS != 4 +# error "The preprocessor definition 'CAN_MAX_NUM_PACKETS' must be equal to '4'" +#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_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_FLAGS_TX_ +#define DEFINED_TYPEDEF_FOR_BUS_FLAGS_TX_ + +struct BUS_FLAGS_TX +{ + boolean_T dirty; + boolean_T stuck; + boolean_T index_broken; + boolean_T phase_broken; + real32_T not_calibrated; + boolean_T ExternalFaultAsserted; + boolean_T UnderVoltageFailure; + boolean_T OverVoltageFailure; + boolean_T OverCurrentFailure; + boolean_T DHESInvalidValue; + boolean_T AS5045CSumError; + boolean_T DHESInvalidSequence; + boolean_T CANInvalidProtocol; + boolean_T CAN_BufferOverRun; + boolean_T SetpointExpired; + boolean_T CAN_TXIsPasv; + boolean_T CAN_RXIsPasv; + boolean_T CAN_IsWarnTX; + boolean_T CAN_IsWarnRX; + boolean_T OverHeating; + boolean_T ADCCalFailure; + boolean_T I2TFailure; + boolean_T EMUROMFault; + boolean_T EMUROMCRCFault; + boolean_T EncoderFault; + boolean_T FirmwareSPITimingError; + boolean_T AS5045CalcError; + boolean_T FirmwarePWMFatalError; + boolean_T CAN_TXWasPasv; + boolean_T CAN_RXWasPasv; + boolean_T CAN_RTRFlagActive; + boolean_T CAN_WasWarn; + boolean_T CAN_DLCError; + boolean_T SiliconRevisionFault; + boolean_T PositionLimitUpper; + boolean_T PositionLimitLower; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_BUS_MSG_STATUS_ +#define DEFINED_TYPEDEF_FOR_BUS_MSG_STATUS_ + +struct BUS_MSG_STATUS +{ + MCControlModes control_mode; + + // control effort (quadrature) + real32_T pwm_fbk; + BUS_FLAGS_TX flags; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_BUS_MESSAGES_TX_ +#define DEFINED_TYPEDEF_FOR_BUS_MESSAGES_TX_ + +// Aggregate of all CAN transmitted messages. +struct BUS_MESSAGES_TX +{ + BUS_MSG_FOC foc; + BUS_MSG_STATUS status; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_BUS_STATUS_TX_ +#define DEFINED_TYPEDEF_FOR_BUS_STATUS_TX_ + +// Aggregate of all events specifying types of transmitted messages. +struct BUS_STATUS_TX +{ + boolean_T foc; + boolean_T status; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_MotorConfig_ +#define DEFINED_TYPEDEF_FOR_MotorConfig_ + +struct MotorConfig +{ + 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 Vcc; + 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_BUS_CAN_PACKET_ +#define DEFINED_TYPEDEF_FOR_BUS_CAN_PACKET_ + +// Fields of a transmitted CAN packet. +struct BUS_CAN_PACKET +{ + // ID of the CAN packet. + uint16_T ID; + + // PAYLOAD of the CAN packet. + uint8_T PAYLOAD[8]; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_BUS_CAN_ +#define DEFINED_TYPEDEF_FOR_BUS_CAN_ + +struct BUS_CAN +{ + // If true, the packet is available to be processed. + boolean_T available; + uint8_T length; + BUS_CAN_PACKET packet; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_BUS_CAN_MULTIPLE_ +#define DEFINED_TYPEDEF_FOR_BUS_CAN_MULTIPLE_ + +struct BUS_CAN_MULTIPLE +{ + BUS_CAN packets[CAN_MAX_NUM_PACKETS]; +}; + +#endif + +// Forward declaration for rtModel +typedef struct tag_RTM_can_encoder_T RT_MODEL_can_encoder_T; + +#endif // RTW_HEADER_can_encoder_types_h_ + +// +// File trailer for generated code. +// +// [EOF] +// diff --git a/emBODY/eBcode/arch-arm/board/amc2c/application/src/model-based-design/control-foc/FOCInnerLoop.cpp b/emBODY/eBcode/arch-arm/board/amc2c/application/src/model-based-design/control-foc/FOCInnerLoop.cpp new file mode 100644 index 0000000000..a85cd41c5b --- /dev/null +++ b/emBODY/eBcode/arch-arm/board/amc2c/application/src/model-based-design/control-foc/FOCInnerLoop.cpp @@ -0,0 +1,771 @@ +// +// Non-Degree Granting Education License -- for use at non-degree +// granting, nonprofit, education, and research organizations only. Not +// for commercial or industrial use. +// +// File: FOCInnerLoop.cpp +// +// Code generated for Simulink model 'control_foc'. +// +// Model version : 5.13 +// Simulink Coder version : 9.9 (R2023a) 19-Nov-2022 +// C/C++ source code generated on : Mon Oct 16 10:08:10 2023 +// +// Target selection: ert.tlc +// Embedded hardware selection: ARM Compatible->ARM Cortex-M +// Code generation objectives: Unspecified +// Validation result: Not run +// +#include "rtwtypes.h" +#include "FOCInnerLoop.h" +#include "control_foc_types.h" + +extern "C" +{ + +#include "rt_nonfinite.h" + +} + +#include +#include "mw_cmsis.h" +#include "arm_math.h" +#include "control_foc_private.h" +#include "zero_crossing_types.h" + +// Forward declaration for local functions +static void control_foc_SystemCore_setup(dsp_simulink_MovingRMS_control_foc_T + *obj); +static void control_foc_SystemCore_setup(dsp_simulink_MovingRMS_control_foc_T + *obj) +{ + real32_T val; + boolean_T flag; + obj->isSetupComplete = false; + obj->isInitialized = 1; + flag = (obj->isInitialized == 1); + if (flag) { + obj->TunablePropsChanged = true; + } + + obj->ForgettingFactor = 0.0F; + obj->TunablePropsChanged = false; + obj->NumChannels = 1; + obj->FrameLength = 1; + if (obj->ForgettingFactor != 0.0F) { + val = obj->ForgettingFactor; + } else { + val = 2.22044605E-16F; + } + + obj->_pobj0.isInitialized = 0; + obj->_pobj0.isInitialized = 0; + flag = (obj->_pobj0.isInitialized == 1); + if (flag) { + obj->_pobj0.TunablePropsChanged = true; + } + + obj->_pobj0.ForgettingFactor = val; + obj->pStatistic = &obj->_pobj0; + obj->isSetupComplete = true; + obj->TunablePropsChanged = false; +} + +// System initialize for atomic system: +void control_foc_MovingRMS_Init(DW_MovingRMS_control_foc_T *localDW) +{ + c_dsp_internal_ExponentialMovingAverage_control_foc_T *obj; + + // Start for MATLABSystem: '/Moving RMS' + localDW->obj.isInitialized = 0; + localDW->obj.NumChannels = -1; + localDW->obj.FrameLength = -1; + localDW->obj.matlabCodegenIsDeleted = false; + localDW->objisempty = true; + control_foc_SystemCore_setup(&localDW->obj); + + // InitializeConditions for MATLABSystem: '/Moving RMS' + obj = localDW->obj.pStatistic; + if (obj->isInitialized == 1) { + obj->pwN = 1.0F; + obj->pmN = 0.0F; + } + + // End of InitializeConditions for MATLABSystem: '/Moving RMS' +} + +// Output and update for atomic system: +void control_foc_MovingRMS(real32_T rtu_0, real32_T rtu_1, + B_MovingRMS_control_foc_T *localB, DW_MovingRMS_control_foc_T *localDW) +{ + c_dsp_internal_ExponentialMovingAverage_control_foc_T *obj; + real32_T a; + real32_T lambda; + real32_T pmLocal; + real32_T varargin_1; + boolean_T p; + + // MATLABSystem: '/Moving RMS' + varargin_1 = localDW->obj.ForgettingFactor; + if ((varargin_1 == rtu_1) || (rtIsNaNF(varargin_1) && rtIsNaNF(rtu_1))) { + } else { + p = (localDW->obj.isInitialized == 1); + if (p) { + localDW->obj.TunablePropsChanged = true; + } + + localDW->obj.ForgettingFactor = rtu_1; + } + + if (localDW->obj.TunablePropsChanged) { + localDW->obj.TunablePropsChanged = false; + obj = localDW->obj.pStatistic; + p = (obj->isInitialized == 1); + if (p) { + obj->TunablePropsChanged = true; + } + + localDW->obj.pStatistic->ForgettingFactor = localDW->obj.ForgettingFactor; + } + + a = std::abs(rtu_0); + obj = localDW->obj.pStatistic; + if (obj->isInitialized != 1) { + obj->isSetupComplete = false; + obj->isInitialized = 1; + obj->pwN = 1.0F; + obj->pmN = 0.0F; + obj->plambda = obj->ForgettingFactor; + obj->isSetupComplete = true; + obj->TunablePropsChanged = false; + obj->pwN = 1.0F; + obj->pmN = 0.0F; + } + + if (obj->TunablePropsChanged) { + obj->TunablePropsChanged = false; + obj->plambda = obj->ForgettingFactor; + } + + varargin_1 = obj->pwN; + pmLocal = obj->pmN; + lambda = obj->plambda; + a = (1.0F - 1.0F / varargin_1) * pmLocal + 1.0F / varargin_1 * (a * a); + obj->pwN = lambda * varargin_1 + 1.0F; + obj->pmN = a; + + // MATLABSystem: '/Moving RMS' + mw_arm_sqrt_f32(a, &localB->MovingRMS); +} + +// Termination for atomic system: +void control_foc_MovingRMS_Term(DW_MovingRMS_control_foc_T *localDW) +{ + c_dsp_internal_ExponentialMovingAverage_control_foc_T *obj; + + // Terminate for MATLABSystem: '/Moving RMS' + if (!localDW->obj.matlabCodegenIsDeleted) { + localDW->obj.matlabCodegenIsDeleted = true; + if ((localDW->obj.isInitialized == 1) && localDW->obj.isSetupComplete) { + obj = localDW->obj.pStatistic; + if (obj->isInitialized == 1) { + obj->isInitialized = 2; + } + + localDW->obj.NumChannels = -1; + localDW->obj.FrameLength = -1; + } + } + + // End of Terminate for MATLABSystem: '/Moving RMS' +} + +// 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; + control_foc_MovingRMS_Init(&localDW->MovingRMS); + control_foc_MovingRMS_Init(&localDW->MovingRMS1); +} + +// 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, + ZCE_FOCInnerLoop_T *localZCE) +{ + // local block i/o variables + real32_T rtb_Saturation2; + real32_T rtb_algDD_o1; + real32_T rtb_algDD_o2; + int32_T i; + real32_T rtb_IaIbIc0[2]; + real32_T DProdOut; + real32_T rtb_Add; + real32_T rtb_Product; + real32_T rtb_SinCos_o1; + real32_T rtb_SinCos_o2; + real32_T rtb_Unary_Minus; + real32_T rtb_algDD_o1_p; + real32_T rtb_algDD_o2_n; + real32_T rtb_sum_alpha; + int8_T tmp; + int8_T tmp_0; + + // Sum: '/Add' incorporates: + // Product: '/Product1' + // Product: '/Product2' + + rtb_Add = rtu_OuterOutputs->motorcurrent.current * + rtu_ConfigurationParameters->motorconfig.Rphase + + rtu_Estimates->jointvelocities.velocity * + rtu_ConfigurationParameters->motorconfig.Kbemf; + + // MinMax: '/Min' + if ((rtu_ConfigurationParameters->motorconfig.Vcc <= + rtu_ConfigurationParameters->motorconfig.Vmax) || rtIsNaNF + (rtu_ConfigurationParameters->motorconfig.Vmax)) { + rtb_sum_alpha = rtu_ConfigurationParameters->motorconfig.Vcc; + } else { + rtb_sum_alpha = rtu_ConfigurationParameters->motorconfig.Vmax; + } + + // Product: '/Product' incorporates: + // Gain: '/Gain4' + // MinMax: '/Min' + + rtb_Product = 0.5F * rtb_sum_alpha * localC->Sum5; + + // Gain: '/Ia+Ib+Ic=0' + rtb_algDD_o1_p = rtu_Sensors->motorsensors.Iabc[1]; + rtb_algDD_o2_n = rtu_Sensors->motorsensors.Iabc[0]; + rtb_Unary_Minus = rtu_Sensors->motorsensors.Iabc[2]; + for (i = 0; i < 2; i++) { + rtb_IaIbIc0[i] = (rtCP_IaIbIc0_Gain[i + 2] * rtb_algDD_o1_p + + rtCP_IaIbIc0_Gain[i] * rtb_algDD_o2_n) + + rtCP_IaIbIc0_Gain[i + 4] * rtb_Unary_Minus; + } + + // End of Gain: '/Ia+Ib+Ic=0' + + // Outputs for Atomic SubSystem: '/Clarke Transform' + // AlgorithmDescriptorDelegate generated from: '/a16' + arm_clarke_f32(rtb_IaIbIc0[0], rtb_IaIbIc0[1], &rtb_algDD_o1_p, + &rtb_algDD_o2_n); + + // End of Outputs for SubSystem: '/Clarke Transform' + + // Gain: '/deg2rad' + rtb_Unary_Minus = 0.0174532924F * rtu_Sensors->motorsensors.angle; + + // Trigonometry: '/SinCos' + rtb_SinCos_o1 = std::sin(rtb_Unary_Minus); + rtb_SinCos_o2 = std::cos(rtb_Unary_Minus); + + // Outputs for Atomic SubSystem: '/Park Transform' + // AlgorithmDescriptorDelegate generated from: '/a16' incorporates: + // Product: '/asin' + // Product: '/bcos' + // Sum: '/sum_Qs' + // UnaryMinus: '/Unary_Minus' + + rtb_algDD_o1 = -(rtb_algDD_o2_n * rtb_SinCos_o2 - rtb_algDD_o1_p * + rtb_SinCos_o1); + + // AlgorithmDescriptorDelegate generated from: '/a16' incorporates: + // Product: '/acos' + // Product: '/bsin' + // Sum: '/sum_Ds' + + rtb_algDD_o2 = rtb_algDD_o1_p * rtb_SinCos_o2 + rtb_algDD_o2_n * rtb_SinCos_o1; + + // End of Outputs for SubSystem: '/Park Transform' + + // Sum: '/Sum' + rtb_Unary_Minus = rtu_OuterOutputs->motorcurrent.current - rtb_algDD_o2; + + // Product: '/PProd Out' + rtb_algDD_o1_p = rtb_Unary_Minus * rtu_ConfigurationParameters->CurLoopPID.P; + + // Product: '/IProd Out' + rtb_algDD_o2_n = rtb_Unary_Minus * rtu_ConfigurationParameters->CurLoopPID.I; + + // SampleTimeMath: '/Tsamp' incorporates: + // SampleTimeMath: '/Tsamp' + // + // About '/Tsamp': + // y = u * K where K = ( w * Ts ) + // + // About '/Tsamp': + // y = u * K where K = ( w * Ts ) + + DProdOut = rtu_ConfigurationParameters->CurLoopPID.N * 3.0E-5F; + + // 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_sum_alpha = 1.0F / (DProdOut + 1.0F); + + // DiscreteTransferFcn: '/Filter Differentiator TF' + if (rtu_OuterOutputs->pid_reset && (localZCE->FilterDifferentiatorTF_Reset_ZCE + != POS_ZCSIG)) { + localDW->FilterDifferentiatorTF_states = 0.0F; + } + + localZCE->FilterDifferentiatorTF_Reset_ZCE = rtu_OuterOutputs->pid_reset; + + // 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 ) + + DProdOut = (DProdOut - 1.0F) * rtb_sum_alpha; + + // DiscreteTransferFcn: '/Filter Differentiator TF' incorporates: + // Product: '/DProd Out' + // Product: '/Divide' + + localDW->FilterDifferentiatorTF_tmp = rtb_Unary_Minus * + rtu_ConfigurationParameters->CurLoopPID.D - DProdOut * + localDW->FilterDifferentiatorTF_states; + + // Product: '/NProd Out' incorporates: + // DiscreteTransferFcn: '/Filter Differentiator TF' + // Math: '/Reciprocal' + // Product: '/DenCoefOut' + // + // About '/Reciprocal': + // Operator: reciprocal + + rtb_Unary_Minus = (localDW->FilterDifferentiatorTF_tmp - + localDW->FilterDifferentiatorTF_states) * rtb_sum_alpha * + rtu_ConfigurationParameters->CurLoopPID.N; + + // Sum: '/SumI1' incorporates: + // Sum: '/Sum Fdbk' + // Sum: '/SumI3' + // UnitDelay: '/Unit Delay' + + localB->SumI1 = (localDW->UnitDelay_DSTATE - ((rtb_algDD_o1_p + + localDW->Integrator_DSTATE) + rtb_Unary_Minus)) + rtb_algDD_o2_n; + + // DiscreteIntegrator: '/Integrator' + if (rtu_OuterOutputs->pid_reset && (localDW->Integrator_PrevResetState <= 0)) + { + localDW->Integrator_DSTATE = 0.0F; + } + + // DiscreteIntegrator: '/Integrator' + localB->Integrator = 3.0E-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_p = ((rtb_algDD_o1_p + localB->Integrator) + rtb_Unary_Minus) + + rtb_Add; + } else { + rtb_algDD_o1_p = rtu_Targets->motorvoltage.voltage * rtb_Product * 0.01F + + rtu_OuterOutputs->current_limiter; + } + + // End of Switch: '/Switch1' + + // Switch: '/Switch2' incorporates: + // Gain: '/Gain2' + // RelationalOperator: '/LowerRelop1' + // RelationalOperator: '/UpperRelop' + // Switch: '/Switch' + + if (rtb_algDD_o1_p > rtb_Product) { + rtb_algDD_o1_p = rtb_Product; + } else if (rtb_algDD_o1_p < -rtb_Product) { + // Switch: '/Switch' incorporates: + // Gain: '/Gain2' + + rtb_algDD_o1_p = -rtb_Product; + } + + // End of Switch: '/Switch2' + + // Product: '/PProd Out' incorporates: + // Gain: '/Gain' + + rtb_Unary_Minus = -rtb_algDD_o1 * rtu_ConfigurationParameters->CurLoopPID.P; + + // DiscreteTransferFcn: '/Filter Differentiator TF' incorporates: + // DiscreteTransferFcn: '/Filter Differentiator TF' + // Gain: '/Gain' + // Product: '/DProd Out' + + if (rtu_OuterOutputs->pid_reset && + (localZCE->FilterDifferentiatorTF_Reset_ZCE_o != POS_ZCSIG)) { + 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 - DProdOut * + localDW->FilterDifferentiatorTF_states_k; + + // Product: '/NProd Out' incorporates: + // DiscreteTransferFcn: '/Filter Differentiator TF' + // Product: '/DenCoefOut' + + DProdOut = (localDW->FilterDifferentiatorTF_tmp_c - + localDW->FilterDifferentiatorTF_states_k) * rtb_sum_alpha * + rtu_ConfigurationParameters->CurLoopPID.N; + + // Sum: '/Sum Fdbk' + rtb_algDD_o2_n = (rtb_Unary_Minus + localDW->Integrator_DSTATE_o) + DProdOut; + + // Switch: '/Switch' incorporates: + // Gain: '/Gain2' + // RelationalOperator: '/u_GTE_up' + // RelationalOperator: '/u_GT_lo' + // Switch: '/Switch1' + + if (rtb_algDD_o2_n >= rtb_Product) { + rtb_sum_alpha = rtb_Product; + } else if (rtb_algDD_o2_n > -rtb_Product) { + // Switch: '/Switch1' + rtb_sum_alpha = rtb_algDD_o2_n; + } else { + rtb_sum_alpha = -rtb_Product; + } + + // Sum: '/Diff' incorporates: + // Switch: '/Switch' + + rtb_algDD_o2_n -= rtb_sum_alpha; + + // Product: '/IProd Out' incorporates: + // Gain: '/Gain' + + rtb_sum_alpha = -rtb_algDD_o1 * rtu_ConfigurationParameters->CurLoopPID.I; + + // Switch: '/Switch1' incorporates: + // Constant: '/Clamping_zero' + // Constant: '/Constant' + // Constant: '/Constant2' + // RelationalOperator: '/fix for DT propagation issue' + + if (rtb_algDD_o2_n > 0.0F) { + tmp = 1; + } else { + tmp = -1; + } + + // Switch: '/Switch2' incorporates: + // Constant: '/Clamping_zero' + // Constant: '/Constant3' + // Constant: '/Constant4' + // RelationalOperator: '/fix for DT propagation issue1' + + if (rtb_sum_alpha > 0.0F) { + tmp_0 = 1; + } else { + tmp_0 = -1; + } + + // Switch: '/Switch' incorporates: + // Constant: '/Clamping_zero' + // Logic: '/AND3' + // RelationalOperator: '/Equal1' + // RelationalOperator: '/Relational Operator' + // Switch: '/Switch1' + // Switch: '/Switch2' + + if ((rtb_algDD_o2_n != 0.0F) && (tmp == tmp_0)) { + // Switch: '/Switch' incorporates: + // Constant: '/Constant1' + + localB->Switch = 0.0F; + } else { + // Switch: '/Switch' + localB->Switch = rtb_sum_alpha; + } + + // End of Switch: '/Switch' + + // DiscreteIntegrator: '/Integrator' + if (rtu_OuterOutputs->pid_reset && (localDW->Integrator_PrevResetState_k <= 0)) + { + localDW->Integrator_DSTATE_o = 0.0F; + } + + // DiscreteIntegrator: '/Integrator' + localB->Integrator_j = 3.0E-5F * localB->Switch + localDW->Integrator_DSTATE_o; + + // Sum: '/Sum' + rtb_algDD_o2_n = (rtb_Unary_Minus + localB->Integrator_j) + DProdOut; + + // Switch: '/Switch2' incorporates: + // RelationalOperator: '/LowerRelop1' + + if (!(rtb_algDD_o2_n > rtb_Product)) { + // Switch: '/Switch' incorporates: + // Gain: '/Gain2' + // RelationalOperator: '/UpperRelop' + + if (rtb_algDD_o2_n < -rtb_Product) { + rtb_Product = -rtb_Product; + } else { + rtb_Product = rtb_algDD_o2_n; + } + + // End of Switch: '/Switch' + } + + // End of Switch: '/Switch2' + + // Outputs for Atomic SubSystem: '/Inverse Park Transform' + // Switch: '/Switch' incorporates: + // Product: '/dsin' + // Product: '/qcos' + // Sum: '/sum_beta' + + rtb_IaIbIc0[0] = rtb_algDD_o1_p * rtb_SinCos_o2 + rtb_Product * rtb_SinCos_o1; + + // End of Outputs for SubSystem: '/Inverse Park Transform' + + // Switch: '/Switch2' incorporates: + // Constant: '/Constant1' + + if (rtu_OuterOutputs->out_en) { + // Gain: '/Gain3' incorporates: + // Product: '/Divide1' + + rtb_Unary_Minus = rtb_algDD_o1_p / + rtu_ConfigurationParameters->motorconfig.Vcc * 100.0F; + + // Outputs for Atomic SubSystem: '/Inverse Park Transform' + // Gain: '/sqrt3_by_two' incorporates: + // Product: '/dcos' + // Product: '/qsin' + // Sum: '/sum_alpha' + // UnaryMinus: '/Unary_Minus' + + rtb_algDD_o2_n = -(rtb_Product * rtb_SinCos_o2 - rtb_algDD_o1_p * + rtb_SinCos_o1) * 0.866025388F; + + // Gain: '/one_by_two' incorporates: + // AlgorithmDescriptorDelegate generated from: '/a16' + + rtb_SinCos_o1 = 0.5F * rtb_IaIbIc0[0]; + + // End of Outputs for SubSystem: '/Inverse Park Transform' + + // Sum: '/add_c' + rtb_SinCos_o2 = (0.0F - rtb_SinCos_o1) - rtb_algDD_o2_n; + + // Sum: '/add_b' + rtb_SinCos_o1 = rtb_algDD_o2_n - rtb_SinCos_o1; + + // Outputs for Atomic SubSystem: '/Inverse Park Transform' + // MinMax: '/Min1' incorporates: + // AlgorithmDescriptorDelegate generated from: '/a16' + + if ((rtb_IaIbIc0[0] <= rtb_SinCos_o1) || rtIsNaNF(rtb_SinCos_o1)) { + rtb_algDD_o2_n = rtb_IaIbIc0[0]; + } else { + rtb_algDD_o2_n = rtb_SinCos_o1; + } + + // End of Outputs for SubSystem: '/Inverse Park Transform' + if ((!(rtb_algDD_o2_n <= rtb_SinCos_o2)) && (!rtIsNaNF(rtb_SinCos_o2))) { + rtb_algDD_o2_n = rtb_SinCos_o2; + } + + // Saturate: '/Saturation1' + if (rtb_Unary_Minus > 100.0F) { + rtb_Unary_Minus = 100.0F; + } else if (rtb_Unary_Minus < -100.0F) { + rtb_Unary_Minus = -100.0F; + } + + // End of Saturate: '/Saturation1' + + // Outputs for Atomic SubSystem: '/Inverse Park Transform' + // Sum: '/Sum1' incorporates: + // AlgorithmDescriptorDelegate generated from: '/a16' + // Constant: '/Constant2' + // Gain: '/Gain1' + // MinMax: '/Min1' + // Product: '/Divide' + // Sum: '/Sum4' + + rtb_Product = (rtb_IaIbIc0[0] - rtb_algDD_o2_n) / + rtu_ConfigurationParameters->motorconfig.Vcc * 100.0F + 5.0F; + + // End of Outputs for SubSystem: '/Inverse Park Transform' + + // Saturate: '/Saturation' + if (rtb_Product > 100.0F) { + rtb_Product = 100.0F; + } else if (rtb_Product < 0.0F) { + rtb_Product = 0.0F; + } + + // Sum: '/Sum1' incorporates: + // Constant: '/Constant2' + // Gain: '/Gain1' + // MinMax: '/Min1' + // Product: '/Divide' + // Sum: '/Sum4' + + rtb_SinCos_o1 = (rtb_SinCos_o1 - rtb_algDD_o2_n) / + rtu_ConfigurationParameters->motorconfig.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; + } + + // Sum: '/Sum1' incorporates: + // Constant: '/Constant2' + // Gain: '/Gain1' + // MinMax: '/Min1' + // Product: '/Divide' + // Sum: '/Sum4' + + rtb_SinCos_o2 = (rtb_SinCos_o2 - rtb_algDD_o2_n) / + rtu_ConfigurationParameters->motorconfig.Vcc * 100.0F + 5.0F; + + // Saturate: '/Saturation' + if (rtb_SinCos_o2 > 100.0F) { + rtb_SinCos_o2 = 100.0F; + } else if (rtb_SinCos_o2 < 0.0F) { + rtb_SinCos_o2 = 0.0F; + } + } else { + rtb_Unary_Minus = 0.0F; + rtb_Product = 0.0F; + rtb_SinCos_o1 = 0.0F; + rtb_SinCos_o2 = 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; + + // Saturate: '/Saturation2' + if (rtu_ConfigurationParameters->estimationconfig.current_rms_lambda > 1.0F) { + // Saturate: '/Saturation2' + rtb_Saturation2 = 1.0F; + } else if (rtu_ConfigurationParameters->estimationconfig.current_rms_lambda < + 0.0F) { + // Saturate: '/Saturation2' + rtb_Saturation2 = 0.0F; + } else { + // Saturate: '/Saturation2' + rtb_Saturation2 = + rtu_ConfigurationParameters->estimationconfig.current_rms_lambda; + } + + // End of Saturate: '/Saturation2' + control_foc_MovingRMS(rtb_algDD_o2, rtb_Saturation2, &localB->MovingRMS, + &localDW->MovingRMS); + + // BusCreator: '/Bus Creator3' + rty_FOCOutputs->Iq_rms.current = localB->MovingRMS.MovingRMS; + control_foc_MovingRMS(rtb_algDD_o1, rtb_Saturation2, &localB->MovingRMS1, + &localDW->MovingRMS1); + + // BusCreator: '/Bus Creator4' + rty_FOCOutputs->Id_rms.current = localB->MovingRMS1.MovingRMS; + + // BusCreator: '/Bus Creator' + rty_FOCOutputs->Vq = rtb_Unary_Minus; + rty_FOCOutputs->Vabc[0] = rtb_Product; + rty_FOCOutputs->Vabc[1] = rtb_SinCos_o1; + rty_FOCOutputs->Vabc[2] = rtb_SinCos_o2; + + // Sum: '/Sum3' + localB->Sum3 = rtb_algDD_o1_p - rtb_Add; +} + +// Update for atomic system: '/FOC inner loop' +void FOCInnerLoop_Update(const ControlOuterOutputs *rtu_OuterOutputs, + 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 = 3.0E-5F * localB->SumI1 + localB->Integrator; + localDW->Integrator_PrevResetState = static_cast + (rtu_OuterOutputs->pid_reset); + + // 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 = 3.0E-5F * localB->Switch + localB->Integrator_j; + localDW->Integrator_PrevResetState_k = static_cast + (rtu_OuterOutputs->pid_reset); +} + +// Termination for atomic system: '/FOC inner loop' +void FOCInnerLoop_Term(DW_FOCInnerLoop_T *localDW) +{ + control_foc_MovingRMS_Term(&localDW->MovingRMS); + control_foc_MovingRMS_Term(&localDW->MovingRMS1); +} + +// +// File trailer for generated code. +// +// [EOF] +// diff --git a/emBODY/eBcode/arch-arm/board/amc2c/application/src/model-based-design/control-foc/FOCInnerLoop.h b/emBODY/eBcode/arch-arm/board/amc2c/application/src/model-based-design/control-foc/FOCInnerLoop.h new file mode 100644 index 0000000000..748c4b4f48 --- /dev/null +++ b/emBODY/eBcode/arch-arm/board/amc2c/application/src/model-based-design/control-foc/FOCInnerLoop.h @@ -0,0 +1,101 @@ +// +// Non-Degree Granting Education License -- for use at non-degree +// granting, nonprofit, education, and research organizations only. Not +// for commercial or industrial use. +// +// File: FOCInnerLoop.h +// +// Code generated for Simulink model 'control_foc'. +// +// Model version : 5.13 +// Simulink Coder version : 9.9 (R2023a) 19-Nov-2022 +// C/C++ source code generated on : Mon Oct 16 10:08:10 2023 +// +// Target selection: ert.tlc +// Embedded hardware selection: ARM Compatible->ARM Cortex-M +// Code generation objectives: Unspecified +// Validation result: Not run +// +#ifndef RTW_HEADER_FOCInnerLoop_h_ +#define RTW_HEADER_FOCInnerLoop_h_ +#include "rtwtypes.h" +#include "control_foc_types.h" +#include "zero_crossing_types.h" + +// Block signals for system '/Moving RMS' +struct B_MovingRMS_control_foc_T { + real32_T MovingRMS; // '/Moving RMS' +}; + +// Block states (default storage) for system '/Moving RMS' +struct DW_MovingRMS_control_foc_T { + dsp_simulink_MovingRMS_control_foc_T obj;// '/Moving RMS' + boolean_T objisempty; // '/Moving RMS' +}; + +// Block signals for system '/FOC inner loop' +struct B_FOCInnerLoop_T { + real32_T SumI1; // '/SumI1' + real32_T Integrator; // '/Integrator' + real32_T Switch; // '/Switch' + real32_T Integrator_j; // '/Integrator' + real32_T Sum3; // '/Sum3' + B_MovingRMS_control_foc_T MovingRMS1;// '/Moving RMS' + B_MovingRMS_control_foc_T MovingRMS; // '/Moving RMS' +}; + +// Block states (default storage) for system '/FOC inner loop' +struct DW_FOCInnerLoop_T { + real32_T FilterDifferentiatorTF_states;// '/Filter Differentiator TF' + real32_T UnitDelay_DSTATE; // '/Unit Delay' + real32_T Integrator_DSTATE; // '/Integrator' + real32_T FilterDifferentiatorTF_states_k;// '/Filter Differentiator TF' + real32_T Integrator_DSTATE_o; // '/Integrator' + real32_T FilterDifferentiatorTF_tmp; // '/Filter Differentiator TF' + real32_T FilterDifferentiatorTF_tmp_c;// '/Filter Differentiator TF' + int8_T Integrator_PrevResetState; // '/Integrator' + int8_T Integrator_PrevResetState_k; // '/Integrator' + DW_MovingRMS_control_foc_T MovingRMS1;// '/Moving RMS' + DW_MovingRMS_control_foc_T MovingRMS;// '/Moving RMS' +}; + +// Zero-crossing (trigger) state for system '/FOC inner loop' +struct ZCV_FOCInnerLoop_T { + real_T FilterDifferentiatorTF_Reset_ZC;// '/Filter Differentiator TF' + real_T FilterDifferentiatorTF_Reset_ZC_c;// '/Filter Differentiator TF' +}; + +// Zero-crossing (trigger) state for system '/FOC inner loop' +struct ZCE_FOCInnerLoop_T { + ZCSigState FilterDifferentiatorTF_Reset_ZCE;// '/Filter Differentiator TF' + 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 control_foc_MovingRMS_Init(DW_MovingRMS_control_foc_T *localDW); +extern void control_foc_MovingRMS(real32_T rtu_0, real32_T rtu_1, + B_MovingRMS_control_foc_T *localB, DW_MovingRMS_control_foc_T *localDW); +extern void FOCInnerLoop_Init(DW_FOCInnerLoop_T *localDW); +extern void FOCInnerLoop_Update(const ControlOuterOutputs *rtu_OuterOutputs, + 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 control_foc_MovingRMS_Term(DW_MovingRMS_control_foc_T *localDW); +extern void FOCInnerLoop_Term(DW_FOCInnerLoop_T *localDW); + +#endif // RTW_HEADER_FOCInnerLoop_h_ + +// +// File trailer for generated code. +// +// [EOF] +// diff --git a/emBODY/eBcode/arch-arm/board/amc2c/application/src/model-based-design/control-foc/control_foc.cpp b/emBODY/eBcode/arch-arm/board/amc2c/application/src/model-based-design/control-foc/control_foc.cpp new file mode 100644 index 0000000000..b266ac80c4 --- /dev/null +++ b/emBODY/eBcode/arch-arm/board/amc2c/application/src/model-based-design/control-foc/control_foc.cpp @@ -0,0 +1,100 @@ +// +// 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.cpp +// +// Code generated for Simulink model 'control_foc'. +// +// Model version : 5.13 +// Simulink Coder version : 9.9 (R2023a) 19-Nov-2022 +// C/C++ source code generated on : Mon Oct 16 10:08:10 2023 +// +// Target selection: ert.tlc +// Embedded hardware selection: ARM Compatible->ARM Cortex-M +// Code generation objectives: Unspecified +// Validation result: Not run +// +#include "control_foc.h" +#include "control_foc_types.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) +{ + // SystemInitialize for Atomic SubSystem: '/FOC inner loop' + FOCInnerLoop_Init(&localDW->FOCinnerloop); + + // End of SystemInitialize for SubSystem: '/FOC inner loop' +} + +// 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) +{ + // 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); + + // End of Outputs for SubSystem: '/FOC inner loop' + + // Update for Atomic SubSystem: '/FOC inner loop' + FOCInnerLoop_Update(&rtu_FOCSlowInputs->controlouteroutputs, + &localB->FOCinnerloop, &localDW->FOCinnerloop); + + // End of Update for SubSystem: '/FOC inner loop' +} + +// Termination for referenced model: 'control_foc' +void control_foc_Term(DW_control_foc_f_T *localDW) +{ + // Terminate for Atomic SubSystem: '/FOC inner loop' + FOCInnerLoop_Term(&localDW->FOCinnerloop); + + // End of Terminate 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) +{ + // 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; +} + +// +// File trailer for generated code. +// +// [EOF] +// diff --git a/emBODY/eBcode/arch-arm/board/amc2c/application/src/model-based-design/control-foc/control_foc.h b/emBODY/eBcode/arch-arm/board/amc2c/application/src/model-based-design/control-foc/control_foc.h new file mode 100644 index 0000000000..e717ead8a1 --- /dev/null +++ b/emBODY/eBcode/arch-arm/board/amc2c/application/src/model-based-design/control-foc/control_foc.h @@ -0,0 +1,239 @@ +// +// 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.h +// +// Code generated for Simulink model 'control_foc'. +// +// Model version : 5.13 +// Simulink Coder version : 9.9 (R2023a) 19-Nov-2022 +// C/C++ source code generated on : Mon Oct 16 10:08:10 2023 +// +// Target selection: ert.tlc +// Embedded hardware selection: ARM Compatible->ARM Cortex-M +// Code generation objectives: Unspecified +// Validation result: Not run +// +#ifndef RTW_HEADER_control_foc_h_ +#define RTW_HEADER_control_foc_h_ +#include "rtwtypes.h" +#include "control_foc_types.h" +#include "FOCInnerLoop.h" +#include + +extern "C" +{ + +#include "rt_nonfinite.h" + +} + +#include "zero_crossing_types.h" + +// Block signals for model 'control_foc' +struct B_control_foc_c_T { + B_FOCInnerLoop_T FOCinnerloop; // '/FOC inner loop' +}; + +// Block states (default storage) for model 'control_foc' +struct DW_control_foc_f_T { + DW_FOCInnerLoop_T FOCinnerloop; // '/FOC inner loop' +}; + +// Zero-crossing (trigger) state for model 'control_foc' +struct ZCV_control_foc_g_T { + ZCV_FOCInnerLoop_T FOCinnerloop; // '/FOC inner loop' +}; + +// Zero-crossing (trigger) state for model 'control_foc' +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_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_Term(DW_control_foc_f_T *localDW); + +//- +// These blocks were eliminated from the model due to optimizations: +// +// Block '/Data Type Duplicate' : Unused code path elimination +// Block '/Direct Lookup Table (n-D)1' : Unused code path elimination +// Block '/Data Type Duplicate' : Unused code path elimination +// Block '/Data Type Propagation' : Unused code path elimination +// Block '/Data Type Duplicate' : Unused code path elimination +// Block '/Data Type Duplicate' : Unused code path elimination +// Block '/Data Type Duplicate1' : Unused code path elimination +// Block '/Data Type Duplicate' : Unused code path elimination +// Block '/Data Type Duplicate1' : Unused code path elimination +// Block '/Data Type Duplicate' : Unused code path elimination +// Block '/Data Type Propagation' : Unused code path elimination +// Block '/Passthrough for tuning' : Eliminate redundant data type conversion +// Block '/Passthrough for tuning' : Eliminate redundant data type conversion +// Block '/Kt' : Eliminated nontunable gain of 1 +// Block '/Offset' : Unused code path elimination +// Block '/Offset' : Unused code path elimination + + +//- +// 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 +// +// '' : 'control_foc' +// '' : 'control_foc/FOC inner loop' +// '' : 'control_foc/FOC inner loop/Clarke Transform' +// '' : 'control_foc/FOC inner loop/Id PID control' +// '' : 'control_foc/FOC inner loop/Inverse Clarke Transform' +// '' : 'control_foc/FOC inner loop/Inverse Park Transform' +// '' : 'control_foc/FOC inner loop/Iq PID control' +// '' : 'control_foc/FOC inner loop/Park Transform' +// '' : 'control_foc/FOC inner loop/Saturation Dynamic' +// '' : 'control_foc/FOC inner loop/Id PID control/Anti-windup' +// '' : 'control_foc/FOC inner loop/Id PID control/D Gain' +// '' : 'control_foc/FOC inner loop/Id PID control/Filter' +// '' : 'control_foc/FOC inner loop/Id PID control/Filter ICs' +// '' : 'control_foc/FOC inner loop/Id PID control/I Gain' +// '' : 'control_foc/FOC inner loop/Id PID control/Ideal P Gain' +// '' : 'control_foc/FOC inner loop/Id PID control/Ideal P Gain Fdbk' +// '' : 'control_foc/FOC inner loop/Id PID control/Integrator' +// '' : 'control_foc/FOC inner loop/Id PID control/Integrator ICs' +// '' : 'control_foc/FOC inner loop/Id PID control/N Copy' +// '' : 'control_foc/FOC inner loop/Id PID control/N Gain' +// '' : 'control_foc/FOC inner loop/Id PID control/P Copy' +// '' : 'control_foc/FOC inner loop/Id PID control/Parallel P Gain' +// '' : 'control_foc/FOC inner loop/Id PID control/Reset Signal' +// '' : 'control_foc/FOC inner loop/Id PID control/Saturation' +// '' : 'control_foc/FOC inner loop/Id PID control/Saturation Fdbk' +// '' : 'control_foc/FOC inner loop/Id PID control/Sum' +// '' : 'control_foc/FOC inner loop/Id PID control/Sum Fdbk' +// '' : 'control_foc/FOC inner loop/Id PID control/Tracking Mode' +// '' : 'control_foc/FOC inner loop/Id PID control/Tracking Mode Sum' +// '' : 'control_foc/FOC inner loop/Id PID control/Tsamp - Integral' +// '' : 'control_foc/FOC inner loop/Id PID control/Tsamp - Ngain' +// '' : 'control_foc/FOC inner loop/Id PID control/postSat Signal' +// '' : 'control_foc/FOC inner loop/Id PID control/preSat Signal' +// '' : 'control_foc/FOC inner loop/Id PID control/Anti-windup/Disc. Clamping Parallel' +// '' : 'control_foc/FOC inner loop/Id PID control/Anti-windup/Disc. Clamping Parallel/Dead Zone' +// '' : 'control_foc/FOC inner loop/Id PID control/Anti-windup/Disc. Clamping Parallel/Dead Zone/External' +// '' : 'control_foc/FOC inner loop/Id PID control/Anti-windup/Disc. Clamping Parallel/Dead Zone/External/Dead Zone Dynamic' +// '' : 'control_foc/FOC inner loop/Id PID control/D Gain/External Parameters' +// '' : 'control_foc/FOC inner loop/Id PID control/Filter/Disc. Trapezoidal Filter' +// '' : 'control_foc/FOC inner loop/Id PID control/Filter/Disc. Trapezoidal Filter/Tsamp' +// '' : 'control_foc/FOC inner loop/Id PID control/Filter/Disc. Trapezoidal Filter/Tsamp/Internal Ts' +// '' : 'control_foc/FOC inner loop/Id PID control/Filter ICs/Internal IC - Filter' +// '' : 'control_foc/FOC inner loop/Id PID control/I Gain/External Parameters' +// '' : 'control_foc/FOC inner loop/Id PID control/Ideal P Gain/Passthrough' +// '' : 'control_foc/FOC inner loop/Id PID control/Ideal P Gain Fdbk/Passthrough' +// '' : 'control_foc/FOC inner loop/Id PID control/Integrator/Discrete' +// '' : 'control_foc/FOC inner loop/Id PID control/Integrator ICs/Internal IC' +// '' : 'control_foc/FOC inner loop/Id PID control/N Copy/External Parameters' +// '' : 'control_foc/FOC inner loop/Id PID control/N Gain/External Parameters' +// '' : 'control_foc/FOC inner loop/Id PID control/P Copy/Disabled' +// '' : 'control_foc/FOC inner loop/Id PID control/Parallel P Gain/External Parameters' +// '' : 'control_foc/FOC inner loop/Id PID control/Reset Signal/External Reset' +// '' : 'control_foc/FOC inner loop/Id PID control/Saturation/External' +// '' : 'control_foc/FOC inner loop/Id PID control/Saturation/External/Saturation Dynamic' +// '' : 'control_foc/FOC inner loop/Id PID control/Saturation Fdbk/Passthrough' +// '' : 'control_foc/FOC inner loop/Id PID control/Sum/Sum_PID' +// '' : 'control_foc/FOC inner loop/Id PID control/Sum Fdbk/Enabled' +// '' : 'control_foc/FOC inner loop/Id PID control/Tracking Mode/Disabled' +// '' : 'control_foc/FOC inner loop/Id PID control/Tracking Mode Sum/Passthrough' +// '' : 'control_foc/FOC inner loop/Id PID control/Tsamp - Integral/TsSignalSpecification' +// '' : 'control_foc/FOC inner loop/Id PID control/Tsamp - Ngain/Passthrough' +// '' : 'control_foc/FOC inner loop/Id PID control/postSat Signal/Feedback_Path' +// '' : 'control_foc/FOC inner loop/Id PID control/preSat Signal/Feedback_Path' +// '' : 'control_foc/FOC inner loop/Inverse Park Transform/Switch_Axis' +// '' : 'control_foc/FOC inner loop/Iq PID control/Anti-windup' +// '' : 'control_foc/FOC inner loop/Iq PID control/D Gain' +// '' : 'control_foc/FOC inner loop/Iq PID control/Filter' +// '' : 'control_foc/FOC inner loop/Iq PID control/Filter ICs' +// '' : 'control_foc/FOC inner loop/Iq PID control/I Gain' +// '' : 'control_foc/FOC inner loop/Iq PID control/Ideal P Gain' +// '' : 'control_foc/FOC inner loop/Iq PID control/Ideal P Gain Fdbk' +// '' : 'control_foc/FOC inner loop/Iq PID control/Integrator' +// '' : 'control_foc/FOC inner loop/Iq PID control/Integrator ICs' +// '' : 'control_foc/FOC inner loop/Iq PID control/N Copy' +// '' : 'control_foc/FOC inner loop/Iq PID control/N Gain' +// '' : 'control_foc/FOC inner loop/Iq PID control/P Copy' +// '' : 'control_foc/FOC inner loop/Iq PID control/Parallel P Gain' +// '' : 'control_foc/FOC inner loop/Iq PID control/Reset Signal' +// '' : 'control_foc/FOC inner loop/Iq PID control/Saturation' +// '' : 'control_foc/FOC inner loop/Iq PID control/Saturation Fdbk' +// '' : 'control_foc/FOC inner loop/Iq PID control/Sum' +// '' : 'control_foc/FOC inner loop/Iq PID control/Sum Fdbk' +// '' : 'control_foc/FOC inner loop/Iq PID control/Tracking Mode' +// '' : 'control_foc/FOC inner loop/Iq PID control/Tracking Mode Sum' +// '' : 'control_foc/FOC inner loop/Iq PID control/Tsamp - Integral' +// '' : 'control_foc/FOC inner loop/Iq PID control/Tsamp - Ngain' +// '' : 'control_foc/FOC inner loop/Iq PID control/postSat Signal' +// '' : 'control_foc/FOC inner loop/Iq PID control/preSat Signal' +// '' : 'control_foc/FOC inner loop/Iq PID control/Anti-windup/Passthrough' +// '' : 'control_foc/FOC inner loop/Iq PID control/D Gain/External Parameters' +// '' : 'control_foc/FOC inner loop/Iq PID control/Filter/Disc. Trapezoidal Filter' +// '' : 'control_foc/FOC inner loop/Iq PID control/Filter/Disc. Trapezoidal Filter/Tsamp' +// '' : 'control_foc/FOC inner loop/Iq PID control/Filter/Disc. Trapezoidal Filter/Tsamp/Internal Ts' +// '' : 'control_foc/FOC inner loop/Iq PID control/Filter ICs/Internal IC - Filter' +// '' : 'control_foc/FOC inner loop/Iq PID control/I Gain/External Parameters' +// '' : 'control_foc/FOC inner loop/Iq PID control/Ideal P Gain/Passthrough' +// '' : 'control_foc/FOC inner loop/Iq PID control/Ideal P Gain Fdbk/Passthrough' +// '' : 'control_foc/FOC inner loop/Iq PID control/Integrator/Discrete' +// '' : 'control_foc/FOC inner loop/Iq PID control/Integrator ICs/Internal IC' +// '' : 'control_foc/FOC inner loop/Iq PID control/N Copy/External Parameters' +// '' : 'control_foc/FOC inner loop/Iq PID control/N Gain/External Parameters' +// '' : 'control_foc/FOC inner loop/Iq PID control/P Copy/Disabled' +// '' : 'control_foc/FOC inner loop/Iq PID control/Parallel P Gain/External Parameters' +// '' : 'control_foc/FOC inner loop/Iq PID control/Reset Signal/External Reset' +// '' : 'control_foc/FOC inner loop/Iq PID control/Saturation/Passthrough' +// '' : 'control_foc/FOC inner loop/Iq PID control/Saturation Fdbk/Passthrough' +// '' : 'control_foc/FOC inner loop/Iq PID control/Sum/Sum_PID' +// '' : 'control_foc/FOC inner loop/Iq PID control/Sum Fdbk/Enabled' +// '' : 'control_foc/FOC inner loop/Iq PID control/Tracking Mode/Enabled' +// '' : 'control_foc/FOC inner loop/Iq PID control/Tracking Mode Sum/Tracking Mode' +// '' : 'control_foc/FOC inner loop/Iq PID control/Tsamp - Integral/TsSignalSpecification' +// '' : 'control_foc/FOC inner loop/Iq PID control/Tsamp - Ngain/Passthrough' +// '' : 'control_foc/FOC inner loop/Iq PID control/postSat Signal/Feedback_Path' +// '' : 'control_foc/FOC inner loop/Iq PID control/preSat Signal/Feedback_Path' +// '' : 'control_foc/FOC inner loop/Park Transform/Switch_Axis' + +#endif // RTW_HEADER_control_foc_h_ + +// +// File trailer for generated code. +// +// [EOF] +// diff --git a/emBODY/eBcode/arch-arm/board/amc2c/application/src/model-based-design/control-foc/control_foc_data.cpp b/emBODY/eBcode/arch-arm/board/amc2c/application/src/model-based-design/control-foc/control_foc_data.cpp new file mode 100644 index 0000000000..840a36d1c4 --- /dev/null +++ b/emBODY/eBcode/arch-arm/board/amc2c/application/src/model-based-design/control-foc/control_foc_data.cpp @@ -0,0 +1,37 @@ +// +// 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 : 5.13 +// Simulink Coder version : 9.9 (R2023a) 19-Nov-2022 +// C/C++ source code generated on : Mon Oct 16 10:08:10 2023 +// +// 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/board/amc2c/application/src/model-based-design/control-foc/control_foc_private.h b/emBODY/eBcode/arch-arm/board/amc2c/application/src/model-based-design/control-foc/control_foc_private.h new file mode 100644 index 0000000000..ce7f44196b --- /dev/null +++ b/emBODY/eBcode/arch-arm/board/amc2c/application/src/model-based-design/control-foc/control_foc_private.h @@ -0,0 +1,58 @@ +// +// 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_private.h +// +// Code generated for Simulink model 'control_foc'. +// +// Model version : 5.13 +// Simulink Coder version : 9.9 (R2023a) 19-Nov-2022 +// C/C++ source code generated on : Mon Oct 16 10:08:10 2023 +// +// Target selection: ert.tlc +// Embedded hardware selection: ARM Compatible->ARM Cortex-M +// Code generation objectives: Unspecified +// Validation result: Not run +// +#ifndef RTW_HEADER_control_foc_private_h_ +#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_ + +// +// File trailer for generated code. +// +// [EOF] +// diff --git a/emBODY/eBcode/arch-arm/board/amc2c/application/src/model-based-design/control-foc/control_foc_types.h b/emBODY/eBcode/arch-arm/board/amc2c/application/src/model-based-design/control-foc/control_foc_types.h new file mode 100644 index 0000000000..45c69cb302 --- /dev/null +++ b/emBODY/eBcode/arch-arm/board/amc2c/application/src/model-based-design/control-foc/control_foc_types.h @@ -0,0 +1,420 @@ +// +// 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_types.h +// +// Code generated for Simulink model 'control_foc'. +// +// Model version : 5.13 +// Simulink Coder version : 9.9 (R2023a) 19-Nov-2022 +// C/C++ source code generated on : Mon Oct 16 10:08:10 2023 +// +// Target selection: ert.tlc +// Embedded hardware selection: ARM Compatible->ARM Cortex-M +// Code generation objectives: Unspecified +// Validation result: Not run +// +#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_ + +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_SensorsData_ +#define DEFINED_TYPEDEF_FOR_SensorsData_ + +struct SensorsData +{ + // position encoders + JointPositions jointpositions; + MotorSensors motorsensors; +}; + +#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 +{ + 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 Vcc; + 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_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 +{ + // 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 struct_c_dsp_internal_ExponentialMovingAverage_control_foc_T +#define struct_c_dsp_internal_ExponentialMovingAverage_control_foc_T + +struct c_dsp_internal_ExponentialMovingAverage_control_foc_T +{ + int32_T isInitialized; + boolean_T isSetupComplete; + boolean_T TunablePropsChanged; + real32_T ForgettingFactor; + real32_T pwN; + real32_T pmN; + real32_T plambda; +}; + +#endif // struct_c_dsp_internal_ExponentialMovingAverage_control_foc_T + +#ifndef struct_cell_wrap_control_foc_T +#define struct_cell_wrap_control_foc_T + +struct cell_wrap_control_foc_T +{ + uint32_T f1[8]; +}; + +#endif // struct_cell_wrap_control_foc_T + +#ifndef struct_dsp_simulink_MovingRMS_control_foc_T +#define struct_dsp_simulink_MovingRMS_control_foc_T + +struct dsp_simulink_MovingRMS_control_foc_T +{ + boolean_T matlabCodegenIsDeleted; + int32_T isInitialized; + boolean_T isSetupComplete; + boolean_T TunablePropsChanged; + cell_wrap_control_foc_T inputVarSize[2]; + real32_T ForgettingFactor; + c_dsp_internal_ExponentialMovingAverage_control_foc_T *pStatistic; + int32_T NumChannels; + int32_T FrameLength; + c_dsp_internal_ExponentialMovingAverage_control_foc_T _pobj0; +}; + +#endif // struct_dsp_simulink_MovingRMS_control_foc_T + +// Forward declaration for rtModel +typedef struct tag_RTM_control_foc_T RT_MODEL_control_foc_T; + +#endif // RTW_HEADER_control_foc_types_h_ + +// +// File trailer for generated code. +// +// [EOF] +// diff --git a/emBODY/eBcode/arch-arm/board/amc2c/application/src/model-based-design/control-outer/control_outer.cpp b/emBODY/eBcode/arch-arm/board/amc2c/application/src/model-based-design/control-outer/control_outer.cpp new file mode 100644 index 0000000000..888b03172d --- /dev/null +++ b/emBODY/eBcode/arch-arm/board/amc2c/application/src/model-based-design/control-outer/control_outer.cpp @@ -0,0 +1,582 @@ +// +// 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_outer.cpp +// +// Code generated for Simulink model 'control_outer'. +// +// Model version : 5.5 +// Simulink Coder version : 9.9 (R2023a) 19-Nov-2022 +// C/C++ source code generated on : Mon Oct 16 10:08:24 2023 +// +// Target selection: ert.tlc +// Embedded hardware selection: ARM Compatible->ARM Cortex-M +// Code generation objectives: Unspecified +// Validation result: Not run +// +#include "control_outer.h" +#include "control_outer_types.h" +#include +#include "rtwtypes.h" +#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) +{ + // InitializeConditions for UnitDelay: '/Delay Input1' + // + // Block description for '/Delay Input1': + // + // Store in Global RAM + + 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' +void control_outer_Enable(DW_control_outer_f_T *localDW) +{ + // Enable for DiscreteIntegrator: '/Discrete-Time Integrator' + localDW->DiscreteTimeIntegrator_SYSTEM_ENABLE = 1U; +} + +// Disable for referenced model: 'control_outer' +void control_outer_Disable(B_control_outer_c_T *localB, DW_control_outer_f_T + *localDW) +{ + // Disable for DiscreteIntegrator: '/Discrete-Time Integrator' + localDW->DiscreteTimeIntegrator_DSTATE = localB->DiscreteTimeIntegrator; +} + +// 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) +{ + int32_T rowIdx; + int32_T tmp; + real32_T rtb_Abs; + real32_T rtb_DProdOut; + real32_T rtb_DProdOut_f; + 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_Switch2; + real32_T rtb_Switch2_f; + boolean_T rtb_Compare; + boolean_T rtb_Compare_m; + boolean_T rtb_FixPtRelationalOperator; + + // Abs: '/Abs' + rtb_Abs = std::abs(rtu_ConfigurationParameters->thresholds.jntVelMax); + + // RelationalOperator: '/Compare' incorporates: + // Constant: '/Constant' + + rtb_Compare_m = (rtu_Flags->control_mode != ControlModes_Current); + + // RelationalOperator: '/Compare' incorporates: + // Constant: '/Constant' + + rty_OuterOutputs->cur_en = (rtu_Flags->control_mode != ControlModes_Voltage); + + // Logic: '/NOR' incorporates: + // Constant: '/Constant' + // Constant: '/Constant' + // Constant: '/Constant' + // RelationalOperator: '/Compare' + // RelationalOperator: '/Compare' + // RelationalOperator: '/Compare' + + rty_OuterOutputs->out_en = ((rtu_Flags->control_mode != + ControlModes_NotConfigured) && (rtu_Flags->control_mode != ControlModes_Idle) + && (rtu_Flags->control_mode != ControlModes_HwFaultCM)); + + // RelationalOperator: '/FixPt Relational Operator' incorporates: + // UnitDelay: '/Delay Input1' + // + // Block description for '/Delay Input1': + // + // Store in Global RAM + + rtb_FixPtRelationalOperator = (rtu_Flags->control_mode != + localDW->DelayInput1_DSTATE); + + // Switch: '/Switch2' + if (rtu_Flags->enable_thermal_protection) { + rtb_Switch2 = rtu_ConfigurationParameters->thresholds.motorNominalCurrents; + } else { + rtb_Switch2 = rtu_ConfigurationParameters->thresholds.motorPeakCurrents; + } + + // End of Switch: '/Switch2' + + // Sum: '/Sum3' + rtb_DenCoefOut = rtu_Targets->jointpositions.position - + rtu_Sensors->jointpositions.position; + + // Product: '/PProd Out' + rtb_PProdOut = rtb_DenCoefOut * rtu_ConfigurationParameters->PosLoopPID.P; + + // SampleTimeMath: '/Tsamp' + // + // About '/Tsamp': + // y = u * K where K = ( w * Ts ) + + rtb_Product = rtu_ConfigurationParameters->PosLoopPID.N * 0.0005F; + + // Math: '/Reciprocal' incorporates: + // Constant: '/Filter Den Constant' + // Sum: '/SumDen' + // + // About '/Reciprocal': + // Operator: reciprocal + + 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' + + 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: '/NProd Out' incorporates: + // DiscreteTransferFcn: '/Filter Differentiator TF' + // Product: '/DenCoefOut' + + rtb_Switch2_f = (localDW->FilterDifferentiatorTF_tmp - + localDW->FilterDifferentiatorTF_states) * rtb_Switch2_f * + rtu_ConfigurationParameters->PosLoopPID.N; + + // Sum: '/SumI1' incorporates: + // Product: '/IProd 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; + + // DiscreteIntegrator: '/Integrator' + if (rtb_FixPtRelationalOperator && (localDW->Integrator_PrevResetState <= 0)) + { + localDW->Integrator_DSTATE = 0.0F; + } + + // DiscreteIntegrator: '/Integrator' + rtb_FilterDifferentiatorTF = 0.0005F * rtb_DProdOut + + 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)) { + localDW->FilterDifferentiatorTF_states_i = 0.0F; + } + + 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; + + // 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; + + // 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; + + // DiscreteIntegrator: '/Integrator' + if (rtb_FixPtRelationalOperator && (localDW->Integrator_PrevResetState_n <= 0)) + { + localDW->Integrator_DSTATE_i = 0.0F; + } + + // DiscreteIntegrator: '/Integrator' + rtb_DProdOut_f = 0.0005F * rtb_FilterDifferentiatorTF_n + + localDW->Integrator_DSTATE_i; + + // Switch: '/Switch3' incorporates: + // Constant: '/Constant1' + // Constant: '/Constant' + // Constant: '/Constant' + // Logic: '/OR' + // RelationalOperator: '/Compare' + // RelationalOperator: '/Compare' + + if (rtb_Compare || (rtu_Flags->control_mode == ControlModes_PositionDirect) || + (rtu_Flags->control_mode == ControlModes_Velocity)) { + // Switch: '/Switch5' incorporates: + // Sum: '/Sum' + // Sum: '/Sum' + + if (rtb_Compare) { + rtb_DenCoefOut = (rtb_PProdOut + rtb_FilterDifferentiatorTF) + + rtb_Switch2_f; + } else { + rtb_DenCoefOut += rtb_PProdOut_o + rtb_DProdOut_f; + } + + // End of Switch: '/Switch5' + } else { + rtb_DenCoefOut = 0.0F; + } + + // Sum: '/Sum2' incorporates: + // Switch: '/Switch3' + + rtb_Switch2_f = rtb_DenCoefOut + rtu_Targets->jointvelocities.velocity; + + // Switch: '/Switch2' incorporates: + // Gain: '/Gain' + // RelationalOperator: '/LowerRelop1' + // RelationalOperator: '/UpperRelop' + // Switch: '/Switch' + + if (rtb_Switch2_f > rtb_Abs) { + rtb_Switch2_f = rtb_Abs; + } else if (rtb_Switch2_f < -rtb_Abs) { + // Switch: '/Switch' incorporates: + // Gain: '/Gain' + + rtb_Switch2_f = -rtb_Abs; + } + + // End of Switch: '/Switch2' + + // Sum: '/Sum1' + rtb_Product = rtb_Switch2_f - rtu_Estimates->jointvelocities.velocity; + + // Product: '/PProd Out' + rtb_Abs = rtb_Product * rtu_ConfigurationParameters->VelLoopPID.P; + + // Product: '/IProd Out' + rtb_Gain1_h = rtb_Product * rtu_ConfigurationParameters->VelLoopPID.I; + + // Product: '/DProd Out' + rtb_PProdOut = rtb_Product * rtu_ConfigurationParameters->VelLoopPID.D; + + // SampleTimeMath: '/Tsamp' + // + // About '/Tsamp': + // y = u * K where K = ( w * Ts ) + + rtb_Product = rtu_ConfigurationParameters->VelLoopPID.N * 0.0005F; + + // Math: '/Reciprocal' incorporates: + // Constant: '/Filter Den Constant' + // Sum: '/SumDen' + // + // About '/Reciprocal': + // Operator: reciprocal + + rtb_DenCoefOut = 1.0F / (rtb_Product + 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_e != POS_ZCSIG)) { + localDW->FilterDifferentiatorTF_states_c = 0.0F; + } + + localZCE->FilterDifferentiatorTF_Reset_ZCE_e = rtb_FixPtRelationalOperator; + localDW->FilterDifferentiatorTF_tmp_p = rtb_PProdOut - (rtb_Product - 1.0F) * + rtb_DenCoefOut * localDW->FilterDifferentiatorTF_states_c; + + // Product: '/NProd Out' incorporates: + // DiscreteTransferFcn: '/Filter Differentiator TF' + // Product: '/DenCoefOut' + + rtb_DenCoefOut = (localDW->FilterDifferentiatorTF_tmp_p - + localDW->FilterDifferentiatorTF_states_c) * rtb_DenCoefOut * + rtu_ConfigurationParameters->VelLoopPID.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; + + // DiscreteIntegrator: '/Integrator' + if (rtb_FixPtRelationalOperator && (localDW->Integrator_PrevResetState_c <= 0)) + { + localDW->Integrator_DSTATE_b = 0.0F; + } + + // DiscreteIntegrator: '/Integrator' + rtb_Product = 0.0005F * rtb_PProdOut + localDW->Integrator_DSTATE_b; + + // Switch: '/Switch1' incorporates: + // Sum: '/Sum' + + if (rtb_Compare_m) { + rtb_Abs = (rtb_Abs + rtb_Product) + rtb_DenCoefOut; + } else { + rtb_Abs = rtu_Targets->motorcurrent.current; + } + + // End of Switch: '/Switch1' + + // Switch: '/Switch2' incorporates: + // 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' + } + + // End of Switch: '/Switch2' + + // BusCreator: '/Bus Creator2' + rty_OuterOutputs->motorcurrent.current = rtb_Switch2; + + // Sum: '/Sum' incorporates: + // Abs: '/Abs1' + + rtb_Abs = rtu_ConfigurationParameters->thresholds.motorPeakCurrents - std::abs + (rtu_Estimates->Iq_filtered.current); + + // CombinatorialLogic: '/Logic' incorporates: + // Constant: '/Constant' + // Gain: '/Gain1' + // Logic: '/Logical Operator' + // Memory: '/Memory' + // RelationalOperator: '/Relational Operator' + // RelationalOperator: '/Compare' + + rowIdx = static_cast(((((rtb_Abs > 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]; + + // 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)) + { + // DiscreteIntegrator: '/Discrete-Time Integrator' + localB->DiscreteTimeIntegrator = 0.0F; + } else { + // DiscreteIntegrator: '/Discrete-Time Integrator' + localB->DiscreteTimeIntegrator = 0.0005F * rtb_Abs + + localDW->DiscreteTimeIntegrator_DSTATE; + } + + // End of DiscreteIntegrator: '/Discrete-Time Integrator' + + // BusCreator: '/Bus Creator1' + rty_OuterOutputs->vel_en = rtb_Compare_m; + rty_OuterOutputs->pid_reset = rtb_FixPtRelationalOperator; + + // Switch: '/Switch1' incorporates: + // Constant: '/Constant1' + // Constant: '/Constant2' + // Constant: '/Constant' + // RelationalOperator: '/Compare' + + if (rtu_Estimates->Iq_filtered.current < 0.0F) { + tmp = -1; + } else { + tmp = 1; + } + + // BusCreator: '/Bus Creator1' incorporates: + // Product: '/Product' + // Switch: '/Switch1' + + rty_OuterOutputs->current_limiter = static_cast(tmp) * + localB->DiscreteTimeIntegrator * rtu_ConfigurationParameters->CurLoopPID.I; + + // Update for UnitDelay: '/Delay Input1' + // + // Block description for '/Delay Input1': + // + // Store in Global RAM + + localDW->DelayInput1_DSTATE = rtu_Flags->control_mode; + + // 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_PrevResetState = static_cast + (rtb_FixPtRelationalOperator); + + // Update for DiscreteTransferFcn: '/Filter Differentiator TF' + localDW->FilterDifferentiatorTF_states_i = + localDW->FilterDifferentiatorTF_tmp_m; + + // Update for DiscreteIntegrator: '/Integrator' incorporates: + // DiscreteIntegrator: '/Integrator' + + localDW->Integrator_DSTATE_i = 0.0005F * rtb_FilterDifferentiatorTF_n + + rtb_DProdOut_f; + localDW->Integrator_PrevResetState_n = static_cast + (rtb_FixPtRelationalOperator); + + // Update for DiscreteTransferFcn: '/Filter Differentiator TF' + 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_PrevResetState_c = static_cast + (rtb_FixPtRelationalOperator); + + // Update for Memory: '/Memory' incorporates: + // CombinatorialLogic: '/Logic' + + localDW->Memory_PreviousInput = rtCP_Logic_table[static_cast(rowIdx)]; + + // Update for DiscreteIntegrator: '/Discrete-Time Integrator' + localDW->DiscreteTimeIntegrator_SYSTEM_ENABLE = 0U; + localDW->DiscreteTimeIntegrator_DSTATE = 0.0005F * rtb_Abs + + localB->DiscreteTimeIntegrator; + localDW->DiscreteTimeIntegrator_PrevResetState = static_cast + (rtb_Compare); +} + +// 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) +{ + // 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; +} + +// +// File trailer for generated code. +// +// [EOF] +// diff --git a/emBODY/eBcode/arch-arm/board/amc2c/application/src/model-based-design/control-outer/control_outer.h b/emBODY/eBcode/arch-arm/board/amc2c/application/src/model-based-design/control-outer/control_outer.h new file mode 100644 index 0000000000..67801b06b5 --- /dev/null +++ b/emBODY/eBcode/arch-arm/board/amc2c/application/src/model-based-design/control-outer/control_outer.h @@ -0,0 +1,301 @@ +// +// 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_outer.h +// +// Code generated for Simulink model 'control_outer'. +// +// Model version : 5.5 +// Simulink Coder version : 9.9 (R2023a) 19-Nov-2022 +// C/C++ source code generated on : Mon Oct 16 10:08:24 2023 +// +// Target selection: ert.tlc +// Embedded hardware selection: ARM Compatible->ARM Cortex-M +// Code generation objectives: Unspecified +// Validation result: Not run +// +#ifndef RTW_HEADER_control_outer_h_ +#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' +struct B_control_outer_c_T { + real32_T DiscreteTimeIntegrator; // '/Discrete-Time Integrator' +}; + +// Block states (default storage) for model 'control_outer' +struct DW_control_outer_f_T { + real32_T FilterDifferentiatorTF_states;// '/Filter Differentiator TF' + real32_T UnitDelay_DSTATE; // '/Unit Delay' + real32_T Integrator_DSTATE; // '/Integrator' + real32_T FilterDifferentiatorTF_states_i;// '/Filter Differentiator TF' + real32_T Integrator_DSTATE_i; // '/Integrator' + real32_T FilterDifferentiatorTF_states_c;// '/Filter Differentiator TF' + real32_T UnitDelay1_DSTATE; // '/Unit Delay1' + real32_T Integrator_DSTATE_b; // '/Integrator' + real32_T DiscreteTimeIntegrator_DSTATE;// '/Discrete-Time Integrator' + ControlModes DelayInput1_DSTATE; // '/Delay Input1' + real32_T FilterDifferentiatorTF_tmp; // '/Filter Differentiator TF' + real32_T FilterDifferentiatorTF_tmp_m;// '/Filter Differentiator TF' + real32_T FilterDifferentiatorTF_tmp_p;// '/Filter Differentiator TF' + int8_T Integrator_PrevResetState; // '/Integrator' + int8_T Integrator_PrevResetState_n; // '/Integrator' + int8_T Integrator_PrevResetState_c; // '/Integrator' + int8_T DiscreteTimeIntegrator_PrevResetState;// '/Discrete-Time Integrator' + uint8_T DiscreteTimeIntegrator_SYSTEM_ENABLE;// '/Discrete-Time Integrator' + boolean_T Memory_PreviousInput; // '/Memory' +}; + +// Zero-crossing (trigger) state for model 'control_outer' +struct ZCV_control_outer_g_T { + real_T FilterDifferentiatorTF_Reset_ZC;// '/Filter Differentiator TF' + real_T FilterDifferentiatorTF_Reset_ZC_f;// '/Filter Differentiator TF' + real_T FilterDifferentiatorTF_Reset_ZC_m;// '/Filter Differentiator TF' +}; + +// Zero-crossing (trigger) state for model 'control_outer' +struct ZCE_control_outer_T { + ZCSigState FilterDifferentiatorTF_Reset_ZCE;// '/Filter Differentiator TF' + ZCSigState FilterDifferentiatorTF_Reset_ZCE_m;// '/Filter Differentiator TF' + 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_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, + ZCE_control_outer_T *localZCE); + +//- +// These blocks were eliminated from the model due to optimizations: +// +// Block '/Data Type Duplicate' : Unused code path elimination +// Block '/Data Type Propagation' : Unused code path elimination +// Block '/Data Type Duplicate' : Unused code path elimination +// Block '/Data Type Propagation' : Unused code path elimination +// Block '/Passthrough for tuning' : Eliminate redundant data type conversion +// Block '/Kt' : Eliminated nontunable gain of 1 +// Block '/Passthrough for tuning' : Eliminate redundant data type conversion +// Block '/Kt' : Eliminated nontunable gain of 1 +// Block '/Passthrough for tuning' : Eliminate redundant data type conversion +// Block '/Kt' : Eliminated nontunable gain of 1 + + +//- +// 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 +// +// '' : 'control_outer' +// '' : 'control_outer/Current Limiter' +// '' : 'control_outer/Enabling Logic' +// '' : 'control_outer/Position PID control' +// '' : 'control_outer/Saturation Dynamic' +// '' : 'control_outer/Saturation Dynamic1' +// '' : 'control_outer/Velocity PID control' +// '' : 'control_outer/Current Limiter/Compare To Zero' +// '' : 'control_outer/Current Limiter/Compare To Zero1' +// '' : 'control_outer/Current Limiter/S-R Flip-Flop' +// '' : 'control_outer/Enabling Logic/Compare To Constant' +// '' : 'control_outer/Enabling Logic/Compare To Constant1' +// '' : 'control_outer/Enabling Logic/Compare To Constant2' +// '' : 'control_outer/Enabling Logic/Compare To Constant3' +// '' : 'control_outer/Enabling Logic/Compare To Constant4' +// '' : 'control_outer/Enabling Logic/Compare To Constant5' +// '' : 'control_outer/Enabling Logic/Compare To Constant6' +// '' : 'control_outer/Enabling Logic/Compare To Constant7' +// '' : 'control_outer/Enabling Logic/Detect Change' +// '' : 'control_outer/Position PID control/Position PID' +// '' : 'control_outer/Position PID control/Position-Direct PID' +// '' : 'control_outer/Position PID control/Position PID/Anti-windup' +// '' : 'control_outer/Position PID control/Position PID/D Gain' +// '' : 'control_outer/Position PID control/Position PID/Filter' +// '' : 'control_outer/Position PID control/Position PID/Filter ICs' +// '' : 'control_outer/Position PID control/Position PID/I Gain' +// '' : 'control_outer/Position PID control/Position PID/Ideal P Gain' +// '' : 'control_outer/Position PID control/Position PID/Ideal P Gain Fdbk' +// '' : 'control_outer/Position PID control/Position PID/Integrator' +// '' : 'control_outer/Position PID control/Position PID/Integrator ICs' +// '' : 'control_outer/Position PID control/Position PID/N Copy' +// '' : 'control_outer/Position PID control/Position PID/N Gain' +// '' : 'control_outer/Position PID control/Position PID/P Copy' +// '' : 'control_outer/Position PID control/Position PID/Parallel P Gain' +// '' : 'control_outer/Position PID control/Position PID/Reset Signal' +// '' : 'control_outer/Position PID control/Position PID/Saturation' +// '' : 'control_outer/Position PID control/Position PID/Saturation Fdbk' +// '' : 'control_outer/Position PID control/Position PID/Sum' +// '' : 'control_outer/Position PID control/Position PID/Sum Fdbk' +// '' : 'control_outer/Position PID control/Position PID/Tracking Mode' +// '' : 'control_outer/Position PID control/Position PID/Tracking Mode Sum' +// '' : 'control_outer/Position PID control/Position PID/Tsamp - Integral' +// '' : 'control_outer/Position PID control/Position PID/Tsamp - Ngain' +// '' : 'control_outer/Position PID control/Position PID/postSat Signal' +// '' : 'control_outer/Position PID control/Position PID/preSat Signal' +// '' : 'control_outer/Position PID control/Position PID/Anti-windup/Passthrough' +// '' : 'control_outer/Position PID control/Position PID/D Gain/External Parameters' +// '' : 'control_outer/Position PID control/Position PID/Filter/Disc. Trapezoidal Filter' +// '' : 'control_outer/Position PID control/Position PID/Filter/Disc. Trapezoidal Filter/Tsamp' +// '' : 'control_outer/Position PID control/Position PID/Filter/Disc. Trapezoidal Filter/Tsamp/Internal Ts' +// '' : 'control_outer/Position PID control/Position PID/Filter ICs/Internal IC - Filter' +// '' : 'control_outer/Position PID control/Position PID/I Gain/External Parameters' +// '' : 'control_outer/Position PID control/Position PID/Ideal P Gain/Passthrough' +// '' : 'control_outer/Position PID control/Position PID/Ideal P Gain Fdbk/Passthrough' +// '' : 'control_outer/Position PID control/Position PID/Integrator/Discrete' +// '' : 'control_outer/Position PID control/Position PID/Integrator ICs/Internal IC' +// '' : 'control_outer/Position PID control/Position PID/N Copy/External Parameters' +// '' : 'control_outer/Position PID control/Position PID/N Gain/External Parameters' +// '' : 'control_outer/Position PID control/Position PID/P Copy/Disabled' +// '' : 'control_outer/Position PID control/Position PID/Parallel P Gain/External Parameters' +// '' : 'control_outer/Position PID control/Position PID/Reset Signal/External Reset' +// '' : 'control_outer/Position PID control/Position PID/Saturation/Passthrough' +// '' : 'control_outer/Position PID control/Position PID/Saturation Fdbk/Passthrough' +// '' : 'control_outer/Position PID control/Position PID/Sum/Sum_PID' +// '' : 'control_outer/Position PID control/Position PID/Sum Fdbk/Enabled' +// '' : 'control_outer/Position PID control/Position PID/Tracking Mode/Enabled' +// '' : 'control_outer/Position PID control/Position PID/Tracking Mode Sum/Tracking Mode' +// '' : 'control_outer/Position PID control/Position PID/Tsamp - Integral/TsSignalSpecification' +// '' : 'control_outer/Position PID control/Position PID/Tsamp - Ngain/Passthrough' +// '' : 'control_outer/Position PID control/Position PID/postSat Signal/Feedback_Path' +// '' : 'control_outer/Position PID control/Position PID/preSat Signal/Feedback_Path' +// '' : 'control_outer/Position PID control/Position-Direct PID/Anti-windup' +// '' : 'control_outer/Position PID control/Position-Direct PID/D Gain' +// '' : 'control_outer/Position PID control/Position-Direct PID/Filter' +// '' : 'control_outer/Position PID control/Position-Direct PID/Filter ICs' +// '' : 'control_outer/Position PID control/Position-Direct PID/I Gain' +// '' : 'control_outer/Position PID control/Position-Direct PID/Ideal P Gain' +// '' : 'control_outer/Position PID control/Position-Direct PID/Ideal P Gain Fdbk' +// '' : 'control_outer/Position PID control/Position-Direct PID/Integrator' +// '' : 'control_outer/Position PID control/Position-Direct PID/Integrator ICs' +// '' : 'control_outer/Position PID control/Position-Direct PID/N Copy' +// '' : 'control_outer/Position PID control/Position-Direct PID/N Gain' +// '' : 'control_outer/Position PID control/Position-Direct PID/P Copy' +// '' : 'control_outer/Position PID control/Position-Direct PID/Parallel P Gain' +// '' : 'control_outer/Position PID control/Position-Direct PID/Reset Signal' +// '' : 'control_outer/Position PID control/Position-Direct PID/Saturation' +// '' : 'control_outer/Position PID control/Position-Direct PID/Saturation Fdbk' +// '' : 'control_outer/Position PID control/Position-Direct PID/Sum' +// '' : 'control_outer/Position PID control/Position-Direct PID/Sum Fdbk' +// '' : 'control_outer/Position PID control/Position-Direct PID/Tracking Mode' +// '' : 'control_outer/Position PID control/Position-Direct PID/Tracking Mode Sum' +// '' : 'control_outer/Position PID control/Position-Direct PID/Tsamp - Integral' +// '' : 'control_outer/Position PID control/Position-Direct PID/Tsamp - Ngain' +// '' : 'control_outer/Position PID control/Position-Direct PID/postSat Signal' +// '' : 'control_outer/Position PID control/Position-Direct PID/preSat Signal' +// '' : 'control_outer/Position PID control/Position-Direct PID/Anti-windup/Passthrough' +// '' : 'control_outer/Position PID control/Position-Direct PID/D Gain/External Parameters' +// '' : 'control_outer/Position PID control/Position-Direct PID/Filter/Disc. Trapezoidal Filter' +// '' : 'control_outer/Position PID control/Position-Direct PID/Filter/Disc. Trapezoidal Filter/Tsamp' +// '' : 'control_outer/Position PID control/Position-Direct PID/Filter/Disc. Trapezoidal Filter/Tsamp/Internal Ts' +// '' : 'control_outer/Position PID control/Position-Direct PID/Filter ICs/Internal IC - Filter' +// '' : 'control_outer/Position PID control/Position-Direct PID/I Gain/External Parameters' +// '' : 'control_outer/Position PID control/Position-Direct PID/Ideal P Gain/Passthrough' +// '' : 'control_outer/Position PID control/Position-Direct PID/Ideal P Gain Fdbk/Passthrough' +// '' : 'control_outer/Position PID control/Position-Direct PID/Integrator/Discrete' +// '' : 'control_outer/Position PID control/Position-Direct PID/Integrator ICs/Internal IC' +// '' : 'control_outer/Position PID control/Position-Direct PID/N Copy/External Parameters' +// '' : 'control_outer/Position PID control/Position-Direct PID/N Gain/External Parameters' +// '' : 'control_outer/Position PID control/Position-Direct PID/P Copy/Disabled' +// '' : 'control_outer/Position PID control/Position-Direct PID/Parallel P Gain/External Parameters' +// '' : 'control_outer/Position PID control/Position-Direct PID/Reset Signal/External Reset' +// '' : 'control_outer/Position PID control/Position-Direct PID/Saturation/Passthrough' +// '' : 'control_outer/Position PID control/Position-Direct PID/Saturation Fdbk/Passthrough' +// '' : 'control_outer/Position PID control/Position-Direct PID/Sum/Sum_PID' +// '' : 'control_outer/Position PID control/Position-Direct PID/Sum Fdbk/Enabled' +// '' : 'control_outer/Position PID control/Position-Direct PID/Tracking Mode/Enabled' +// '' : 'control_outer/Position PID control/Position-Direct PID/Tracking Mode Sum/Tracking Mode' +// '' : 'control_outer/Position PID control/Position-Direct PID/Tsamp - Integral/TsSignalSpecification' +// '' : 'control_outer/Position PID control/Position-Direct PID/Tsamp - Ngain/Passthrough' +// '' : 'control_outer/Position PID control/Position-Direct PID/postSat Signal/Feedback_Path' +// '' : 'control_outer/Position PID control/Position-Direct PID/preSat Signal/Feedback_Path' +// '' : 'control_outer/Velocity PID control/Anti-windup' +// '' : 'control_outer/Velocity PID control/D Gain' +// '' : 'control_outer/Velocity PID control/Filter' +// '' : 'control_outer/Velocity PID control/Filter ICs' +// '' : 'control_outer/Velocity PID control/I Gain' +// '' : 'control_outer/Velocity PID control/Ideal P Gain' +// '' : 'control_outer/Velocity PID control/Ideal P Gain Fdbk' +// '' : 'control_outer/Velocity PID control/Integrator' +// '' : 'control_outer/Velocity PID control/Integrator ICs' +// '' : 'control_outer/Velocity PID control/N Copy' +// '' : 'control_outer/Velocity PID control/N Gain' +// '' : 'control_outer/Velocity PID control/P Copy' +// '' : 'control_outer/Velocity PID control/Parallel P Gain' +// '' : 'control_outer/Velocity PID control/Reset Signal' +// '' : 'control_outer/Velocity PID control/Saturation' +// '' : 'control_outer/Velocity PID control/Saturation Fdbk' +// '' : 'control_outer/Velocity PID control/Sum' +// '' : 'control_outer/Velocity PID control/Sum Fdbk' +// '' : 'control_outer/Velocity PID control/Tracking Mode' +// '' : 'control_outer/Velocity PID control/Tracking Mode Sum' +// '' : 'control_outer/Velocity PID control/Tsamp - Integral' +// '' : 'control_outer/Velocity PID control/Tsamp - Ngain' +// '' : 'control_outer/Velocity PID control/postSat Signal' +// '' : 'control_outer/Velocity PID control/preSat Signal' +// '' : 'control_outer/Velocity PID control/Anti-windup/Passthrough' +// '' : 'control_outer/Velocity PID control/D Gain/External Parameters' +// '' : 'control_outer/Velocity PID control/Filter/Disc. Trapezoidal Filter' +// '' : 'control_outer/Velocity PID control/Filter/Disc. Trapezoidal Filter/Tsamp' +// '' : 'control_outer/Velocity PID control/Filter/Disc. Trapezoidal Filter/Tsamp/Internal Ts' +// '' : 'control_outer/Velocity PID control/Filter ICs/Internal IC - Filter' +// '' : 'control_outer/Velocity PID control/I Gain/External Parameters' +// '' : 'control_outer/Velocity PID control/Ideal P Gain/Passthrough' +// '' : 'control_outer/Velocity PID control/Ideal P Gain Fdbk/Passthrough' +// '' : 'control_outer/Velocity PID control/Integrator/Discrete' +// '' : 'control_outer/Velocity PID control/Integrator ICs/Internal IC' +// '' : 'control_outer/Velocity PID control/N Copy/External Parameters' +// '' : 'control_outer/Velocity PID control/N Gain/External Parameters' +// '' : 'control_outer/Velocity PID control/P Copy/Disabled' +// '' : 'control_outer/Velocity PID control/Parallel P Gain/External Parameters' +// '' : 'control_outer/Velocity PID control/Reset Signal/External Reset' +// '' : 'control_outer/Velocity PID control/Saturation/Passthrough' +// '' : 'control_outer/Velocity PID control/Saturation Fdbk/Passthrough' +// '' : 'control_outer/Velocity PID control/Sum/Sum_PID' +// '' : 'control_outer/Velocity PID control/Sum Fdbk/Enabled' +// '' : 'control_outer/Velocity PID control/Tracking Mode/Enabled' +// '' : 'control_outer/Velocity PID control/Tracking Mode Sum/Tracking Mode' +// '' : 'control_outer/Velocity PID control/Tsamp - Integral/TsSignalSpecification' +// '' : 'control_outer/Velocity PID control/Tsamp - Ngain/Passthrough' +// '' : 'control_outer/Velocity PID control/postSat Signal/Feedback_Path' +// '' : 'control_outer/Velocity PID control/preSat Signal/Feedback_Path' + +#endif // RTW_HEADER_control_outer_h_ + +// +// File trailer for generated code. +// +// [EOF] +// diff --git a/emBODY/eBcode/arch-arm/board/amc2c/application/src/model-based-design/control-outer/control_outer_private.h b/emBODY/eBcode/arch-arm/board/amc2c/application/src/model-based-design/control-outer/control_outer_private.h new file mode 100644 index 0000000000..9a33d0f211 --- /dev/null +++ b/emBODY/eBcode/arch-arm/board/amc2c/application/src/model-based-design/control-outer/control_outer_private.h @@ -0,0 +1,53 @@ +// +// 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_outer_private.h +// +// Code generated for Simulink model 'control_outer'. +// +// Model version : 5.5 +// Simulink Coder version : 9.9 (R2023a) 19-Nov-2022 +// C/C++ source code generated on : Mon Oct 16 10:08:24 2023 +// +// Target selection: ert.tlc +// Embedded hardware selection: ARM Compatible->ARM Cortex-M +// Code generation objectives: Unspecified +// Validation result: Not run +// +#ifndef RTW_HEADER_control_outer_private_h_ +#define RTW_HEADER_control_outer_private_h_ +#include "rtwtypes.h" +#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 + // Referenced by: '/Logic' + +#endif // RTW_HEADER_control_outer_private_h_ + +// +// File trailer for generated code. +// +// [EOF] +// diff --git a/emBODY/eBcode/arch-arm/board/amc2c/application/src/model-based-design/control-outer/control_outer_types.h b/emBODY/eBcode/arch-arm/board/amc2c/application/src/model-based-design/control-outer/control_outer_types.h new file mode 100644 index 0000000000..616a15f16c --- /dev/null +++ b/emBODY/eBcode/arch-arm/board/amc2c/application/src/model-based-design/control-outer/control_outer_types.h @@ -0,0 +1,335 @@ +// +// 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_outer_types.h +// +// Code generated for Simulink model 'control_outer'. +// +// Model version : 5.5 +// Simulink Coder version : 9.9 (R2023a) 19-Nov-2022 +// C/C++ source code generated on : Mon Oct 16 10:08:24 2023 +// +// Target selection: ert.tlc +// Embedded hardware selection: ARM Compatible->ARM Cortex-M +// Code generation objectives: Unspecified +// Validation result: Not run +// +#ifndef RTW_HEADER_control_outer_types_h_ +#define RTW_HEADER_control_outer_types_h_ +#include "rtwtypes.h" +#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 +{ + 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 Vcc; + 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_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_SensorsData_ +#define DEFINED_TYPEDEF_FOR_SensorsData_ + +struct SensorsData +{ + // position encoders + JointPositions jointpositions; + MotorSensors motorsensors; +}; + +#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_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 + +// Forward declaration for rtModel +typedef struct tag_RTM_control_outer_T RT_MODEL_control_outer_T; + +#endif // RTW_HEADER_control_outer_types_h_ + +// +// File trailer for generated code. +// +// [EOF] +// diff --git a/emBODY/eBcode/arch-arm/board/amc2c/application/src/model-based-design/estimator/estimation_velocity.cpp b/emBODY/eBcode/arch-arm/board/amc2c/application/src/model-based-design/estimator/estimation_velocity.cpp new file mode 100644 index 0000000000..86e75716d0 --- /dev/null +++ b/emBODY/eBcode/arch-arm/board/amc2c/application/src/model-based-design/estimator/estimation_velocity.cpp @@ -0,0 +1,504 @@ +// +// Non-Degree Granting Education License -- for use at non-degree +// granting, nonprofit, education, and research organizations only. Not +// for commercial or industrial use. +// +// File: estimation_velocity.cpp +// +// Code generated for Simulink model 'estimation_velocity'. +// +// Model version : 5.1 +// Simulink Coder version : 9.9 (R2023a) 19-Nov-2022 +// C/C++ source code generated on : Mon Oct 16 10:08:35 2023 +// +// Target selection: ert.tlc +// Embedded hardware selection: ARM Compatible->ARM Cortex-M +// Code generation objectives: Unspecified +// Validation result: Not run +// +#include "estimation_velocity.h" +#include "estimation_velocity_types.h" +#include "rtwtypes.h" +#include +#include +#include "mw_cmsis.h" +#include "rt_hypotf_snf.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); +static void estimation_velocity_xgeqp3(const real32_T A[32], real32_T b_A[32], + real32_T tau[2], int32_T jpvt[2]); +static real32_T estimation_velocity_xnrm2(int32_T n, const real32_T x[32], + int32_T ix0) +{ + real32_T absxk; + real32_T y; + y = 0.0F; + if (n < 1) { + } else if (n == 1) { + y = std::abs(x[ix0 - 1]); + } else { + int32_T kend; + real32_T scale; + scale = 1.29246971E-26F; + kend = ix0 + n; + for (int32_T k = ix0; k < kend; k++) { + absxk = std::abs(x[k - 1]); + if (absxk > scale) { + real32_T t; + t = scale / absxk; + y = y * t * t + 1.0F; + scale = absxk; + } else { + real32_T t; + t = absxk / scale; + y += t * t; + } + } + + mw_arm_sqrt_f32(y, &absxk); + y = scale * absxk; + } + + return y; +} + +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 d; + int32_T exitg1; + int32_T i; + int32_T ix; + int32_T jy; + int32_T kend; + int32_T kend_tmp; + int32_T knt; + int32_T lastv; + int32_T nmip1; + real32_T vn1[2]; + real32_T vn2[2]; + real32_T work[2]; + real32_T absxk; + real32_T b_atmp; + real32_T scale; + real32_T t; + boolean_T exitg2; + for (b_k = 0; b_k < 2; b_k++) { + jpvt[b_k] = b_k + 1; + } + + tau[0] = 0.0F; + tau[1] = 0.0F; + 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; + b_atmp = 0.0F; + scale = 1.29246971E-26F; + for (lastv = ix + 1; lastv <= ix + 16; lastv++) { + absxk = std::abs(A[lastv - 1]); + if (absxk > scale) { + t = scale / absxk; + b_atmp = b_atmp * t * t + 1.0F; + scale = absxk; + } else { + t = absxk / scale; + b_atmp += t * t; + } + } + + mw_arm_sqrt_f32(b_atmp, &b_atmp); + b_atmp *= scale; + vn1[b_k] = b_atmp; + vn2[b_k] = b_atmp; + } + + for (b_k = 0; b_k < 2; b_k++) { + i = b_k + 1; + kend_tmp = b_k << 4; + kend = kend_tmp + b_k; + nmip1 = 2 - b_k; + knt = 0; + if (2 - b_k > 1) { + scale = vn1[b_k]; + for (lastv = 2; lastv <= nmip1; lastv++) { + absxk = vn1[b_k + 1]; + if (absxk > scale) { + knt = 1; + scale = absxk; + } + } + } + + nmip1 = b_k + knt; + if (nmip1 + 1 != b_k + 1) { + ix = nmip1 << 4; + for (lastv = 0; lastv < 16; lastv++) { + knt = ix + lastv; + scale = b_A[knt]; + jy = kend_tmp + lastv; + b_A[knt] = b_A[jy]; + b_A[jy] = scale; + } + + ix = jpvt[nmip1]; + jpvt[nmip1] = jpvt[b_k]; + jpvt[b_k] = ix; + vn1[nmip1] = vn1[b_k]; + vn2[nmip1] = vn2[b_k]; + } + + ix = kend + 2; + b_atmp = b_A[kend]; + tau[b_k] = 0.0F; + scale = estimation_velocity_xnrm2(15 - b_k, b_A, kend + 2); + if (scale != 0.0F) { + absxk = b_A[kend]; + scale = rt_hypotf_snf(absxk, scale); + if (absxk >= 0.0F) { + scale = -scale; + } + + if (std::abs(scale) < 9.86076132E-32F) { + knt = -1; + do { + knt++; + nmip1 = (kend - b_k) - 1; + for (lastv = ix; lastv <= nmip1 + 17; lastv++) { + b_A[lastv - 1] *= 1.01412048E+31F; + } + + scale *= 1.01412048E+31F; + b_atmp *= 1.01412048E+31F; + } while ((std::abs(scale) < 9.86076132E-32F) && (knt + 1 < 20)); + + scale = rt_hypotf_snf(b_atmp, estimation_velocity_xnrm2(15 - b_k, b_A, + kend + 2)); + if (b_atmp >= 0.0F) { + scale = -scale; + } + + tau[b_k] = (scale - b_atmp) / scale; + b_atmp = 1.0F / (b_atmp - scale); + for (lastv = ix; lastv <= nmip1 + 17; lastv++) { + b_A[lastv - 1] *= b_atmp; + } + + for (lastv = 0; lastv <= knt; lastv++) { + scale *= 9.86076132E-32F; + } + + b_atmp = scale; + } else { + tau[b_k] = (scale - absxk) / scale; + b_atmp = 1.0F / (absxk - scale); + nmip1 = (kend - b_k) - 1; + for (lastv = ix; lastv <= nmip1 + 17; lastv++) { + b_A[lastv - 1] *= b_atmp; + } + + b_atmp = scale; + } + } + + b_A[kend] = b_atmp; + if (b_k + 1 < 2) { + b_A[kend] = 1.0F; + kend_tmp = kend + 17; + if (tau[0] != 0.0F) { + lastv = 16; + ix = kend - 1; + while ((lastv > 0) && (b_A[ix + 16] == 0.0F)) { + lastv--; + ix--; + } + + knt = 0; + exitg2 = false; + while ((!exitg2) && (knt + 1 > 0)) { + ix = (knt << 4) + kend; + jy = ix + 17; + do { + exitg1 = 0; + if (jy <= (ix + lastv) + 16) { + if (b_A[jy - 1] != 0.0F) { + exitg1 = 1; + } else { + jy++; + } + } else { + knt--; + exitg1 = 2; + } + } while (exitg1 == 0); + + if (exitg1 == 1) { + exitg2 = true; + } + } + } else { + lastv = 0; + knt = -1; + } + + if (lastv > 0) { + if (knt + 1 != 0) { + if (knt >= 0) { + std::memset(&work[0], 0, static_cast(knt + 1) * sizeof + (real32_T)); + } + + nmip1 = ((knt << 4) + kend) + 17; + for (ix = kend_tmp; ix <= nmip1; ix += 16) { + scale = 0.0F; + d = ix + lastv; + for (jy = ix; jy < d; jy++) { + scale += b_A[(kend + jy) - ix] * b_A[jy - 1]; + } + + jy = ((ix - kend) - 17) >> 4; + work[jy] += scale; + } + } + + if (!(-tau[0] == 0.0F)) { + jy = kend; + for (kend_tmp = 0; kend_tmp <= knt; kend_tmp++) { + scale = work[kend_tmp]; + if (scale != 0.0F) { + scale *= -tau[0]; + nmip1 = jy + 17; + ix = (lastv + jy) + 16; + for (d = nmip1; d <= ix; d++) { + b_A[d - 1] += b_A[((kend + d) - jy) - 17] * scale; + } + } + + jy += 16; + } + } + } + + b_A[kend] = b_atmp; + } + + for (nmip1 = i + 1; nmip1 < 3; nmip1++) { + if (vn1[1] != 0.0F) { + b_atmp = std::abs(b_A[b_k + 16]) / vn1[1]; + b_atmp = 1.0F - b_atmp * b_atmp; + if (b_atmp < 0.0F) { + b_atmp = 0.0F; + } + + scale = vn1[1] / vn2[1]; + scale = scale * scale * b_atmp; + if (scale <= 0.000345266977F) { + ix = b_k + 17; + b_atmp = 0.0F; + scale = 1.29246971E-26F; + for (lastv = ix + 1; lastv < 33; lastv++) { + absxk = std::abs(b_A[lastv - 1]); + if (absxk > scale) { + t = scale / absxk; + b_atmp = b_atmp * t * t + 1.0F; + scale = absxk; + } else { + t = absxk / scale; + b_atmp += t * t; + } + } + + mw_arm_sqrt_f32(b_atmp, &b_atmp); + b_atmp *= scale; + vn1[1] = b_atmp; + vn2[1] = b_atmp; + } else { + mw_arm_sqrt_f32(b_atmp, &b_atmp); + vn1[1] *= b_atmp; + } + } + } + } +} + +// System initialize for referenced model: 'estimation_velocity' +void estimation_velocity_Init(DW_estimation_velocity_f_T *localDW) +{ + int32_T idxIn; + + // InitializeConditions for S-Function (sdspsreg2): '/Delay Line' + idxIn = 0; + for (int32_T i = 0; i < 15; i++) { + localDW->DelayLine_Buff[idxIn] = 0.0F; + idxIn++; + } + + 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) +{ + int32_T jpvt[2]; + int32_T bIndx; + int32_T i; + int32_T i_0; + int32_T jpvt_tmp; + int32_T rankA; + int32_T rtb_QRSolver_tmp; + real32_T b_A[32]; + real32_T rtb_DelayLine[16]; + real32_T rtb_QRSolver_0[2]; + real32_T tau[2]; + real32_T tol; + + // S-Function (sdspsreg2): '/Delay Line' + for (rankA = 0; rankA < 15 - localDW->DelayLine_BUFF_OFFSET; rankA++) { + rtb_DelayLine[rankA] = localDW->DelayLine_Buff + [localDW->DelayLine_BUFF_OFFSET + rankA]; + } + + for (rankA = 0; rankA < localDW->DelayLine_BUFF_OFFSET; rankA++) { + rtb_DelayLine[(rankA - localDW->DelayLine_BUFF_OFFSET) + 15] = + localDW->DelayLine_Buff[rankA]; + } + + rtb_DelayLine[15] = rtu_SensorsData->jointpositions.position; + + // MATLABSystem: '/QR Solver' incorporates: + // Constant: '/Constant' + + estimation_velocity_xgeqp3(rtCP_Constant_Value_c, b_A, tau, jpvt); + rankA = 0; + tol = 1.90734863E-5F * std::abs(b_A[0]); + while ((rankA < 2) && (!(std::abs(b_A[(rankA << 4) + rankA]) <= tol))) { + rankA++; + } + + for (i_0 = 0; i_0 < 2; i_0++) { + rtb_QRSolver_0[i_0] = 0.0F; + if (tau[i_0] != 0.0F) { + tol = rtb_DelayLine[i_0]; + for (i = i_0 + 2; i < 17; i++) { + tol += b_A[((i_0 << 4) + i) - 1] * rtb_DelayLine[i - 1]; + } + + tol *= tau[i_0]; + if (tol != 0.0F) { + rtb_DelayLine[i_0] -= tol; + for (i = i_0 + 2; i < 17; i++) { + rtb_DelayLine[i - 1] -= b_A[((i_0 << 4) + i) - 1] * tol; + } + } + } + } + + for (i = 0; i < rankA; i++) { + rtb_QRSolver_0[jpvt[i] - 1] = rtb_DelayLine[i]; + } + + for (i_0 = rankA; i_0 >= 1; i_0--) { + jpvt_tmp = jpvt[i_0 - 1]; + rtb_QRSolver_tmp = (i_0 - 1) << 4; + rtb_QRSolver_0[jpvt_tmp - 1] /= b_A[(rtb_QRSolver_tmp + i_0) - 1]; + i = i_0 - 2; + for (bIndx = 0; bIndx <= i; bIndx++) { + rtb_QRSolver_0[jpvt[0] - 1] -= rtb_QRSolver_0[jpvt_tmp - 1] * + b_A[rtb_QRSolver_tmp]; + } + } + + // MultiPortSwitch: '/Index Vector' incorporates: + // Constant: '/Constant' + // Delay: '/Delay' + // Gain: '/Gain' + // MATLABSystem: '/QR Solver' + // S-Function (sdspsreg2): '/Delay Line' + // Sum: '/Sum' + + switch (rtu_ConfigurationParameters->estimationconfig.velocity_mode) { + case EstimationVelocityModes_Disabled: + rty_EstimatedVelocity->velocity = 0.0F; + break; + + case EstimationVelocityModes_MovingAverage: + rty_EstimatedVelocity->velocity = (rtu_SensorsData->jointpositions.position + - localDW->Delay_DSTATE[localDW->CircBufIdx]) * 62.5F; + break; + + default: + rty_EstimatedVelocity->velocity = rtb_QRSolver_0[0]; + break; + } + + // End of MultiPortSwitch: '/Index Vector' + + // Update for S-Function (sdspsreg2): '/Delay Line' + localDW->DelayLine_Buff[localDW->DelayLine_BUFF_OFFSET] = + rtu_SensorsData->jointpositions.position; + localDW->DelayLine_BUFF_OFFSET++; + while (localDW->DelayLine_BUFF_OFFSET >= 15) { + localDW->DelayLine_BUFF_OFFSET -= 15; + } + + // 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; + if (localDW->CircBufIdx < 15U) { + localDW->CircBufIdx++; + } else { + localDW->CircBufIdx = 0U; + } + + // 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. +// +// [EOF] +// diff --git a/emBODY/eBcode/arch-arm/board/amc2c/application/src/model-based-design/estimator/estimation_velocity.h b/emBODY/eBcode/arch-arm/board/amc2c/application/src/model-based-design/estimator/estimation_velocity.h new file mode 100644 index 0000000000..1a7186fb29 --- /dev/null +++ b/emBODY/eBcode/arch-arm/board/amc2c/application/src/model-based-design/estimator/estimation_velocity.h @@ -0,0 +1,96 @@ +// +// Non-Degree Granting Education License -- for use at non-degree +// granting, nonprofit, education, and research organizations only. Not +// for commercial or industrial use. +// +// File: estimation_velocity.h +// +// Code generated for Simulink model 'estimation_velocity'. +// +// Model version : 5.1 +// Simulink Coder version : 9.9 (R2023a) 19-Nov-2022 +// C/C++ source code generated on : Mon Oct 16 10:08:35 2023 +// +// Target selection: ert.tlc +// Embedded hardware selection: ARM Compatible->ARM Cortex-M +// Code generation objectives: Unspecified +// Validation result: Not run +// +#ifndef RTW_HEADER_estimation_velocity_h_ +#define RTW_HEADER_estimation_velocity_h_ +#include "rtwtypes.h" +#include "estimation_velocity_types.h" +#include "rtGetNaN.h" +#include + +extern "C" +{ + +#include "rt_nonfinite.h" + +} + +// Block states (default storage) for model 'estimation_velocity' +struct DW_estimation_velocity_f_T { + real32_T DelayLine_Buff[15]; // '/Delay Line' + real32_T Delay_DSTATE[16]; // '/Delay' + int32_T DelayLine_BUFF_OFFSET; // '/Delay Line' + uint32_T CircBufIdx; // '/Delay' + dsp_simulink_QRSolver_estimation_velocity_T obj;// '/QR Solver' + 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); + +//- +// These blocks were eliminated from the model due to optimizations: +// +// Block '/Check Signal Attributes' : Unused code path elimination +// Block '/Check Signal Attributes' : Unused code path elimination +// Block '/Check Signal Attributes' : Unused code path elimination + + +//- +// 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 +// +// '' : 'estimation_velocity' +// '' : 'estimation_velocity/Least Squares Polynomial Fit' +// '' : 'estimation_velocity/Least Squares Polynomial Fit/Check Signal Attributes' +// '' : 'estimation_velocity/Least Squares Polynomial Fit/Check Signal Attributes1' +// '' : 'estimation_velocity/Least Squares Polynomial Fit/Check Signal Attributes2' + +#endif // RTW_HEADER_estimation_velocity_h_ + +// +// File trailer for generated code. +// +// [EOF] +// diff --git a/emBODY/eBcode/arch-arm/board/amc2c/application/src/model-based-design/estimator/estimation_velocity_private.h b/emBODY/eBcode/arch-arm/board/amc2c/application/src/model-based-design/estimator/estimation_velocity_private.h new file mode 100644 index 0000000000..2c0d442ea6 --- /dev/null +++ b/emBODY/eBcode/arch-arm/board/amc2c/application/src/model-based-design/estimator/estimation_velocity_private.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: estimation_velocity_private.h +// +// Code generated for Simulink model 'estimation_velocity'. +// +// Model version : 5.1 +// Simulink Coder version : 9.9 (R2023a) 19-Nov-2022 +// C/C++ source code generated on : Mon Oct 16 10:08:35 2023 +// +// Target selection: ert.tlc +// Embedded hardware selection: ARM Compatible->ARM Cortex-M +// Code generation objectives: Unspecified +// Validation result: Not run +// +#ifndef RTW_HEADER_estimation_velocity_private_h_ +#define RTW_HEADER_estimation_velocity_private_h_ +#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 + // Referenced by: '/Constant' + +#endif // RTW_HEADER_estimation_velocity_private_h_ + +// +// File trailer for generated code. +// +// [EOF] +// diff --git a/emBODY/eBcode/arch-arm/board/amc2c/application/src/model-based-design/estimator/estimation_velocity_types.h b/emBODY/eBcode/arch-arm/board/amc2c/application/src/model-based-design/estimator/estimation_velocity_types.h new file mode 100644 index 0000000000..e980ff4519 --- /dev/null +++ b/emBODY/eBcode/arch-arm/board/amc2c/application/src/model-based-design/estimator/estimation_velocity_types.h @@ -0,0 +1,236 @@ +// +// Non-Degree Granting Education License -- for use at non-degree +// granting, nonprofit, education, and research organizations only. Not +// for commercial or industrial use. +// +// File: estimation_velocity_types.h +// +// Code generated for Simulink model 'estimation_velocity'. +// +// Model version : 5.1 +// Simulink Coder version : 9.9 (R2023a) 19-Nov-2022 +// C/C++ source code generated on : Mon Oct 16 10:08:35 2023 +// +// Target selection: ert.tlc +// Embedded hardware selection: ARM Compatible->ARM Cortex-M +// Code generation objectives: Unspecified +// Validation result: Not run +// +#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_ + +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_SensorsData_ +#define DEFINED_TYPEDEF_FOR_SensorsData_ + +struct SensorsData +{ + // position encoders + JointPositions jointpositions; + MotorSensors motorsensors; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_MotorConfig_ +#define DEFINED_TYPEDEF_FOR_MotorConfig_ + +struct MotorConfig +{ + 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 Vcc; + 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_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 + +struct dsp_simulink_QRSolver_estimation_velocity_T +{ + int32_T isInitialized; +}; + +#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_ + +// +// File trailer for generated code. +// +// [EOF] +// diff --git a/emBODY/eBcode/arch-arm/board/amc2c/application/src/model-based-design/filter-current/filter_current.cpp b/emBODY/eBcode/arch-arm/board/amc2c/application/src/model-based-design/filter-current/filter_current.cpp new file mode 100644 index 0000000000..b66295e70f --- /dev/null +++ b/emBODY/eBcode/arch-arm/board/amc2c/application/src/model-based-design/filter-current/filter_current.cpp @@ -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: filter_current.cpp +// +// Code generated for Simulink model 'filter_current'. +// +// Model version : 5.1 +// Simulink Coder version : 9.9 (R2023a) 19-Nov-2022 +// C/C++ source code generated on : Mon Oct 16 10:08:44 2023 +// +// Target selection: ert.tlc +// Embedded hardware selection: ARM Compatible->ARM Cortex-M +// Code generation objectives: Unspecified +// Validation result: Not run +// +#include "filter_current.h" +#include "filter_current_types.h" +#include "rtwtypes.h" +#include +#include +#include "filter_current_private.h" + +// Forward declaration for local functions +static void filter_current_MedianFilterCG_resetImpl + (c_dsp_internal_MedianFilterCG_filter_current_T *obj); +static void filter_current_MedianFilterCG_trickleDownMax + (c_dsp_internal_MedianFilterCG_filter_current_T *obj, real32_T i); +static void filter_current_MedianFilterCG_trickleDownMin + (c_dsp_internal_MedianFilterCG_filter_current_T *obj, real32_T i); +static void filter_current_MedianFilterCG_resetImpl + (c_dsp_internal_MedianFilterCG_filter_current_T *obj) +{ + real32_T cnt1; + real32_T cnt2; + std::memset(&obj->pBuf[0], 0, sizeof(real32_T) << 5U); + std::memset(&obj->pPos[0], 0, sizeof(real32_T) << 5U); + std::memset(&obj->pHeap[0], 0, sizeof(real32_T) << 5U); + obj->pWinLen = 32.0F; + obj->pIdx = obj->pWinLen; + 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); + } else { + obj->pMinHeapLength = std::floor(cnt1); + } + + cnt1 = obj->pWinLen / 2.0F; + if (cnt1 < 0.0F) { + obj->pMaxHeapLength = std::ceil(cnt1); + } else { + obj->pMaxHeapLength = std::floor(cnt1); + } + + cnt1 = 1.0F; + cnt2 = obj->pWinLen; + for (int32_T i = 0; i < 32; i++) { + if (static_cast(std::fmod(32.0F - static_cast(i), 2.0F)) == + 0) { + obj->pPos[31 - i] = cnt1; + cnt1++; + } else { + obj->pPos[31 - i] = cnt2; + cnt2--; + } + + obj->pHeap[static_cast(obj->pPos[31 - i]) - 1] = 32.0F - + static_cast(i); + } +} + +static void filter_current_MedianFilterCG_trickleDownMax + (c_dsp_internal_MedianFilterCG_filter_current_T *obj, real32_T i) +{ + boolean_T exitg1; + exitg1 = false; + while ((!exitg1) && (i >= -obj->pMaxHeapLength)) { + real32_T ind2; + real32_T temp; + real32_T tmp; + real32_T u_tmp; + if ((i < -1.0F) && (i > -obj->pMaxHeapLength) && (obj->pBuf + [static_cast(obj->pHeap[static_cast(i + obj->pMidHeap) + - 1]) - 1] < obj->pBuf[static_cast(obj->pHeap + [static_cast((i - 1.0F) + obj->pMidHeap) - 1]) - 1])) { + i--; + } + + u_tmp = i / 2.0F; + if (u_tmp < 0.0F) { + temp = std::ceil(u_tmp); + } else { + temp = std::floor(u_tmp); + } + + 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])) + { + exitg1 = true; + } else { + if (u_tmp < 0.0F) { + temp = std::ceil(u_tmp); + } else { + temp = std::floor(u_tmp); + } + + u_tmp = temp + obj->pMidHeap; + temp = obj->pHeap[static_cast(u_tmp) - 1]; + obj->pHeap[static_cast(u_tmp) - 1] = tmp; + obj->pHeap[static_cast(ind2) - 1] = temp; + obj->pPos[static_cast(obj->pHeap[static_cast(u_tmp) - 1]) + - 1] = u_tmp; + obj->pPos[static_cast(obj->pHeap[static_cast(ind2) - 1]) + - 1] = ind2; + i *= 2.0F; + } + } +} + +static void filter_current_MedianFilterCG_trickleDownMin + (c_dsp_internal_MedianFilterCG_filter_current_T *obj, real32_T i) +{ + boolean_T exitg1; + exitg1 = false; + while ((!exitg1) && (i <= obj->pMinHeapLength)) { + real32_T ind1; + real32_T tmp; + real32_T tmp_0; + real32_T u_tmp; + if ((i > 1.0F) && (i < obj->pMinHeapLength) && (obj->pBuf + [static_cast(obj->pHeap[static_cast((i + 1.0F) + + obj->pMidHeap) - 1]) - 1] < obj->pBuf[static_cast(obj-> + pHeap[static_cast(i + obj->pMidHeap) - 1]) - 1])) { + i++; + } + + u_tmp = i / 2.0F; + if (u_tmp < 0.0F) { + tmp = std::ceil(u_tmp); + } else { + tmp = std::floor(u_tmp); + } + + 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])) { + exitg1 = true; + } else { + if (u_tmp < 0.0F) { + tmp = std::ceil(u_tmp); + } else { + tmp = std::floor(u_tmp); + } + + u_tmp = tmp + obj->pMidHeap; + obj->pHeap[static_cast(ind1) - 1] = obj->pHeap + [static_cast(u_tmp) - 1]; + obj->pHeap[static_cast(u_tmp) - 1] = tmp_0; + obj->pPos[static_cast(obj->pHeap[static_cast(ind1) - 1]) + - 1] = ind1; + obj->pPos[static_cast(obj->pHeap[static_cast(u_tmp) - 1]) + - 1] = u_tmp; + i *= 2.0F; + } + } +} + +// System initialize for referenced model: 'filter_current' +void filter_current_Init(DW_filter_current_f_T *localDW) +{ + // Start for MATLABSystem: '/Median Filter' + localDW->obj.matlabCodegenIsDeleted = false; + localDW->objisempty = true; + localDW->obj.isInitialized = 1; + localDW->obj.NumChannels = 1; + localDW->obj.pMID.isInitialized = 0; + localDW->obj.isSetupComplete = true; + + // InitializeConditions for MATLABSystem: '/Median Filter' + if (localDW->obj.pMID.isInitialized == 1) { + filter_current_MedianFilterCG_resetImpl(&localDW->obj.pMID); + } + + // End of InitializeConditions for MATLABSystem: '/Median Filter' +} + +// Output and update for referenced model: 'filter_current' +void filter_current(const ControlOutputs *rtu_ControlOutputs, MotorCurrent + *rty_FilteredCurrent, DW_filter_current_f_T *localDW) +{ + c_dsp_internal_MedianFilterCG_filter_current_T *obj; + int32_T vprev_tmp; + real32_T ind2; + real32_T p; + real32_T temp; + real32_T u_tmp; + real32_T vprev; + boolean_T exitg1; + boolean_T flag; + + // MATLABSystem: '/Median Filter' + obj = &localDW->obj.pMID; + if (localDW->obj.pMID.isInitialized != 1) { + localDW->obj.pMID.isInitialized = 1; + localDW->obj.pMID.isSetupComplete = true; + filter_current_MedianFilterCG_resetImpl(&localDW->obj.pMID); + } + + 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; + 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) { + localDW->obj.pMID.pIdx = 1.0F; + } + + if (p > localDW->obj.pMID.pMidHeap) { + if (vprev < rtu_ControlOutputs->Iq_fbk.current) { + vprev = p - localDW->obj.pMID.pMidHeap; + filter_current_MedianFilterCG_trickleDownMin(&localDW->obj.pMID, vprev * + 2.0F); + } else { + vprev = p - localDW->obj.pMID.pMidHeap; + exitg1 = false; + while ((!exitg1) && (vprev > 0.0F)) { + u_tmp = 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 + + 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 + [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(ind2) - + 1]) - 1] = ind2; + vprev = std::floor(vprev / 2.0F); + } + } + + if (vprev == 0.0F) { + filter_current_MedianFilterCG_trickleDownMax(&localDW->obj.pMID, -1.0F); + } + } + } else if (p < localDW->obj.pMID.pMidHeap) { + if (rtu_ControlOutputs->Iq_fbk.current < vprev) { + vprev = p - localDW->obj.pMID.pMidHeap; + filter_current_MedianFilterCG_trickleDownMax(&localDW->obj.pMID, vprev * + 2.0F); + } else { + 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); + } else { + p = -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]); + if (!flag) { + exitg1 = true; + } else { + if (u_tmp < 0.0F) { + p = std::ceil(u_tmp); + } else { + p = -0.0F; + } + + p += 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]; + 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(ind2) - + 1]) - 1] = ind2; + if (u_tmp < 0.0F) { + vprev = std::ceil(u_tmp); + } else { + vprev = -0.0F; + } + } + } + + if (vprev == 0.0F) { + filter_current_MedianFilterCG_trickleDownMin(&localDW->obj.pMID, 1.0F); + } + } + } else { + if (localDW->obj.pMID.pMaxHeapLength != 0.0F) { + filter_current_MedianFilterCG_trickleDownMax(&localDW->obj.pMID, -1.0F); + } + + if (localDW->obj.pMID.pMinHeapLength > 0.0F) { + filter_current_MedianFilterCG_trickleDownMin(&localDW->obj.pMID, 1.0F); + } + } + + vprev = 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; + + // End of MATLABSystem: '/Median Filter' +} + +// Termination for referenced model: 'filter_current' +void filter_current_Term(DW_filter_current_f_T *localDW) +{ + // Terminate for MATLABSystem: '/Median Filter' + if (!localDW->obj.matlabCodegenIsDeleted) { + localDW->obj.matlabCodegenIsDeleted = true; + if ((localDW->obj.isInitialized == 1) && localDW->obj.isSetupComplete) { + localDW->obj.NumChannels = -1; + if (localDW->obj.pMID.isInitialized == 1) { + localDW->obj.pMID.isInitialized = 2; + } + } + } + + // 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. +// +// [EOF] +// diff --git a/emBODY/eBcode/arch-arm/board/amc2c/application/src/model-based-design/filter-current/filter_current.h b/emBODY/eBcode/arch-arm/board/amc2c/application/src/model-based-design/filter-current/filter_current.h new file mode 100644 index 0000000000..9823d83e51 --- /dev/null +++ b/emBODY/eBcode/arch-arm/board/amc2c/application/src/model-based-design/filter-current/filter_current.h @@ -0,0 +1,72 @@ +// +// Non-Degree Granting Education License -- for use at non-degree +// granting, nonprofit, education, and research organizations only. Not +// for commercial or industrial use. +// +// File: filter_current.h +// +// Code generated for Simulink model 'filter_current'. +// +// Model version : 5.1 +// Simulink Coder version : 9.9 (R2023a) 19-Nov-2022 +// C/C++ source code generated on : Mon Oct 16 10:08:44 2023 +// +// Target selection: ert.tlc +// Embedded hardware selection: ARM Compatible->ARM Cortex-M +// Code generation objectives: Unspecified +// Validation result: Not run +// +#ifndef RTW_HEADER_filter_current_h_ +#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 { + dsp_simulink_MedianFilter_filter_current_T obj;// '/Median Filter' + 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_Term(DW_filter_current_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 +// +// '' : 'filter_current' + +#endif // RTW_HEADER_filter_current_h_ + +// +// File trailer for generated code. +// +// [EOF] +// diff --git a/emBODY/eBcode/arch-arm/board/amc2c/application/src/model-based-design/filter-current/filter_current_private.h b/emBODY/eBcode/arch-arm/board/amc2c/application/src/model-based-design/filter-current/filter_current_private.h new file mode 100644 index 0000000000..a1a7cd3546 --- /dev/null +++ b/emBODY/eBcode/arch-arm/board/amc2c/application/src/model-based-design/filter-current/filter_current_private.h @@ -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: filter_current_private.h +// +// Code generated for Simulink model 'filter_current'. +// +// Model version : 5.1 +// Simulink Coder version : 9.9 (R2023a) 19-Nov-2022 +// C/C++ source code generated on : Mon Oct 16 10:08:44 2023 +// +// Target selection: ert.tlc +// Embedded hardware selection: ARM Compatible->ARM Cortex-M +// Code generation objectives: Unspecified +// Validation result: Not run +// +#ifndef RTW_HEADER_filter_current_private_h_ +#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_ + +// +// File trailer for generated code. +// +// [EOF] +// diff --git a/emBODY/eBcode/arch-arm/board/amc2c/application/src/model-based-design/filter-current/filter_current_types.h b/emBODY/eBcode/arch-arm/board/amc2c/application/src/model-based-design/filter-current/filter_current_types.h new file mode 100644 index 0000000000..8156afd4b1 --- /dev/null +++ b/emBODY/eBcode/arch-arm/board/amc2c/application/src/model-based-design/filter-current/filter_current_types.h @@ -0,0 +1,112 @@ +// +// Non-Degree Granting Education License -- for use at non-degree +// granting, nonprofit, education, and research organizations only. Not +// for commercial or industrial use. +// +// File: filter_current_types.h +// +// Code generated for Simulink model 'filter_current'. +// +// Model version : 5.1 +// Simulink Coder version : 9.9 (R2023a) 19-Nov-2022 +// C/C++ source code generated on : Mon Oct 16 10:08:44 2023 +// +// Target selection: ert.tlc +// Embedded hardware selection: ARM Compatible->ARM Cortex-M +// Code generation objectives: Unspecified +// Validation result: Not run +// +#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_ + +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 struct_c_dsp_internal_MedianFilterCG_filter_current_T +#define struct_c_dsp_internal_MedianFilterCG_filter_current_T + +struct c_dsp_internal_MedianFilterCG_filter_current_T +{ + int32_T isInitialized; + boolean_T isSetupComplete; + real32_T pWinLen; + real32_T pBuf[32]; + real32_T pHeap[32]; + real32_T pMidHeap; + real32_T pIdx; + real32_T pPos[32]; + real32_T pMinHeapLength; + real32_T pMaxHeapLength; +}; + +#endif // struct_c_dsp_internal_MedianFilterCG_filter_current_T + +#ifndef struct_cell_wrap_filter_current_T +#define struct_cell_wrap_filter_current_T + +struct cell_wrap_filter_current_T +{ + uint32_T f1[8]; +}; + +#endif // struct_cell_wrap_filter_current_T + +#ifndef struct_dsp_simulink_MedianFilter_filter_current_T +#define struct_dsp_simulink_MedianFilter_filter_current_T + +struct dsp_simulink_MedianFilter_filter_current_T +{ + boolean_T matlabCodegenIsDeleted; + int32_T isInitialized; + boolean_T isSetupComplete; + cell_wrap_filter_current_T inputVarSize; + int32_T NumChannels; + c_dsp_internal_MedianFilterCG_filter_current_T pMID; +}; + +#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_ + +// +// File trailer for generated code. +// +// [EOF] +// diff --git a/emBODY/eBcode/arch-arm/board/amc2c/application/src/model-based-design/sharedutils/const_params.cpp b/emBODY/eBcode/arch-arm/board/amc2c/application/src/model-based-design/sharedutils/const_params.cpp new file mode 100644 index 0000000000..42af1352d5 --- /dev/null +++ b/emBODY/eBcode/arch-arm/board/amc2c/application/src/model-based-design/sharedutils/const_params.cpp @@ -0,0 +1,28 @@ +// +// const_params.cpp +// +// Non-Degree Granting Education License -- for use at non-degree +// granting, nonprofit, education, and research organizations only. Not +// for commercial or industrial use. +// +// Code generation for model "estimation_velocity". +// +// Model version : 5.1 +// Simulink Coder version : 9.9 (R2023a) 19-Nov-2022 +// C++ source code generated on : Thu Oct 12 19:04:35 2023 + +#include "rtwtypes.h" + +extern const real32_T rtCP_pooled_Az3IVI54Pn7X[32]; +const real32_T rtCP_pooled_Az3IVI54Pn7X[32] = { 0.0F, 0.001F, 0.002F, 0.003F, + 0.004F, 0.005F, 0.006F, 0.007F, 0.008F, 0.009F, 0.01F, 0.011F, 0.012F, 0.013F, + 0.014F, 0.015F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, + 1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F } ; + +extern const real32_T rtCP_pooled_IgamRjjg0YgF[6]; +const real32_T rtCP_pooled_IgamRjjg0YgF[6] = { 0.666666687F, -0.333333343F, + -0.333333343F, 0.666666687F, -0.333333343F, -0.333333343F } ; + +extern const boolean_T rtCP_pooled_kUC6nmgO8rex[16]; +const boolean_T rtCP_pooled_kUC6nmgO8rex[16] = { false, true, false, false, true, + true, false, false, true, false, true, true, false, false, false, false } ; diff --git a/emBODY/eBcode/arch-arm/board/amc2c/application/src/model-based-design/sharedutils/multiword_types.h b/emBODY/eBcode/arch-arm/board/amc2c/application/src/model-based-design/sharedutils/multiword_types.h new file mode 100644 index 0000000000..10130f73b1 --- /dev/null +++ b/emBODY/eBcode/arch-arm/board/amc2c/application/src/model-based-design/sharedutils/multiword_types.h @@ -0,0 +1,50 @@ +// +// Non-Degree Granting Education License -- for use at non-degree +// granting, nonprofit, education, and research organizations only. Not +// for commercial or industrial use. +// +// File: multiword_types.h +// +// Code generated for Simulink model 'SupervisorFSM_RX'. +// +// Model version : 4.6 +// Simulink Coder version : 9.7 (R2022a) 13-Nov-2021 +// C/C++ source code generated on : Tue Jun 7 15:41:38 2022 +// +#ifndef MULTIWORD_TYPES_H +#define MULTIWORD_TYPES_H +#include "rtwtypes.h" + +// +// MultiWord supporting definitions + +typedef long int long_T; + +// +// MultiWord types + +typedef struct { + uint32_T chunks[2]; +} int64m_T; + +typedef struct { + int64m_T re; + int64m_T im; +} cint64m_T; + +typedef struct { + uint32_T chunks[2]; +} uint64m_T; + +typedef struct { + uint64m_T re; + uint64m_T im; +} cuint64m_T; + +#endif // MULTIWORD_TYPES_H + +// +// File trailer for generated code. +// +// [EOF] +// diff --git a/emBODY/eBcode/arch-arm/board/amc2c/application/src/model-based-design/sharedutils/mw_cmsis.h b/emBODY/eBcode/arch-arm/board/amc2c/application/src/model-based-design/sharedutils/mw_cmsis.h new file mode 100644 index 0000000000..5ab7118a5c --- /dev/null +++ b/emBODY/eBcode/arch-arm/board/amc2c/application/src/model-based-design/sharedutils/mw_cmsis.h @@ -0,0 +1,75 @@ +/* Copyright 2015-2021 The MathWorks, Inc. */ + +/**************************************************** +* * +* Wrapper functions for CMSIS functions * +* * +****************************************************/ + +#ifndef MW_CMSIS_H +#define MW_CMSIS_H + +#include "arm_math.h" +#include "rtwtypes.h" + +#define mw_arm_abs_f32(pSrc, pDst, blockSize) arm_abs_f32((float32_t *)pSrc, (float32_t *)pDst, blockSize) +#define mw_arm_abs_q7(pSrc, pDst, blockSize) arm_abs_q7((q7_t *)pSrc, (q7_t *)pDst, blockSize) +#define mw_arm_abs_q15(pSrc, pDst, blockSize) arm_abs_q15((q15_t *)pSrc, (q15_t *)pDst, blockSize) +#define mw_arm_abs_q31(pSrc, pDst, blockSize) arm_abs_q31((q31_t *)pSrc, (q31_t *)pDst, blockSize) + +#define mw_arm_sqrt_q15(in, pOut) arm_sqrt_q15((q15_t)in,(q15_t *)pOut) +#define mw_arm_sqrt_q31(in, pOut) arm_sqrt_q31((q31_t)in,(q31_t *)pOut) +#define mw_arm_sqrt_f32(in, pOut) arm_sqrt_f32((float32_t)in,(float32_t *)pOut) + +#define mw_arm_float_to_q31(pSrc, pDst, blockSize) arm_float_to_q31((float32_t *)pSrc, (q31_t *)pDst, blockSize) +#define mw_arm_float_to_q15(pSrc, pDst, blockSize) arm_float_to_q15((float32_t *)pSrc, (q15_t *)pDst, blockSize) +#define mw_arm_float_to_q7(pSrc, pDst, blockSize) arm_float_to_q7((float32_t *)pSrc, (q7_t *)pDst, blockSize) + +#define mw_arm_q15_to_float(pSrc, pDst, blockSize) arm_q15_to_float((q15_t *)pSrc, (float32_t *)pDst, blockSize) +#define mw_arm_q15_to_q31(pSrc, pDst, blockSize) arm_q15_to_q31((q15_t *)pSrc, (q31_t *)pDst, blockSize) +#define mw_arm_q15_to_q7(pSrc, pDst, blockSize) arm_q15_to_q7((q15_t *)pSrc, (q7_t *)pDst, blockSize) + +#define mw_arm_q31_to_float(pSrc, pDst, blockSize) arm_q31_to_float((q31_t *)pSrc, (float32_t *)pDst, blockSize) +#define mw_arm_q31_to_q15(pSrc, pDst, blockSize) arm_q31_to_q15((q31_t *)pSrc, (q15_t *)pDst, blockSize) +#define mw_arm_q31_to_q7(pSrc, pDst, blockSize) arm_q31_to_q7((q31_t *)pSrc, (q7_t *)pDst, blockSize) + +#define mw_arm_q7_to_float(pSrc, pDst, blockSize) arm_q7_to_float((q7_t *)pSrc, (float32_t *)pDst, blockSize) +#define mw_arm_q7_to_q31(pSrc, pDst, blockSize) arm_q7_to_q31((q7_t *)pSrc, (q31_t *)pDst, blockSize) +#define mw_arm_q7_to_q15(pSrc, pDst, blockSize) arm_q7_to_q15((q7_t *)pSrc, (q15_t *)pDst, blockSize) + +#define mw_arm_add_f32(pSrcA, pSrcB, pDst, blockSize) arm_add_f32((float32_t *)pSrcA, (float32_t *)pSrcB, (float32_t *)pDst, blockSize) +#define mw_arm_add_q31(pSrcA, pSrcB, pDst, blockSize) arm_add_q31((q31_t *)pSrcA, (q31_t *)pSrcB, (q31_t *)pDst, blockSize) +#define mw_arm_add_q15(pSrcA, pSrcB, pDst, blockSize) arm_add_q15((q15_t *)pSrcA, (q15_t *)pSrcB, (q15_t *)pDst, blockSize) +#define mw_arm_add_q7(pSrcA, pSrcB, pDst, blockSize) arm_add_q7((q7_t *)pSrcA, (q7_t *)pSrcB, (q7_t *)pDst, blockSize) + +#define mw_arm_sub_f32(pSrcA, pSrcB, pDst, blockSize) arm_sub_f32((float32_t *)pSrcA, (float32_t *)pSrcB, (float32_t *)pDst, blockSize) +#define mw_arm_sub_q31(pSrcA, pSrcB, pDst, blockSize) arm_sub_q31((q31_t *)pSrcA, (q31_t *)pSrcB, (q31_t *)pDst, blockSize) +#define mw_arm_sub_q15(pSrcA, pSrcB, pDst, blockSize) arm_sub_q15((q15_t *)pSrcA, (q15_t *)pSrcB, (q15_t *)pDst, blockSize) +#define mw_arm_sub_q7(pSrcA, pSrcB, pDst, blockSize) arm_sub_q7((q7_t *)pSrcA, (q7_t *)pSrcB, (q7_t *)pDst, blockSize) + +#define mw_arm_mult_f32(pSrcA, pSrcB, pDst, blockSize) arm_mult_f32((float32_t *)pSrcA, (float32_t *)pSrcB, (float32_t *)pDst, blockSize) +#define mw_arm_mult_q31(pSrcA, pSrcB, pDst, blockSize) arm_mult_q31((q31_t *)pSrcA, (q31_t *)pSrcB, (q31_t *)pDst, blockSize) +#define mw_arm_mult_q15(pSrcA, pSrcB, pDst, blockSize) arm_mult_q15((q15_t *)pSrcA, (q15_t *)pSrcB, (q15_t *)pDst, blockSize) +#define mw_arm_mult_q7(pSrcA, pSrcB, pDst, blockSize) arm_mult_q7((q7_t *)pSrcA, (q7_t *)pSrcB, (q7_t *)pDst, blockSize) + +#define mw_arm_cmplx_conj_f32(pSrc, pDst, numSamples) arm_cmplx_conj_f32((float32_t *)pSrc, (float32_t *)pDst, numSamples) +#define mw_arm_cmplx_conj_q31(pSrc, pDst, numSamples) arm_cmplx_conj_q31((q31_t *)pSrc, (q31_t *)pDst, numSamples) +#define mw_arm_cmplx_conj_q15(pSrc, pDst, numSamples) arm_cmplx_conj_q15((q15_t *)pSrc, (q15_t *)pDst, numSamples) + +#define mw_arm_cmplx_mult_cmplx_f32(pSrcA, pSrcB, pDst, blockSize) arm_cmplx_mult_cmplx_f32((float32_t *)pSrcA, (float32_t *)pSrcB, (float32_t *)pDst, blockSize) +#define mw_arm_cmplx_mult_cmplx_q31(pSrcA, pSrcB, pDst, blockSize) arm_cmplx_mult_cmplx_q31((q31_t *)pSrcA, (q31_t *)pSrcB, (q31_t *)pDst, blockSize) +#define mw_arm_cmplx_mult_cmplx_q15(pSrcA, pSrcB, pDst, blockSize) arm_cmplx_mult_cmplx_q15((q15_t *)pSrcA, (q15_t *)pSrcB, (q15_t *)pDst, blockSize) + +#define mw_arm_cmplx_mult_real_f32(pSrcA, pSrcB, pDst, blockSize) arm_cmplx_mult_real_f32((float32_t *)pSrcA, (float32_t *)pSrcB, (float32_t *)pDst, blockSize) +#define mw_arm_cmplx_mult_real_q31(pSrcA, pSrcB, pDst, blockSize) arm_cmplx_mult_real_q31((q31_t *)pSrcA, (q31_t *)pSrcB, (q31_t *)pDst, blockSize) +#define mw_arm_cmplx_mult_real_q15(pSrcA, pSrcB, pDst, blockSize) arm_cmplx_mult_real_q15((q15_t *)pSrcA, (q15_t *)pSrcB, (q15_t *)pDst, blockSize) + +#define mw_arm_rshift_q15(pSrc, shiftBits, pDst, blockSize) arm_shift_q15 ((q15_t *)pSrc, -(shiftBits),(q15_t *)pDst, blockSize) +#define mw_arm_rshift_q31(pSrc, shiftBits, pDst, blockSize) arm_shift_q31 ((q31_t *)pSrc, -(shiftBits), (q31_t *)pDst, blockSize) +#define mw_arm_rshift_q7(pSrc, shiftBits, pDst, blockSize) arm_shift_q7 ((q7_t *)pSrc, -(shiftBits), (q7_t *)pDst, blockSize) + +#define mw_arm_shift_q15(pSrc, shiftBits, pDst, blockSize) arm_shift_q15 ((q15_t *)pSrc, shiftBits,(q15_t *)pDst, blockSize) +#define mw_arm_shift_q31(pSrc, shiftBits, pDst, blockSize) arm_shift_q31 ((q31_t *)pSrc, shiftBits, (q31_t *)pDst, blockSize) +#define mw_arm_shift_q7(pSrc, shiftBits, pDst, blockSize) arm_shift_q7 ((q7_t *)pSrc, shiftBits, (q7_t *)pDst, blockSize) + +#endif diff --git a/emBODY/eBcode/arch-arm/board/amc2c/application/src/model-based-design/sharedutils/rtGetInf.cpp b/emBODY/eBcode/arch-arm/board/amc2c/application/src/model-based-design/sharedutils/rtGetInf.cpp new file mode 100644 index 0000000000..193fa55fea --- /dev/null +++ b/emBODY/eBcode/arch-arm/board/amc2c/application/src/model-based-design/sharedutils/rtGetInf.cpp @@ -0,0 +1,111 @@ +// +// Non-Degree Granting Education License -- for use at non-degree +// granting, nonprofit, education, and research organizations only. Not +// for commercial or industrial use. +// +// File: rtGetInf.cpp +// +// Code generated for Simulink model 'control_foc'. +// +// Model version : 5.12 +// Simulink Coder version : 9.9 (R2023a) 19-Nov-2022 +// C/C++ source code generated on : Thu Oct 12 19:04:15 2023 +// +#include "rtwtypes.h" + +extern "C" +{ + +#include "rtGetInf.h" + +} + +#include + +extern "C" +{ + +#include "rt_nonfinite.h" + +} + +#define NumBitsPerChar 8U + +extern "C" +{ + // + // Initialize rtInf needed by the generated code. + // Inf is initialized as non-signaling. Assumes IEEE. + // + real_T rtGetInf(void) + { + size_t bitsPerReal = sizeof(real_T) * (NumBitsPerChar); + real_T inf = 0.0; + if (bitsPerReal == 32U) { + inf = rtGetInfF(); + } else { + union { + LittleEndianIEEEDouble bitVal; + real_T fltVal; + } tmpVal; + + tmpVal.bitVal.words.wordH = 0x7FF00000U; + tmpVal.bitVal.words.wordL = 0x00000000U; + inf = tmpVal.fltVal; + } + + return inf; + } + + // + // Initialize rtInfF needed by the generated code. + // Inf is initialized as non-signaling. Assumes IEEE. + // + real32_T rtGetInfF(void) + { + IEEESingle infF; + infF.wordL.wordLuint = 0x7F800000U; + return infF.wordL.wordLreal; + } + + // + // Initialize rtMinusInf needed by the generated code. + // Inf is initialized as non-signaling. Assumes IEEE. + // + real_T rtGetMinusInf(void) + { + size_t bitsPerReal = sizeof(real_T) * (NumBitsPerChar); + real_T minf = 0.0; + if (bitsPerReal == 32U) { + minf = rtGetMinusInfF(); + } else { + union { + LittleEndianIEEEDouble bitVal; + real_T fltVal; + } tmpVal; + + tmpVal.bitVal.words.wordH = 0xFFF00000U; + tmpVal.bitVal.words.wordL = 0x00000000U; + minf = tmpVal.fltVal; + } + + return minf; + } + + // + // Initialize rtMinusInfF needed by the generated code. + // Inf is initialized as non-signaling. Assumes IEEE. + // + real32_T rtGetMinusInfF(void) + { + IEEESingle minfF; + minfF.wordL.wordLuint = 0xFF800000U; + return minfF.wordL.wordLreal; + } +} + +// +// File trailer for generated code. +// +// [EOF] +// diff --git a/emBODY/eBcode/arch-arm/board/amc2c/application/src/model-based-design/sharedutils/rtGetInf.h b/emBODY/eBcode/arch-arm/board/amc2c/application/src/model-based-design/sharedutils/rtGetInf.h new file mode 100644 index 0000000000..f4b4e66b7d --- /dev/null +++ b/emBODY/eBcode/arch-arm/board/amc2c/application/src/model-based-design/sharedutils/rtGetInf.h @@ -0,0 +1,48 @@ +// +// Non-Degree Granting Education License -- for use at non-degree +// granting, nonprofit, education, and research organizations only. Not +// for commercial or industrial use. +// +// File: rtGetInf.h +// +// Code generated for Simulink model 'control_foc'. +// +// Model version : 5.12 +// Simulink Coder version : 9.9 (R2023a) 19-Nov-2022 +// C/C++ source code generated on : Thu Oct 12 19:04:15 2023 +// +#ifndef RTW_HEADER_rtGetInf_h_ +#define RTW_HEADER_rtGetInf_h_ + +extern "C" +{ + +#include "rt_nonfinite.h" + +} + +#include "rtwtypes.h" +#ifdef __cplusplus + +extern "C" +{ + +#endif + + extern real_T rtGetInf(void); + extern real32_T rtGetInfF(void); + extern real_T rtGetMinusInf(void); + extern real32_T rtGetMinusInfF(void); + +#ifdef __cplusplus + +} // extern "C" + +#endif +#endif // RTW_HEADER_rtGetInf_h_ + +// +// File trailer for generated code. +// +// [EOF] +// diff --git a/emBODY/eBcode/arch-arm/board/amc2c/application/src/model-based-design/sharedutils/rtGetNaN.cpp b/emBODY/eBcode/arch-arm/board/amc2c/application/src/model-based-design/sharedutils/rtGetNaN.cpp new file mode 100644 index 0000000000..1cbc65d5be --- /dev/null +++ b/emBODY/eBcode/arch-arm/board/amc2c/application/src/model-based-design/sharedutils/rtGetNaN.cpp @@ -0,0 +1,77 @@ +// +// Non-Degree Granting Education License -- for use at non-degree +// granting, nonprofit, education, and research organizations only. Not +// for commercial or industrial use. +// +// File: rtGetNaN.cpp +// +// Code generated for Simulink model 'control_foc'. +// +// Model version : 5.12 +// Simulink Coder version : 9.9 (R2023a) 19-Nov-2022 +// C/C++ source code generated on : Thu Oct 12 19:04:15 2023 +// +#include "rtwtypes.h" + +extern "C" +{ + +#include "rtGetNaN.h" + +} + +#include + +extern "C" +{ + +#include "rt_nonfinite.h" + +} + +#define NumBitsPerChar 8U + +extern "C" +{ + // + // Initialize rtNaN needed by the generated code. + // NaN is initialized as non-signaling. Assumes IEEE. + // + real_T rtGetNaN(void) + { + size_t bitsPerReal = sizeof(real_T) * (NumBitsPerChar); + real_T nan = 0.0; + if (bitsPerReal == 32U) { + nan = rtGetNaNF(); + } else { + union { + LittleEndianIEEEDouble bitVal; + real_T fltVal; + } tmpVal; + + tmpVal.bitVal.words.wordH = 0xFFF80000U; + tmpVal.bitVal.words.wordL = 0x00000000U; + nan = tmpVal.fltVal; + } + + return nan; + } + + // + // Initialize rtNaNF needed by the generated code. + // NaN is initialized as non-signaling. Assumes IEEE. + // + real32_T rtGetNaNF(void) + { + IEEESingle nanF = { { 0.0F } }; + + nanF.wordL.wordLuint = 0xFFC00000U; + return nanF.wordL.wordLreal; + } +} + +// +// File trailer for generated code. +// +// [EOF] +// diff --git a/emBODY/eBcode/arch-arm/board/amc2c/application/src/model-based-design/sharedutils/rtGetNaN.h b/emBODY/eBcode/arch-arm/board/amc2c/application/src/model-based-design/sharedutils/rtGetNaN.h new file mode 100644 index 0000000000..e5977f8ddf --- /dev/null +++ b/emBODY/eBcode/arch-arm/board/amc2c/application/src/model-based-design/sharedutils/rtGetNaN.h @@ -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: rtGetNaN.h +// +// Code generated for Simulink model 'control_foc'. +// +// Model version : 5.12 +// Simulink Coder version : 9.9 (R2023a) 19-Nov-2022 +// C/C++ source code generated on : Thu Oct 12 19:04:15 2023 +// +#ifndef RTW_HEADER_rtGetNaN_h_ +#define RTW_HEADER_rtGetNaN_h_ + +extern "C" +{ + +#include "rt_nonfinite.h" + +} + +#include "rtwtypes.h" +#ifdef __cplusplus + +extern "C" +{ + +#endif + + extern real_T rtGetNaN(void); + extern real32_T rtGetNaNF(void); + +#ifdef __cplusplus + +} // extern "C" + +#endif +#endif // RTW_HEADER_rtGetNaN_h_ + +// +// File trailer for generated code. +// +// [EOF] +// diff --git a/emBODY/eBcode/arch-arm/board/amc2c/application/src/model-based-design/sharedutils/rt_hypotf_snf.cpp b/emBODY/eBcode/arch-arm/board/amc2c/application/src/model-based-design/sharedutils/rt_hypotf_snf.cpp new file mode 100644 index 0000000000..f9ea2b7bf9 --- /dev/null +++ b/emBODY/eBcode/arch-arm/board/amc2c/application/src/model-based-design/sharedutils/rt_hypotf_snf.cpp @@ -0,0 +1,56 @@ +// +// 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_snf.cpp +// +// Code generated for Simulink model 'estimation_velocity'. +// +// Model version : 5.1 +// Simulink Coder version : 9.9 (R2023a) 19-Nov-2022 +// C/C++ source code generated on : Thu Oct 12 19:04:35 2023 +// +#include "rtwtypes.h" +#include "rt_hypotf_snf.h" +#include + +extern "C" +{ + +#include "rt_nonfinite.h" + +} + +#include "mw_cmsis.h" + +real32_T rt_hypotf_snf(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 if (rtIsNaNF(b)) { + y = (rtNaNF); + } else { + y = a * 1.41421354F; + } + + return y; +} + +// +// File trailer for generated code. +// +// [EOF] +// diff --git a/emBODY/eBcode/arch-arm/board/amc2c/application/src/model-based-design/sharedutils/rt_hypotf_snf.h b/emBODY/eBcode/arch-arm/board/amc2c/application/src/model-based-design/sharedutils/rt_hypotf_snf.h new file mode 100644 index 0000000000..9d642a440b --- /dev/null +++ b/emBODY/eBcode/arch-arm/board/amc2c/application/src/model-based-design/sharedutils/rt_hypotf_snf.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_snf.h +// +// Code generated for Simulink model 'estimation_velocity'. +// +// Model version : 5.1 +// Simulink Coder version : 9.9 (R2023a) 19-Nov-2022 +// C/C++ source code generated on : Thu Oct 12 19:04:35 2023 +// +#ifndef RTW_HEADER_rt_hypotf_snf_h_ +#define RTW_HEADER_rt_hypotf_snf_h_ +#include "rtwtypes.h" + +extern real32_T rt_hypotf_snf(real32_T u0, real32_T u1); + +#endif // RTW_HEADER_rt_hypotf_snf_h_ + +// +// File trailer for generated code. +// +// [EOF] +// diff --git a/emBODY/eBcode/arch-arm/board/amc2c/application/src/model-based-design/sharedutils/rt_nonfinite.cpp b/emBODY/eBcode/arch-arm/board/amc2c/application/src/model-based-design/sharedutils/rt_nonfinite.cpp new file mode 100644 index 0000000000..c645b927d6 --- /dev/null +++ b/emBODY/eBcode/arch-arm/board/amc2c/application/src/model-based-design/sharedutils/rt_nonfinite.cpp @@ -0,0 +1,116 @@ +// +// 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_nonfinite.cpp +// +// Code generated for Simulink model 'control_foc'. +// +// Model version : 5.12 +// Simulink Coder version : 9.9 (R2023a) 19-Nov-2022 +// C/C++ source code generated on : Thu Oct 12 19:04:15 2023 +// +extern "C" +{ + +#include "rtGetNaN.h" + +} + +extern "C" +{ + +#include "rtGetInf.h" + +} + +#include +#include "rtwtypes.h" + +extern "C" +{ + +#include "rt_nonfinite.h" + +} + +#define NumBitsPerChar 8U + +extern "C" +{ + real_T rtInf; + real_T rtMinusInf; + real_T rtNaN; + real32_T rtInfF; + real32_T rtMinusInfF; + real32_T rtNaNF; +} + +extern "C" +{ + // + // Initialize the rtInf, rtMinusInf, and rtNaN needed by the + // generated code. NaN is initialized as non-signaling. Assumes IEEE. + // + void rt_InitInfAndNaN(size_t realSize) + { + (void) (realSize); + rtNaN = rtGetNaN(); + rtNaNF = rtGetNaNF(); + rtInf = rtGetInf(); + rtInfF = rtGetInfF(); + rtMinusInf = rtGetMinusInf(); + rtMinusInfF = rtGetMinusInfF(); + } + + // Test if value is infinite + boolean_T rtIsInf(real_T value) + { + return (boolean_T)((value==rtInf || value==rtMinusInf) ? 1U : 0U); + } + + // Test if single-precision value is infinite + boolean_T rtIsInfF(real32_T value) + { + return (boolean_T)(((value)==rtInfF || (value)==rtMinusInfF) ? 1U : 0U); + } + + // Test if value is not a number + boolean_T rtIsNaN(real_T value) + { + boolean_T result = (boolean_T) 0; + size_t bitsPerReal = sizeof(real_T) * (NumBitsPerChar); + if (bitsPerReal == 32U) { + result = rtIsNaNF((real32_T)value); + } else { + union { + LittleEndianIEEEDouble bitVal; + real_T fltVal; + } tmpVal; + + tmpVal.fltVal = value; + result = (boolean_T)((tmpVal.bitVal.words.wordH & 0x7FF00000) == + 0x7FF00000 && + ( (tmpVal.bitVal.words.wordH & 0x000FFFFF) != 0 || + (tmpVal.bitVal.words.wordL != 0) )); + } + + return result; + } + + // Test if single-precision value is not a number + boolean_T rtIsNaNF(real32_T value) + { + IEEESingle tmp; + tmp.wordL.wordLreal = value; + return (boolean_T)( (tmp.wordL.wordLuint & 0x7F800000) == 0x7F800000 && + (tmp.wordL.wordLuint & 0x007FFFFF) != 0 ); + } +} + +// +// File trailer for generated code. +// +// [EOF] +// diff --git a/emBODY/eBcode/arch-arm/board/amc2c/application/src/model-based-design/sharedutils/rt_nonfinite.h b/emBODY/eBcode/arch-arm/board/amc2c/application/src/model-based-design/sharedutils/rt_nonfinite.h new file mode 100644 index 0000000000..d8c148ffca --- /dev/null +++ b/emBODY/eBcode/arch-arm/board/amc2c/application/src/model-based-design/sharedutils/rt_nonfinite.h @@ -0,0 +1,69 @@ +// +// 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_nonfinite.h +// +// Code generated for Simulink model 'control_foc'. +// +// Model version : 5.12 +// Simulink Coder version : 9.9 (R2023a) 19-Nov-2022 +// C/C++ source code generated on : Thu Oct 12 19:04:15 2023 +// +#ifndef RTW_HEADER_rt_nonfinite_h_ +#define RTW_HEADER_rt_nonfinite_h_ +#include +#include "rtwtypes.h" +#define NOT_USING_NONFINITE_LITERALS 1 +#ifdef __cplusplus + +extern "C" +{ + +#endif + + extern real_T rtInf; + extern real_T rtMinusInf; + extern real_T rtNaN; + extern real32_T rtInfF; + extern real32_T rtMinusInfF; + extern real32_T rtNaNF; + extern void rt_InitInfAndNaN(size_t realSize); + extern boolean_T rtIsInf(real_T value); + extern boolean_T rtIsInfF(real32_T value); + extern boolean_T rtIsNaN(real_T value); + extern boolean_T rtIsNaNF(real32_T value); + struct BigEndianIEEEDouble { + struct { + uint32_T wordH; + uint32_T wordL; + } words; + }; + + struct LittleEndianIEEEDouble { + struct { + uint32_T wordL; + uint32_T wordH; + } words; + }; + + struct IEEESingle { + union { + real32_T wordLreal; + uint32_T wordLuint; + } wordL; + }; + +#ifdef __cplusplus + +} // extern "C" + +#endif +#endif // RTW_HEADER_rt_nonfinite_h_ + +// +// File trailer for generated code. +// +// [EOF] +// diff --git a/emBODY/eBcode/arch-arm/board/amc2c/application/src/model-based-design/sharedutils/rt_roundd_snf.cpp b/emBODY/eBcode/arch-arm/board/amc2c/application/src/model-based-design/sharedutils/rt_roundd_snf.cpp new file mode 100644 index 0000000000..e3a2580eee --- /dev/null +++ b/emBODY/eBcode/arch-arm/board/amc2c/application/src/model-based-design/sharedutils/rt_roundd_snf.cpp @@ -0,0 +1,40 @@ +// +// 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_roundd_snf.cpp +// +// Code generated for Simulink model 'SupervisorFSM_RX'. +// +// Model version : 6.4 +// Simulink Coder version : 9.9 (R2023a) 19-Nov-2022 +// C/C++ source code generated on : Thu Oct 12 19:03:17 2023 +// +#include "rtwtypes.h" +#include "rt_roundd_snf.h" +#include + +real_T rt_roundd_snf(real_T u) +{ + real_T y; + if (std::abs(u) < 4.503599627370496E+15) { + if (u >= 0.5) { + y = std::floor(u + 0.5); + } else if (u > -0.5) { + y = u * 0.0; + } else { + y = std::ceil(u - 0.5); + } + } else { + y = u; + } + + return y; +} + +// +// File trailer for generated code. +// +// [EOF] +// diff --git a/emBODY/eBcode/arch-arm/board/amc2c/application/src/model-based-design/sharedutils/rt_roundd_snf.h b/emBODY/eBcode/arch-arm/board/amc2c/application/src/model-based-design/sharedutils/rt_roundd_snf.h new file mode 100644 index 0000000000..2ada1193b1 --- /dev/null +++ b/emBODY/eBcode/arch-arm/board/amc2c/application/src/model-based-design/sharedutils/rt_roundd_snf.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_roundd_snf.h +// +// Code generated for Simulink model 'SupervisorFSM_RX'. +// +// Model version : 6.4 +// Simulink Coder version : 9.9 (R2023a) 19-Nov-2022 +// C/C++ source code generated on : Thu Oct 12 19:03:17 2023 +// +#ifndef RTW_HEADER_rt_roundd_snf_h_ +#define RTW_HEADER_rt_roundd_snf_h_ +#include "rtwtypes.h" + +extern real_T rt_roundd_snf(real_T u); + +#endif // RTW_HEADER_rt_roundd_snf_h_ + +// +// File trailer for generated code. +// +// [EOF] +// diff --git a/emBODY/eBcode/arch-arm/board/amc2c/application/src/model-based-design/sharedutils/rtw_defines.h b/emBODY/eBcode/arch-arm/board/amc2c/application/src/model-based-design/sharedutils/rtw_defines.h new file mode 100644 index 0000000000..afa803fc57 --- /dev/null +++ b/emBODY/eBcode/arch-arm/board/amc2c/application/src/model-based-design/sharedutils/rtw_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: rtw_defines.h +// +// Code generated for Simulink model 'SupervisorFSM_RX'. +// +// Model version : 6.4 +// Simulink Coder version : 9.9 (R2023a) 19-Nov-2022 +// C/C++ source code generated on : Thu Oct 12 19:03:17 2023 +// + +#ifndef RTW_HEADER_rtw_defines_h_ +#define RTW_HEADER_rtw_defines_h_ +#include "rtwtypes.h" + +// Exported data define +// Definition for custom storage class: Define +#define CAN_MAX_NUM_PACKETS 4 // Maximum number of TX/RX packets handled per time instance. +#endif // RTW_HEADER_rtw_defines_h_ + +// +// File trailer for generated code. +// +// [EOF] +// diff --git a/emBODY/eBcode/arch-arm/board/amc2c/application/src/model-based-design/sharedutils/rtw_enable_disable_motors.c b/emBODY/eBcode/arch-arm/board/amc2c/application/src/model-based-design/sharedutils/rtw_enable_disable_motors.c new file mode 100644 index 0000000000..a54169b84b --- /dev/null +++ b/emBODY/eBcode/arch-arm/board/amc2c/application/src/model-based-design/sharedutils/rtw_enable_disable_motors.c @@ -0,0 +1,52 @@ +/* + * Copyright (C) 2021 iCub Tech Facility - Istituto Italiano di Tecnologia + * Author: German David Rodriguez Arenas + * email: german.rodriguez@iit.it + * Permission is granted to copy, distribute, and/or modify this program + * under the terms of the GNU General Public License, version 2 or any + * later version published by the Free Software Foundation. + * + * A copy of the license can be found at + * http://www.robotcub.org/icub/license/gpl.txt + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General + * Public License for more details +*/ + +#include "rtw_enable_disable_motors.h" + + + +#if defined(STM32HAL_BOARD_AMCBLDC) || defined(STM32HAL_BOARD_AMC2C) + +#ifndef __cplusplus + #error this file must be compiled in C++ mode when deployed on the target +#endif + +#include "embot_hw_motor.h" + +#else + #warning: choose a STM32HAL_BOARD_* +#endif + +void rtw_enableMotor() +{ +#if defined(STM32HAL_BOARD_AMCBLDC) || defined(STM32HAL_BOARD_AMC2C) + embot::hw::motor::enable(embot::hw::MOTOR::one, true); +#else + #warning: choose a STM32HAL_BOARD_* +#endif +} + +void rtw_disableMotor() +{ +#if defined(STM32HAL_BOARD_AMCBLDC) || defined(STM32HAL_BOARD_AMC2C) + embot::hw::motor::enable(embot::hw::MOTOR::one, false); +#else + #warning: choose a STM32HAL_BOARD_* +#endif +} + +// - end-of-file (leave a blank line after)---------------------------------------------------------------------------- diff --git a/emBODY/eBcode/arch-arm/board/amc2c/application/src/model-based-design/sharedutils/rtw_enable_disable_motors.h b/emBODY/eBcode/arch-arm/board/amc2c/application/src/model-based-design/sharedutils/rtw_enable_disable_motors.h new file mode 100644 index 0000000000..98a7abfb5c --- /dev/null +++ b/emBODY/eBcode/arch-arm/board/amc2c/application/src/model-based-design/sharedutils/rtw_enable_disable_motors.h @@ -0,0 +1,33 @@ +/* + * Copyright (C) 2021 iCub Tech Facility - Istituto Italiano di Tecnologia + * Author: German David Rodriguez Arenas + * email: german.rodriguez@iit.it + * Permission is granted to copy, distribute, and/or modify this program + * under the terms of the GNU General Public License, version 2 or any + * later version published by the Free Software Foundation. + * + * A copy of the license can be found at + * http://www.robotcub.org/icub/license/gpl.txt + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General + * Public License for more details +*/ + +#ifndef RTW_ENABLE_DISABLE_MOTORS +#define RTW_ENABLE_DISABLE_MOTORS + +#ifdef __cplusplus + extern "C" { +#endif /* __cplusplus */ + +void rtw_enableMotor(); + +void rtw_disableMotor(); + +#ifdef __cplusplus + } /* extern "C" */ +#endif /* __cplusplus */ + +#endif /* RTW_ENABLE_DISABLE_MOTORS */ \ No newline at end of file diff --git a/emBODY/eBcode/arch-arm/board/amc2c/application/src/model-based-design/sharedutils/rtw_motor_config.c b/emBODY/eBcode/arch-arm/board/amc2c/application/src/model-based-design/sharedutils/rtw_motor_config.c new file mode 100644 index 0000000000..4f3a091a2d --- /dev/null +++ b/emBODY/eBcode/arch-arm/board/amc2c/application/src/model-based-design/sharedutils/rtw_motor_config.c @@ -0,0 +1,58 @@ +/* + * Copyright (C) 2021 iCub Tech Facility - Istituto Italiano di Tecnologia + * Author: Valentina Gaggero + * email: valentina.gaggero@iit.it + * Permission is granted to copy, distribute, and/or modify this program + * under the terms of the GNU General Public License, version 2 or any + * later version published by the Free Software Foundation. + * + * A copy of the license can be found at + * http://www.robotcub.org/icub/license/gpl.txt + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General + * Public License for more details +*/ + +#include "rtw_motor_config.h" + +#if defined(STM32HAL_BOARD_AMCBLDC) || defined(STM32HAL_BOARD_AMC2C) + +#ifndef __cplusplus + #error this file must be compiled in C++ mode when deployed on the target +#endif + +#include "embot_hw_motor.h" + +#else + #warning: choose a STM32HAL_BOARD_* +#endif + +void rtw_configMotor(uint8_t has_quad_enc, int16_t rotor_enc_resolution, uint8_t pole_pairs, uint8_t has_hall_sens, uint8_t swapBC, uint16_t hall_sens_offset) +{ +#if defined(STM32HAL_BOARD_AMCBLDC) || defined(STM32HAL_BOARD_AMC2C) + +// embot::hw::motor::config( +// embot::hw::MOTOR::one, +// has_quad_enc, +// rotor_enc_resolution, +// pole_pairs, +// has_hall_sens, +// swapBC, // it was 1, but better use its param +// hall_sens_offset); // it was 120*65536/360, but better using its param +// // because: supervisor calls rtw_configMotor( , swapBC = 1, hall_sens_offset = 21845) and 120*65536/360 = 21845 + + // marco.accame: but now we use embot::hw::motor::Config + const embot::hw::motor::Config cfg { + rotor_enc_resolution, pole_pairs, hall_sens_offset, + (0 == has_quad_enc) ? false : true, + (0 == has_hall_sens) ? false : true, + (0 == swapBC) ? false : true + }; + embot::hw::motor::configure(embot::hw::MOTOR::one, cfg); + +#else + #warning: choose a STM32HAL_BOARD_* +#endif +} \ No newline at end of file diff --git a/emBODY/eBcode/arch-arm/board/amc2c/application/src/model-based-design/sharedutils/rtw_motor_config.h b/emBODY/eBcode/arch-arm/board/amc2c/application/src/model-based-design/sharedutils/rtw_motor_config.h new file mode 100644 index 0000000000..f7d9094710 --- /dev/null +++ b/emBODY/eBcode/arch-arm/board/amc2c/application/src/model-based-design/sharedutils/rtw_motor_config.h @@ -0,0 +1,36 @@ +/* + * Copyright (C) 2021 iCub Tech Facility - Istituto Italiano di Tecnologia + * Author: Valentina Gaggero + * email: valentina.gaggero@iit.it + * Permission is granted to copy, distribute, and/or modify this program + * under the terms of the GNU General Public License, version 2 or any + * later version published by the Free Software Foundation. + * + * A copy of the license can be found at + * http://www.robotcub.org/icub/license/gpl.txt + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General + * Public License for more details +*/ + +#ifndef RTW_MOTOR_CONFIG_MOTORS +#define RTW_MOTOR_CONFIG_MOTORS + +#ifdef __cplusplus + extern "C" { +#endif /* __cplusplus */ + +#include "stdint.h" + +/*inizialize the motor encoder using the data read in the configuration files (yarprobotinterface) +swapBC = 1 +hall_sens_offset = 120 *65536/360 ; icubDeg*/ +void rtw_configMotor(uint8_t has_quad_enc, int16_t rotor_enc_resolution, uint8_t pole_pairs, uint8_t has_hall_sens, uint8_t swapBC, uint16_t hall_sens_offset); + +#ifdef __cplusplus + } /* extern "C" */ +#endif /* __cplusplus */ + +#endif /* RTW_MOTOR_CONFIG_MOTORS */ \ No newline at end of file diff --git a/emBODY/eBcode/arch-arm/board/amc2c/application/src/model-based-design/sharedutils/rtw_mutex.h b/emBODY/eBcode/arch-arm/board/amc2c/application/src/model-based-design/sharedutils/rtw_mutex.h new file mode 100644 index 0000000000..7b04ad7727 --- /dev/null +++ b/emBODY/eBcode/arch-arm/board/amc2c/application/src/model-based-design/sharedutils/rtw_mutex.h @@ -0,0 +1,61 @@ +/* + * Copyright (C) 2021 iCub Tech Facility - Istituto Italiano di Tecnologia + * Author: Ugo Pattacini + * email: ugo.pattacini@iit.it + * Permission is granted to copy, distribute, and/or modify this program + * under the terms of the GNU General Public License, version 2 or any + * later version published by the Free Software Foundation. + * + * A copy of the license can be found at + * http://www.robotcub.org/icub/license/gpl.txt + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General + * Public License for more details +*/ + +#ifndef RTW_MUTEX_H +#define RTW_MUTEX_H + +#include "stm32hal.h" + +#define rtw_mutex_init() +#define rtw_mutex_destroy() + +inline bool IsException(void) +{ + return(__get_IPSR() != 0U); +} + +inline void rtw_mutex_lock(void) +{ + if(false == IsException()) + { +#if defined(STM32HAL_BOARD_AMCBLDC) + NVIC_DisableIRQ(DMA1_Channel2_IRQn); +#elif defined(STM32HAL_BOARD_AMC2C) + NVIC_DisableIRQ(DMA2_Stream0_IRQn); +#else + #warning: choose a STM32HAL_BOARD_* +#endif + } +} + +inline void rtw_mutex_unlock(void) +{ + if(false == IsException()) + { +#if defined(STM32HAL_BOARD_AMCBLDC) + NVIC_EnableIRQ(DMA1_Channel2_IRQn); +#elif defined(STM32HAL_BOARD_AMC2C) + NVIC_EnableIRQ(DMA2_Stream0_IRQn); +#else + #warning: choose a STM32HAL_BOARD_* +#endif + } +} + +#endif // RTW_MUTEX_H + +// - end-of-file (leave a blank line after)---------------------------------------------------------------------------- diff --git a/emBODY/eBcode/arch-arm/board/amc2c/application/src/model-based-design/sharedutils/rtwtypes.h b/emBODY/eBcode/arch-arm/board/amc2c/application/src/model-based-design/sharedutils/rtwtypes.h new file mode 100644 index 0000000000..70f3d64acb --- /dev/null +++ b/emBODY/eBcode/arch-arm/board/amc2c/application/src/model-based-design/sharedutils/rtwtypes.h @@ -0,0 +1,154 @@ +// +// Non-Degree Granting Education License -- for use at non-degree +// granting, nonprofit, education, and research organizations only. Not +// for commercial or industrial use. +// +// File: rtwtypes.h +// +// Code generated for Simulink model 'SupervisorFSM_RX'. +// +// Model version : 6.4 +// Simulink Coder version : 9.9 (R2023a) 19-Nov-2022 +// C/C++ source code generated on : Thu Oct 12 19:03:17 2023 +// +#ifndef RTWTYPES_H +#define RTWTYPES_H + +// Logical type definitions +#if (!defined(__cplusplus)) +#ifndef false +#define false (0U) +#endif + +#ifndef true +#define true (1U) +#endif +#endif + +//=======================================================================* +// Target hardware information +// Device type: ARM Compatible->ARM Cortex-M +// Number of bits: char: 8 short: 16 int: 32 +// long: 32 +// native word size: 32 +// Byte ordering: LittleEndian +// Signed integer division rounds to: Zero +// Shift right on a signed integer as arithmetic shift: on +// ======================================================================= + +//=======================================================================* +// Fixed width word size data types: * +// int8_T, int16_T, int32_T - signed 8, 16, or 32 bit integers * +// uint8_T, uint16_T, uint32_T - unsigned 8, 16, or 32 bit integers * +// real32_T, real64_T - 32 and 64 bit floating point numbers * +// ======================================================================= +typedef signed char int8_T; +typedef unsigned char uint8_T; +typedef short int16_T; +typedef unsigned short uint16_T; +typedef int int32_T; +typedef unsigned int uint32_T; +typedef float real32_T; +typedef double real64_T; + +//===========================================================================* +// Generic type definitions: boolean_T, char_T, byte_T, int_T, uint_T, * +// real_T, time_T, ulong_T. * +// =========================================================================== +typedef double real_T; +typedef double time_T; +typedef unsigned char boolean_T; +typedef int int_T; +typedef unsigned int uint_T; +typedef unsigned long ulong_T; +typedef char char_T; +typedef unsigned char uchar_T; +typedef char_T byte_T; + +//===========================================================================* +// Complex number type definitions * +// =========================================================================== +#define CREAL_T + +typedef struct { + real32_T re; + real32_T im; +} creal32_T; + +typedef struct { + real64_T re; + real64_T im; +} creal64_T; + +typedef struct { + real_T re; + real_T im; +} creal_T; + +#define CINT8_T + +typedef struct { + int8_T re; + int8_T im; +} cint8_T; + +#define CUINT8_T + +typedef struct { + uint8_T re; + uint8_T im; +} cuint8_T; + +#define CINT16_T + +typedef struct { + int16_T re; + int16_T im; +} cint16_T; + +#define CUINT16_T + +typedef struct { + uint16_T re; + uint16_T im; +} cuint16_T; + +#define CINT32_T + +typedef struct { + int32_T re; + int32_T im; +} cint32_T; + +#define CUINT32_T + +typedef struct { + uint32_T re; + uint32_T im; +} cuint32_T; + +//=======================================================================* +// Min and Max: * +// int8_T, int16_T, int32_T - signed 8, 16, or 32 bit integers * +// uint8_T, uint16_T, uint32_T - unsigned 8, 16, or 32 bit integers * +// ======================================================================= +#define MAX_int8_T ((int8_T)(127)) +#define MIN_int8_T ((int8_T)(-128)) +#define MAX_uint8_T ((uint8_T)(255U)) +#define MAX_int16_T ((int16_T)(32767)) +#define MIN_int16_T ((int16_T)(-32768)) +#define MAX_uint16_T ((uint16_T)(65535U)) +#define MAX_int32_T ((int32_T)(2147483647)) +#define MIN_int32_T ((int32_T)(-2147483647-1)) +#define MAX_uint32_T ((uint32_T)(0xFFFFFFFFU)) + +// Block D-Work pointer type +typedef void * pointer_T; + +#endif // RTWTYPES_H + +// +// File trailer for generated code. +// +// [EOF] +// diff --git a/emBODY/eBcode/arch-arm/board/amc2c/application/src/model-based-design/sharedutils/uMultiWord2Double.cpp b/emBODY/eBcode/arch-arm/board/amc2c/application/src/model-based-design/sharedutils/uMultiWord2Double.cpp new file mode 100644 index 0000000000..4723380c4b --- /dev/null +++ b/emBODY/eBcode/arch-arm/board/amc2c/application/src/model-based-design/sharedutils/uMultiWord2Double.cpp @@ -0,0 +1,36 @@ +// +// Non-Degree Granting Education License -- for use at non-degree +// granting, nonprofit, education, and research organizations only. Not +// for commercial or industrial use. +// +// File: uMultiWord2Double.cpp +// +// Code generated for Simulink model 'SupervisorFSM_RX'. +// +// Model version : 4.6 +// Simulink Coder version : 9.7 (R2022a) 13-Nov-2021 +// C/C++ source code generated on : Tue Jun 7 15:41:38 2022 +// +#include "uMultiWord2Double.h" +#include +#include "rtwtypes.h" + +real_T uMultiWord2Double(const uint32_T u1[], int32_T n1, int32_T e1) +{ + real_T y; + int32_T exp_0; + y = 0.0; + exp_0 = e1; + for (int32_T i = 0; i < n1; i++) { + y += std::ldexp(static_cast(u1[i]), exp_0); + exp_0 += 32; + } + + return y; +} + +// +// File trailer for generated code. +// +// [EOF] +// diff --git a/emBODY/eBcode/arch-arm/board/amc2c/application/src/model-based-design/sharedutils/uMultiWord2Double.h b/emBODY/eBcode/arch-arm/board/amc2c/application/src/model-based-design/sharedutils/uMultiWord2Double.h new file mode 100644 index 0000000000..5bfd3a8801 --- /dev/null +++ b/emBODY/eBcode/arch-arm/board/amc2c/application/src/model-based-design/sharedutils/uMultiWord2Double.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: uMultiWord2Double.h +// +// Code generated for Simulink model 'SupervisorFSM_RX'. +// +// Model version : 4.6 +// Simulink Coder version : 9.7 (R2022a) 13-Nov-2021 +// C/C++ source code generated on : Tue Jun 7 15:41:38 2022 +// +#ifndef RTW_HEADER_uMultiWord2Double_h_ +#define RTW_HEADER_uMultiWord2Double_h_ +#include "rtwtypes.h" + +extern real_T uMultiWord2Double(const uint32_T u1[], int32_T n1, int32_T e1); + +#endif // RTW_HEADER_uMultiWord2Double_h_ + +// +// File trailer for generated code. +// +// [EOF] +// diff --git a/emBODY/eBcode/arch-arm/board/amc2c/application/src/model-based-design/sharedutils/uMultiWordShl.cpp b/emBODY/eBcode/arch-arm/board/amc2c/application/src/model-based-design/sharedutils/uMultiWordShl.cpp new file mode 100644 index 0000000000..deabb16dcf --- /dev/null +++ b/emBODY/eBcode/arch-arm/board/amc2c/application/src/model-based-design/sharedutils/uMultiWordShl.cpp @@ -0,0 +1,73 @@ +// +// Non-Degree Granting Education License -- for use at non-degree +// granting, nonprofit, education, and research organizations only. Not +// for commercial or industrial use. +// +// File: uMultiWordShl.cpp +// +// Code generated for Simulink model 'SupervisorFSM_RX'. +// +// Model version : 4.6 +// Simulink Coder version : 9.7 (R2022a) 13-Nov-2021 +// C/C++ source code generated on : Tue Jun 7 15:41:38 2022 +// +#include "uMultiWordShl.h" +#include "rtwtypes.h" + +void uMultiWordShl(const uint32_T u1[], int32_T n1, uint32_T n2, uint32_T y[], + int32_T n) +{ + int32_T i; + int32_T nb; + int32_T nc; + uint32_T u1i; + uint32_T ys; + nb = static_cast(n2 >> 5); + ys = (u1[n1 - 1] & 2147483648U) != 0U ? MAX_uint32_T : 0U; + nc = nb > n ? n : nb; + u1i = 0U; + for (i = 0; i < nc; i++) { + y[i] = 0U; + } + + if (nb < n) { + uint32_T nl; + nl = n2 - (static_cast(nb) << 5); + nb += n1; + if (nb > n) { + nb = n; + } + + nb -= i; + if (nl > 0U) { + for (nc = 0; nc < nb; nc++) { + uint32_T yi; + yi = u1i >> (32U - nl); + u1i = u1[nc]; + y[i] = u1i << nl | yi; + i++; + } + + if (i < n) { + y[i] = u1i >> (32U - nl) | ys << nl; + i++; + } + } else { + for (nc = 0; nc < nb; nc++) { + y[i] = u1[nc]; + i++; + } + } + } + + while (i < n) { + y[i] = ys; + i++; + } +} + +// +// File trailer for generated code. +// +// [EOF] +// diff --git a/emBODY/eBcode/arch-arm/board/amc2c/application/src/model-based-design/sharedutils/uMultiWordShl.h b/emBODY/eBcode/arch-arm/board/amc2c/application/src/model-based-design/sharedutils/uMultiWordShl.h new file mode 100644 index 0000000000..c37431318b --- /dev/null +++ b/emBODY/eBcode/arch-arm/board/amc2c/application/src/model-based-design/sharedutils/uMultiWordShl.h @@ -0,0 +1,27 @@ +// +// Non-Degree Granting Education License -- for use at non-degree +// granting, nonprofit, education, and research organizations only. Not +// for commercial or industrial use. +// +// File: uMultiWordShl.h +// +// Code generated for Simulink model 'SupervisorFSM_RX'. +// +// Model version : 4.6 +// Simulink Coder version : 9.7 (R2022a) 13-Nov-2021 +// C/C++ source code generated on : Tue Jun 7 15:41:38 2022 +// +#ifndef RTW_HEADER_uMultiWordShl_h_ +#define RTW_HEADER_uMultiWordShl_h_ +#include "rtwtypes.h" + +extern void uMultiWordShl(const uint32_T u1[], int32_T n1, uint32_T n2, uint32_T + y[], int32_T n); + +#endif // RTW_HEADER_uMultiWordShl_h_ + +// +// File trailer for generated code. +// +// [EOF] +// diff --git a/emBODY/eBcode/arch-arm/board/amc2c/application/src/model-based-design/sharedutils/zero_crossing_types.h b/emBODY/eBcode/arch-arm/board/amc2c/application/src/model-based-design/sharedutils/zero_crossing_types.h new file mode 100644 index 0000000000..c8659f419a --- /dev/null +++ b/emBODY/eBcode/arch-arm/board/amc2c/application/src/model-based-design/sharedutils/zero_crossing_types.h @@ -0,0 +1,44 @@ +// +// Non-Degree Granting Education License -- for use at non-degree +// granting, nonprofit, education, and research organizations only. Not +// for commercial or industrial use. +// +// File: zero_crossing_types.h +// +// Code generated for Simulink model 'control_foc'. +// +// Model version : 5.12 +// Simulink Coder version : 9.9 (R2023a) 19-Nov-2022 +// C/C++ source code generated on : Thu Oct 12 19:04:15 2023 +// +#ifndef ZERO_CROSSING_TYPES_H +#define ZERO_CROSSING_TYPES_H +#include "rtwtypes.h" + +// Trigger directions: falling, either, and rising +typedef enum { + FALLING_ZERO_CROSSING = -1, + ANY_ZERO_CROSSING = 0, + RISING_ZERO_CROSSING = 1 +} ZCDirection; + +// Previous state of a trigger signal +typedef uint8_T ZCSigState; + +// Initial value of a trigger zero crossing signal +#define UNINITIALIZED_ZCSIG 0x03U +#define NEG_ZCSIG 0x02U +#define POS_ZCSIG 0x01U +#define ZERO_ZCSIG 0x00U + +// Current state of a trigger signal +typedef enum { FALLING_ZCEVENT = -1, NO_ZCEVENT = 0, RISING_ZCEVENT = 1 } + ZCEventType; + +#endif // ZERO_CROSSING_TYPES_H + +// +// File trailer for generated code. +// +// [EOF] +// diff --git a/emBODY/eBcode/arch-arm/board/amc2c/application/src/model-based-design/supervisor-rx/SupervisorFSM_RX.cpp b/emBODY/eBcode/arch-arm/board/amc2c/application/src/model-based-design/supervisor-rx/SupervisorFSM_RX.cpp new file mode 100644 index 0000000000..c8e28b9410 --- /dev/null +++ b/emBODY/eBcode/arch-arm/board/amc2c/application/src/model-based-design/supervisor-rx/SupervisorFSM_RX.cpp @@ -0,0 +1,1553 @@ +// +// 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 : 6.4 +// Simulink Coder version : 9.9 (R2023a) 19-Nov-2022 +// C/C++ source code generated on : Mon Oct 16 10:07:15 2023 +// +// 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 +#include "rt_roundd_snf.h" +#include "SupervisorFSM_RX_private.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_g = 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; +} + +// System initialize for function-call system: '/SetpointHandler' +void SupervisorFSM_RX_SetpointHandler_Init(void) +{ + // SystemInitialize for Chart: '/Chart1' + SupervisorFSM_RX_B.targets.jointpositions.position = 0.0F; + SupervisorFSM_RX_B.targets.jointvelocities.velocity = 0.0F; + SupervisorFSM_RX_B.targets.motorcurrent.current = 0.0F; + SupervisorFSM_RX_B.targets.motorvoltage.voltage = 0.0F; +} + +// 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_i; + } + + 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_i; + } + + 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_i = 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_i = + 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_i = + 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_i = + 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_i = + 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_i = 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_i = + 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_i = + 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_i = + 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_i = + 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_i = 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_i = 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_i = 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_i = + 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_i = 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_i = + 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_i = 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' +} + +// System initialize for function-call system: '/Limits Handler' +void SupervisorFSM_RX_LimitsHandler_Init(void) +{ + // SystemInitialize for Chart: '/Chart' + SupervisorFSM_RX_B.thresholds.jntPosMin = 0.0F; + SupervisorFSM_RX_B.thresholds.jntPosMax = 0.0F; + SupervisorFSM_RX_B.thresholds.hardwareJntPosMin = 0.0F; + SupervisorFSM_RX_B.thresholds.hardwareJntPosMax = 0.0F; + SupervisorFSM_RX_B.thresholds.rotorPosMin = 0.0F; + SupervisorFSM_RX_B.thresholds.rotorPosMax = 0.0F; + SupervisorFSM_RX_B.thresholds.jntVelMax = 0.0F; + SupervisorFSM_RX_B.thresholds.velocityTimeout = 0U; + SupervisorFSM_RX_B.thresholds.motorNominalCurrents = 0.0F; + SupervisorFSM_RX_B.thresholds.motorPeakCurrents = 0.0F; + SupervisorFSM_RX_B.thresholds.motorOverloadCurrents = 0.0F; + SupervisorFSM_RX_B.thresholds.motorPwmLimit = 0U; + SupervisorFSM_RX_B.thresholds.motorCriticalTemperature = 0.0F; +} + +// 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' +} + +// System initialize for function-call system: '/PID Handler' +void SupervisorFSM_RX_PIDHandler_Init(void) +{ + // SystemInitialize for Chart: '/Chart' + SupervisorFSM_RX_B.CurrentPID.OutMax = 0.0F; + SupervisorFSM_RX_B.CurrentPID.OutMin = 0.0F; + SupervisorFSM_RX_B.CurrentPID.P = 0.0F; + SupervisorFSM_RX_B.CurrentPID.I = 0.0F; + SupervisorFSM_RX_B.CurrentPID.D = 0.0F; + SupervisorFSM_RX_B.CurrentPID.N = 0.0F; + SupervisorFSM_RX_B.CurrentPID.I0 = 0.0F; + SupervisorFSM_RX_B.CurrentPID.D0 = 0.0F; + SupervisorFSM_RX_B.CurrentPID.shift_factor = 0U; + SupervisorFSM_RX_B.VelocityPID.OutMax = 0.0F; + SupervisorFSM_RX_B.VelocityPID.OutMin = 0.0F; + SupervisorFSM_RX_B.VelocityPID.P = 0.0F; + SupervisorFSM_RX_B.VelocityPID.I = 0.0F; + SupervisorFSM_RX_B.VelocityPID.D = 0.0F; + SupervisorFSM_RX_B.VelocityPID.N = 0.0F; + SupervisorFSM_RX_B.VelocityPID.I0 = 0.0F; + SupervisorFSM_RX_B.VelocityPID.D0 = 0.0F; + SupervisorFSM_RX_B.VelocityPID.shift_factor = 0U; + SupervisorFSM_RX_B.PositionPID.OutMax = 0.0F; + SupervisorFSM_RX_B.PositionPID.OutMin = 0.0F; + SupervisorFSM_RX_B.PositionPID.P = 0.0F; + SupervisorFSM_RX_B.PositionPID.I = 0.0F; + SupervisorFSM_RX_B.PositionPID.D = 0.0F; + SupervisorFSM_RX_B.PositionPID.N = 0.0F; + SupervisorFSM_RX_B.PositionPID.I0 = 0.0F; + SupervisorFSM_RX_B.PositionPID.D0 = 0.0F; + SupervisorFSM_RX_B.PositionPID.shift_factor = 0U; + SupervisorFSM_RX_B.OpenLoopPID.OutMax = 0.0F; + SupervisorFSM_RX_B.OpenLoopPID.OutMin = 0.0F; + SupervisorFSM_RX_B.OpenLoopPID.P = 0.0F; + SupervisorFSM_RX_B.OpenLoopPID.I = 0.0F; + SupervisorFSM_RX_B.OpenLoopPID.D = 0.0F; + SupervisorFSM_RX_B.OpenLoopPID.N = 0.0F; + SupervisorFSM_RX_B.OpenLoopPID.I0 = 0.0F; + SupervisorFSM_RX_B.OpenLoopPID.D0 = 0.0F; + SupervisorFSM_RX_B.OpenLoopPID.shift_factor = 0U; +} + +// 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) +{ + 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, 21845U); +} + +// System initialize for function-call system: '/CAN Message Handler' +void SupervisorFSM_RX_CANMessageHandler_Init(void) +{ + // SystemInitialize for Chart: '/CAN Event Dispatcher' + SupervisorFSM_RX_B.newSetpoint.current = 0.0F; + SupervisorFSM_RX_B.newSetpoint.voltage = 0.0F; + SupervisorFSM_RX_B.newSetpoint.velocity = 0.0F; + SupervisorFSM_RX_B.newLimit.overload = 0.0F; + SupervisorFSM_RX_B.newLimit.peak = 0.0F; + SupervisorFSM_RX_B.newLimit.nominal = 0.0F; + SupervisorFSM_RX_B.newLimit.type = ControlModes_NotConfigured; + SupervisorFSM_RX_B.newPID.motor = false; + SupervisorFSM_RX_B.newPID.Kp = 0.0F; + SupervisorFSM_RX_B.newPID.Ki = 0.0F; + SupervisorFSM_RX_B.newPID.Kd = 0.0F; + SupervisorFSM_RX_B.newPID.Ks = 0U; + SupervisorFSM_RX_B.motorConfig.has_hall_sens = false; + SupervisorFSM_RX_B.motorConfig.has_quadrature_encoder = false; + SupervisorFSM_RX_B.motorConfig.has_speed_quadrature_encoder = false; + SupervisorFSM_RX_B.motorConfig.has_torque_sens = false; + SupervisorFSM_RX_B.motorConfig.use_index = false; + SupervisorFSM_RX_B.motorConfig.enable_verbosity = false; + SupervisorFSM_RX_B.motorConfig.rotor_encoder_resolution = 0; + SupervisorFSM_RX_B.motorConfig.rotor_index_offset = 0; + SupervisorFSM_RX_B.motorConfig.encoder_tolerance = 0U; + SupervisorFSM_RX_B.motorConfig.pole_pairs = 0U; + SupervisorFSM_RX_B.motorConfig.Kbemf = 0.0F; + SupervisorFSM_RX_B.motorConfig.Rphase = 0.0F; + SupervisorFSM_RX_B.motorConfig.Imin = 0.0F; + SupervisorFSM_RX_B.motorConfig.Imax = 0.0F; + SupervisorFSM_RX_B.motorConfig.Vcc = 0.0F; + SupervisorFSM_RX_B.motorConfig.Vmax = 0.0F; + SupervisorFSM_RX_B.motorConfig.resistance = 0.0F; + SupervisorFSM_RX_B.motorConfig.inductance = 0.0F; + SupervisorFSM_RX_B.motorConfig.thermal_resistance = 0.0F; + SupervisorFSM_RX_B.motorConfig.thermal_time_constant = 0.0F; + SupervisorFSM_RX_B.estimationConfig.velocity_mode = + EstimationVelocityModes_Disabled; + SupervisorFSM_RX_B.estimationConfig.current_rms_lambda = 0.0F; + + // SystemInitialize for Function Call SubSystem: '/SetpointHandler' + SupervisorFSM_RX_SetpointHandler_Init(); + + // End of SystemInitialize for SubSystem: '/SetpointHandler' + + // SystemInitialize for Function Call SubSystem: '/Limits Handler' + SupervisorFSM_RX_LimitsHandler_Init(); + + // End of SystemInitialize for SubSystem: '/Limits Handler' + + // SystemInitialize for Function Call SubSystem: '/PID Handler' + SupervisorFSM_RX_PIDHandler_Init(); + + // End of SystemInitialize for SubSystem: '/PID Handler' +} + +// 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 Chart: '/SupervisorRX State Handler' incorporates: + // SubSystem: '/CAN Message Handler' + + SupervisorFSM_RX_CANMessageHandler_Init(); + + // 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_g; + 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_g: + 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/board/amc2c/application/src/model-based-design/supervisor-rx/SupervisorFSM_RX.h b/emBODY/eBcode/arch-arm/board/amc2c/application/src/model-based-design/supervisor-rx/SupervisorFSM_RX.h new file mode 100644 index 0000000000..172c268ce4 --- /dev/null +++ b/emBODY/eBcode/arch-arm/board/amc2c/application/src/model-based-design/supervisor-rx/SupervisorFSM_RX.h @@ -0,0 +1,209 @@ +// +// 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 : 6.4 +// Simulink Coder version : 9.9 (R2023a) 19-Nov-2022 +// C/C++ source code generated on : Mon Oct 16 10:07:15 2023 +// +// 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" + +// Block signals for model 'SupervisorFSM_RX' +#ifndef SupervisorFSM_RX_MDLREF_HIDE_CHILD_ + +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_i; // '/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' +}; + +#endif //SupervisorFSM_RX_MDLREF_HIDE_CHILD_ + +// Block states (default storage) for model 'SupervisorFSM_RX' +#ifndef SupervisorFSM_RX_MDLREF_HIDE_CHILD_ + +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' +}; + +#endif //SupervisorFSM_RX_MDLREF_HIDE_CHILD_ + +// Invariant block signals for model 'SupervisorFSM_RX' +#ifndef SupervisorFSM_RX_MDLREF_HIDE_CHILD_ + +struct ConstB_SupervisorFSM_RX_h_T { + boolean_T Constant5; // '/Constant5' +}; + +#endif //SupervisorFSM_RX_MDLREF_HIDE_CHILD_ + +#ifndef SupervisorFSM_RX_MDLREF_HIDE_CHILD_ + +// Real-time Model Data Structure +struct tag_RTM_SupervisorFSM_RX_T { + const char_T **errorStatus; +}; + +#endif //SupervisorFSM_RX_MDLREF_HIDE_CHILD_ + +#ifndef SupervisorFSM_RX_MDLREF_HIDE_CHILD_ + +struct MdlrefDW_SupervisorFSM_RX_T { + RT_MODEL_SupervisorFSM_RX_T rtm; +}; + +#endif //SupervisorFSM_RX_MDLREF_HIDE_CHILD_ + +// +// 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); + +#ifndef SupervisorFSM_RX_MDLREF_HIDE_CHILD_ + +extern void SupervisorFSM_RX_SetpointHandler_Init(void); +extern void SupervisorFSM_RX_SetpointHandler(void); +extern void SupervisorFSM_RX_ControlModeHandlerMotor0(void); +extern void SupervisorFSM_RX_LimitsHandler_Init(void); +extern void SupervisorFSM_RX_LimitsHandler(void); +extern void SupervisorFSM_RX_PIDHandler_Init(void); +extern void SupervisorFSM_RX_PIDHandler(void); +extern void SupervisorFSM_RX_CANMessageHandler_Init(void); +extern void SupervisorFSM_RX_CANMessageHandler(void); + +#endif //SupervisorFSM_RX_MDLREF_HIDE_CHILD_ + +#ifndef SupervisorFSM_RX_MDLREF_HIDE_CHILD_ + +extern MdlrefDW_SupervisorFSM_RX_T SupervisorFSM_RX_MdlrefDW; + +#endif //SupervisorFSM_RX_MDLREF_HIDE_CHILD_ + +#ifndef SupervisorFSM_RX_MDLREF_HIDE_CHILD_ + +// Block signals (default storage) +extern B_SupervisorFSM_RX_c_T SupervisorFSM_RX_B; + +#endif //SupervisorFSM_RX_MDLREF_HIDE_CHILD_ + +#ifndef SupervisorFSM_RX_MDLREF_HIDE_CHILD_ + +// Block states (default storage) +extern DW_SupervisorFSM_RX_f_T SupervisorFSM_RX_DW; + +#endif //SupervisorFSM_RX_MDLREF_HIDE_CHILD_ + +//- +// 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/board/amc2c/application/src/model-based-design/supervisor-rx/SupervisorFSM_RX_data.cpp b/emBODY/eBcode/arch-arm/board/amc2c/application/src/model-based-design/supervisor-rx/SupervisorFSM_RX_data.cpp new file mode 100644 index 0000000000..6f35ff4953 --- /dev/null +++ b/emBODY/eBcode/arch-arm/board/amc2c/application/src/model-based-design/supervisor-rx/SupervisorFSM_RX_data.cpp @@ -0,0 +1,31 @@ +// +// 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_data.cpp +// +// Code generated for Simulink model 'SupervisorFSM_RX'. +// +// Model version : 6.4 +// Simulink Coder version : 9.9 (R2023a) 19-Nov-2022 +// C/C++ source code generated on : Mon Oct 16 10:07:15 2023 +// +// 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' +}; + +// +// File trailer for generated code. +// +// [EOF] +// diff --git a/emBODY/eBcode/arch-arm/board/amc2c/application/src/model-based-design/supervisor-rx/SupervisorFSM_RX_private.h b/emBODY/eBcode/arch-arm/board/amc2c/application/src/model-based-design/supervisor-rx/SupervisorFSM_RX_private.h new file mode 100644 index 0000000000..097cc2634b --- /dev/null +++ b/emBODY/eBcode/arch-arm/board/amc2c/application/src/model-based-design/supervisor-rx/SupervisorFSM_RX_private.h @@ -0,0 +1,51 @@ +// +// 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 : 6.4 +// Simulink Coder version : 9.9 (R2023a) 19-Nov-2022 +// C/C++ source code generated on : Mon Oct 16 10:07:15 2023 +// +// 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" +#include "SupervisorFSM_RX.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 + +// Invariant block signals (default storage) +extern const ConstB_SupervisorFSM_RX_h_T SupervisorFSM_RX_ConstB; + +#endif // RTW_HEADER_SupervisorFSM_RX_private_h_ + +// +// File trailer for generated code. +// +// [EOF] +// diff --git a/emBODY/eBcode/arch-arm/board/amc2c/application/src/model-based-design/supervisor-rx/SupervisorFSM_RX_types.h b/emBODY/eBcode/arch-arm/board/amc2c/application/src/model-based-design/supervisor-rx/SupervisorFSM_RX_types.h new file mode 100644 index 0000000000..cba2bbcde9 --- /dev/null +++ b/emBODY/eBcode/arch-arm/board/amc2c/application/src/model-based-design/supervisor-rx/SupervisorFSM_RX_types.h @@ -0,0 +1,589 @@ +// +// 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 : 6.4 +// Simulink Coder version : 9.9 (R2023a) 19-Nov-2022 +// C/C++ source code generated on : Mon Oct 16 10:07:15 2023 +// +// 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_SensorsData_ +#define DEFINED_TYPEDEF_FOR_SensorsData_ + +struct SensorsData +{ + // position encoders + JointPositions jointpositions; + 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_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 +{ + 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 Vcc; + 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/board/amc2c/application/src/model-based-design/supervisor-tx/SupervisorFSM_TX.cpp b/emBODY/eBcode/arch-arm/board/amc2c/application/src/model-based-design/supervisor-tx/SupervisorFSM_TX.cpp new file mode 100644 index 0000000000..2fb588e15a --- /dev/null +++ b/emBODY/eBcode/arch-arm/board/amc2c/application/src/model-based-design/supervisor-tx/SupervisorFSM_TX.cpp @@ -0,0 +1,200 @@ +// +// 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_TX.cpp +// +// Code generated for Simulink model 'SupervisorFSM_TX'. +// +// Model version : 6.1 +// Simulink Coder version : 9.9 (R2023a) 19-Nov-2022 +// C/C++ source code generated on : Mon Oct 16 10:07:34 2023 +// +// Target selection: ert.tlc +// Embedded hardware selection: ARM Compatible->ARM Cortex-M +// Code generation objectives: Unspecified +// Validation result: Not run +// +#include "SupervisorFSM_TX.h" +#include "SupervisorFSM_TX_types.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); + +// Function for Chart: '/SupervisorFSM_TX' +static MCControlModes SupervisorFSM_TX_convert(ControlModes controlmode) +{ + MCControlModes mccontrolmode; + switch (controlmode) { + case ControlModes_Idle: + mccontrolmode = MCControlModes_Idle; + break; + + case ControlModes_Current: + mccontrolmode = MCControlModes_Current; + break; + + case ControlModes_Velocity: + mccontrolmode = MCControlModes_SpeedVoltage; + break; + + case ControlModes_Voltage: + mccontrolmode = MCControlModes_OpenLoop; + break; + + case ControlModes_HwFaultCM: + mccontrolmode = MCControlModes_HWFault; + break; + + default: + mccontrolmode = MCControlModes_Idle; + break; + } + + return mccontrolmode; +} + +// System initialize for referenced model: 'SupervisorFSM_TX' +void SupervisorFSM_TX_Init(BUS_MESSAGES_TX *rty_MessagesTx) +{ + // SystemInitialize for Chart: '/SupervisorFSM_TX' + rty_MessagesTx->foc.current = 0.0F; + rty_MessagesTx->foc.position = 0.0F; + rty_MessagesTx->foc.velocity = 0.0F; + rty_MessagesTx->status.control_mode = MCControlModes_Idle; + rty_MessagesTx->status.pwm_fbk = 0.0F; + rty_MessagesTx->status.flags.dirty = false; + rty_MessagesTx->status.flags.stuck = false; + rty_MessagesTx->status.flags.index_broken = false; + rty_MessagesTx->status.flags.phase_broken = false; + rty_MessagesTx->status.flags.not_calibrated = 0.0F; + rty_MessagesTx->status.flags.ExternalFaultAsserted = false; + rty_MessagesTx->status.flags.UnderVoltageFailure = false; + rty_MessagesTx->status.flags.OverVoltageFailure = false; + rty_MessagesTx->status.flags.OverCurrentFailure = false; + rty_MessagesTx->status.flags.DHESInvalidValue = false; + rty_MessagesTx->status.flags.AS5045CSumError = false; + rty_MessagesTx->status.flags.DHESInvalidSequence = false; + rty_MessagesTx->status.flags.CANInvalidProtocol = false; + rty_MessagesTx->status.flags.CAN_BufferOverRun = false; + rty_MessagesTx->status.flags.SetpointExpired = false; + rty_MessagesTx->status.flags.CAN_TXIsPasv = false; + rty_MessagesTx->status.flags.CAN_RXIsPasv = false; + rty_MessagesTx->status.flags.CAN_IsWarnTX = false; + rty_MessagesTx->status.flags.CAN_IsWarnRX = false; + rty_MessagesTx->status.flags.OverHeating = false; + rty_MessagesTx->status.flags.ADCCalFailure = false; + rty_MessagesTx->status.flags.I2TFailure = false; + rty_MessagesTx->status.flags.EMUROMFault = false; + rty_MessagesTx->status.flags.EMUROMCRCFault = false; + rty_MessagesTx->status.flags.EncoderFault = false; + rty_MessagesTx->status.flags.FirmwareSPITimingError = false; + rty_MessagesTx->status.flags.AS5045CalcError = false; + rty_MessagesTx->status.flags.FirmwarePWMFatalError = false; + rty_MessagesTx->status.flags.CAN_TXWasPasv = false; + rty_MessagesTx->status.flags.CAN_RXWasPasv = false; + rty_MessagesTx->status.flags.CAN_RTRFlagActive = false; + rty_MessagesTx->status.flags.CAN_WasWarn = false; + rty_MessagesTx->status.flags.CAN_DLCError = false; + rty_MessagesTx->status.flags.SiliconRevisionFault = false; + rty_MessagesTx->status.flags.PositionLimitUpper = false; + rty_MessagesTx->status.flags.PositionLimitLower = false; +} + +// 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) +{ + // Chart: '/SupervisorFSM_TX' + if (SupervisorFSM_TX_DW.is_active_c3_SupervisorFSM_TX == 0U) { + SupervisorFSM_TX_DW.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->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++; + } + + if (SupervisorFSM_TX_DW.ev_focEventCounter > 0U) { + SupervisorFSM_TX_B.ev_foc = !SupervisorFSM_TX_B.ev_foc; + SupervisorFSM_TX_DW.ev_focEventCounter--; + } + + if (SupervisorFSM_TX_DW.ev_statusEventCounter > 0U) { + SupervisorFSM_TX_B.ev_status = !SupervisorFSM_TX_B.ev_status; + SupervisorFSM_TX_DW.ev_statusEventCounter--; + } + + // End of Chart: '/SupervisorFSM_TX' + + // RelationalOperator: '/FixPt Relational Operator' incorporates: + // UnitDelay: '/Delay Input1' + // + // Block description for '/Delay Input1': + // + // Store in Global RAM + + rty_StatusTx->foc = (SupervisorFSM_TX_B.ev_foc != + SupervisorFSM_TX_DW.DelayInput1_DSTATE); + + // RelationalOperator: '/FixPt Relational Operator' incorporates: + // UnitDelay: '/Delay Input1' + // + // Block description for '/Delay Input1': + // + // Store in Global RAM + + rty_StatusTx->status = (SupervisorFSM_TX_B.ev_status != + SupervisorFSM_TX_DW.DelayInput1_DSTATE_d); + + // Update for UnitDelay: '/Delay Input1' + // + // Block description for '/Delay Input1': + // + // Store in Global RAM + + SupervisorFSM_TX_DW.DelayInput1_DSTATE = SupervisorFSM_TX_B.ev_foc; + + // Update for UnitDelay: '/Delay Input1' + // + // Block description for '/Delay Input1': + // + // Store in Global RAM + + SupervisorFSM_TX_DW.DelayInput1_DSTATE_d = SupervisorFSM_TX_B.ev_status; +} + +// Model initialize function +void SupervisorFSM_TX_initialize(const char_T **rt_errorStatus) +{ + RT_MODEL_SupervisorFSM_TX_T *const SupervisorFSM_TX_M = + &(SupervisorFSM_TX_MdlrefDW.rtm); + + // Registration code + + // initialize error status + rtmSetErrorStatusPointer(SupervisorFSM_TX_M, rt_errorStatus); +} + +// +// File trailer for generated code. +// +// [EOF] +// diff --git a/emBODY/eBcode/arch-arm/board/amc2c/application/src/model-based-design/supervisor-tx/SupervisorFSM_TX.h b/emBODY/eBcode/arch-arm/board/amc2c/application/src/model-based-design/supervisor-tx/SupervisorFSM_TX.h new file mode 100644 index 0000000000..627859513e --- /dev/null +++ b/emBODY/eBcode/arch-arm/board/amc2c/application/src/model-based-design/supervisor-tx/SupervisorFSM_TX.h @@ -0,0 +1,118 @@ +// +// 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_TX.h +// +// Code generated for Simulink model 'SupervisorFSM_TX'. +// +// Model version : 6.1 +// Simulink Coder version : 9.9 (R2023a) 19-Nov-2022 +// C/C++ source code generated on : Mon Oct 16 10:07:34 2023 +// +// Target selection: ert.tlc +// Embedded hardware selection: ARM Compatible->ARM Cortex-M +// Code generation objectives: Unspecified +// Validation result: Not run +// +#ifndef RTW_HEADER_SupervisorFSM_TX_h_ +#define RTW_HEADER_SupervisorFSM_TX_h_ +#include "rtwtypes.h" +#include "SupervisorFSM_TX_types.h" + +// Block signals for model 'SupervisorFSM_TX' +#ifndef SupervisorFSM_TX_MDLREF_HIDE_CHILD_ + +struct B_SupervisorFSM_TX_c_T { + boolean_T ev_foc; // '/SupervisorFSM_TX' + boolean_T ev_status; // '/SupervisorFSM_TX' +}; + +#endif //SupervisorFSM_TX_MDLREF_HIDE_CHILD_ + +// Block states (default storage) for model 'SupervisorFSM_TX' +#ifndef SupervisorFSM_TX_MDLREF_HIDE_CHILD_ + +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' +}; + +#endif //SupervisorFSM_TX_MDLREF_HIDE_CHILD_ + +#ifndef SupervisorFSM_TX_MDLREF_HIDE_CHILD_ + +// Real-time Model Data Structure +struct tag_RTM_SupervisorFSM_TX_T { + const char_T **errorStatus; +}; + +#endif //SupervisorFSM_TX_MDLREF_HIDE_CHILD_ + +#ifndef SupervisorFSM_TX_MDLREF_HIDE_CHILD_ + +struct MdlrefDW_SupervisorFSM_TX_T { + RT_MODEL_SupervisorFSM_TX_T rtm; +}; + +#endif //SupervisorFSM_TX_MDLREF_HIDE_CHILD_ + +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); + +// Model reference registration function +extern void SupervisorFSM_TX_initialize(const char_T **rt_errorStatus); + +#ifndef SupervisorFSM_TX_MDLREF_HIDE_CHILD_ + +extern MdlrefDW_SupervisorFSM_TX_T SupervisorFSM_TX_MdlrefDW; + +#endif //SupervisorFSM_TX_MDLREF_HIDE_CHILD_ + +#ifndef SupervisorFSM_TX_MDLREF_HIDE_CHILD_ + +// Block signals (default storage) +extern B_SupervisorFSM_TX_c_T SupervisorFSM_TX_B; + +#endif //SupervisorFSM_TX_MDLREF_HIDE_CHILD_ + +#ifndef SupervisorFSM_TX_MDLREF_HIDE_CHILD_ + +// Block states (default storage) +extern DW_SupervisorFSM_TX_f_T SupervisorFSM_TX_DW; + +#endif //SupervisorFSM_TX_MDLREF_HIDE_CHILD_ + +//- +// 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_TX' +// '' : 'SupervisorFSM_TX/Detect Change' +// '' : 'SupervisorFSM_TX/Detect Change1' +// '' : 'SupervisorFSM_TX/SupervisorFSM_TX' + +#endif // RTW_HEADER_SupervisorFSM_TX_h_ + +// +// File trailer for generated code. +// +// [EOF] +// diff --git a/emBODY/eBcode/arch-arm/board/amc2c/application/src/model-based-design/supervisor-tx/SupervisorFSM_TX_private.h b/emBODY/eBcode/arch-arm/board/amc2c/application/src/model-based-design/supervisor-tx/SupervisorFSM_TX_private.h new file mode 100644 index 0000000000..3ed3c99690 --- /dev/null +++ b/emBODY/eBcode/arch-arm/board/amc2c/application/src/model-based-design/supervisor-tx/SupervisorFSM_TX_private.h @@ -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: SupervisorFSM_TX_private.h +// +// Code generated for Simulink model 'SupervisorFSM_TX'. +// +// Model version : 6.1 +// Simulink Coder version : 9.9 (R2023a) 19-Nov-2022 +// C/C++ source code generated on : Mon Oct 16 10:07:34 2023 +// +// Target selection: ert.tlc +// Embedded hardware selection: ARM Compatible->ARM Cortex-M +// Code generation objectives: Unspecified +// Validation result: Not run +// +#ifndef RTW_HEADER_SupervisorFSM_TX_private_h_ +#define RTW_HEADER_SupervisorFSM_TX_private_h_ +#include "rtwtypes.h" +#include "SupervisorFSM_TX_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_SupervisorFSM_TX_private_h_ + +// +// File trailer for generated code. +// +// [EOF] +// diff --git a/emBODY/eBcode/arch-arm/board/amc2c/application/src/model-based-design/supervisor-tx/SupervisorFSM_TX_types.h b/emBODY/eBcode/arch-arm/board/amc2c/application/src/model-based-design/supervisor-tx/SupervisorFSM_TX_types.h new file mode 100644 index 0000000000..2249f16b75 --- /dev/null +++ b/emBODY/eBcode/arch-arm/board/amc2c/application/src/model-based-design/supervisor-tx/SupervisorFSM_TX_types.h @@ -0,0 +1,438 @@ +// +// 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_TX_types.h +// +// Code generated for Simulink model 'SupervisorFSM_TX'. +// +// Model version : 6.1 +// Simulink Coder version : 9.9 (R2023a) 19-Nov-2022 +// C/C++ source code generated on : Mon Oct 16 10:07:34 2023 +// +// Target selection: ert.tlc +// Embedded hardware selection: ARM Compatible->ARM Cortex-M +// Code generation objectives: Unspecified +// Validation result: Not run +// +#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_ + +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_SensorsData_ +#define DEFINED_TYPEDEF_FOR_SensorsData_ + +struct SensorsData +{ + // position encoders + JointPositions jointpositions; + MotorSensors motorsensors; +}; + +#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_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_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_MotorConfig_ +#define DEFINED_TYPEDEF_FOR_MotorConfig_ + +struct MotorConfig +{ + 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 Vcc; + 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_BUS_STATUS_TX_ +#define DEFINED_TYPEDEF_FOR_BUS_STATUS_TX_ + +// Aggregate of all events specifying types of transmitted messages. +struct BUS_STATUS_TX +{ + boolean_T foc; + boolean_T status; +}; + +#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_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_FLAGS_TX_ +#define DEFINED_TYPEDEF_FOR_BUS_FLAGS_TX_ + +struct BUS_FLAGS_TX +{ + boolean_T dirty; + boolean_T stuck; + boolean_T index_broken; + boolean_T phase_broken; + real32_T not_calibrated; + boolean_T ExternalFaultAsserted; + boolean_T UnderVoltageFailure; + boolean_T OverVoltageFailure; + boolean_T OverCurrentFailure; + boolean_T DHESInvalidValue; + boolean_T AS5045CSumError; + boolean_T DHESInvalidSequence; + boolean_T CANInvalidProtocol; + boolean_T CAN_BufferOverRun; + boolean_T SetpointExpired; + boolean_T CAN_TXIsPasv; + boolean_T CAN_RXIsPasv; + boolean_T CAN_IsWarnTX; + boolean_T CAN_IsWarnRX; + boolean_T OverHeating; + boolean_T ADCCalFailure; + boolean_T I2TFailure; + boolean_T EMUROMFault; + boolean_T EMUROMCRCFault; + boolean_T EncoderFault; + boolean_T FirmwareSPITimingError; + boolean_T AS5045CalcError; + boolean_T FirmwarePWMFatalError; + boolean_T CAN_TXWasPasv; + boolean_T CAN_RXWasPasv; + boolean_T CAN_RTRFlagActive; + boolean_T CAN_WasWarn; + boolean_T CAN_DLCError; + boolean_T SiliconRevisionFault; + boolean_T PositionLimitUpper; + boolean_T PositionLimitLower; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_BUS_MSG_STATUS_ +#define DEFINED_TYPEDEF_FOR_BUS_MSG_STATUS_ + +struct BUS_MSG_STATUS +{ + MCControlModes control_mode; + + // control effort (quadrature) + real32_T pwm_fbk; + BUS_FLAGS_TX flags; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_BUS_MESSAGES_TX_ +#define DEFINED_TYPEDEF_FOR_BUS_MESSAGES_TX_ + +// Aggregate of all CAN transmitted messages. +struct BUS_MESSAGES_TX +{ + BUS_MSG_FOC foc; + BUS_MSG_STATUS status; +}; + +#endif + +// Forward declaration for rtModel +typedef struct tag_RTM_SupervisorFSM_TX_T RT_MODEL_SupervisorFSM_TX_T; + +#endif // RTW_HEADER_SupervisorFSM_TX_types_h_ + +// +// File trailer for generated code. +// +// [EOF] +// diff --git a/emBODY/eBcode/arch-arm/board/amc2c/application/src/model-based-design/thermal-model/thermal_model.cpp b/emBODY/eBcode/arch-arm/board/amc2c/application/src/model-based-design/thermal-model/thermal_model.cpp new file mode 100644 index 0000000000..aec0660ff2 --- /dev/null +++ b/emBODY/eBcode/arch-arm/board/amc2c/application/src/model-based-design/thermal-model/thermal_model.cpp @@ -0,0 +1,99 @@ +// +// Non-Degree Granting Education License -- for use at non-degree +// granting, nonprofit, education, and research organizations only. Not +// for commercial or industrial use. +// +// File: thermal_model.cpp +// +// Code generated for Simulink model 'thermal_model'. +// +// Model version : 5.21 +// Simulink Coder version : 9.9 (R2023a) 19-Nov-2022 +// C/C++ source code generated on : Mon Oct 16 10:08:48 2023 +// +// Target selection: ert.tlc +// Embedded hardware selection: ARM Compatible->ARM Cortex-M +// Code generation objectives: Unspecified +// Validation result: Not run +// +#include "thermal_model.h" +#include "thermal_model_types.h" +#include "mw_cmsis.h" +#include "rtwtypes.h" +#include "thermal_model_private.h" +#include + +// System initialize for referenced model: 'thermal_model' +void thermal_model_Init(DW_thermal_model_f_T *localDW) +{ + // InitializeConditions for DiscreteFilter: '/DigitalFilter' + localDW->DigitalFilter_states = 0.0F; +} + +// Output and update for referenced model: 'thermal_model' +void thermal_model(const ControlOutputs *rtu_ControlOutputs, const + ConfigurationParameters *rtu_ConfigurationParameters, + MotorTemperature *rty_MotorTemperature, DW_thermal_model_f_T * + localDW) +{ + real32_T rtb_Product1[2]; + real32_T rtb_Square[2]; + real32_T rtb_Gain2; + real32_T rtb_Sum_a; + real32_T rtb_Sum_p_tmp; + + // SignalConversion generated from: '/Square' + rtb_Product1[0] = rtu_ControlOutputs->Iq_rms.current; + rtb_Product1[1] = rtu_ControlOutputs->Id_rms.current; + + // Math: '/Square' + mw_arm_mult_f32(&rtb_Product1[0], &rtb_Product1[0], &rtb_Square[0], 2U); + + // Gain: '/Gain2' + rtb_Gain2 = 2.0F * + rtu_ConfigurationParameters->motorconfig.thermal_time_constant; + + // DiscreteFilter: '/DigitalFilter' incorporates: + // Constant: '/Ts' + // Constant: '/Constant2' + // Product: '/Divide' + // Product: '/Divide1' + // Product: '/Product' + // Product: '/Product1' + // Sum: '/Sum1' + // Sum: '/Sum' + // Sum: '/Sum1' + + rtb_Sum_p_tmp = 0.01F * rtu_ConfigurationParameters->motorconfig.resistance * + rtu_ConfigurationParameters->motorconfig.thermal_resistance / (rtb_Gain2 + + 0.01F) * (rtb_Square[0] + rtb_Square[1]); + rtb_Sum_a = rtb_Sum_p_tmp + localDW->DigitalFilter_states; + localDW->DigitalFilter_states = rtb_Sum_p_tmp - (0.01F - rtb_Gain2) / + (rtb_Gain2 + 0.01F) * rtb_Sum_a; + + // BusCreator: '/Bus Creator1' incorporates: + // Sum: '/Sum' + + rty_MotorTemperature->temperature = rtb_Sum_a + + rtu_ConfigurationParameters->environment_temperature; +} + +// Model initialize function +void thermal_model_initialize(const char_T **rt_errorStatus, + RT_MODEL_thermal_model_T *const thermal_model_M, DW_thermal_model_f_T *localDW) +{ + // Registration code + + // initialize error status + rtmSetErrorStatusPointer(thermal_model_M, rt_errorStatus); + + // states (dwork) + (void) std::memset(static_cast(localDW), 0, + sizeof(DW_thermal_model_f_T)); +} + +// +// File trailer for generated code. +// +// [EOF] +// diff --git a/emBODY/eBcode/arch-arm/board/amc2c/application/src/model-based-design/thermal-model/thermal_model.h b/emBODY/eBcode/arch-arm/board/amc2c/application/src/model-based-design/thermal-model/thermal_model.h new file mode 100644 index 0000000000..26abe8dd4d --- /dev/null +++ b/emBODY/eBcode/arch-arm/board/amc2c/application/src/model-based-design/thermal-model/thermal_model.h @@ -0,0 +1,78 @@ +// +// Non-Degree Granting Education License -- for use at non-degree +// granting, nonprofit, education, and research organizations only. Not +// for commercial or industrial use. +// +// File: thermal_model.h +// +// Code generated for Simulink model 'thermal_model'. +// +// Model version : 5.21 +// Simulink Coder version : 9.9 (R2023a) 19-Nov-2022 +// C/C++ source code generated on : Mon Oct 16 10:08:48 2023 +// +// Target selection: ert.tlc +// Embedded hardware selection: ARM Compatible->ARM Cortex-M +// Code generation objectives: Unspecified +// Validation result: Not run +// +#ifndef RTW_HEADER_thermal_model_h_ +#define RTW_HEADER_thermal_model_h_ +#include "rtwtypes.h" +#include "thermal_model_types.h" +#include + +// Block states (default storage) for model 'thermal_model' +struct DW_thermal_model_f_T { + real32_T DigitalFilter_states; // '/DigitalFilter' +}; + +// Real-time Model Data Structure +struct tag_RTM_thermal_model_T { + const char_T **errorStatus; +}; + +struct MdlrefDW_thermal_model_T { + DW_thermal_model_f_T rtdw; + RT_MODEL_thermal_model_T rtm; +}; + +// Model reference registration function +extern void thermal_model_initialize(const char_T **rt_errorStatus, + RT_MODEL_thermal_model_T *const thermal_model_M, DW_thermal_model_f_T *localDW); +extern void thermal_model_Init(DW_thermal_model_f_T *localDW); +extern void thermal_model(const ControlOutputs *rtu_ControlOutputs, const + ConfigurationParameters *rtu_ConfigurationParameters, MotorTemperature + *rty_MotorTemperature, DW_thermal_model_f_T *localDW); + +//- +// These blocks were eliminated from the model due to optimizations: +// +// Block '/Cast To Single1' : Eliminate redundant data type conversion +// Block '/Cast To Single2' : Eliminate redundant data type conversion + + +//- +// 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 +// +// '' : 'thermal_model' +// '' : 'thermal_model/Compute coefficients' + +#endif // RTW_HEADER_thermal_model_h_ + +// +// File trailer for generated code. +// +// [EOF] +// diff --git a/emBODY/eBcode/arch-arm/board/amc2c/application/src/model-based-design/thermal-model/thermal_model_private.h b/emBODY/eBcode/arch-arm/board/amc2c/application/src/model-based-design/thermal-model/thermal_model_private.h new file mode 100644 index 0000000000..6f3ab2a2d6 --- /dev/null +++ b/emBODY/eBcode/arch-arm/board/amc2c/application/src/model-based-design/thermal-model/thermal_model_private.h @@ -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: thermal_model_private.h +// +// Code generated for Simulink model 'thermal_model'. +// +// Model version : 5.21 +// Simulink Coder version : 9.9 (R2023a) 19-Nov-2022 +// C/C++ source code generated on : Mon Oct 16 10:08:48 2023 +// +// Target selection: ert.tlc +// Embedded hardware selection: ARM Compatible->ARM Cortex-M +// Code generation objectives: Unspecified +// Validation result: Not run +// +#ifndef RTW_HEADER_thermal_model_private_h_ +#define RTW_HEADER_thermal_model_private_h_ +#include "rtwtypes.h" +#include "thermal_model_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_thermal_model_private_h_ + +// +// File trailer for generated code. +// +// [EOF] +// diff --git a/emBODY/eBcode/arch-arm/board/amc2c/application/src/model-based-design/thermal-model/thermal_model_types.h b/emBODY/eBcode/arch-arm/board/amc2c/application/src/model-based-design/thermal-model/thermal_model_types.h new file mode 100644 index 0000000000..4947b9781f --- /dev/null +++ b/emBODY/eBcode/arch-arm/board/amc2c/application/src/model-based-design/thermal-model/thermal_model_types.h @@ -0,0 +1,223 @@ +// +// Non-Degree Granting Education License -- for use at non-degree +// granting, nonprofit, education, and research organizations only. Not +// for commercial or industrial use. +// +// File: thermal_model_types.h +// +// Code generated for Simulink model 'thermal_model'. +// +// Model version : 5.21 +// Simulink Coder version : 9.9 (R2023a) 19-Nov-2022 +// C/C++ source code generated on : Mon Oct 16 10:08:48 2023 +// +// Target selection: ert.tlc +// Embedded hardware selection: ARM Compatible->ARM Cortex-M +// Code generation objectives: Unspecified +// Validation result: Not run +// +#ifndef RTW_HEADER_thermal_model_types_h_ +#define RTW_HEADER_thermal_model_types_h_ +#include "rtwtypes.h" +#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_MotorConfig_ +#define DEFINED_TYPEDEF_FOR_MotorConfig_ + +struct MotorConfig +{ + 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 Vcc; + 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_MotorTemperature_ +#define DEFINED_TYPEDEF_FOR_MotorTemperature_ + +struct MotorTemperature +{ + // motor temperature + real32_T temperature; +}; + +#endif + +// Forward declaration for rtModel +typedef struct tag_RTM_thermal_model_T RT_MODEL_thermal_model_T; + +#endif // RTW_HEADER_thermal_model_types_h_ + +// +// File trailer for generated code. +// +// [EOF] +// diff --git a/emBODY/eBcode/arch-arm/board/amc2c/bsp/embot_hw_motor_bsp_amc2c.cpp b/emBODY/eBcode/arch-arm/board/amc2c/bsp/embot_hw_motor_bsp_amc2c.cpp index f37f44f5ff..adab8045a3 100644 --- a/emBODY/eBcode/arch-arm/board/amc2c/bsp/embot_hw_motor_bsp_amc2c.cpp +++ b/emBODY/eBcode/arch-arm/board/amc2c/bsp/embot_hw_motor_bsp_amc2c.cpp @@ -385,7 +385,7 @@ namespace embot::hw::motor::bsp::amc2c { hTIM1.Instance = TIM1; hTIM1.Init.Prescaler = 0; hTIM1.Init.CounterMode = TIM_COUNTERMODE_CENTERALIGNED1; - hTIM1.Init.Period = 1219; + hTIM1.Init.Period = embot::hw::motor::bsp::amc2c::PWMvals.TIM1_period; // 2000; // 1219; hTIM1.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1; hTIM1.Init.RepetitionCounter = 0; hTIM1.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_ENABLE; @@ -440,7 +440,7 @@ namespace embot::hw::motor::bsp::amc2c { Error_Handler(); } sConfigOC.OCMode = TIM_OCMODE_PWM2; - sConfigOC.Pulse = 895; // was 752, changed due to different clock source from amc-bldc + sConfigOC.Pulse = embot::hw::motor::bsp::amc2c::PWMvals.TIM1_ocPulse ; // 1469; //895; // was 752, changed due to different clock source from amc-bldc if (HAL_TIM_PWM_ConfigChannel(&hTIM1, &sConfigOC, TIM_CHANNEL_6) != HAL_OK) { Error_Handler(); @@ -749,12 +749,21 @@ namespace embot::hw::motor::bsp::amc2c { } /** Configure Regular Channel */ + +#if 0 + sConfig.Channel = ADC_CHANNEL_0; sConfig.Rank = ADC_REGULAR_RANK_1; sConfig.SamplingTime = ADC_SAMPLETIME_8CYCLES_5; sConfig.SingleDiff = ADC_SINGLE_ENDED; sConfig.OffsetNumber = ADC_OFFSET_1; - sConfig.Offset = 29789; +// sConfig.Offset = 29789; +// sConfig.OffsetRightShift = DISABLE; +// sConfig.OffsetSignedSaturation = ENABLE; +// sConfig.Offset = 0; +// sConfig.OffsetRightShift = DISABLE; +// sConfig.OffsetSignedSaturation = DISABLE; + sConfig.Offset = 29496; sConfig.OffsetRightShift = DISABLE; sConfig.OffsetSignedSaturation = ENABLE; if (HAL_ADC_ConfigChannel(&hADC1, &sConfig) != HAL_OK) @@ -766,6 +775,7 @@ namespace embot::hw::motor::bsp::amc2c { sConfig.Channel = ADC_CHANNEL_1; sConfig.Rank = ADC_REGULAR_RANK_2; sConfig.OffsetNumber = ADC_OFFSET_2; + sConfig.Offset = 29634; if (HAL_ADC_ConfigChannel(&hADC1, &sConfig) != HAL_OK) { Error_Handler(); @@ -775,10 +785,46 @@ namespace embot::hw::motor::bsp::amc2c { sConfig.Channel = ADC_CHANNEL_3; sConfig.Rank = ADC_REGULAR_RANK_3; sConfig.OffsetNumber = ADC_OFFSET_3; + sConfig.Offset = 29020; if (HAL_ADC_ConfigChannel(&hADC1, &sConfig) != HAL_OK) { Error_Handler(); } + +#else +// zero offset in ADC + sConfig.Channel = ADC_CHANNEL_0; + sConfig.Rank = ADC_REGULAR_RANK_1; + sConfig.SamplingTime = ADC_SAMPLETIME_8CYCLES_5; + sConfig.SingleDiff = ADC_SINGLE_ENDED; + sConfig.OffsetNumber = ADC_OFFSET_NONE; + sConfig.Offset = 0; + sConfig.OffsetRightShift = DISABLE; + sConfig.OffsetSignedSaturation = DISABLE; + if (HAL_ADC_ConfigChannel(&hADC1, &sConfig) != HAL_OK) + { + Error_Handler(); + } + /** Configure Regular Channel + */ + sConfig.Channel = ADC_CHANNEL_1; + sConfig.Rank = ADC_REGULAR_RANK_2; + + + if (HAL_ADC_ConfigChannel(&hADC1, &sConfig) != HAL_OK) + { + Error_Handler(); + } + /** Configure Regular Channel + */ + sConfig.Channel = ADC_CHANNEL_3; + sConfig.Rank = ADC_REGULAR_RANK_3; + + if (HAL_ADC_ConfigChannel(&hADC1, &sConfig) != HAL_OK) + { + Error_Handler(); + } +#endif /* USER CODE BEGIN ADC1_Init 2 */ /* USER CODE END ADC1_Init 2 */ @@ -1264,8 +1310,8 @@ namespace embot::hw::motor::bsp::amc2c { // HAL_NVIC_EnableIRQ(TIM1_UP_IRQn); HAL_NVIC_SetPriority(TIM1_TRG_COM_IRQn, 5, 0); HAL_NVIC_EnableIRQ(TIM1_TRG_COM_IRQn); - HAL_NVIC_SetPriority(TIM1_CC_IRQn, 5, 0); - HAL_NVIC_EnableIRQ(TIM1_CC_IRQn); +// HAL_NVIC_SetPriority(TIM1_CC_IRQn, 5, 0); +// HAL_NVIC_EnableIRQ(TIM1_CC_IRQn); /* USER CODE BEGIN TIM1_MspInit 1 */ /* USER CODE END TIM1_MspInit 1 */ @@ -1333,7 +1379,7 @@ namespace embot::hw::motor::bsp::amc2c { #endif // HAL_NVIC_DisableIRQ(TIM1_UP_IRQn); HAL_NVIC_DisableIRQ(TIM1_TRG_COM_IRQn); - HAL_NVIC_DisableIRQ(TIM1_CC_IRQn); +// HAL_NVIC_DisableIRQ(TIM1_CC_IRQn); /* USER CODE BEGIN TIM1_MspDeInit 1 */ /* USER CODE END TIM1_MspDeInit 1 */ diff --git a/emBODY/eBcode/arch-arm/board/amc2c/bsp/embot_hw_motor_bsp_amc2c.h b/emBODY/eBcode/arch-arm/board/amc2c/bsp/embot_hw_motor_bsp_amc2c.h index 470c25a0d7..b0439a9593 100644 --- a/emBODY/eBcode/arch-arm/board/amc2c/bsp/embot_hw_motor_bsp_amc2c.h +++ b/emBODY/eBcode/arch-arm/board/amc2c/bsp/embot_hw_motor_bsp_amc2c.h @@ -43,7 +43,34 @@ namespace embot::hw::motor::bsp::amc2c { // pwm extern TIM_HandleTypeDef hTIM1; + struct PWMvalues + { // the default is for pwm @ 20 us ( + uint32_t TIM1_period { 2000 }; // 2000 {1219}; + static constexpr float prop { 0.7345 }; + uint32_t TIM1_ocPulse { static_cast(prop*static_cast(TIM1_period)) }; // 1469 {895} // actually a proprotion of TIM1_period + uint32_t value100perc {TIM1_period-1}; + constexpr PWMvalues() = default; + constexpr PWMvalues(uint32_t pwmnanosec) : TIM1_period(pwmnanosec/10) + { + TIM1_ocPulse = static_cast(prop*static_cast(TIM1_period)); + value100perc = TIM1_period-1; + }; + }; + + // 20000 is 20.0 us + // 15000 is 15.0 us + // 12500 is 12.5 us + // 12190 is ... as amcbldc + constexpr PWMvalues PWMvals20us {20000}; + constexpr PWMvalues PWMvals15us {15000}; + constexpr PWMvalues PWMvals12dot5us {12500}; + constexpr PWMvalues PWMvals12dot190us {12190}; // 36.57 + + constexpr PWMvalues PWMvals {PWMvals20us}; + //constexpr PWMvalues PWMvals {PWMvals15us}; + + } diff --git a/emBODY/eBcode/arch-arm/board/amc2c/bsp/motorhal/adcm.c b/emBODY/eBcode/arch-arm/board/amc2c/bsp/motorhal/adcm.c index 6d3ac3bf5b..6d14d5a2e3 100644 --- a/emBODY/eBcode/arch-arm/board/amc2c/bsp/motorhal/adcm.c +++ b/emBODY/eBcode/arch-arm/board/amc2c/bsp/motorhal/adcm.c @@ -59,7 +59,7 @@ extern ADC_HandleTypeDef hadc2; // add in here minimal changes // -------------------------------------------------------------------------------------------------------------------- -extern adcm_fp_int16_int16_uint16_t adcm_FP_on_acquisition_of_currents = NULL; +extern adcm_fp_uint16_uint16_uint16_t adcm_FP_on_acquisition_of_currents = NULL; #endif // #if defined(MOTORHAL_changes) @@ -69,7 +69,7 @@ extern adcm_fp_int16_int16_uint16_t adcm_FP_on_acquisition_of_currents = NULL; /* Structure to hold the ADC1/ADC2 DMA data */ typedef struct { - int16_t adc1; + uint16_t adc1; uint16_t adc2; } __attribute__((aligned(4),packed)) DualModeAdcData_t; @@ -80,7 +80,7 @@ typedef struct static DualModeAdcData_t AdcMotRawData[2*NUMBER_OF_ADC_CHANNELS]; /* Unpacked data */ -static int16_t adcMotCurrents[NUMBER_OF_ADC_CHANNELS]; +static uint16_t adcMotCurrents[NUMBER_OF_ADC_CHANNELS]; // in adc units [0, 64k) static uint16_t adcMotVoltages[NUMBER_OF_ADC_CHANNELS]; /* Channels assigned to phase current measurement */ @@ -123,35 +123,11 @@ static void adcMotGetSamples(const DualModeAdcData_t *pBuf) /*DEBUG*/HAL_GPIO_WritePin(TP4_GPIO_Port, TP4_Pin, GPIO_PIN_RESET); #endif -// static int32_t counter = 0; -// static int32_t avg_r_0, avg_r_1, avg_r_2 = 0; #if defined(MOTORHAL_changes) if(NULL != adcm_FP_on_acquisition_of_currents) { - adcm_FP_on_acquisition_of_currents(adcMotCurrents[0], adcMotCurrents[1], adcMotCurrents[2]); -// TODO: Remove, used only for debug -// counter++; -// avg_r_0 = adcMotCurrents[0]; -// avg_r_1 = adcMotCurrents[1]; -// avg_r_2 = adcMotCurrents[2]; - AVG0 = adcMotCurrents[0]; - AVG1 = adcMotCurrents[1]; - AVG2 = adcMotCurrents[2] ; - -// if(counter % 50 == 0) -// { -// AVG0 = avg_r_0 / counter; -// AVG1 = avg_r_1 / counter; -// AVG2 = avg_r_2 / counter; -// //embot::core::print(std::to_string(AVG0) + ";" + std::to_string(AVG1) + ";" + std::to_string(AVG2)); - -// counter = 0; -// avg_r_0 = 0; -// avg_r_1 = 0; -// avg_r_2 = 0; -// } - + adcm_FP_on_acquisition_of_currents(adcMotCurrents[0], adcMotCurrents[1], adcMotCurrents[2]); } #endif } diff --git a/emBODY/eBcode/arch-arm/board/amc2c/bsp/motorhal/adcm.h b/emBODY/eBcode/arch-arm/board/amc2c/bsp/motorhal/adcm.h index 7a0893630d..eebee436a0 100644 --- a/emBODY/eBcode/arch-arm/board/amc2c/bsp/motorhal/adcm.h +++ b/emBODY/eBcode/arch-arm/board/amc2c/bsp/motorhal/adcm.h @@ -57,9 +57,9 @@ extern void AdcMotSetOffset(AdcMotPhase_t Phase, uint16_t Offset); extern void AdcMotHalfTransfer_cb(ADC_HandleTypeDef *hadc); extern void AdcMotTransferComplete_cb(ADC_HandleTypeDef *hadc); -// we need to be bale to specify a user-defined function when the currents are available -typedef void (*adcm_fp_int16_int16_uint16_t) (int16_t, int16_t, int16_t); -extern adcm_fp_int16_int16_uint16_t adcm_FP_on_acquisition_of_currents; +// we need to be able to specify a user-defined function when the raw adc acquistion of currents are available +typedef void (*adcm_fp_uint16_uint16_uint16_t) (uint16_t, uint16_t, uint16_t); +extern adcm_fp_uint16_uint16_uint16_t adcm_FP_on_acquisition_of_currents; #endif diff --git a/emBODY/eBcode/arch-arm/board/amc2c/bsp/motorhal/hall.c b/emBODY/eBcode/arch-arm/board/amc2c/bsp/motorhal/hall.c index 9abe58f334..a66b8528d7 100644 --- a/emBODY/eBcode/arch-arm/board/amc2c/bsp/motorhal/hall.c +++ b/emBODY/eBcode/arch-arm/board/amc2c/bsp/motorhal/hall.c @@ -54,6 +54,14 @@ #define HALL_INPUT_POS (LSB(HALL_INPUT_MASK)) #define HALL_INPUT() ((HALL_INPUT_PORT&HALL_INPUT_MASK)>>HALL_INPUT_POS) +// marco.accame, note-667: +// HALL_INPUT() gets three bits in this order: H3|H2|H1, where: +// - H3 is the status of {MOT_HALL3_GPIO_Port, MOT_HALL3_Pin} = PD14 +// - H2 is the status of {MOT_HALL2_GPIO_Port, MOT_HALL2_Pin} = PD13 +// - H1 is the status of {MOT_HALL1_GPIO_Port, MOT_HALL1_Pin} = PD12 +// because it reads the whole status of the port D, mask the bits 14, 13, 12 and then shifts down by 12 positions +// so: it is correct but we read a byte where bit0 is H1, bit 1 is H2 and bit 2 is H3 + /* Shorthand */ #define _U_ PWM_PHASE_U #define _V_ PWM_PHASE_V diff --git a/emBODY/eBcode/arch-arm/board/amc2c/bsp/motorhal/hall.h b/emBODY/eBcode/arch-arm/board/amc2c/bsp/motorhal/hall.h index b215c7b02f..bbeb64bfe4 100644 --- a/emBODY/eBcode/arch-arm/board/amc2c/bsp/motorhal/hall.h +++ b/emBODY/eBcode/arch-arm/board/amc2c/bsp/motorhal/hall.h @@ -42,6 +42,8 @@ extern void HallTest(void); #if defined(MOTORHAL_changes) +// it contains a byte w/ only three significant bits: 0b00000xyz +// where x = H3, y = H2, z = H1, so it reads H3|H2|H1 extern uint8_t hall_INPUT(); #endif // #if defined(MOTORHAL_changes) diff --git a/emBODY/eBcode/arch-arm/board/amc2c/bsp/motorhal/motorhal.cpp b/emBODY/eBcode/arch-arm/board/amc2c/bsp/motorhal/motorhal.cpp index e7afd793ae..3f2b87dad4 100644 --- a/emBODY/eBcode/arch-arm/board/amc2c/bsp/motorhal/motorhal.cpp +++ b/emBODY/eBcode/arch-arm/board/amc2c/bsp/motorhal/motorhal.cpp @@ -23,6 +23,7 @@ #include "enc.h" #include "hall.h" #include "pwm.h" +#include "motorhal_faults.h" #include "embot_core.h" #include @@ -41,21 +42,34 @@ namespace embot::hw::motor::adc { struct Converter { - static int32_t raw2current(int32_t r) + static constexpr uint32_t defOffset {29789}; + uint32_t offset {defOffset}; + + constexpr Converter() = default; + constexpr Converter(uint32_t o) : offset(o) {}; + + void clear() + { + set(defOffset); + } + + void set(uint32_t o) { - //int32_t c = (r/1000) * 7872; - //return c; - return 1; + offset = o; + } + + int32_t raw2current(uint32_t r) + { + int32_t t = static_cast(r - offset)*33000; + return t/(256*1024); } - static int32_t current2raw(int32_t c) + uint32_t current2raw(int32_t c) { - //int32_t r = (c*7872) / 1000; - //return r; - return 1; + int32_t t = c*256*1024; + return t/33000 + offset; } - constexpr Converter() = default; }; @@ -67,21 +81,25 @@ struct Calibrator volatile bool _done {false}; volatile size_t _count {0}; Currents _currents {}; - std::array cumulativerawvalues {0, 0, 0}; + std::array cumulativerawvalues {0, 0, 0}; + + + std::array _conv {}; constexpr Calibrator() = default; void init() { + for(auto &i : _conv) { i.clear(); } // but the clear is not necessary _count = 0; _done = false; cumulativerawvalues.fill(0); - set({oncurrents, this}); + embot::hw::motor::adc::set({oncurrentscalib, this}); } void stop() { - set({}); + embot::hw::motor::adc::set({}); _count = 0; _done = true; } @@ -113,14 +131,16 @@ struct Calibrator } - static void oncurrents(void *owner, const Currents * const currents) + static void oncurrentscalib(void *owner, const Currents * const currents) { Calibrator *o = reinterpret_cast(owner); - + + // currents is in mA + // i use Converter::current2raw() because technically Currents does not contain the raw ADC values but transformed values - o->cumulativerawvalues[0] += Converter::current2raw(currents->u); - o->cumulativerawvalues[1] += Converter::current2raw(currents->v); - o->cumulativerawvalues[2] += Converter::current2raw(currents->w); + o->cumulativerawvalues[0] += o->_conv[0].current2raw(currents->u); + o->cumulativerawvalues[1] += o->_conv[1].current2raw(currents->v); + o->cumulativerawvalues[2] += o->_conv[2].current2raw(currents->w); o->_count++; @@ -129,20 +149,19 @@ struct Calibrator // time to do business: prepare average currents, impose the offset to ADC, stop the calibrator // dont use the >> _shift because ... 1. this operation is done only once, so who bother. - // 2. cumulativerawvalues is int64_t. it may be (dont think so) negative so it is too complicate to optimize + // 2. cumulativerawvalues is int64_t, so shift does niot work for negative values // impose offset to adc. -// volatile int64_t cc[3] {0, 0, 0}; -// cc[0] = o->cumulativerawvalues[0] / _maxcount; -// cc[1] = o->cumulativerawvalues[1] / _maxcount; -// cc[2] = o->cumulativerawvalues[2] / _maxcount; - AdcMotSetOffset(ADCMOT_PHASE_U, AdcMotGetOffset(ADCMOT_PHASE_U) + (o->cumulativerawvalues[0] / _maxcount)); - AdcMotSetOffset(ADCMOT_PHASE_V, AdcMotGetOffset(ADCMOT_PHASE_V) + (o->cumulativerawvalues[1] / _maxcount)); - AdcMotSetOffset(ADCMOT_PHASE_W, AdcMotGetOffset(ADCMOT_PHASE_W) + (o->cumulativerawvalues[2] / _maxcount)); + volatile int64_t cc[3] {0, 0, 0}; + cc[0] = o->cumulativerawvalues[0] / o->_count; + cc[1] = o->cumulativerawvalues[1] / o->_count; + cc[2] = o->cumulativerawvalues[2] / o->_count; - // stop - o->stop(); + o->_conv[0].set(cc[0]); + o->_conv[1].set(cc[1]); + o->_conv[2].set(cc[2]); - return; + // stop: deregister this callback + o->stop(); } } @@ -154,23 +173,28 @@ struct adcm_Internals Calibrator calibrator {}; static void dummy_adc_callback(void *owner, const Currents * const currents) {} - static constexpr OnCurrents dummyADCcbk { dummy_adc_callback, nullptr }; + static constexpr OnCurrents dummyADCcbk { dummy_adc_callback, nullptr }; adcm_Internals() = default; }; adcm_Internals _adcm_internals {}; - -void adcm_on_acquisition_of_currents(int16_t c1, int16_t c2, int16_t c3) +// this is the callback executed inside the ADC handler +// {c1, c2, c3} are in adc units [0, 64k) +// so, we need to transform them into currents expressed in milli-ampere + // and call the callback imposed by the embot::hw::motor::setCallbackOnCurrents() +void adcm_on_acquisition_of_currents(uint16_t c1, uint16_t c2, uint16_t c3) { - // important note: {c1, c2, c3} are the raw ADC acquisitions as in adcMotCurrents[0, 1, 2] - // the callback requires converted currents. but to be fair in adcm.c there is no concept of how to convert them as in the amcbldc - // so, boh ... for now i do nothing - Currents currents = {Converter::raw2current(c1), Converter::raw2current(c2), Converter::raw2current(c3)}; + // important note: {{c1, c2, c3} are in adc units [0, 64k). no conversion yet. + Currents currents = + { + _adcm_internals.calibrator._conv[0].raw2current(c1), + _adcm_internals.calibrator._conv[1].raw2current(c2), + _adcm_internals.calibrator._conv[2].raw2current(c3), + }; _adcm_internals.config.oncurrents.execute(¤ts); -} - +} bool init(const Configuration &config) { @@ -278,6 +302,7 @@ struct enc_Internals Configuration config {}; Mode mode {}; enc_Conversion conversion {}; + int32_t forcedvalue {0}; enc_Internals() = default; }; @@ -366,7 +391,6 @@ extern bool start(const Mode& mode) return ret; } - extern bool isstarted() { return _enc_internals.started; @@ -376,7 +400,7 @@ extern int32_t getvalue() { if(false == _enc_internals.started) { - return 0; + return _enc_internals.forcedvalue; } int32_t v = __HAL_TIM_GetCounter(&ENC_TIM); @@ -384,6 +408,11 @@ extern int32_t getvalue() return _enc_internals.conversion.convert(v); } +void force(int32_t value) +{ + _enc_internals.forcedvalue = value; +} + } // namespace embot::hw::motor::enc { @@ -402,6 +431,7 @@ struct hall_Table static constexpr int16_t hallAngleStep = 60.0 * Deg2iCub; // 60 degrees scaled by the range of possible values static constexpr int16_t minHallAngleDelta = 30.0 * Deg2iCub; // 30 degrees scaled by the range of possible values + // it keeps the table from hall values H3h2H1 = [1, 2, 3, 4, 5, 6] to the center of the sector expressed in icub degrees static constexpr uint16_t hallAngleTable[] = { /* ABC (?) */ @@ -415,10 +445,19 @@ struct hall_Table /* HHH ERROR */ static_cast(0) }; + // it is the lookup table from hall value H3H2H1 = [1, 2, 3, 4, 5, 6] to sector. + // the sector number x is centered in angle x*60 [deg] and is 60 deg wide + static constexpr uint8_t hallSectorTable[] = {255, 4, 2, 3, 0, 5, 1, 255}; + static constexpr uint16_t status2angle(uint8_t hallstatus) { return hallAngleTable[hallstatus & 0x07]; } + + static constexpr uint8_t status2sector(uint8_t hallstatus) + { + return hallSectorTable[hallstatus & 0x07]; + } static constexpr uint8_t angle2sector(uint16_t angle) { @@ -430,26 +469,50 @@ struct hall_Table struct hall_Data { - uint8_t order[3] = {0, 1, 2}; - int32_t counter {0}; - int32_t angle {0}; - volatile uint8_t status {0}; + static constexpr uint8_t PREV = 0; + static constexpr uint8_t CURR = 1; - void reset(Mode::SWAP s = Mode::SWAP::none) + // in icub degrees + int32_t angle {0}; + + std::array h3h2h1 = {0, 0}; + + static constexpr bool isH3H2H1valid(uint8_t v) { - counter = 0; - angle = 0; - status = 0; - set(s); + return (0 != v) && (v < 7); + } + + static constexpr bool isTransitionOfH3H2H1valid(uint8_t prev, uint8_t curr) + { // the prev and curr values must be valid + // the following lut tells which h3h2h1 is expected for clockwise and counter clockwise rotation + // given the value of the previous h3h2h1 which must be in range [1, 6] + constexpr uint8_t nextclockwise[7] = {255, 5, 3, 1, 6, 4, 2}; + constexpr uint8_t nextcounterclockwise[7] = {255, 3, 6, 2, 5, 1, 4}; + + if(false == isH3H2H1valid(prev)) + { + return false; + } + + if((curr == prev) || (curr == nextclockwise[prev]) || (curr == nextcounterclockwise[prev])) + { // it is ok if the transition happens by a single step clockwise or counterclockise + return true; + } + + return false; } - void set(Mode::SWAP s) + bool firstacquisition {true}; + + void reset() { - order[0] = 0; - order[1] = (Mode::SWAP::BC == s) ? 2 : 1; - order[2] = (Mode::SWAP::BC == s) ? 1 : 2; + angle = 0; + h3h2h1[hall_Data::CURR] = h3h2h1[hall_Data::PREV] = 0; + firstacquisition = true; } + + hall_Data() = default; }; @@ -459,7 +522,8 @@ struct hall_Internals bool started {false}; Configuration config {}; Mode mode {}; - hall_Data data {}; + hall_Data data {}; + void reset() { @@ -471,32 +535,222 @@ struct hall_Internals } hall_Internals() = default; -}; + + + uint8_t input() + { + // we get the values of the three bits: H3|H2|H1. they correspond to C|B|A + uint8_t x = hall_INPUT(); + + uint8_t v = x & 0b111; // already masked by hall_INPUT() but better be sure + + if(embot::hw::motor::hall::Mode::SWAP::BC == mode.swap) + { + // in case swapBC is true, then we swap second (H2) w/ third (H3), so we have v = H2H3H1 = BCA + + v = ((x & 0b001) ) | // gets 0|0|H1 and keeps it where it is + ((x & 0b010) << 1) | // gets 0|H2|0 and moves it up by one position + ((x & 0b100) >> 1) ; // gets H3|0|0 and moves it down by one position + } + return v; + } + + void acquire() + { + // actually ... we + // get the input and validate it vs possible states and possible transitions + + if(true == validate(input())) + { + // i assign angle + data.angle = mode.offset + hall_Table::status2angle(data.h3h2h1[hall_Data::CURR]); + + // i may do.... + + switch(config.encodertuning) + { + case Configuration::ENCODERtuning::forcevalue: + { + embot::hw::motor::enc::force(data.angle); + } break; + + case Configuration::ENCODERtuning::adjust: + { + encoderadjust(); + } break; + + case Configuration::ENCODERtuning::none: + default: + { + // do nothing + } break; + } -hall_Internals _hall_internals {}; + } + } + + + bool validate(uint8_t v) + { + bool r {true}; + + // 1. validate h3h2h1 + + if(false == hall_Data::isH3H2H1valid(v)) + { + // error +// data.h3h2h1[hall_Data::CURR] = data.h3h2h1[hall_Data::PREV]; + + // RAISE ERROR STATE + motorhal_set_fault(DHESInvalidValue); + return false; + } + + // 2. update h3h2h1 status + + // first time of call after a reset() + if(true == data.firstacquisition) + { + data.firstacquisition = false; + // i use current value v for both + data.h3h2h1[hall_Data::PREV] = data.h3h2h1[hall_Data::CURR] = v; + } + else + { + // i store previous h3h2h1 and i assign the new h3h2h1 + data.h3h2h1[hall_Data::PREV] = data.h3h2h1[hall_Data::CURR]; + data.h3h2h1[hall_Data::CURR] = v; + } + + // 3. validate transition from previous h3h2h1 to the current one. + // we can directly use a transition table or to verify that the sectors that corresponds to the two h3h2h1 values + // are the same or spaced by one + + if(data.h3h2h1[hall_Data::PREV] != data.h3h2h1[hall_Data::CURR]) + { + if(false == hall_Data::isTransitionOfH3H2H1valid(data.h3h2h1[hall_Data::PREV], data.h3h2h1[hall_Data::CURR])) + { + motorhal_set_fault(DHESInvalidSequence); + return false; + } + } + + return true; + } + + void encoderadjust() + { + // in here we do what is done for amcbldc in function s_pwm_updateHallStatus() for the case when MainConf.encoder.resolution is not zero. + // that means we have an encoder and we want the hall to help its acquisition. + +#if 0 + if (!s_pwm_hallStatus_old) + { + hallCounter = 0; + encoder_Reset(); + encoder_Force(angle); + } + else + { + // Check if the motor is rotating forward (counterclockwise) + bool forward = ((sector-sector_old+numHallSectors)%numHallSectors)==1; + + if (forward) // forward + { + ++hallCounter; + angle -= minHallAngleDelta; // -30 deg + } + else + { + --hallCounter; + angle += minHallAngleDelta; // +30 deg + } + /* + 0) Use the Hall sensors to rotate until the wrap-around border is reached, + then reset the encoder value + */ + if (calibration_step == 0) + { + encoder_Force(angle); + + if ((sector == 0 && sector_old == 5) || (sector == 5 && sector_old == 0)) + { + encoder_Reset(); + calibration_step = 1; + } + } + /* + 1) Use the hall sensors to rotate. While rotating, store the encoder angle + every time a sector border is crossed. When all 6 borders are crossed, + compute the offset to apply to the encoder by least squares fitting. After + the offset is applied set encoderCalibrated to true + */ + else if (calibration_step == 1) + { + encoder_Force(angle); + + // use the current sector if forward rotation or previous if reverse + uint8_t sector_index = forward ? sector : (sector+1)%numHallSectors; + + // keep track of the encoder value between sectors + border[sector_index] = encoder_GetUncalibrated(); + // found the s-th border, put a 1 in the mask + border_flag |= 1 << sector_index; + + // If all sectors are found apply least squares fitting by computing average of + // difference between measured values on borders and expected hall angle + if (border_flag == 63) // 111111 + { + calibration_step = 2; + + int32_t offset = int16_t(border[0] - MainConf.pwm.hall_offset + minHallAngleDelta); + offset += int16_t(border[1] - MainConf.pwm.hall_offset + minHallAngleDelta - hallAngleStep); + offset += int16_t(border[2] - MainConf.pwm.hall_offset + minHallAngleDelta - 2 * hallAngleStep); + offset += int16_t(border[3] - MainConf.pwm.hall_offset + minHallAngleDelta - 3 * hallAngleStep); + offset += int16_t(border[4] - MainConf.pwm.hall_offset + minHallAngleDelta - 4 * hallAngleStep); + offset += int16_t(border[5] - MainConf.pwm.hall_offset + minHallAngleDelta - 5 * hallAngleStep); + + offset /= numHallSectors; + + embot::core::print("CALIBRATED\n"); + + encoder_Calibrate(int16_t(offset)); + } + } + /* + 2) Update the forced value even if it is not used when the encoder is calibrated. + Reset the encoder angle after a full rotation to avoid desynching + */ + else if (calibration_step == 2) + { + encoder_Force(angle); + // reset the angle after full rotation + if ((sector == 0 && sector_old == 5) || (sector == 5 && sector_old == 0)) + { + encoder_Reset(); + } + } -static uint8_t hall_Get() -{ - uint8_t x = hall_INPUT(); // we get the values of the three bits: H3|H2|H1. they are also called C|B|A - // and now we move them according to order[]. - // if we have order = {0, 1, 2} we dont reorder anything, so we have v = x = H3H2H1 = CBA. - // but in case swapBC is true, then we have order {0, 2, 1} which swap second w/third, so v = H2H3H1 = BCA +#endif + + } + - uint8_t v = (((x>>0)&0x1) >> _hall_internals.data.order[0]) | // H1 (pos 0) is moved to the final position - (((x>>1)&0x1) >> _hall_internals.data.order[1]) | // H2 (pos 1) is moved to the final position - (((x>>2)&0x1) >> _hall_internals.data.order[2]); // H3 (pos 2) is moved to the final position - return v; -} + static void capture(hall_Internals &hint) + { + hint.acquire(); + } +}; + + +hall_Internals _hall_internals {}; static void hall_OnCapture(TIM_HandleTypeDef *htim) { - _hall_internals.data.status = hall_Get(); - - // in here, we surely fill angle. - _hall_internals.data.angle = _hall_internals.mode.offset + hall_Table::status2angle(_hall_internals.data.status); + hall_Internals::capture(_hall_internals); } @@ -527,10 +781,8 @@ bool start(const Mode &mode) bool ret {true}; _hall_internals.mode = mode; - - _hall_internals.data.set(_hall_internals.mode.swap); - - _hall_internals.data.status = hall_Get(); + + hall_Internals::capture(_hall_internals); // marco.accame: i stop things @@ -574,7 +826,7 @@ bool start(const Mode &mode) _hall_internals.started = ret; // i read it again - _hall_internals.data.status = hall_Get(); + hall_Internals::capture(_hall_internals); return ret; @@ -587,7 +839,7 @@ bool isstarted() uint8_t getstatus() { - return _hall_internals.data.status; + return _hall_internals.data.h3h2h1[hall_Data::CURR]; } int32_t getangle() @@ -682,6 +934,38 @@ extern void set(uint16_t u, uint16_t v, uint16_t w) // PwmComparePhaseW = w; // _pwm_internals.pwmcomparevalue_protect(false); } + +uint16_t map2integer(float x) +{ + static constexpr float maxpwmdiv100 = static_cast(embot::hw::motor::bsp::amc2c::PWMvals.value100perc) / 100.0; + uint16_t r = 0; + if((x > 0) && (x < 100.0)) + { + float t = x*maxpwmdiv100; + r = static_cast(t); + } + else if(x >= 100.0) + { + r = embot::hw::motor::bsp::amc2c::PWMvals.value100perc; + } + + return r; +} + +extern void pwmset(float u, float v, float w) +{ + PwmPhaseSet(map2integer(u), map2integer(v), map2integer(w)); +} + +extern void setperc(float u, float v, float w) +{ + if(true == _pwm_internals.calibrating) + { + u = v = w = 0; + } + + pwmset(u, v, w); +} extern void enable(bool on) { diff --git a/emBODY/eBcode/arch-arm/board/amc2c/bsp/motorhal/motorhal.h b/emBODY/eBcode/arch-arm/board/amc2c/bsp/motorhal/motorhal.h index ba51c1c09a..359009363b 100644 --- a/emBODY/eBcode/arch-arm/board/amc2c/bsp/motorhal/motorhal.h +++ b/emBODY/eBcode/arch-arm/board/amc2c/bsp/motorhal/motorhal.h @@ -23,6 +23,7 @@ this API exposes what is required to move the motor using embot::hw::motor namespace embot::hw::motor::adc { // we keep int32_t even if the adc gets only int16_t values +// the unit is [mA] struct Currents { int32_t u {0}; @@ -124,7 +125,8 @@ bool init(const Configuration &config); bool deinit(); bool start(const Mode& mode); bool isstarted(); -int32_t getvalue(); +int32_t getvalue(); +void force(int32_t value); } // namespace embot::hw::motor::enc { @@ -134,8 +136,10 @@ namespace embot::hw::motor::hall { struct Configuration { enum class ACQUISITION { deferred }; + enum class ENCODERtuning { none, forcevalue, adjust }; ACQUISITION acquisition { ACQUISITION::deferred }; + ENCODERtuning encodertuning { ENCODERtuning::none }; constexpr Configuration() = default; constexpr bool isvalid() const { return true; } @@ -182,7 +186,8 @@ struct Configuration void init(const Configuration &config); void deinit(); void enable(bool on); -void set(uint16_t u, uint16_t v, uint16_t w); +void set(uint16_t u, uint16_t v, uint16_t w); +void setperc(float u, float v, float w); } // namespace embot::hw::motor::pwm { diff --git a/emBODY/eBcode/arch-arm/board/amc2c/bsp/motorhal/motorhal_faults.c b/emBODY/eBcode/arch-arm/board/amc2c/bsp/motorhal/motorhal_faults.c new file mode 100644 index 0000000000..771967ff5e --- /dev/null +++ b/emBODY/eBcode/arch-arm/board/amc2c/bsp/motorhal/motorhal_faults.c @@ -0,0 +1,54 @@ + +/* + * Copyright (C) 2023 iCub Tech - Istituto Italiano di Tecnologia + * Author: Alessandro Scalzo + * email: alessandro.scalzo@iit.it +*/ + + +#if 0 + The files motorhal_config.[h, c] contain macros and code which cuts aways un-necessary dependencies + for the files provided by Giorgio Zini for the motor control of the amcbldc. These files are: + encoder.[c, h], pwm.[c, h], analog.[c, h]. The dependencies cut away are those which offer + generic services such as logging, leds, flash storage. +#endif + +// CODE SHAPER +#if defined(USE_STM32HAL) +#include "motorhal_faults.h" +#endif + +static faultmask_t faultmask; + +unsigned long motorhal_get_faultmask() +{ + return faultmask.faults; +} + +unsigned long motorhal_clear_faultmask() +{ + unsigned long ret = faultmask.faults; + faultmask.faults = 0; + return ret; +} + + +bool motorhal_check_fault(int n) +{ + return (faultmask.faults & (1< data[0] in ICUBPROTO_PERIODICCMD_STATUS */ + + /*b0*/ unsigned ExternalFaultAsserted:1; /* External Fault */ + /*b1*/ unsigned UnderVoltageFailure:1; /* Undervoltage */ + /*b2*/ unsigned OverVoltageFailure:1; /* Overvoltage */ + /*b3*/ unsigned OverCurrentFailure:1; /* OverCurrent */ + /*b4*/ unsigned DHESInvalidValue:1; /* an invalid value of HES has been detected */ + /*b5*/ unsigned AS5045CSumError:1; + /*b6*/ unsigned DHESInvalidSequence:1; /* an invalid sequence of HES activation has been detected */ + /*b7*/ unsigned CANInvalidProtocol:1; /* can protocol incompatible with EMS */ + + + /********************************************************************************************************/ + /* second byte ==> data[4] in ICUBPROTO_PERIODICCMD_STATUS */ + + /*b0*/ unsigned CAN_BufferOverRun:1; /* A CAN fifo full event has happened. + This should never happen because this FW on the 2FOC should + be able to handle CAN with full load. This might happen in + certain blocking operation are requested like save to eeprom + parameters. TODO: verify it. */ + /*b1*/ unsigned SetpointExpired:1; /* UNUSED: put here for mad in msg */ + /*b2*/ unsigned CAN_TXIsPasv:1; /* can IS is TX passive mode */ + /*b3*/ unsigned CAN_RXIsPasv:1; /* can IS in RX passive mode */ + /*b4*/ unsigned CAN_IsWarnTX:1; /* can IS in bus warn in tx (exist only one error for rx and tx, so the bits are used together)*/ + /*b5*/ unsigned CAN_IsWarnRX:1; /* can IS in bus warn in rx*/ + /*b6*/ unsigned unused3:1; /* UNUSED: put here for mad in msg */ + /*b7*/ unsigned unused4:1; /* UNUSED: put here for mad in msg */ + + + /********************************************************************************************************/ + /* third byte (not used in ICUBPROTO_PERIODICCMD_STATUS) */ + + /*b0*/ unsigned ADCCalFailure:1; /* ADC calibration failure */ + /*b1*/ unsigned I2TFailure:1; /* I2T protection */ + /*b2*/ unsigned EMUROMFault:1; /* EMUROM Fault */ + /*b3*/ unsigned EMUROMCRCFault:1; /* EMUROM CRC Fault */ + /*b4*/ unsigned EncoderFault:1; /* Encoder Fault. 1 when homing with zero signal fail */ + /*b5*/ unsigned FirmwareSPITimingError:1; /* SPI reading has been interrupted before finishing by foc loop */ + /*b6*/ unsigned AS5045CalcError:1; + + /*b7*/ unsigned FirmwarePWMFatalError:1; /* This is true when the FOC loop tried to delay the PWM update (see below) + but the wait for the PWM counter timed out (PWM counter freezed?) + This should never happen, and that may indicate + a firmware BUG or abnormal firmware behaviour due to + unpredictable and exotic reasons (malfunctions, voodoo + magic, hardware bugs, gravity inversions, alien invasions, + end of the world). + In any case please consider that this is certainly NOT due + to the firmware developer, but more likely it's electronic + eng. full responsibility :-) */ + + + /********************************************************************************************************/ + /* fourth byte (not used in ICUBPROTO_PERIODICCMD_STATUS) */ + /*b0*/ unsigned CAN_TXWasPasv:1; /* can has been in TX passive mode at least one time. */ + /*b1*/ unsigned CAN_RXWasPasv:1; /* can has been in RX passive */ + /*b2*/ unsigned CAN_RTRFlagActive:1; /* an RTR flag has been seen over the wire. This is not OK for LorCan */ + /*b3*/ unsigned CAN_WasWarn:1; /* can has been in bus warn at least one time */ + /*b4*/ unsigned CAN_DLCError:1; /* at least one DLC error has been seen */ + /*b5*/ unsigned SiliconRevisionFault:1; /* see comments below */ + /*b6*/ unsigned PositionLimitUpper:1; + /*b7*/ unsigned PositionLimitLower:1; + } ; + + unsigned long faults; + } faultmask_t; + + enum { ExternalFaultAsserted = 0, UnderVoltageFailure, OverVoltageFailure, OverCurrentFailure, + DHESInvalidValue, AS5045CSumError, DHESInvalidSequence, CANInvalidProtocol, + CAN_BufferOverRun, SetpointExpired, CAN_TXIsPasv, CAN_RXIsPasv, + CAN_IsWarnTX, CAN_IsWarnRX, unused3, unused4, + ADCCalFailure, I2TFailure, EMUROMFault, EMUROMCRCFault, + EncoderFault, FirmwareSPITimingError, AS5045CalcError, FirmwarePWMFatalError, + CAN_TXWasPasv, CAN_RXWasPasv, CAN_RTRFlagActive, CAN_WasWarn, + CAN_DLCError, SiliconRevisionFault, PositionLimitUpper, PositionLimitLower}; + + extern unsigned long motorhal_get_faultmask(); + extern unsigned long motorhal_clear_faultmask(); + + extern bool motorhal_check_fault(int n); + extern bool motorhal_set_fault(int n); + extern bool motorhal_clear_fault(int n); + + +#ifdef __cplusplus + } /* extern "C" */ +#endif /* __cplusplus */ + +#endif // MOTORHAL_CONFIG_H + +// - end-of-file (leave a blank line after)---------------------------------------------------------------------------- + diff --git a/emBODY/eBcode/arch-arm/board/amc2c/bsp/motorhal/pwm.c b/emBODY/eBcode/arch-arm/board/amc2c/bsp/motorhal/pwm.c index 044d3d0b52..00853a7330 100644 --- a/emBODY/eBcode/arch-arm/board/amc2c/bsp/motorhal/pwm.c +++ b/emBODY/eBcode/arch-arm/board/amc2c/bsp/motorhal/pwm.c @@ -137,10 +137,15 @@ void PwmPhaseSet(uint16_t u, uint16_t v, uint16_t w) /* Must be interrupt-safe */ // taskDISABLE_INTERRUPTS(); - PwmComparePhaseU = (u<1219) ? (u) : (1219); - PwmComparePhaseV = (v<1219) ? (v) : (1219); - PwmComparePhaseW = (w<1219) ? (w) : (1219); +// PwmComparePhaseU = (u<1219) ? (u) : (1219); +// PwmComparePhaseV = (v<1219) ? (v) : (1219); +// PwmComparePhaseW = (w<1219) ? (w) : (1219); // taskENABLE_INTERRUPTS(); + + // we remove from here the check vs max value because we assume that the caller has already done it + PwmComparePhaseU = u; + PwmComparePhaseV = v; + PwmComparePhaseW = w; __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_1, PwmComparePhaseU); __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_2, PwmComparePhaseV); __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_3, PwmComparePhaseW); @@ -421,9 +426,9 @@ void PwmTest(void) #endif // #if defined(MOTORHALCONFIG_DONTUSE_TESTS) + #if defined(MOTORHAL_changes) -// nothing else is required #endif // #if defined(MOTORHAL_changes) diff --git a/emBODY/eBcode/arch-arm/board/amc2c/test/bin/.placeholder.txt b/emBODY/eBcode/arch-arm/board/amc2c/test/bin/.placeholder.txt new file mode 100644 index 0000000000..b0eb8200ba --- /dev/null +++ b/emBODY/eBcode/arch-arm/board/amc2c/test/bin/.placeholder.txt @@ -0,0 +1 @@ +.placeholder diff --git a/emBODY/eBcode/arch-arm/board/amc2c/test/cfg/amc2c-template-appl.sct b/emBODY/eBcode/arch-arm/board/amc2c/test/cfg/amc2c-template-appl.sct new file mode 100644 index 0000000000..34c8df018e --- /dev/null +++ b/emBODY/eBcode/arch-arm/board/amc2c/test/cfg/amc2c-template-appl.sct @@ -0,0 +1,12 @@ + + +LR_IROM1 0x08100000 0x00100000 { ; load region size_region + ER_IROM1 0x08100000 0x00100000 { ; load address = execution address + *.o (RESET, +First) + *(InRoot$$Sections) + .ANY (+RO) + } + RW_IRAM1 0x10000000 0x00048000{ ; RW data + .ANY (+RW +ZI) + } +} diff --git a/emBODY/eBcode/arch-arm/board/amc2c/test/cfg/stm32hal.h7.startup.amc.CM4.s b/emBODY/eBcode/arch-arm/board/amc2c/test/cfg/stm32hal.h7.startup.amc.CM4.s new file mode 100644 index 0000000000..b3a6dea97d --- /dev/null +++ b/emBODY/eBcode/arch-arm/board/amc2c/test/cfg/stm32hal.h7.startup.amc.CM4.s @@ -0,0 +1,621 @@ +;******************** (C) COPYRIGHT 2019 STMicroelectronics ******************** +;* File Name : startup_stm32h745xx.s +;* @author MCD Application Team +;* Description : STM32H7xx devices vector table for MDK-ARM toolchain. +;* This module performs: +;* - Set the initial SP +;* - Set the initial PC == Reset_Handler +;* - Set the vector table entries with the exceptions ISR address +;* - Branches to __main in the C library (which eventually +;* calls main()). +;* After Reset the Cortex-M processor is in Thread mode, +;* priority is Privileged, and the Stack is set to Main. +;* <<< Use Configuration Wizard in Context Menu >>> +;****************************************************************************** +;* @attention +;* +;* Copyright (c) 2019 STMicroelectronics. +;* All rights reserved. +;* +;* This software component is licensed by ST under BSD 3-Clause license, +;* the "License"; You may not use this file except in compliance with the +;* License. You may obtain a copy of the License at: +;* opensource.org/licenses/BSD-3-Clause +;* +;****************************************************************************** + +; Amount of memory (in bytes) allocated for Stack +; Tailor this value to your application needs +; Stack Configuration +; Stack Size (in Bytes) <0x0-0xFFFFFFFF:8> +; + +Stack_Size EQU 0x800 + + AREA STACK, NOINIT, READWRITE, ALIGN=3 +Stack_Mem SPACE Stack_Size +__initial_sp + + +; Heap Configuration +; Heap Size (in Bytes) <0x0-0xFFFFFFFF:8> +; + +Heap_Size EQU 0x8000 + + AREA HEAP, NOINIT, READWRITE, ALIGN=3 +__heap_base +Heap_Mem SPACE Heap_Size +__heap_limit + + PRESERVE8 + THUMB + + +; Vector Table Mapped to Address 0 at Reset + AREA RESET, DATA, READONLY + EXPORT __Vectors + EXPORT __Vectors_End + EXPORT __Vectors_Size + +__Vectors DCD __initial_sp ; Top of Stack + DCD Reset_Handler ; Reset Handler + DCD NMI_Handler ; NMI Handler + DCD HardFault_Handler ; Hard Fault Handler + DCD MemManage_Handler ; MPU Fault Handler + DCD BusFault_Handler ; Bus Fault Handler + DCD UsageFault_Handler ; Usage Fault Handler + DCD 0 ; Reserved + DCD 0 ; Reserved + DCD 0 ; Reserved + DCD 0 ; Reserved + DCD SVC_Handler ; SVCall Handler + DCD DebugMon_Handler ; Debug Monitor Handler + DCD 0 ; Reserved + DCD PendSV_Handler ; PendSV Handler + DCD SysTick_Handler ; SysTick Handler + + ; External Interrupts + DCD WWDG_IRQHandler ; Window WatchDog interrupt ( wwdg1_it, wwdg2_it) + DCD PVD_AVD_IRQHandler ; PVD/AVD through EXTI Line detection + DCD TAMP_STAMP_IRQHandler ; Tamper and TimeStamps through the EXTI line + DCD RTC_WKUP_IRQHandler ; RTC Wakeup through the EXTI line + DCD FLASH_IRQHandler ; FLASH + DCD RCC_IRQHandler ; RCC + DCD EXTI0_IRQHandler ; EXTI Line0 + DCD EXTI1_IRQHandler ; EXTI Line1 + DCD EXTI2_IRQHandler ; EXTI Line2 + DCD EXTI3_IRQHandler ; EXTI Line3 + DCD EXTI4_IRQHandler ; EXTI Line4 + DCD DMA1_Stream0_IRQHandler ; DMA1 Stream 0 + DCD DMA1_Stream1_IRQHandler ; DMA1 Stream 1 + DCD DMA1_Stream2_IRQHandler ; DMA1 Stream 2 + DCD DMA1_Stream3_IRQHandler ; DMA1 Stream 3 + DCD DMA1_Stream4_IRQHandler ; DMA1 Stream 4 + DCD DMA1_Stream5_IRQHandler ; DMA1 Stream 5 + DCD DMA1_Stream6_IRQHandler ; DMA1 Stream 6 + DCD ADC_IRQHandler ; ADC1, ADC2 + DCD FDCAN1_IT0_IRQHandler ; FDCAN1 interrupt line 0 + DCD FDCAN2_IT0_IRQHandler ; FDCAN2 interrupt line 0 + DCD FDCAN1_IT1_IRQHandler ; FDCAN1 interrupt line 1 + DCD FDCAN2_IT1_IRQHandler ; FDCAN2 interrupt line 1 + DCD EXTI9_5_IRQHandler ; External Line[9:5]s + DCD TIM1_BRK_IRQHandler ; TIM1 Break interrupt + DCD TIM1_UP_IRQHandler ; TIM1 Update Interrupt + DCD TIM1_TRG_COM_IRQHandler ; TIM1 Trigger and Commutation Interrupt + DCD TIM1_CC_IRQHandler ; TIM1 Capture Compare + DCD TIM2_IRQHandler ; TIM2 + DCD TIM3_IRQHandler ; TIM3 + DCD TIM4_IRQHandler ; TIM4 + DCD I2C1_EV_IRQHandler ; I2C1 Event + DCD I2C1_ER_IRQHandler ; I2C1 Error + DCD I2C2_EV_IRQHandler ; I2C2 Event + DCD I2C2_ER_IRQHandler ; I2C2 Error + DCD SPI1_IRQHandler ; SPI1 + DCD SPI2_IRQHandler ; SPI2 + DCD USART1_IRQHandler ; USART1 + DCD USART2_IRQHandler ; USART2 + DCD USART3_IRQHandler ; USART3 + DCD EXTI15_10_IRQHandler ; External Line[15:10] + DCD RTC_Alarm_IRQHandler ; RTC Alarm (A and B) through EXTI Line + DCD 0 ; Reserved + DCD TIM8_BRK_TIM12_IRQHandler ; TIM8 Break Interrupt and TIM12 global interrupt + DCD TIM8_UP_TIM13_IRQHandler ; TIM8 Update Interrupt and TIM13 global interrupt + DCD TIM8_TRG_COM_TIM14_IRQHandler ; TIM8 Trigger and Commutation Interrupt and TIM14 global interrupt + DCD TIM8_CC_IRQHandler ; TIM8 Capture Compare Interrupt + DCD DMA1_Stream7_IRQHandler ; DMA1 Stream7 + DCD FMC_IRQHandler ; FMC + DCD SDMMC1_IRQHandler ; SDMMC1 + DCD TIM5_IRQHandler ; TIM5 + DCD SPI3_IRQHandler ; SPI3 + DCD UART4_IRQHandler ; UART4 + DCD UART5_IRQHandler ; UART5 + DCD TIM6_DAC_IRQHandler ; TIM6 and DAC1&2 underrun errors + DCD TIM7_IRQHandler ; TIM7 + DCD DMA2_Stream0_IRQHandler ; DMA2 Stream 0 + DCD DMA2_Stream1_IRQHandler ; DMA2 Stream 1 + DCD DMA2_Stream2_IRQHandler ; DMA2 Stream 2 + DCD DMA2_Stream3_IRQHandler ; DMA2 Stream 3 + DCD DMA2_Stream4_IRQHandler ; DMA2 Stream 4 + DCD ETH_IRQHandler ; Ethernet + DCD ETH_WKUP_IRQHandler ; Ethernet Wakeup through EXTI line + DCD FDCAN_CAL_IRQHandler ; FDCAN calibration unit interrupt + DCD CM7_SEV_IRQHandler ; CM7 Send event interrupt for CM4 + DCD CM4_SEV_IRQHandler ; CM4 Send event interrupt for CM7 + DCD 0 ; Reserved + DCD 0 ; Reserved + DCD DMA2_Stream5_IRQHandler ; DMA2 Stream 5 + DCD DMA2_Stream6_IRQHandler ; DMA2 Stream 6 + DCD DMA2_Stream7_IRQHandler ; DMA2 Stream 7 + DCD USART6_IRQHandler ; USART6 + DCD I2C3_EV_IRQHandler ; I2C3 event + DCD I2C3_ER_IRQHandler ; I2C3 error + DCD OTG_HS_EP1_OUT_IRQHandler ; USB OTG HS End Point 1 Out + DCD OTG_HS_EP1_IN_IRQHandler ; USB OTG HS End Point 1 In + DCD OTG_HS_WKUP_IRQHandler ; USB OTG HS Wakeup through EXTI + DCD OTG_HS_IRQHandler ; USB OTG HS + DCD DCMI_IRQHandler ; DCMI + DCD 0 ; Reserved + DCD RNG_IRQHandler ; Rng + DCD FPU_IRQHandler ; FPU + DCD UART7_IRQHandler ; UART7 + DCD UART8_IRQHandler ; UART8 + DCD SPI4_IRQHandler ; SPI4 + DCD SPI5_IRQHandler ; SPI5 + DCD SPI6_IRQHandler ; SPI6 + DCD SAI1_IRQHandler ; SAI1 + DCD LTDC_IRQHandler ; LTDC + DCD LTDC_ER_IRQHandler ; LTDC error + DCD DMA2D_IRQHandler ; DMA2D + DCD SAI2_IRQHandler ; SAI2 + DCD QUADSPI_IRQHandler ; QUADSPI + DCD LPTIM1_IRQHandler ; LPTIM1 + DCD CEC_IRQHandler ; HDMI_CEC + DCD I2C4_EV_IRQHandler ; I2C4 Event + DCD I2C4_ER_IRQHandler ; I2C4 Error + DCD SPDIF_RX_IRQHandler ; SPDIF_RX + DCD OTG_FS_EP1_OUT_IRQHandler ; USB OTG FS End Point 1 Out + DCD OTG_FS_EP1_IN_IRQHandler ; USB OTG FS End Point 1 In + DCD OTG_FS_WKUP_IRQHandler ; USB OTG FS Wakeup through EXTI + DCD OTG_FS_IRQHandler ; USB OTG FS + DCD DMAMUX1_OVR_IRQHandler ; DMAMUX1 Overrun interrupt + DCD HRTIM1_Master_IRQHandler ; HRTIM Master Timer global Interrupts + DCD HRTIM1_TIMA_IRQHandler ; HRTIM Timer A global Interrupt + DCD HRTIM1_TIMB_IRQHandler ; HRTIM Timer B global Interrupt + DCD HRTIM1_TIMC_IRQHandler ; HRTIM Timer C global Interrupt + DCD HRTIM1_TIMD_IRQHandler ; HRTIM Timer D global Interrupt + DCD HRTIM1_TIME_IRQHandler ; HRTIM Timer E global Interrupt + DCD HRTIM1_FLT_IRQHandler ; HRTIM Fault global Interrupt + DCD DFSDM1_FLT0_IRQHandler ; DFSDM Filter0 Interrupt + DCD DFSDM1_FLT1_IRQHandler ; DFSDM Filter1 Interrupt + DCD DFSDM1_FLT2_IRQHandler ; DFSDM Filter2 Interrupt + DCD DFSDM1_FLT3_IRQHandler ; DFSDM Filter3 Interrupt + DCD SAI3_IRQHandler ; SAI3 global Interrupt + DCD SWPMI1_IRQHandler ; Serial Wire Interface 1 global interrupt + DCD TIM15_IRQHandler ; TIM15 global Interrupt + DCD TIM16_IRQHandler ; TIM16 global Interrupt + DCD TIM17_IRQHandler ; TIM17 global Interrupt + DCD MDIOS_WKUP_IRQHandler ; MDIOS Wakeup Interrupt + DCD MDIOS_IRQHandler ; MDIOS global Interrupt + DCD JPEG_IRQHandler ; JPEG global Interrupt + DCD MDMA_IRQHandler ; MDMA global Interrupt + DCD 0 ; Reserved + DCD SDMMC2_IRQHandler ; SDMMC2 global Interrupt + DCD HSEM1_IRQHandler ; HSEM1 global Interrupt + DCD HSEM2_IRQHandler ; HSEM2 global Interrupt + DCD ADC3_IRQHandler ; ADC3 global Interrupt + DCD DMAMUX2_OVR_IRQHandler ; DMAMUX Overrun interrupt + DCD BDMA_Channel0_IRQHandler ; BDMA Channel 0 global Interrupt + DCD BDMA_Channel1_IRQHandler ; BDMA Channel 1 global Interrupt + DCD BDMA_Channel2_IRQHandler ; BDMA Channel 2 global Interrupt + DCD BDMA_Channel3_IRQHandler ; BDMA Channel 3 global Interrupt + DCD BDMA_Channel4_IRQHandler ; BDMA Channel 4 global Interrupt + DCD BDMA_Channel5_IRQHandler ; BDMA Channel 5 global Interrupt + DCD BDMA_Channel6_IRQHandler ; BDMA Channel 6 global Interrupt + DCD BDMA_Channel7_IRQHandler ; BDMA Channel 7 global Interrupt + DCD COMP1_IRQHandler ; COMP1 global Interrupt + DCD LPTIM2_IRQHandler ; LP TIM2 global interrupt + DCD LPTIM3_IRQHandler ; LP TIM3 global interrupt + DCD LPTIM4_IRQHandler ; LP TIM4 global interrupt + DCD LPTIM5_IRQHandler ; LP TIM5 global interrupt + DCD LPUART1_IRQHandler ; LP UART1 interrupt + DCD WWDG_RST_IRQHandler ; Window Watchdog reset interrupt (exti_d2_wwdg_it, exti_d1_wwdg_it) + DCD CRS_IRQHandler ; Clock Recovery Global Interrupt + DCD ECC_IRQHandler ; ECC diagnostic Global Interrupt + DCD SAI4_IRQHandler ; SAI4 global interrupt + DCD 0 ; Reserved + DCD HOLD_CORE_IRQHandler ; Hold core interrupt + DCD WAKEUP_PIN_IRQHandler ; Interrupt for all 6 wake-up pins + + +__Vectors_End + +__Vectors_Size EQU __Vectors_End - __Vectors + + AREA |.text|, CODE, READONLY + +; Reset handler +Reset_Handler PROC + EXPORT Reset_Handler [WEAK] + IMPORT SystemInit + IMPORT __main + + LDR R0, =SystemInit + BLX R0 + LDR R0, =__main + BX R0 + ENDP + +; Dummy Exception Handlers (infinite loops which can be modified) + +NMI_Handler PROC + EXPORT NMI_Handler [WEAK] + B . + ENDP +HardFault_Handler\ + PROC + EXPORT HardFault_Handler [WEAK] + B . + ENDP +MemManage_Handler\ + PROC + EXPORT MemManage_Handler [WEAK] + B . + ENDP +BusFault_Handler\ + PROC + EXPORT BusFault_Handler [WEAK] + B . + ENDP +UsageFault_Handler\ + PROC + EXPORT UsageFault_Handler [WEAK] + B . + ENDP +SVC_Handler PROC + EXPORT SVC_Handler [WEAK] + B . + ENDP +DebugMon_Handler\ + PROC + EXPORT DebugMon_Handler [WEAK] + B . + ENDP +PendSV_Handler PROC + EXPORT PendSV_Handler [WEAK] + B . + ENDP +SysTick_Handler PROC + EXPORT SysTick_Handler [WEAK] + B . + ENDP + +Default_Handler PROC + + EXPORT WWDG_IRQHandler [WEAK] + EXPORT PVD_AVD_IRQHandler [WEAK] + EXPORT TAMP_STAMP_IRQHandler [WEAK] + EXPORT RTC_WKUP_IRQHandler [WEAK] + EXPORT FLASH_IRQHandler [WEAK] + EXPORT RCC_IRQHandler [WEAK] + EXPORT EXTI0_IRQHandler [WEAK] + EXPORT EXTI1_IRQHandler [WEAK] + EXPORT EXTI2_IRQHandler [WEAK] + EXPORT EXTI3_IRQHandler [WEAK] + EXPORT EXTI4_IRQHandler [WEAK] + EXPORT DMA1_Stream0_IRQHandler [WEAK] + EXPORT DMA1_Stream1_IRQHandler [WEAK] + EXPORT DMA1_Stream2_IRQHandler [WEAK] + EXPORT DMA1_Stream3_IRQHandler [WEAK] + EXPORT DMA1_Stream4_IRQHandler [WEAK] + EXPORT DMA1_Stream5_IRQHandler [WEAK] + EXPORT DMA1_Stream6_IRQHandler [WEAK] + EXPORT DMA1_Stream7_IRQHandler [WEAK] + EXPORT ADC_IRQHandler [WEAK] + EXPORT FDCAN1_IT0_IRQHandler [WEAK] + EXPORT FDCAN2_IT0_IRQHandler [WEAK] + EXPORT FDCAN1_IT1_IRQHandler [WEAK] + EXPORT FDCAN2_IT1_IRQHandler [WEAK] + EXPORT EXTI9_5_IRQHandler [WEAK] + EXPORT TIM1_BRK_IRQHandler [WEAK] + EXPORT TIM1_UP_IRQHandler [WEAK] + EXPORT TIM1_TRG_COM_IRQHandler [WEAK] + EXPORT TIM1_CC_IRQHandler [WEAK] + EXPORT TIM2_IRQHandler [WEAK] + EXPORT TIM3_IRQHandler [WEAK] + EXPORT TIM4_IRQHandler [WEAK] + EXPORT I2C1_EV_IRQHandler [WEAK] + EXPORT I2C1_ER_IRQHandler [WEAK] + EXPORT I2C2_EV_IRQHandler [WEAK] + EXPORT I2C2_ER_IRQHandler [WEAK] + EXPORT SPI1_IRQHandler [WEAK] + EXPORT SPI2_IRQHandler [WEAK] + EXPORT USART1_IRQHandler [WEAK] + EXPORT USART2_IRQHandler [WEAK] + EXPORT USART3_IRQHandler [WEAK] + EXPORT EXTI15_10_IRQHandler [WEAK] + EXPORT RTC_Alarm_IRQHandler [WEAK] + EXPORT TIM8_BRK_TIM12_IRQHandler [WEAK] + EXPORT TIM8_UP_TIM13_IRQHandler [WEAK] + EXPORT TIM8_TRG_COM_TIM14_IRQHandler [WEAK] + EXPORT TIM8_CC_IRQHandler [WEAK] + EXPORT DMA1_Stream7_IRQHandler [WEAK] + EXPORT FMC_IRQHandler [WEAK] + EXPORT SDMMC1_IRQHandler [WEAK] + EXPORT TIM5_IRQHandler [WEAK] + EXPORT SPI3_IRQHandler [WEAK] + EXPORT UART4_IRQHandler [WEAK] + EXPORT UART5_IRQHandler [WEAK] + EXPORT TIM6_DAC_IRQHandler [WEAK] + EXPORT TIM7_IRQHandler [WEAK] + EXPORT DMA2_Stream0_IRQHandler [WEAK] + EXPORT DMA2_Stream1_IRQHandler [WEAK] + EXPORT DMA2_Stream2_IRQHandler [WEAK] + EXPORT DMA2_Stream3_IRQHandler [WEAK] + EXPORT DMA2_Stream4_IRQHandler [WEAK] + EXPORT ETH_IRQHandler [WEAK] + EXPORT ETH_WKUP_IRQHandler [WEAK] + EXPORT FDCAN_CAL_IRQHandler [WEAK] + EXPORT CM7_SEV_IRQHandler [WEAK] + EXPORT CM4_SEV_IRQHandler [WEAK] + EXPORT DMA2_Stream5_IRQHandler [WEAK] + EXPORT DMA2_Stream6_IRQHandler [WEAK] + EXPORT DMA2_Stream7_IRQHandler [WEAK] + EXPORT USART6_IRQHandler [WEAK] + EXPORT I2C3_EV_IRQHandler [WEAK] + EXPORT I2C3_ER_IRQHandler [WEAK] + EXPORT OTG_HS_EP1_OUT_IRQHandler [WEAK] + EXPORT OTG_HS_EP1_IN_IRQHandler [WEAK] + EXPORT OTG_HS_WKUP_IRQHandler [WEAK] + EXPORT OTG_HS_IRQHandler [WEAK] + EXPORT DCMI_IRQHandler [WEAK] + EXPORT RNG_IRQHandler [WEAK] + EXPORT FPU_IRQHandler [WEAK] + EXPORT UART7_IRQHandler [WEAK] + EXPORT UART8_IRQHandler [WEAK] + EXPORT SPI4_IRQHandler [WEAK] + EXPORT SPI5_IRQHandler [WEAK] + EXPORT SPI6_IRQHandler [WEAK] + EXPORT SAI1_IRQHandler [WEAK] + EXPORT LTDC_IRQHandler [WEAK] + EXPORT LTDC_ER_IRQHandler [WEAK] + EXPORT DMA2D_IRQHandler [WEAK] + EXPORT SAI2_IRQHandler [WEAK] + EXPORT QUADSPI_IRQHandler [WEAK] + EXPORT LPTIM1_IRQHandler [WEAK] + EXPORT CEC_IRQHandler [WEAK] + EXPORT I2C4_EV_IRQHandler [WEAK] + EXPORT I2C4_ER_IRQHandler [WEAK] + EXPORT SPDIF_RX_IRQHandler [WEAK] + EXPORT OTG_FS_EP1_OUT_IRQHandler [WEAK] + EXPORT OTG_FS_EP1_IN_IRQHandler [WEAK] + EXPORT OTG_FS_WKUP_IRQHandler [WEAK] + EXPORT OTG_FS_IRQHandler [WEAK] + EXPORT DMAMUX1_OVR_IRQHandler [WEAK] + EXPORT HRTIM1_Master_IRQHandler [WEAK] + EXPORT HRTIM1_TIMA_IRQHandler [WEAK] + EXPORT HRTIM1_TIMB_IRQHandler [WEAK] + EXPORT HRTIM1_TIMC_IRQHandler [WEAK] + EXPORT HRTIM1_TIMD_IRQHandler [WEAK] + EXPORT HRTIM1_TIME_IRQHandler [WEAK] + EXPORT HRTIM1_FLT_IRQHandler [WEAK] + EXPORT DFSDM1_FLT0_IRQHandler [WEAK] + EXPORT DFSDM1_FLT1_IRQHandler [WEAK] + EXPORT DFSDM1_FLT2_IRQHandler [WEAK] + EXPORT DFSDM1_FLT3_IRQHandler [WEAK] + EXPORT SAI3_IRQHandler [WEAK] + EXPORT SWPMI1_IRQHandler [WEAK] + EXPORT TIM15_IRQHandler [WEAK] + EXPORT TIM16_IRQHandler [WEAK] + EXPORT TIM17_IRQHandler [WEAK] + EXPORT MDIOS_WKUP_IRQHandler [WEAK] + EXPORT MDIOS_IRQHandler [WEAK] + EXPORT JPEG_IRQHandler [WEAK] + EXPORT MDMA_IRQHandler [WEAK] + EXPORT SDMMC2_IRQHandler [WEAK] + EXPORT HSEM1_IRQHandler [WEAK] + EXPORT HSEM2_IRQHandler [WEAK] + EXPORT ADC3_IRQHandler [WEAK] + EXPORT DMAMUX2_OVR_IRQHandler [WEAK] + EXPORT BDMA_Channel0_IRQHandler [WEAK] + EXPORT BDMA_Channel1_IRQHandler [WEAK] + EXPORT BDMA_Channel2_IRQHandler [WEAK] + EXPORT BDMA_Channel3_IRQHandler [WEAK] + EXPORT BDMA_Channel4_IRQHandler [WEAK] + EXPORT BDMA_Channel5_IRQHandler [WEAK] + EXPORT BDMA_Channel6_IRQHandler [WEAK] + EXPORT BDMA_Channel7_IRQHandler [WEAK] + EXPORT COMP1_IRQHandler [WEAK] + EXPORT LPTIM2_IRQHandler [WEAK] + EXPORT LPTIM3_IRQHandler [WEAK] + EXPORT LPTIM4_IRQHandler [WEAK] + EXPORT LPTIM5_IRQHandler [WEAK] + EXPORT LPUART1_IRQHandler [WEAK] + EXPORT WWDG_RST_IRQHandler [WEAK] + EXPORT CRS_IRQHandler [WEAK] + EXPORT ECC_IRQHandler [WEAK] + EXPORT SAI4_IRQHandler [WEAK] + EXPORT HOLD_CORE_IRQHandler [WEAK] + EXPORT WAKEUP_PIN_IRQHandler [WEAK] + + +WWDG_IRQHandler +PVD_AVD_IRQHandler +TAMP_STAMP_IRQHandler +RTC_WKUP_IRQHandler +FLASH_IRQHandler +RCC_IRQHandler +EXTI0_IRQHandler +EXTI1_IRQHandler +EXTI2_IRQHandler +EXTI3_IRQHandler +EXTI4_IRQHandler +DMA1_Stream0_IRQHandler +DMA1_Stream1_IRQHandler +DMA1_Stream2_IRQHandler +DMA1_Stream3_IRQHandler +DMA1_Stream4_IRQHandler +DMA1_Stream5_IRQHandler +DMA1_Stream6_IRQHandler +ADC_IRQHandler +FDCAN1_IT0_IRQHandler +FDCAN2_IT0_IRQHandler +FDCAN1_IT1_IRQHandler +FDCAN2_IT1_IRQHandler +EXTI9_5_IRQHandler +TIM1_BRK_IRQHandler +TIM1_UP_IRQHandler +TIM1_TRG_COM_IRQHandler +TIM1_CC_IRQHandler +TIM2_IRQHandler +TIM3_IRQHandler +TIM4_IRQHandler +I2C1_EV_IRQHandler +I2C1_ER_IRQHandler +I2C2_EV_IRQHandler +I2C2_ER_IRQHandler +SPI1_IRQHandler +SPI2_IRQHandler +USART1_IRQHandler +USART2_IRQHandler +USART3_IRQHandler +EXTI15_10_IRQHandler +RTC_Alarm_IRQHandler +TIM8_BRK_TIM12_IRQHandler +TIM8_UP_TIM13_IRQHandler +TIM8_TRG_COM_TIM14_IRQHandler +TIM8_CC_IRQHandler +DMA1_Stream7_IRQHandler +FMC_IRQHandler +SDMMC1_IRQHandler +TIM5_IRQHandler +SPI3_IRQHandler +UART4_IRQHandler +UART5_IRQHandler +TIM6_DAC_IRQHandler +TIM7_IRQHandler +DMA2_Stream0_IRQHandler +DMA2_Stream1_IRQHandler +DMA2_Stream2_IRQHandler +DMA2_Stream3_IRQHandler +DMA2_Stream4_IRQHandler +ETH_IRQHandler +ETH_WKUP_IRQHandler +FDCAN_CAL_IRQHandler +CM7_SEV_IRQHandler +CM4_SEV_IRQHandler +DMA2_Stream5_IRQHandler +DMA2_Stream6_IRQHandler +DMA2_Stream7_IRQHandler +USART6_IRQHandler +I2C3_EV_IRQHandler +I2C3_ER_IRQHandler +OTG_HS_EP1_OUT_IRQHandler +OTG_HS_EP1_IN_IRQHandler +OTG_HS_WKUP_IRQHandler +OTG_HS_IRQHandler +DCMI_IRQHandler +RNG_IRQHandler +FPU_IRQHandler +UART7_IRQHandler +UART8_IRQHandler +SPI4_IRQHandler +SPI5_IRQHandler +SPI6_IRQHandler +SAI1_IRQHandler +LTDC_IRQHandler +LTDC_ER_IRQHandler +DMA2D_IRQHandler +SAI2_IRQHandler +QUADSPI_IRQHandler +LPTIM1_IRQHandler +CEC_IRQHandler +I2C4_EV_IRQHandler +I2C4_ER_IRQHandler +SPDIF_RX_IRQHandler +OTG_FS_EP1_OUT_IRQHandler +OTG_FS_EP1_IN_IRQHandler +OTG_FS_WKUP_IRQHandler +OTG_FS_IRQHandler +DMAMUX1_OVR_IRQHandler +HRTIM1_Master_IRQHandler +HRTIM1_TIMA_IRQHandler +HRTIM1_TIMB_IRQHandler +HRTIM1_TIMC_IRQHandler +HRTIM1_TIMD_IRQHandler +HRTIM1_TIME_IRQHandler +HRTIM1_FLT_IRQHandler +DFSDM1_FLT0_IRQHandler +DFSDM1_FLT1_IRQHandler +DFSDM1_FLT2_IRQHandler +DFSDM1_FLT3_IRQHandler +SAI3_IRQHandler +SWPMI1_IRQHandler +TIM15_IRQHandler +TIM16_IRQHandler +TIM17_IRQHandler +MDIOS_WKUP_IRQHandler +MDIOS_IRQHandler +JPEG_IRQHandler +MDMA_IRQHandler +SDMMC2_IRQHandler +HSEM1_IRQHandler +HSEM2_IRQHandler +ADC3_IRQHandler +DMAMUX2_OVR_IRQHandler +BDMA_Channel0_IRQHandler +BDMA_Channel1_IRQHandler +BDMA_Channel2_IRQHandler +BDMA_Channel3_IRQHandler +BDMA_Channel4_IRQHandler +BDMA_Channel5_IRQHandler +BDMA_Channel6_IRQHandler +BDMA_Channel7_IRQHandler +COMP1_IRQHandler +LPTIM2_IRQHandler +LPTIM3_IRQHandler +LPTIM4_IRQHandler +LPTIM5_IRQHandler +LPUART1_IRQHandler +WWDG_RST_IRQHandler +CRS_IRQHandler +ECC_IRQHandler +SAI4_IRQHandler +HOLD_CORE_IRQHandler +WAKEUP_PIN_IRQHandler + + B . + + ENDP + + ALIGN + +;******************************************************************************* +; User Stack and Heap initialization +;******************************************************************************* + IF :DEF:__MICROLIB + + EXPORT __initial_sp + EXPORT __heap_base + EXPORT __heap_limit + + ELSE + + IMPORT __use_two_region_memory + EXPORT __user_initial_stackheap + +__user_initial_stackheap + + LDR R0, = Heap_Mem + LDR R1, =(Stack_Mem + Stack_Size) + LDR R2, = (Heap_Mem + Heap_Size) + LDR R3, = Stack_Mem + BX LR + + ALIGN + + ENDIF + + END + +;************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE***** diff --git a/emBODY/eBcode/arch-arm/board/amc2c/test/proj/amc2c-appl-test.uvoptx b/emBODY/eBcode/arch-arm/board/amc2c/test/proj/amc2c-appl-test.uvoptx new file mode 100644 index 0000000000..2a2b5d84fa --- /dev/null +++ b/emBODY/eBcode/arch-arm/board/amc2c/test/proj/amc2c-appl-test.uvoptx @@ -0,0 +1,1951 @@ + + + + 1.0 + +
### uVision Project, (C) Keil Software
+ + + *.c + *.s*; *.src; *.a* + *.obj; *.o + *.lib + *.txt; *.h; *.inc; *.md + *.plm + *.cpp + 0 + + + + 0 + 0 + + + + appl-can-stlink + 0x4 + ARM-ADS + + 12000000 + + 1 + 1 + 0 + 1 + 0 + + + 1 + 65535 + 0 + 0 + 0 + + + 79 + 66 + 8 + .\lst\ + + + 1 + 1 + 1 + 0 + 1 + 1 + 0 + 1 + 0 + 0 + 0 + 0 + + + 1 + 1 + 1 + 1 + 1 + 1 + 1 + 0 + 0 + + + 1 + 0 + 0 + + 18 + + 0 + 1 + 1 + 1 + 1 + 1 + 1 + 1 + 1 + 1 + 1 + 1 + 1 + 1 + 0 + 1 + 1 + 1 + 1 + 0 + 0 + 1 + 0 + 0 + 6 + + + + + + + + + + + STLink\ST-LINKIII-KEIL_SWO.dll + + + + 0 + ULP2CM3 + -UP0948199 -O206 -S8 -C0 -P00000003 -N00("ARM CoreSight SW-DP") -D00(6BA02477) -L00(0) -TO65554 -TC200000000 -TT10000000 -TP18 -TDX0 -TDD0 -TDS8000 -TDT0 -TDC1F -TIE80000001 -TIP9 -FO7 -FD10000000 -FC8000 -FN1 -FF0STM32H7x_2048.FLM -FS08000000 -FL0200000 -FP0($$Device:STM32H745IIKx$CMSIS\Flash\STM32H7x_2048.FLM) + + + 0 + UL2CM3 + UL2CM3(-S0 -C0 -P0 ) -FN1 -FC8000 -FD10000000 -FF0STM32H7x_2048 -FL0200000 -FS08000000 -FP0($$Device:STM32H745IIKx$CMSIS\Flash\STM32H7x_2048.FLM) + + + 0 + ST-LINKIII-KEIL_SWO + -U005700373137510539383538 -O462 -SF24000 -C0 -A3 -I0 -HNlocalhost -HP7184 -P1 -N00("ARM CoreSight SW-DP") -D00(6BA02477) -L00(0) -TO19 -TC200000000 -TT400000000 -TP21 -TDS8021 -TDT0 -TDC1F -TIE80000000 -TIP9 -FO7 -FD10000000 -FC8000 -FN1 -FF0STM32H7x_2048.FLM -FS08000000 -FL0200000 -FP0($$Device:STM32H745IIKx$CMSIS\Flash\STM32H7x_2048.FLM) + + + 0 + ARMRTXEVENTFLAGS + -L200 -Z14 -C0 -M1 -T1 + + + 0 + DLGTARM + (1010=-1,-1,-1,-1,0)(1007=-1,-1,-1,-1,0)(1008=-1,-1,-1,-1,0)(1009=-1,-1,-1,-1,0)(1012=-1,-1,-1,-1,0) + + + 0 + ARMDBGFLAGS + + + + 0 + DLGUARM + (105=-1,-1,-1,-1,0) + + + + + + 0 + 1 + signalled + + + 1 + 1 + TXreceivedback + + + 2 + 1 + tde.str + + + 3 + 1 + tout,0x0A + + + 4 + 1 + replyinfo + + + 5 + 1 + dd + + + + + 0 + 2 + osRtxInfo + + + + + 1 + 0 + 0x5C004000 + 0 + + + + 0 + + + 0 + 1 + 0 + 0 + 0 + 0 + 0 + 1 + 0 + 0 + 0 + 0 + 0 + 0 + 0 + 0 + 0 + 0 + 0 + 1 + 1 + 0 + 0 + 0 + + + + 0 + 0 + 0 + + + + + + + + + + + OS Support\Event Viewer + 35905 + + + + 0 + 0 + 0 + 2 + 10000000 + + + + + + appl-can-ulpro + 0x4 + ARM-ADS + + 12000000 + + 1 + 1 + 0 + 1 + 0 + + + 1 + 65535 + 0 + 0 + 0 + + + 79 + 66 + 8 + .\lst\ + + + 1 + 1 + 1 + 0 + 1 + 1 + 0 + 1 + 0 + 0 + 0 + 0 + + + 1 + 1 + 1 + 1 + 1 + 1 + 1 + 0 + 0 + + + 1 + 0 + 1 + + 18 + + 0 + 1 + 1 + 1 + 1 + 1 + 1 + 1 + 1 + 1 + 1 + 1 + 1 + 1 + 0 + 1 + 1 + 1 + 1 + 0 + 0 + 1 + 0 + 0 + 1 + + + + + + + + + + + BIN\ULP2CM3.DLL + + + + 0 + ULP2CM3 + -UP1123199 -O462 -S12 -C0 -P00000003 -N00("ARM CoreSight SW-DP") -D00(6BA02477) -L00(0) -TO32819 -TC200000000 -TT400000000 -TP18 -TDX0 -TDD0 -TDS8001 -TDT0 -TDC1F -TIE80000001 -TIP9 -FO7 -FD10000000 -FC8000 -FN1 -FF0STM32H7x_2048.FLM -FS08000000 -FL0200000 -FP0($$Device:STM32H745IIKx$CMSIS\Flash\STM32H7x_2048.FLM) + + + 0 + UL2CM3 + UL2CM3(-S0 -C0 -P0 ) -FN1 -FC8000 -FD10000000 -FF0STM32H7x_2048 -FL0200000 -FS08000000 -FP0($$Device:STM32H745IIKx$CMSIS\Flash\STM32H7x_2048.FLM) + + + 0 + ST-LINKIII-KEIL_SWO + -U005700373137510539383538 -O462 -SF24000 -C0 -A3 -I0 -HNlocalhost -HP7184 -P1 -N00("ARM CoreSight SW-DP") -D00(6BA02477) -L00(0) -TO19 -TC200000000 -TT400000000 -TP21 -TDS8021 -TDT0 -TDC1F -TIE80000000 -TIP9 -FO7 -FD10000000 -FC8000 -FN1 -FF0STM32H7x_2048.FLM -FS08000000 -FL0200000 -FP0($$Device:STM32H745IIKx$CMSIS\Flash\STM32H7x_2048.FLM) + + + 0 + ARMRTXEVENTFLAGS + -L200 -Z8 -C0 -M1 -T1 + + + 0 + DLGTARM + (1010=-1,-1,-1,-1,0)(1007=-1,-1,-1,-1,0)(1008=-1,-1,-1,-1,0)(1009=-1,-1,-1,-1,0)(1012=-1,-1,-1,-1,0) + + + 0 + ARMDBGFLAGS + + + + 0 + DLGUARM + + + + + + + 0 + 1 + currs + + + 1 + 1 + cc,0x0A + + + 2 + 1 + o,0x0A + + + 3 + 1 + currents->u,0x0A + + + + + 0 + 2 + osRtxInfo + + + + + 1 + 0 + 0x5C004000 + 0 + + + + 0 + + + 0 + 1 + 1 + 0 + 0 + 0 + 0 + 1 + 0 + 0 + 0 + 0 + 0 + 0 + 0 + 0 + 0 + 0 + 0 + 0 + 1 + 0 + 0 + 0 + + + + 0 + 0 + 0 + + + + + + + + + + + OS Support\Event Viewer + 35905 + + + + 0 + 0 + 0 + 2 + 10000000 + + + + + + appl-can-ulpro-test + 0x4 + ARM-ADS + + 12000000 + + 0 + 1 + 1 + 0 + 0 + + + 1 + 65535 + 0 + 0 + 0 + + + 79 + 66 + 8 + .\lst\ + + + 1 + 1 + 1 + 0 + 1 + 1 + 0 + 1 + 0 + 0 + 0 + 0 + + + 1 + 1 + 1 + 1 + 1 + 1 + 1 + 0 + 0 + + + 0 + 0 + 0 + + 18 + + 1 + 0 + 1 + 1 + 1 + 1 + 1 + 1 + 1 + 1 + 0 + 1 + 1 + 1 + 0 + 1 + 1 + 1 + 1 + 0 + 0 + 1 + 0 + 0 + -1 + + + + + + + + + + + + + + + 0 + + + 0 + 0 + 0 + 0 + 0 + 0 + 0 + 0 + 0 + 0 + 0 + 0 + 0 + 0 + 0 + 0 + 0 + 0 + 0 + 0 + 0 + 0 + 0 + 0 + + + + 0 + 0 + 0 + + + + + + + + + + + + + main + 1 + 0 + 0 + 0 + + 1 + 1 + 8 + 0 + 0 + 0 + ..\src\amc2c-main2.cpp + amc2c-main2.cpp + 0 + 0 + + + + + embot::app::board::amc2c + 1 + 0 + 0 + 0 + + 2 + 2 + 8 + 0 + 0 + 0 + ..\src\app-board-amc2c\embot_app_board_amc2c_info.cpp + embot_app_board_amc2c_info.cpp + 0 + 0 + + + 2 + 3 + 8 + 0 + 0 + 0 + ..\src\app-board-amc2c\embot_app_board_amc2c_test01.cpp + embot_app_board_amc2c_test01.cpp + 0 + 0 + + + 2 + 4 + 8 + 0 + 0 + 0 + ..\src\app-board-amc2c\embot_app_board_amc2c_test.cpp + embot_app_board_amc2c_test.cpp + 0 + 0 + + + 2 + 5 + 8 + 0 + 0 + 0 + ..\src\app-board-amc2c\embot_app_board_amc2c_theCANagentCORE.cpp + embot_app_board_amc2c_theCANagentCORE.cpp + 0 + 0 + + + + + theMBD + 1 + 0 + 0 + 0 + + 3 + 6 + 8 + 0 + 0 + 0 + ..\src\app-board-amc2c\embot_app_board_amc2c_theMBD.cpp + embot_app_board_amc2c_theMBD.cpp + 0 + 0 + + + + + embot::app::bldc + 0 + 0 + 0 + 0 + + 4 + 7 + 8 + 0 + 0 + 0 + ..\..\..\..\embot\app\bldc\embot_app_bldc_theApplication.cpp + embot_app_bldc_theApplication.cpp + 0 + 0 + + + 4 + 8 + 8 + 0 + 0 + 0 + ..\..\..\..\embot\app\bldc\embot_app_bldc_theCOMM.cpp + embot_app_bldc_theCOMM.cpp + 0 + 0 + + + 4 + 9 + 8 + 0 + 0 + 0 + ..\..\..\..\embot\app\bldc\embot_app_bldc_theCTRL.cpp + embot_app_bldc_theCTRL.cpp + 0 + 0 + + + 4 + 10 + 8 + 0 + 0 + 0 + ..\..\..\..\embot\app\bldc\embot_app_bldc_theMSGbroker.cpp + embot_app_bldc_theMSGbroker.cpp + 0 + 0 + + + + + embot::app::application + 0 + 0 + 0 + 0 + + 5 + 11 + 8 + 0 + 0 + 0 + ..\..\..\..\embot\app\embot_app_theLEDmanager.cpp + embot_app_theLEDmanager.cpp + 0 + 0 + + + 5 + 12 + 8 + 0 + 0 + 0 + ..\..\..\..\embot\app\embot_app_application_theCANparserCORE.cpp + embot_app_application_theCANparserCORE.cpp + 0 + 0 + + + 5 + 13 + 8 + 0 + 0 + 0 + ..\..\..\..\embot\app\embot_app_application_theCANtracer.cpp + embot_app_application_theCANtracer.cpp + 0 + 0 + + + 5 + 14 + 8 + 0 + 0 + 0 + ..\..\..\..\embot\app\embot_app_scope.cpp + embot_app_scope.cpp + 0 + 0 + + + + + stm32hal + 0 + 0 + 0 + 0 + + 6 + 15 + 4 + 0 + 0 + 0 + ..\..\..\..\libs\lowlevel\stm32hal\lib\stm32hal.h7.amc2c.v1A0.lib + stm32hal.h7.amc2c.v1A0.lib + 0 + 0 + + + 6 + 16 + 2 + 0 + 0 + 0 + ..\cfg\stm32hal.h7.startup.amc.CM4.s + stm32hal.h7.startup.amc.CM4.s + 0 + 0 + + + + + rtos + 0 + 0 + 0 + 0 + + 7 + 17 + 4 + 0 + 0 + 0 + ..\..\..\..\libs\highlevel\abslayer\osal\lib\osal.cm4.dbg.lib + osal.cm4.dbg.lib + 0 + 0 + + + 7 + 18 + 1 + 0 + 0 + 0 + ..\..\..\..\libs\midware\eventviewer\src\eventviewer.c + eventviewer.c + 0 + 0 + + + + + embot::core + 0 + 0 + 0 + 0 + + 8 + 19 + 8 + 0 + 0 + 0 + ..\..\..\..\..\..\..\..\icub-firmware-shared\embot\core\embot_core.cpp + embot_core.cpp + 0 + 0 + + + 8 + 20 + 8 + 0 + 0 + 0 + ..\..\..\..\..\..\..\..\icub-firmware-shared\embot\core\embot_core_binary.cpp + embot_core_binary.cpp + 0 + 0 + + + + + embot::tools + 0 + 0 + 0 + 0 + + 9 + 21 + 8 + 0 + 0 + 0 + ..\..\..\..\..\..\..\..\icub-firmware-shared\embot\tools\embot_tools.cpp + embot_tools.cpp + 0 + 0 + + + + + embot::hw + 1 + 0 + 0 + 0 + + 10 + 22 + 8 + 0 + 0 + 0 + ..\..\..\..\embot\hw\embot_hw.cpp + embot_hw.cpp + 0 + 0 + + + 10 + 23 + 8 + 0 + 0 + 0 + ..\..\..\..\embot\hw\embot_hw_bsp.cpp + embot_hw_bsp.cpp + 0 + 0 + + + 10 + 24 + 8 + 0 + 0 + 0 + ..\..\..\..\embot\hw\embot_hw_gpio.cpp + embot_hw_gpio.cpp + 0 + 0 + + + 10 + 25 + 8 + 0 + 0 + 0 + ..\..\..\..\embot\hw\embot_hw_led.cpp + embot_hw_led.cpp + 0 + 0 + + + 10 + 26 + 8 + 0 + 0 + 0 + ..\..\..\..\embot\hw\embot_hw_sys.cpp + embot_hw_sys.cpp + 0 + 0 + + + 10 + 27 + 8 + 0 + 0 + 0 + ..\..\..\..\embot\hw\embot_hw_button.cpp + embot_hw_button.cpp + 0 + 0 + + + 10 + 28 + 8 + 0 + 0 + 0 + ..\..\..\..\embot\hw\embot_hw_can.cpp + embot_hw_can.cpp + 0 + 0 + + + 10 + 29 + 8 + 0 + 0 + 0 + ..\..\..\..\embot\hw\embot_hw_flash.cpp + embot_hw_flash.cpp + 0 + 0 + + + 10 + 30 + 8 + 0 + 0 + 0 + ..\..\..\..\embot\hw\embot_hw_timer.cpp + embot_hw_timer.cpp + 0 + 0 + + + 10 + 31 + 8 + 0 + 0 + 0 + ..\..\..\..\embot\hw\embot_hw_types.cpp + embot_hw_types.cpp + 0 + 0 + + + 10 + 32 + 8 + 0 + 0 + 0 + ..\..\..\..\embot\hw\embot_hw_motor.cpp + embot_hw_motor.cpp + 0 + 0 + + + 10 + 33 + 8 + 0 + 0 + 0 + ..\..\..\..\embot\hw\embot_hw_testpoint.cpp + embot_hw_testpoint.cpp + 0 + 0 + + + 10 + 34 + 8 + 0 + 0 + 0 + ..\..\..\..\embot\hw\embot_hw_mtx.cpp + embot_hw_mtx.cpp + 0 + 0 + + + 10 + 35 + 8 + 0 + 0 + 0 + ..\..\..\..\embot\hw\embot_hw_icc_sig.cpp + embot_hw_icc_sig.cpp + 0 + 0 + + + 10 + 36 + 8 + 0 + 0 + 0 + ..\..\..\..\embot\hw\embot_hw_icc_mem.cpp + embot_hw_icc_mem.cpp + 0 + 0 + + + 10 + 37 + 8 + 0 + 0 + 0 + ..\..\..\..\embot\hw\embot_hw_icc_ltr.cpp + embot_hw_icc_ltr.cpp + 0 + 0 + + + 10 + 38 + 8 + 0 + 0 + 0 + ..\..\..\..\embot\hw\embot_hw_icc_printer.cpp + embot_hw_icc_printer.cpp + 0 + 0 + + + + + embot::hw::lowlevel-0optimized + 0 + 0 + 0 + 0 + + 11 + 39 + 8 + 0 + 0 + 0 + ..\..\..\..\embot\hw\embot_hw_lowlevel.cpp + embot_hw_lowlevel.cpp + 0 + 0 + + + + + embot::hw::bsp + 1 + 0 + 0 + 0 + + 12 + 40 + 8 + 0 + 0 + 0 + ..\..\bsp\embot_hw_bsp_amc2c.cpp + embot_hw_bsp_amc2c.cpp + 0 + 0 + + + 12 + 41 + 8 + 0 + 0 + 0 + ..\..\bsp\embot_hw_can_bsp_amc2c.cpp + embot_hw_can_bsp_amc2c.cpp + 0 + 0 + + + 12 + 42 + 8 + 0 + 0 + 0 + ..\..\bsp\embot_hw_flash_bsp_amc2c.cpp + embot_hw_flash_bsp_amc2c.cpp + 0 + 0 + + + 12 + 43 + 8 + 0 + 0 + 0 + ..\..\bsp\embot_hw_timer_bsp_amc2c.cpp + embot_hw_timer_bsp_amc2c.cpp + 0 + 0 + + + 12 + 44 + 8 + 0 + 0 + 0 + ..\..\bsp\embot_hw_motor_bsp_amc2c.cpp + embot_hw_motor_bsp_amc2c.cpp + 0 + 0 + + + 12 + 45 + 8 + 0 + 0 + 0 + ..\..\bsp\embot_hw_mtx_bsp_amc2c.cpp + embot_hw_mtx_bsp_amc2c.cpp + 0 + 0 + + + 12 + 46 + 8 + 0 + 0 + 0 + ..\..\bsp\embot_hw_icc_bsp_amc2c.cpp + embot_hw_icc_bsp_amc2c.cpp + 0 + 0 + + + + + embot::os + 0 + 0 + 0 + 0 + + 13 + 47 + 8 + 0 + 0 + 0 + ..\..\..\..\embot\os\embot_os.cpp + embot_os.cpp + 0 + 0 + + + 13 + 48 + 8 + 0 + 0 + 0 + ..\..\..\..\embot\os\embot_os_Action.cpp + embot_os_Action.cpp + 0 + 0 + + + 13 + 49 + 8 + 0 + 0 + 0 + ..\..\..\..\embot\os\embot_os_theCallbackManager.cpp + embot_os_theCallbackManager.cpp + 0 + 0 + + + 13 + 50 + 8 + 0 + 0 + 0 + ..\..\..\..\embot\os\embot_os_theScheduler.cpp + embot_os_theScheduler.cpp + 0 + 0 + + + 13 + 51 + 8 + 0 + 0 + 0 + ..\..\..\..\embot\os\embot_os_theTimerManager.cpp + embot_os_theTimerManager.cpp + 0 + 0 + + + 13 + 52 + 8 + 0 + 0 + 0 + ..\..\..\..\embot\os\embot_os_Thread.cpp + embot_os_Thread.cpp + 0 + 0 + + + 13 + 53 + 8 + 0 + 0 + 0 + ..\..\..\..\embot\os\embot_os_Timer.cpp + embot_os_Timer.cpp + 0 + 0 + + + 13 + 54 + 8 + 0 + 0 + 0 + ..\..\..\..\embot\os\embot_os_rtos.cpp + embot_os_rtos.cpp + 0 + 0 + + + + + embot::prot::can + 0 + 0 + 0 + 0 + + 14 + 55 + 8 + 0 + 0 + 0 + ..\..\..\..\embot\prot\can\embot_prot_can.cpp + embot_prot_can.cpp + 0 + 0 + + + 14 + 56 + 8 + 0 + 0 + 0 + ..\..\..\..\embot\prot\can\embot_prot_can_analog_periodic.cpp + embot_prot_can_analog_periodic.cpp + 0 + 0 + + + 14 + 57 + 8 + 0 + 0 + 0 + ..\..\..\..\embot\prot\can\embot_prot_can_analog_polling.cpp + embot_prot_can_analog_polling.cpp + 0 + 0 + + + 14 + 58 + 8 + 0 + 0 + 0 + ..\..\..\..\embot\prot\can\embot_prot_can_bootloader.cpp + embot_prot_can_bootloader.cpp + 0 + 0 + + + 14 + 59 + 8 + 0 + 0 + 0 + ..\..\..\..\embot\prot\can\embot_prot_can_inertial_periodic.cpp + embot_prot_can_inertial_periodic.cpp + 0 + 0 + + + 14 + 60 + 8 + 0 + 0 + 0 + ..\..\..\..\embot\prot\can\embot_prot_can_motor_periodic.cpp + embot_prot_can_motor_periodic.cpp + 0 + 0 + + + 14 + 61 + 8 + 0 + 0 + 0 + ..\..\..\..\embot\prot\can\embot_prot_can_motor_polling.cpp + embot_prot_can_motor_polling.cpp + 0 + 0 + + + 14 + 62 + 8 + 0 + 0 + 0 + ..\..\..\..\embot\prot\can\embot_prot_can_skin_periodic.cpp + embot_prot_can_skin_periodic.cpp + 0 + 0 + + + + + others-cmsis-math + 0 + 0 + 0 + 0 + + 15 + 63 + 4 + 0 + 0 + 0 + ..\..\..\..\libs\lowlevel\cmsis\dsp\v160\Lib\ARM\arm_cortexM4lf_math.lib + arm_cortexM4lf_math.lib + 0 + 0 + + + + + others::motorhal + 1 + 0 + 0 + 0 + + 16 + 64 + 8 + 0 + 0 + 0 + ..\..\bsp\motorhal\motorhal.cpp + motorhal.cpp + 0 + 0 + + + 16 + 65 + 8 + 0 + 0 + 0 + ..\..\bsp\motorhal\motorhal_config.c + motorhal_config.c + 0 + 0 + + + 16 + 66 + 8 + 0 + 0 + 0 + ..\..\bsp\motorhal\adcm.c + adcm.c + 0 + 0 + + + 16 + 67 + 8 + 0 + 0 + 0 + ..\..\bsp\motorhal\enc.c + enc.c + 0 + 0 + + + 16 + 68 + 8 + 0 + 0 + 0 + ..\..\bsp\motorhal\hall.c + hall.c + 0 + 0 + + + 16 + 69 + 8 + 0 + 0 + 0 + ..\..\bsp\motorhal\pwm.c + pwm.c + 0 + 0 + + + + + mbd + 0 + 0 + 0 + 0 + + 17 + 70 + 8 + 0 + 0 + 0 + ..\..\..\amcbldc\application\src\model-based-design\sharedutils\const_params.cpp + const_params.cpp + 0 + 0 + + + 17 + 71 + 8 + 0 + 0 + 0 + ..\..\..\amcbldc\application\src\model-based-design\sharedutils\rt_hypotf_snf.cpp + rt_hypotf_snf.cpp + 0 + 0 + + + 17 + 72 + 8 + 0 + 0 + 0 + ..\..\..\amcbldc\application\src\model-based-design\sharedutils\rt_nonfinite.cpp + rt_nonfinite.cpp + 0 + 0 + + + 17 + 73 + 8 + 0 + 0 + 0 + ..\..\..\amcbldc\application\src\model-based-design\sharedutils\rt_roundd_snf.cpp + rt_roundd_snf.cpp + 0 + 0 + + + 17 + 74 + 8 + 0 + 0 + 0 + ..\..\..\amcbldc\application\src\model-based-design\sharedutils\rtGetInf.cpp + rtGetInf.cpp + 0 + 0 + + + 17 + 75 + 8 + 0 + 0 + 0 + ..\..\..\amcbldc\application\src\model-based-design\sharedutils\rtGetNaN.cpp + rtGetNaN.cpp + 0 + 0 + + + 17 + 76 + 8 + 0 + 0 + 0 + ..\..\..\amcbldc\application\src\model-based-design\sharedutils\uMultiWord2Double.cpp + uMultiWord2Double.cpp + 0 + 0 + + + 17 + 77 + 8 + 0 + 0 + 0 + ..\..\..\amcbldc\application\src\model-based-design\sharedutils\uMultiWordShl.cpp + uMultiWordShl.cpp + 0 + 0 + + + 17 + 78 + 8 + 0 + 0 + 0 + ..\..\..\amcbldc\application\src\model-based-design\sharedutils\rtw_enable_disable_motors.c + rtw_enable_disable_motors.c + 0 + 0 + + + 17 + 79 + 8 + 0 + 0 + 0 + ..\..\..\amcbldc\application\src\model-based-design\sharedutils\rtw_motor_config.c + rtw_motor_config.c + 0 + 0 + + + + + mbd::can-encoder + 0 + 0 + 0 + 0 + + 18 + 80 + 8 + 0 + 0 + 0 + ..\..\..\amcbldc\application\src\model-based-design\can-encoder\can_encoder.cpp + can_encoder.cpp + 0 + 0 + + + + + mbd::can-decoder + 0 + 0 + 0 + 0 + + 19 + 81 + 8 + 0 + 0 + 0 + ..\..\..\amcbldc\application\src\model-based-design\can-decoder\can_decoder.cpp + can_decoder.cpp + 0 + 0 + + + + + mbd::supervisor-rx + 0 + 0 + 0 + 0 + + 20 + 82 + 8 + 0 + 0 + 0 + ..\..\..\amcbldc\application\src\model-based-design\supervisor-rx\SupervisorFSM_RX.cpp + SupervisorFSM_RX.cpp + 0 + 0 + + + 20 + 83 + 8 + 0 + 0 + 0 + ..\..\..\amcbldc\application\src\model-based-design\supervisor-rx\SupervisorFSM_RX_data.cpp + SupervisorFSM_RX_data.cpp + 0 + 0 + + + + + mbd::supervisor-tx + 0 + 0 + 0 + 0 + + 21 + 84 + 8 + 0 + 0 + 0 + ..\..\..\amcbldc\application\src\model-based-design\supervisor-tx\SupervisorFSM_TX.cpp + SupervisorFSM_TX.cpp + 0 + 0 + + + + + mbd::control-outer + 0 + 0 + 0 + 0 + + 22 + 85 + 8 + 0 + 0 + 0 + ..\..\..\amcbldc\application\src\model-based-design\control-outer\control_outer.cpp + control_outer.cpp + 0 + 0 + + + + + mbd::control-foc + 0 + 0 + 0 + 0 + + 23 + 86 + 8 + 0 + 0 + 0 + ..\..\..\amcbldc\application\src\model-based-design\control-foc\control_foc.cpp + control_foc.cpp + 0 + 0 + + + 23 + 87 + 8 + 0 + 0 + 0 + ..\..\..\amcbldc\application\src\model-based-design\control-foc\control_foc_data.cpp + control_foc_data.cpp + 0 + 0 + + + 23 + 88 + 8 + 0 + 0 + 0 + ..\..\..\amcbldc\application\src\model-based-design\control-foc\FOCInnerLoop.cpp + FOCInnerLoop.cpp + 0 + 0 + + + + + mdb::estimator + 0 + 0 + 0 + 0 + + 24 + 89 + 8 + 0 + 0 + 0 + ..\..\..\amcbldc\application\src\model-based-design\estimator\estimation_velocity.cpp + estimation_velocity.cpp + 0 + 0 + + + + + mbd::filter-current + 0 + 0 + 0 + 0 + + 25 + 90 + 8 + 0 + 0 + 0 + ..\..\..\amcbldc\application\src\model-based-design\filter-current\filter_current.cpp + filter_current.cpp + 0 + 0 + + + + + mbd::amc-bldc + 0 + 0 + 0 + 0 + + 26 + 91 + 8 + 0 + 0 + 0 + ..\..\..\amcbldc\application\src\model-based-design\amc-bldc\AMC_BLDC.cpp + AMC_BLDC.cpp + 0 + 0 + + + + + mbd::thermal-model + 0 + 0 + 0 + 0 + + 27 + 92 + 8 + 0 + 0 + 0 + ..\..\..\amcbldc\application\src\model-based-design\thermal-model\thermal_model.cpp + thermal_model.cpp + 0 + 0 + + + + diff --git a/emBODY/eBcode/arch-arm/board/amc2c/test/proj/amc2c-appl-test.uvprojx b/emBODY/eBcode/arch-arm/board/amc2c/test/proj/amc2c-appl-test.uvprojx new file mode 100644 index 0000000000..77e510e99f --- /dev/null +++ b/emBODY/eBcode/arch-arm/board/amc2c/test/proj/amc2c-appl-test.uvprojx @@ -0,0 +1,3507 @@ + + + + 2.1 + +
### uVision Project, (C) Keil Software
+ + + + appl-can-stlink + 0x4 + ARM-ADS + 6190000::V6.19::ARMCLANG + 1 + + + STM32H745IIKx:CM4 + STMicroelectronics + Keil.STM32H7xx_DFP.3.1.1 + https://www.keil.com/pack/ + IRAM(0x10000000,0x00048000) IROM(0x08100000,0x00100000) CPUTYPE("Cortex-M4") FPU2 CLOCK(12000000) ELITTLE + + + UL2CM3(-S0 -C0 -P0 -FD10000000 -FC8000 -FN1 -FF0STM32H7x_2048 -FS08000000 -FL0200000 -FP0($$Device:STM32H745IIKx$CMSIS\Flash\STM32H7x_2048.FLM)) + 0 + $$Device:STM32H745IIKx$Drivers\CMSIS\Device\ST\STM32H7xx\Include\stm32h7xx.h + + + + + + + + + + $$Device:STM32H745IIKx$CMSIS\SVD\STM32H745_CM4.svd + 0 + 0 + + + + + + + 0 + 0 + 0 + 0 + 1 + + .\obj\ + amc2c + 1 + 0 + 1 + 1 + 1 + .\lst\ + 1 + 0 + 0 + + 0 + 0 + + + 0 + 0 + 0 + 0 + + + 0 + 0 + + + 0 + 0 + 0 + 0 + + + 0 + 0 + + + 0 + 0 + 0 + 0 + + 0 + + + + 0 + 0 + 0 + 0 + 0 + 1 + 0 + 0 + 0 + 0 + 3 + + + 0 + + + SARMCM3.DLL + -REMAP -MPU + DCM.DLL + -pCM4 + SARMCM3.DLL + -MPU + TCM.DLL + -pCM4 + + + + 1 + 0 + 0 + 0 + 16 + + + + + 1 + 0 + 0 + 1 + 1 + 4096 + + 1 + BIN\UL2CM3.DLL + + + + + + 0 + + + + 0 + 1 + 1 + 1 + 1 + 1 + 1 + 1 + 0 + 1 + 1 + 0 + 1 + 1 + 0 + 0 + 1 + 1 + 1 + 1 + 1 + 1 + 1 + 1 + 1 + 0 + 0 + "Cortex-M4" + + 0 + 0 + 0 + 1 + 1 + 0 + 0 + 2 + 0 + 0 + 0 + 0 + 0 + 8 + 0 + 0 + 0 + 0 + 3 + 3 + 0 + 0 + 0 + 0 + 0 + 0 + 0 + 0 + 0 + 0 + 1 + 0 + 0 + 0 + 0 + 1 + 0 + + + 0 + 0x0 + 0x0 + + + 0 + 0x0 + 0x0 + + + 0 + 0x0 + 0x0 + + + 0 + 0x0 + 0x0 + + + 0 + 0x0 + 0x0 + + + 0 + 0x0 + 0x0 + + + 0 + 0x10000000 + 0x48000 + + + 1 + 0x8100000 + 0x100000 + + + 1 + 0x20000000 + 0x20000 + + + 1 + 0x0 + 0x0 + + + 1 + 0x0 + 0x0 + + + 1 + 0x0 + 0x0 + + + 1 + 0x8100000 + 0x100000 + + + 1 + 0x0 + 0x0 + + + 0 + 0x0 + 0x0 + + + 0 + 0x0 + 0x0 + + + 0 + 0x0 + 0x0 + + + 0 + 0x10000000 + 0x48000 + + + 0 + 0x0 + 0x0 + + + + + + 1 + 5 + 0 + 0 + 1 + 0 + 0 + 0 + 0 + 0 + 3 + 0 + 1 + 0 + 0 + 0 + 3 + 8 + 1 + 1 + 0 + 0 + 0 + + -Wno-pragma-pack -Wno-deprecated-register -DEMBOT_USE_rtos_osal + USE_STM32HAL STM32HAL_BOARD_AMC2C STM32HAL_DRIVER_V1A0 + + ..\..\..\..\libs\lowlevel\stm32hal\api;..\..\..\..\libs\highlevel\abslayer\osal\api;..\..\..\..\..\..\..\..\icub-firmware-shared\embot\core;..\..\..\..\embot\hw;..\..\..\..\embot\os;..\..\..\..\embot\app;..\..\..\..\embot\app;..\..\..\..\libs\midware\eventviewer\api;..\..\..\..\libs\highlevel\services\embenv\api;..\..\..\..\embot\prot\can;..\..\..\..\..\..\..\..\icub-firmware-shared\can\canProtocolLib;..\..\..\..\..\..\..\..\icub-firmware-shared\embot\tools;..\..\bsp;..\..\..\..\embot\app\eth;..\..\..\..\embot\app\bldc;..\src\app-board-amc2c;..\..\..\amcbldc\application\src\model-based-design\sharedutils;..\..\..\amcbldc\application\src\model-based-design\can-decoder;..\..\..\amcbldc\application\src\model-based-design\can-encoder;..\..\..\amcbldc\application\src\model-based-design\supervisor-rx;..\..\..\amcbldc\application\src\model-based-design\supervisor-tx;..\..\..\amcbldc\application\src\model-based-design\control-outer;..\..\..\amcbldc\application\src\model-based-design\control-foc;..\..\..\amcbldc\application\src\model-based-design\estimator;..\..\..\amcbldc\application\src\model-based-design\amc-bldc;..\..\..\amcbldc\application\src\model-based-design\filter-current;..\..\..\amcbldc\application\src\model-based-design\thermal-model;..\..\..\..\libs\lowlevel\cmsis\dsp\v160\Include;..\..\bsp\motorhal + + + + 1 + 0 + 0 + 0 + 0 + 0 + 0 + 0 + 0 + 4 + + + + + + + + + 0 + 0 + 0 + 0 + 1 + 0 + 0x08000000 + 0x20000000 + + ..\cfg\amc2c-template-appl.sct + + + --diag_suppress=L6329 + + + + + + + + main + + + amc2c-main2.cpp + 8 + ..\src\amc2c-main2.cpp + + + + + embot::app::board::amc2c + + + embot_app_board_amc2c_info.cpp + 8 + ..\src\app-board-amc2c\embot_app_board_amc2c_info.cpp + + + embot_app_board_amc2c_test01.cpp + 8 + ..\src\app-board-amc2c\embot_app_board_amc2c_test01.cpp + + + 2 + 0 + 0 + 0 + 0 + 2 + 2 + 2 + 2 + 2 + 11 + + + 1 + + + + 2 + 1 + 2 + 2 + 2 + 2 + 2 + 2 + 2 + 2 + 0 + 2 + 2 + 2 + 2 + 2 + 0 + 0 + 2 + 2 + 2 + 2 + 2 + + + + + + + + + + + + embot_app_board_amc2c_test.cpp + 8 + ..\src\app-board-amc2c\embot_app_board_amc2c_test.cpp + + + 2 + 0 + 0 + 0 + 0 + 0 + 2 + 2 + 2 + 2 + 11 + + + 1 + + + + 2 + 0 + 2 + 2 + 2 + 2 + 2 + 2 + 2 + 2 + 0 + 2 + 2 + 2 + 2 + 2 + 0 + 0 + 2 + 2 + 2 + 2 + 2 + + + + + + + + + + + + embot_app_board_amc2c_theCANagentCORE.cpp + 8 + ..\src\app-board-amc2c\embot_app_board_amc2c_theCANagentCORE.cpp + + + + + theMBD + + + embot_app_board_amc2c_theMBD.cpp + 8 + ..\src\app-board-amc2c\embot_app_board_amc2c_theMBD.cpp + + + + + embot::app::bldc + + + embot_app_bldc_theApplication.cpp + 8 + ..\..\..\..\embot\app\bldc\embot_app_bldc_theApplication.cpp + + + embot_app_bldc_theCOMM.cpp + 8 + ..\..\..\..\embot\app\bldc\embot_app_bldc_theCOMM.cpp + + + embot_app_bldc_theCTRL.cpp + 8 + ..\..\..\..\embot\app\bldc\embot_app_bldc_theCTRL.cpp + + + embot_app_bldc_theMSGbroker.cpp + 8 + ..\..\..\..\embot\app\bldc\embot_app_bldc_theMSGbroker.cpp + + + + + embot::app::application + + + embot_app_theLEDmanager.cpp + 8 + ..\..\..\..\embot\app\embot_app_theLEDmanager.cpp + + + embot_app_application_theCANparserCORE.cpp + 8 + ..\..\..\..\embot\app\embot_app_application_theCANparserCORE.cpp + + + embot_app_application_theCANtracer.cpp + 8 + ..\..\..\..\embot\app\embot_app_application_theCANtracer.cpp + + + embot_app_scope.cpp + 8 + ..\..\..\..\embot\app\embot_app_scope.cpp + + + + + stm32hal + + + stm32hal.h7.amc2c.v1A0.lib + 4 + ..\..\..\..\libs\lowlevel\stm32hal\lib\stm32hal.h7.amc2c.v1A0.lib + + + stm32hal.h7.startup.amc.CM4.s + 2 + ..\cfg\stm32hal.h7.startup.amc.CM4.s + + + + + rtos + + + osal.cm4.dbg.lib + 4 + ..\..\..\..\libs\highlevel\abslayer\osal\lib\osal.cm4.dbg.lib + + + eventviewer.c + 1 + ..\..\..\..\libs\midware\eventviewer\src\eventviewer.c + + + + + embot::core + + + embot_core.cpp + 8 + ..\..\..\..\..\..\..\..\icub-firmware-shared\embot\core\embot_core.cpp + + + embot_core_binary.cpp + 8 + ..\..\..\..\..\..\..\..\icub-firmware-shared\embot\core\embot_core_binary.cpp + + + + + embot::tools + + + embot_tools.cpp + 8 + ..\..\..\..\..\..\..\..\icub-firmware-shared\embot\tools\embot_tools.cpp + + + + + embot::hw + + + embot_hw.cpp + 8 + ..\..\..\..\embot\hw\embot_hw.cpp + + + embot_hw_bsp.cpp + 8 + ..\..\..\..\embot\hw\embot_hw_bsp.cpp + + + embot_hw_gpio.cpp + 8 + ..\..\..\..\embot\hw\embot_hw_gpio.cpp + + + embot_hw_led.cpp + 8 + ..\..\..\..\embot\hw\embot_hw_led.cpp + + + embot_hw_sys.cpp + 8 + ..\..\..\..\embot\hw\embot_hw_sys.cpp + + + embot_hw_button.cpp + 8 + ..\..\..\..\embot\hw\embot_hw_button.cpp + + + embot_hw_can.cpp + 8 + ..\..\..\..\embot\hw\embot_hw_can.cpp + + + embot_hw_flash.cpp + 8 + ..\..\..\..\embot\hw\embot_hw_flash.cpp + + + embot_hw_timer.cpp + 8 + ..\..\..\..\embot\hw\embot_hw_timer.cpp + + + embot_hw_types.cpp + 8 + ..\..\..\..\embot\hw\embot_hw_types.cpp + + + embot_hw_motor.cpp + 8 + ..\..\..\..\embot\hw\embot_hw_motor.cpp + + + embot_hw_testpoint.cpp + 8 + ..\..\..\..\embot\hw\embot_hw_testpoint.cpp + + + embot_hw_mtx.cpp + 8 + ..\..\..\..\embot\hw\embot_hw_mtx.cpp + + + embot_hw_icc_sig.cpp + 8 + ..\..\..\..\embot\hw\embot_hw_icc_sig.cpp + + + embot_hw_icc_mem.cpp + 8 + ..\..\..\..\embot\hw\embot_hw_icc_mem.cpp + + + embot_hw_icc_ltr.cpp + 8 + ..\..\..\..\embot\hw\embot_hw_icc_ltr.cpp + + + embot_hw_icc_printer.cpp + 8 + ..\..\..\..\embot\hw\embot_hw_icc_printer.cpp + + + + + embot::hw::lowlevel-0optimized + + + 0 + 0 + 0 + 0 + 0 + 2 + 2 + 2 + 2 + 2 + 11 + + + 1 + + + + 2 + 1 + 2 + 2 + 2 + 2 + 2 + 2 + 2 + 2 + 0 + 2 + 2 + 2 + 2 + 2 + 0 + 0 + 2 + 2 + 2 + 2 + 2 + + + + + + + + + 2 + 2 + 2 + 2 + 2 + 2 + 2 + 2 + 2 + 0 + + + + + + + + + + + + embot_hw_lowlevel.cpp + 8 + ..\..\..\..\embot\hw\embot_hw_lowlevel.cpp + + + 2 + 0 + 0 + 0 + 0 + 2 + 2 + 2 + 2 + 2 + 11 + + + 1 + + + + 2 + 1 + 2 + 2 + 2 + 2 + 2 + 2 + 2 + 2 + 0 + 2 + 2 + 2 + 2 + 2 + 0 + 0 + 2 + 2 + 2 + 2 + 2 + + + + + + + + + + + + + + embot::hw::bsp + + + embot_hw_bsp_amc2c.cpp + 8 + ..\..\bsp\embot_hw_bsp_amc2c.cpp + + + embot_hw_can_bsp_amc2c.cpp + 8 + ..\..\bsp\embot_hw_can_bsp_amc2c.cpp + + + embot_hw_flash_bsp_amc2c.cpp + 8 + ..\..\bsp\embot_hw_flash_bsp_amc2c.cpp + + + embot_hw_timer_bsp_amc2c.cpp + 8 + ..\..\bsp\embot_hw_timer_bsp_amc2c.cpp + + + embot_hw_motor_bsp_amc2c.cpp + 8 + ..\..\bsp\embot_hw_motor_bsp_amc2c.cpp + + + embot_hw_mtx_bsp_amc2c.cpp + 8 + ..\..\bsp\embot_hw_mtx_bsp_amc2c.cpp + + + embot_hw_icc_bsp_amc2c.cpp + 8 + ..\..\bsp\embot_hw_icc_bsp_amc2c.cpp + + + + + embot::os + + + embot_os.cpp + 8 + ..\..\..\..\embot\os\embot_os.cpp + + + embot_os_Action.cpp + 8 + ..\..\..\..\embot\os\embot_os_Action.cpp + + + embot_os_theCallbackManager.cpp + 8 + ..\..\..\..\embot\os\embot_os_theCallbackManager.cpp + + + embot_os_theScheduler.cpp + 8 + ..\..\..\..\embot\os\embot_os_theScheduler.cpp + + + embot_os_theTimerManager.cpp + 8 + ..\..\..\..\embot\os\embot_os_theTimerManager.cpp + + + embot_os_Thread.cpp + 8 + ..\..\..\..\embot\os\embot_os_Thread.cpp + + + embot_os_Timer.cpp + 8 + ..\..\..\..\embot\os\embot_os_Timer.cpp + + + embot_os_rtos.cpp + 8 + ..\..\..\..\embot\os\embot_os_rtos.cpp + + + + + embot::prot::can + + + embot_prot_can.cpp + 8 + ..\..\..\..\embot\prot\can\embot_prot_can.cpp + + + embot_prot_can_analog_periodic.cpp + 8 + ..\..\..\..\embot\prot\can\embot_prot_can_analog_periodic.cpp + + + embot_prot_can_analog_polling.cpp + 8 + ..\..\..\..\embot\prot\can\embot_prot_can_analog_polling.cpp + + + embot_prot_can_bootloader.cpp + 8 + ..\..\..\..\embot\prot\can\embot_prot_can_bootloader.cpp + + + embot_prot_can_inertial_periodic.cpp + 8 + ..\..\..\..\embot\prot\can\embot_prot_can_inertial_periodic.cpp + + + embot_prot_can_motor_periodic.cpp + 8 + ..\..\..\..\embot\prot\can\embot_prot_can_motor_periodic.cpp + + + embot_prot_can_motor_polling.cpp + 8 + ..\..\..\..\embot\prot\can\embot_prot_can_motor_polling.cpp + + + embot_prot_can_skin_periodic.cpp + 8 + ..\..\..\..\embot\prot\can\embot_prot_can_skin_periodic.cpp + + + + + others-cmsis-math + + + arm_cortexM4lf_math.lib + 4 + ..\..\..\..\libs\lowlevel\cmsis\dsp\v160\Lib\ARM\arm_cortexM4lf_math.lib + + + + + others::motorhal + + + motorhal.cpp + 8 + ..\..\bsp\motorhal\motorhal.cpp + + + motorhal_config.c + 8 + ..\..\bsp\motorhal\motorhal_config.c + + + adcm.c + 8 + ..\..\bsp\motorhal\adcm.c + + + enc.c + 8 + ..\..\bsp\motorhal\enc.c + + + hall.c + 8 + ..\..\bsp\motorhal\hall.c + + + pwm.c + 8 + ..\..\bsp\motorhal\pwm.c + + + + + mbd + + + const_params.cpp + 8 + ..\..\..\amcbldc\application\src\model-based-design\sharedutils\const_params.cpp + + + rt_hypotf_snf.cpp + 8 + ..\..\..\amcbldc\application\src\model-based-design\sharedutils\rt_hypotf_snf.cpp + + + rt_nonfinite.cpp + 8 + ..\..\..\amcbldc\application\src\model-based-design\sharedutils\rt_nonfinite.cpp + + + rt_roundd_snf.cpp + 8 + ..\..\..\amcbldc\application\src\model-based-design\sharedutils\rt_roundd_snf.cpp + + + rtGetInf.cpp + 8 + ..\..\..\amcbldc\application\src\model-based-design\sharedutils\rtGetInf.cpp + + + rtGetNaN.cpp + 8 + ..\..\..\amcbldc\application\src\model-based-design\sharedutils\rtGetNaN.cpp + + + uMultiWord2Double.cpp + 8 + ..\..\..\amcbldc\application\src\model-based-design\sharedutils\uMultiWord2Double.cpp + + + uMultiWordShl.cpp + 8 + ..\..\..\amcbldc\application\src\model-based-design\sharedutils\uMultiWordShl.cpp + + + rtw_enable_disable_motors.c + 8 + ..\..\..\amcbldc\application\src\model-based-design\sharedutils\rtw_enable_disable_motors.c + + + rtw_motor_config.c + 8 + ..\..\..\amcbldc\application\src\model-based-design\sharedutils\rtw_motor_config.c + + + + + mbd::can-encoder + + + can_encoder.cpp + 8 + ..\..\..\amcbldc\application\src\model-based-design\can-encoder\can_encoder.cpp + + + + + mbd::can-decoder + + + can_decoder.cpp + 8 + ..\..\..\amcbldc\application\src\model-based-design\can-decoder\can_decoder.cpp + + + + + mbd::supervisor-rx + + + SupervisorFSM_RX.cpp + 8 + ..\..\..\amcbldc\application\src\model-based-design\supervisor-rx\SupervisorFSM_RX.cpp + + + SupervisorFSM_RX_data.cpp + 8 + ..\..\..\amcbldc\application\src\model-based-design\supervisor-rx\SupervisorFSM_RX_data.cpp + + + + + mbd::supervisor-tx + + + SupervisorFSM_TX.cpp + 8 + ..\..\..\amcbldc\application\src\model-based-design\supervisor-tx\SupervisorFSM_TX.cpp + + + + + mbd::control-outer + + + control_outer.cpp + 8 + ..\..\..\amcbldc\application\src\model-based-design\control-outer\control_outer.cpp + + + + + mbd::control-foc + + + control_foc.cpp + 8 + ..\..\..\amcbldc\application\src\model-based-design\control-foc\control_foc.cpp + + + control_foc_data.cpp + 8 + ..\..\..\amcbldc\application\src\model-based-design\control-foc\control_foc_data.cpp + + + FOCInnerLoop.cpp + 8 + ..\..\..\amcbldc\application\src\model-based-design\control-foc\FOCInnerLoop.cpp + + + + + mdb::estimator + + + estimation_velocity.cpp + 8 + ..\..\..\amcbldc\application\src\model-based-design\estimator\estimation_velocity.cpp + + + + + mbd::filter-current + + + filter_current.cpp + 8 + ..\..\..\amcbldc\application\src\model-based-design\filter-current\filter_current.cpp + + + + + mbd::amc-bldc + + + AMC_BLDC.cpp + 8 + ..\..\..\amcbldc\application\src\model-based-design\amc-bldc\AMC_BLDC.cpp + + + + + mbd::thermal-model + + + thermal_model.cpp + 8 + ..\..\..\amcbldc\application\src\model-based-design\thermal-model\thermal_model.cpp + + + + + + + appl-can-ulpro + 0x4 + ARM-ADS + 6190000::V6.19::ARMCLANG + 1 + + + STM32H745IIKx:CM4 + STMicroelectronics + Keil.STM32H7xx_DFP.3.1.1 + https://www.keil.com/pack/ + IRAM(0x10000000,0x00048000) IROM(0x08100000,0x00100000) CPUTYPE("Cortex-M4") FPU2 CLOCK(12000000) ELITTLE + + + UL2CM3(-S0 -C0 -P0 -FD10000000 -FC8000 -FN1 -FF0STM32H7x_2048 -FS08000000 -FL0200000 -FP0($$Device:STM32H745IIKx$CMSIS\Flash\STM32H7x_2048.FLM)) + 0 + $$Device:STM32H745IIKx$Drivers\CMSIS\Device\ST\STM32H7xx\Include\stm32h7xx.h + + + + + + + + + + $$Device:STM32H745IIKx$CMSIS\SVD\STM32H745_CM4.svd + 0 + 0 + + + + + + + 0 + 0 + 0 + 0 + 1 + + .\obj\ + amc2c + 1 + 0 + 1 + 1 + 1 + .\lst\ + 1 + 0 + 0 + + 0 + 0 + + + 0 + 0 + 0 + 0 + + + 0 + 0 + + + 0 + 0 + 0 + 0 + + + 0 + 0 + + + 0 + 0 + 0 + 0 + + 0 + + + + 0 + 0 + 0 + 0 + 0 + 1 + 0 + 0 + 0 + 0 + 3 + + + 0 + + + SARMCM3.DLL + -REMAP -MPU + DCM.DLL + -pCM4 + SARMCM3.DLL + -MPU + TCM.DLL + -pCM4 + + + + 1 + 0 + 0 + 0 + 16 + + + + + 1 + 0 + 0 + 1 + 1 + 4096 + + 1 + BIN\UL2CM3.DLL + + + + + + 0 + + + + 0 + 1 + 1 + 1 + 1 + 1 + 1 + 1 + 0 + 1 + 1 + 0 + 1 + 1 + 0 + 0 + 1 + 1 + 1 + 1 + 1 + 1 + 1 + 1 + 1 + 0 + 0 + "Cortex-M4" + + 0 + 0 + 0 + 1 + 1 + 0 + 0 + 2 + 0 + 0 + 0 + 0 + 0 + 8 + 0 + 0 + 0 + 0 + 3 + 3 + 0 + 0 + 0 + 0 + 0 + 0 + 0 + 0 + 0 + 0 + 1 + 0 + 0 + 0 + 0 + 1 + 0 + + + 0 + 0x0 + 0x0 + + + 0 + 0x0 + 0x0 + + + 0 + 0x0 + 0x0 + + + 0 + 0x0 + 0x0 + + + 0 + 0x0 + 0x0 + + + 0 + 0x0 + 0x0 + + + 0 + 0x10000000 + 0x48000 + + + 1 + 0x8100000 + 0x100000 + + + 1 + 0x20000000 + 0x20000 + + + 1 + 0x0 + 0x0 + + + 1 + 0x0 + 0x0 + + + 1 + 0x0 + 0x0 + + + 1 + 0x8100000 + 0x100000 + + + 1 + 0x0 + 0x0 + + + 0 + 0x0 + 0x0 + + + 0 + 0x0 + 0x0 + + + 0 + 0x0 + 0x0 + + + 0 + 0x10000000 + 0x48000 + + + 0 + 0x0 + 0x0 + + + + + + 1 + 5 + 0 + 0 + 1 + 0 + 0 + 0 + 0 + 0 + 3 + 0 + 1 + 0 + 0 + 0 + 3 + 8 + 1 + 1 + 0 + 0 + 0 + + -Wno-pragma-pack -Wno-deprecated-register -DEMBOT_USE_rtos_osal + USE_STM32HAL STM32HAL_BOARD_AMC2C STM32HAL_DRIVER_V1A0 + + ..\..\..\..\libs\lowlevel\stm32hal\api;..\..\..\..\libs\highlevel\abslayer\osal\api;..\..\..\..\..\..\..\..\icub-firmware-shared\embot\core;..\..\..\..\embot\hw;..\..\..\..\embot\os;..\..\..\..\embot\app;..\..\..\..\embot\app;..\..\..\..\libs\midware\eventviewer\api;..\..\..\..\libs\highlevel\services\embenv\api;..\..\..\..\embot\prot\can;..\..\..\..\..\..\..\..\icub-firmware-shared\can\canProtocolLib;..\..\..\..\..\..\..\..\icub-firmware-shared\embot\tools;..\..\bsp;..\..\..\..\embot\app\eth;..\..\..\..\embot\app\bldc;..\src\app-board-amc2c;..\..\..\amcbldc\application\src\model-based-design\sharedutils;..\..\..\amcbldc\application\src\model-based-design\can-decoder;..\..\..\amcbldc\application\src\model-based-design\can-encoder;..\..\..\amcbldc\application\src\model-based-design\supervisor-rx;..\..\..\amcbldc\application\src\model-based-design\supervisor-tx;..\..\..\amcbldc\application\src\model-based-design\control-outer;..\..\..\amcbldc\application\src\model-based-design\control-foc;..\..\..\amcbldc\application\src\model-based-design\estimator;..\..\..\amcbldc\application\src\model-based-design\amc-bldc;..\..\..\amcbldc\application\src\model-based-design\filter-current;..\..\..\amcbldc\application\src\model-based-design\thermal-model;..\..\..\..\libs\lowlevel\cmsis\dsp\v160\Include;..\..\bsp\motorhal + + + + 1 + 0 + 0 + 0 + 0 + 0 + 0 + 0 + 0 + 4 + + + + + + + + + 0 + 0 + 0 + 0 + 1 + 0 + 0x08000000 + 0x20000000 + + ..\cfg\amc2c-template-appl.sct + + + --diag_suppress=L6329 + + + + + + + + main + + + amc2c-main2.cpp + 8 + ..\src\amc2c-main2.cpp + + + + + embot::app::board::amc2c + + + embot_app_board_amc2c_info.cpp + 8 + ..\src\app-board-amc2c\embot_app_board_amc2c_info.cpp + + + embot_app_board_amc2c_test01.cpp + 8 + ..\src\app-board-amc2c\embot_app_board_amc2c_test01.cpp + + + embot_app_board_amc2c_test.cpp + 8 + ..\src\app-board-amc2c\embot_app_board_amc2c_test.cpp + + + 2 + 0 + 0 + 0 + 0 + 0 + 2 + 2 + 2 + 2 + 11 + + + 1 + + + + 2 + 0 + 2 + 2 + 2 + 2 + 2 + 2 + 2 + 2 + 0 + 2 + 2 + 2 + 2 + 2 + 0 + 0 + 2 + 2 + 2 + 2 + 2 + + + + + + + + + + + + embot_app_board_amc2c_theCANagentCORE.cpp + 8 + ..\src\app-board-amc2c\embot_app_board_amc2c_theCANagentCORE.cpp + + + + + theMBD + + + embot_app_board_amc2c_theMBD.cpp + 8 + ..\src\app-board-amc2c\embot_app_board_amc2c_theMBD.cpp + + + + + embot::app::bldc + + + embot_app_bldc_theApplication.cpp + 8 + ..\..\..\..\embot\app\bldc\embot_app_bldc_theApplication.cpp + + + embot_app_bldc_theCOMM.cpp + 8 + ..\..\..\..\embot\app\bldc\embot_app_bldc_theCOMM.cpp + + + embot_app_bldc_theCTRL.cpp + 8 + ..\..\..\..\embot\app\bldc\embot_app_bldc_theCTRL.cpp + + + embot_app_bldc_theMSGbroker.cpp + 8 + ..\..\..\..\embot\app\bldc\embot_app_bldc_theMSGbroker.cpp + + + + + embot::app::application + + + embot_app_theLEDmanager.cpp + 8 + ..\..\..\..\embot\app\embot_app_theLEDmanager.cpp + + + embot_app_application_theCANparserCORE.cpp + 8 + ..\..\..\..\embot\app\embot_app_application_theCANparserCORE.cpp + + + embot_app_application_theCANtracer.cpp + 8 + ..\..\..\..\embot\app\embot_app_application_theCANtracer.cpp + + + embot_app_scope.cpp + 8 + ..\..\..\..\embot\app\embot_app_scope.cpp + + + + + stm32hal + + + stm32hal.h7.amc2c.v1A0.lib + 4 + ..\..\..\..\libs\lowlevel\stm32hal\lib\stm32hal.h7.amc2c.v1A0.lib + + + stm32hal.h7.startup.amc.CM4.s + 2 + ..\cfg\stm32hal.h7.startup.amc.CM4.s + + + + + rtos + + + osal.cm4.dbg.lib + 4 + ..\..\..\..\libs\highlevel\abslayer\osal\lib\osal.cm4.dbg.lib + + + eventviewer.c + 1 + ..\..\..\..\libs\midware\eventviewer\src\eventviewer.c + + + + + embot::core + + + embot_core.cpp + 8 + ..\..\..\..\..\..\..\..\icub-firmware-shared\embot\core\embot_core.cpp + + + embot_core_binary.cpp + 8 + ..\..\..\..\..\..\..\..\icub-firmware-shared\embot\core\embot_core_binary.cpp + + + + + embot::tools + + + embot_tools.cpp + 8 + ..\..\..\..\..\..\..\..\icub-firmware-shared\embot\tools\embot_tools.cpp + + + + + embot::hw + + + embot_hw.cpp + 8 + ..\..\..\..\embot\hw\embot_hw.cpp + + + embot_hw_bsp.cpp + 8 + ..\..\..\..\embot\hw\embot_hw_bsp.cpp + + + embot_hw_gpio.cpp + 8 + ..\..\..\..\embot\hw\embot_hw_gpio.cpp + + + embot_hw_led.cpp + 8 + ..\..\..\..\embot\hw\embot_hw_led.cpp + + + embot_hw_sys.cpp + 8 + ..\..\..\..\embot\hw\embot_hw_sys.cpp + + + embot_hw_button.cpp + 8 + ..\..\..\..\embot\hw\embot_hw_button.cpp + + + embot_hw_can.cpp + 8 + ..\..\..\..\embot\hw\embot_hw_can.cpp + + + embot_hw_flash.cpp + 8 + ..\..\..\..\embot\hw\embot_hw_flash.cpp + + + embot_hw_timer.cpp + 8 + ..\..\..\..\embot\hw\embot_hw_timer.cpp + + + embot_hw_types.cpp + 8 + ..\..\..\..\embot\hw\embot_hw_types.cpp + + + embot_hw_motor.cpp + 8 + ..\..\..\..\embot\hw\embot_hw_motor.cpp + + + embot_hw_testpoint.cpp + 8 + ..\..\..\..\embot\hw\embot_hw_testpoint.cpp + + + embot_hw_mtx.cpp + 8 + ..\..\..\..\embot\hw\embot_hw_mtx.cpp + + + embot_hw_icc_sig.cpp + 8 + ..\..\..\..\embot\hw\embot_hw_icc_sig.cpp + + + embot_hw_icc_mem.cpp + 8 + ..\..\..\..\embot\hw\embot_hw_icc_mem.cpp + + + embot_hw_icc_ltr.cpp + 8 + ..\..\..\..\embot\hw\embot_hw_icc_ltr.cpp + + + embot_hw_icc_printer.cpp + 8 + ..\..\..\..\embot\hw\embot_hw_icc_printer.cpp + + + + + embot::hw::lowlevel-0optimized + + + 0 + 0 + 0 + 0 + 0 + 2 + 2 + 2 + 2 + 2 + 11 + + + 1 + + + + 2 + 1 + 2 + 2 + 2 + 2 + 2 + 2 + 2 + 2 + 0 + 2 + 2 + 2 + 2 + 2 + 0 + 0 + 2 + 2 + 2 + 2 + 2 + + + + + + + + + 2 + 2 + 2 + 2 + 2 + 2 + 2 + 2 + 2 + 0 + + + + + + + + + + + + embot_hw_lowlevel.cpp + 8 + ..\..\..\..\embot\hw\embot_hw_lowlevel.cpp + + + 2 + 0 + 0 + 0 + 0 + 2 + 2 + 2 + 2 + 2 + 11 + + + 1 + + + + 2 + 1 + 2 + 2 + 2 + 2 + 2 + 2 + 2 + 2 + 0 + 2 + 2 + 2 + 2 + 2 + 0 + 0 + 2 + 2 + 2 + 2 + 2 + + + + + + + + + + + + + + embot::hw::bsp + + + embot_hw_bsp_amc2c.cpp + 8 + ..\..\bsp\embot_hw_bsp_amc2c.cpp + + + embot_hw_can_bsp_amc2c.cpp + 8 + ..\..\bsp\embot_hw_can_bsp_amc2c.cpp + + + embot_hw_flash_bsp_amc2c.cpp + 8 + ..\..\bsp\embot_hw_flash_bsp_amc2c.cpp + + + embot_hw_timer_bsp_amc2c.cpp + 8 + ..\..\bsp\embot_hw_timer_bsp_amc2c.cpp + + + embot_hw_motor_bsp_amc2c.cpp + 8 + ..\..\bsp\embot_hw_motor_bsp_amc2c.cpp + + + embot_hw_mtx_bsp_amc2c.cpp + 8 + ..\..\bsp\embot_hw_mtx_bsp_amc2c.cpp + + + embot_hw_icc_bsp_amc2c.cpp + 8 + ..\..\bsp\embot_hw_icc_bsp_amc2c.cpp + + + + + embot::os + + + embot_os.cpp + 8 + ..\..\..\..\embot\os\embot_os.cpp + + + embot_os_Action.cpp + 8 + ..\..\..\..\embot\os\embot_os_Action.cpp + + + embot_os_theCallbackManager.cpp + 8 + ..\..\..\..\embot\os\embot_os_theCallbackManager.cpp + + + embot_os_theScheduler.cpp + 8 + ..\..\..\..\embot\os\embot_os_theScheduler.cpp + + + embot_os_theTimerManager.cpp + 8 + ..\..\..\..\embot\os\embot_os_theTimerManager.cpp + + + embot_os_Thread.cpp + 8 + ..\..\..\..\embot\os\embot_os_Thread.cpp + + + embot_os_Timer.cpp + 8 + ..\..\..\..\embot\os\embot_os_Timer.cpp + + + embot_os_rtos.cpp + 8 + ..\..\..\..\embot\os\embot_os_rtos.cpp + + + + + embot::prot::can + + + embot_prot_can.cpp + 8 + ..\..\..\..\embot\prot\can\embot_prot_can.cpp + + + embot_prot_can_analog_periodic.cpp + 8 + ..\..\..\..\embot\prot\can\embot_prot_can_analog_periodic.cpp + + + embot_prot_can_analog_polling.cpp + 8 + ..\..\..\..\embot\prot\can\embot_prot_can_analog_polling.cpp + + + embot_prot_can_bootloader.cpp + 8 + ..\..\..\..\embot\prot\can\embot_prot_can_bootloader.cpp + + + embot_prot_can_inertial_periodic.cpp + 8 + ..\..\..\..\embot\prot\can\embot_prot_can_inertial_periodic.cpp + + + embot_prot_can_motor_periodic.cpp + 8 + ..\..\..\..\embot\prot\can\embot_prot_can_motor_periodic.cpp + + + embot_prot_can_motor_polling.cpp + 8 + ..\..\..\..\embot\prot\can\embot_prot_can_motor_polling.cpp + + + embot_prot_can_skin_periodic.cpp + 8 + ..\..\..\..\embot\prot\can\embot_prot_can_skin_periodic.cpp + + + + + others-cmsis-math + + + arm_cortexM4lf_math.lib + 4 + ..\..\..\..\libs\lowlevel\cmsis\dsp\v160\Lib\ARM\arm_cortexM4lf_math.lib + + + + + others::motorhal + + + motorhal.cpp + 8 + ..\..\bsp\motorhal\motorhal.cpp + + + motorhal_config.c + 8 + ..\..\bsp\motorhal\motorhal_config.c + + + adcm.c + 8 + ..\..\bsp\motorhal\adcm.c + + + enc.c + 8 + ..\..\bsp\motorhal\enc.c + + + hall.c + 8 + ..\..\bsp\motorhal\hall.c + + + pwm.c + 8 + ..\..\bsp\motorhal\pwm.c + + + + + mbd + + + const_params.cpp + 8 + ..\..\..\amcbldc\application\src\model-based-design\sharedutils\const_params.cpp + + + rt_hypotf_snf.cpp + 8 + ..\..\..\amcbldc\application\src\model-based-design\sharedutils\rt_hypotf_snf.cpp + + + rt_nonfinite.cpp + 8 + ..\..\..\amcbldc\application\src\model-based-design\sharedutils\rt_nonfinite.cpp + + + rt_roundd_snf.cpp + 8 + ..\..\..\amcbldc\application\src\model-based-design\sharedutils\rt_roundd_snf.cpp + + + rtGetInf.cpp + 8 + ..\..\..\amcbldc\application\src\model-based-design\sharedutils\rtGetInf.cpp + + + rtGetNaN.cpp + 8 + ..\..\..\amcbldc\application\src\model-based-design\sharedutils\rtGetNaN.cpp + + + uMultiWord2Double.cpp + 8 + ..\..\..\amcbldc\application\src\model-based-design\sharedutils\uMultiWord2Double.cpp + + + uMultiWordShl.cpp + 8 + ..\..\..\amcbldc\application\src\model-based-design\sharedutils\uMultiWordShl.cpp + + + rtw_enable_disable_motors.c + 8 + ..\..\..\amcbldc\application\src\model-based-design\sharedutils\rtw_enable_disable_motors.c + + + rtw_motor_config.c + 8 + ..\..\..\amcbldc\application\src\model-based-design\sharedutils\rtw_motor_config.c + + + + + mbd::can-encoder + + + can_encoder.cpp + 8 + ..\..\..\amcbldc\application\src\model-based-design\can-encoder\can_encoder.cpp + + + + + mbd::can-decoder + + + can_decoder.cpp + 8 + ..\..\..\amcbldc\application\src\model-based-design\can-decoder\can_decoder.cpp + + + + + mbd::supervisor-rx + + + SupervisorFSM_RX.cpp + 8 + ..\..\..\amcbldc\application\src\model-based-design\supervisor-rx\SupervisorFSM_RX.cpp + + + SupervisorFSM_RX_data.cpp + 8 + ..\..\..\amcbldc\application\src\model-based-design\supervisor-rx\SupervisorFSM_RX_data.cpp + + + + + mbd::supervisor-tx + + + SupervisorFSM_TX.cpp + 8 + ..\..\..\amcbldc\application\src\model-based-design\supervisor-tx\SupervisorFSM_TX.cpp + + + + + mbd::control-outer + + + control_outer.cpp + 8 + ..\..\..\amcbldc\application\src\model-based-design\control-outer\control_outer.cpp + + + + + mbd::control-foc + + + control_foc.cpp + 8 + ..\..\..\amcbldc\application\src\model-based-design\control-foc\control_foc.cpp + + + control_foc_data.cpp + 8 + ..\..\..\amcbldc\application\src\model-based-design\control-foc\control_foc_data.cpp + + + FOCInnerLoop.cpp + 8 + ..\..\..\amcbldc\application\src\model-based-design\control-foc\FOCInnerLoop.cpp + + + + + mdb::estimator + + + estimation_velocity.cpp + 8 + ..\..\..\amcbldc\application\src\model-based-design\estimator\estimation_velocity.cpp + + + + + mbd::filter-current + + + filter_current.cpp + 8 + ..\..\..\amcbldc\application\src\model-based-design\filter-current\filter_current.cpp + + + + + mbd::amc-bldc + + + AMC_BLDC.cpp + 8 + ..\..\..\amcbldc\application\src\model-based-design\amc-bldc\AMC_BLDC.cpp + + + + + mbd::thermal-model + + + thermal_model.cpp + 8 + ..\..\..\amcbldc\application\src\model-based-design\thermal-model\thermal_model.cpp + + + + + + + appl-can-ulpro-test + 0x4 + ARM-ADS + 6180000::V6.18::ARMCLANG + 1 + + + STM32H745IIKx:CM4 + STMicroelectronics + Keil.STM32H7xx_DFP.3.1.1 + https://www.keil.com/pack/ + IRAM(0x10000000,0x00048000) IROM(0x08100000,0x00100000) CPUTYPE("Cortex-M4") FPU2 CLOCK(12000000) ELITTLE + + + UL2CM3(-S0 -C0 -P0 -FD10000000 -FC8000 -FN1 -FF0STM32H7x_2048 -FS08000000 -FL0200000 -FP0($$Device:STM32H745IIKx$CMSIS\Flash\STM32H7x_2048.FLM)) + 0 + $$Device:STM32H745IIKx$Drivers\CMSIS\Device\ST\STM32H7xx\Include\stm32h7xx.h + + + + + + + + + + $$Device:STM32H745IIKx$CMSIS\SVD\STM32H745_CM4.svd + 0 + 0 + + + + + + + 0 + 0 + 0 + 0 + 1 + + .\obj\ + amc2c + 1 + 0 + 1 + 1 + 1 + .\lst\ + 1 + 0 + 0 + + 0 + 0 + + + 0 + 0 + 0 + 0 + + + 0 + 0 + + + 0 + 0 + 0 + 0 + + + 0 + 0 + + + 0 + 0 + 0 + 0 + + 0 + + + + 0 + 0 + 0 + 0 + 0 + 1 + 0 + 0 + 0 + 0 + 3 + + + 0 + + + SARMCM3.DLL + -REMAP -MPU + DCM.DLL + -pCM4 + SARMCM3.DLL + -MPU + TCM.DLL + -pCM4 + + + + 1 + 0 + 0 + 0 + 16 + + + + + 1 + 0 + 0 + 1 + 1 + 4096 + + 1 + BIN\UL2CM3.DLL + + + + + + 0 + + + + 0 + 1 + 1 + 1 + 1 + 1 + 1 + 1 + 0 + 1 + 1 + 0 + 1 + 1 + 0 + 0 + 1 + 1 + 1 + 1 + 1 + 1 + 1 + 1 + 1 + 0 + 0 + "Cortex-M4" + + 0 + 0 + 0 + 1 + 1 + 0 + 0 + 2 + 0 + 0 + 0 + 0 + 0 + 8 + 0 + 0 + 0 + 0 + 3 + 3 + 0 + 0 + 0 + 0 + 0 + 0 + 0 + 0 + 0 + 0 + 1 + 0 + 0 + 0 + 0 + 1 + 0 + + + 0 + 0x0 + 0x0 + + + 0 + 0x0 + 0x0 + + + 0 + 0x0 + 0x0 + + + 0 + 0x0 + 0x0 + + + 0 + 0x0 + 0x0 + + + 0 + 0x0 + 0x0 + + + 0 + 0x10000000 + 0x48000 + + + 1 + 0x8100000 + 0x100000 + + + 1 + 0x20000000 + 0x20000 + + + 1 + 0x0 + 0x0 + + + 1 + 0x0 + 0x0 + + + 1 + 0x0 + 0x0 + + + 1 + 0x8100000 + 0x100000 + + + 1 + 0x0 + 0x0 + + + 0 + 0x0 + 0x0 + + + 0 + 0x0 + 0x0 + + + 0 + 0x0 + 0x0 + + + 0 + 0x10000000 + 0x48000 + + + 0 + 0x0 + 0x0 + + + + + + 1 + 2 + 0 + 0 + 1 + 0 + 0 + 0 + 0 + 0 + 3 + 0 + 1 + 0 + 0 + 0 + 3 + 8 + 1 + 1 + 0 + 0 + 0 + + -Wno-pragma-pack -Wno-deprecated-register -DEMBOT_USE_rtos_osal + USE_STM32HAL STM32HAL_BOARD_AMC2C STM32HAL_DRIVER_V1A0 MOTORHALCONFIG_enabletests + + ..\..\..\..\libs\lowlevel\stm32hal\api;..\..\..\..\libs\highlevel\abslayer\osal\api;..\..\..\..\..\..\..\..\icub-firmware-shared\embot\core;..\..\..\..\embot\hw;..\..\..\..\embot\os;..\..\..\..\embot\app;..\..\..\..\embot\app;..\..\..\..\libs\midware\eventviewer\api;..\..\..\..\libs\highlevel\services\embenv\api;..\..\..\..\embot\prot\can;..\..\..\..\..\..\..\..\icub-firmware-shared\can\canProtocolLib;..\..\..\..\..\..\..\..\icub-firmware-shared\embot\tools;..\..\bsp;..\..\..\..\embot\app\eth;..\..\..\..\embot\app\bldc;..\src\app-board-amc2c;..\..\..\amcbldc\application\src\model-based-design\sharedutils;..\..\..\amcbldc\application\src\model-based-design\can-decoder;..\..\..\amcbldc\application\src\model-based-design\can-encoder;..\..\..\amcbldc\application\src\model-based-design\supervisor-rx;..\..\..\amcbldc\application\src\model-based-design\supervisor-tx;..\..\..\amcbldc\application\src\model-based-design\control-outer;..\..\..\amcbldc\application\src\model-based-design\control-foc;..\..\..\amcbldc\application\src\model-based-design\estimator;..\..\..\amcbldc\application\src\model-based-design\amc-bldc;..\..\..\amcbldc\application\src\model-based-design\filter-current;..\..\..\..\libs\lowlevel\cmsis\dsp\v160\Include;..\..\bsp\motorhal + + + + 1 + 0 + 0 + 0 + 0 + 0 + 0 + 0 + 0 + 4 + + + + + + + + + 0 + 0 + 0 + 0 + 1 + 0 + 0x08000000 + 0x20000000 + + ..\cfg\amc2c-template-appl.sct + + + --diag_suppress=L6329 + + + + + + + + main + + + amc2c-main2.cpp + 8 + ..\src\amc2c-main2.cpp + + + + + embot::app::board::amc2c + + + embot_app_board_amc2c_info.cpp + 8 + ..\src\app-board-amc2c\embot_app_board_amc2c_info.cpp + + + embot_app_board_amc2c_test01.cpp + 8 + ..\src\app-board-amc2c\embot_app_board_amc2c_test01.cpp + + + 2 + 0 + 0 + 0 + 0 + 0 + 2 + 2 + 2 + 2 + 11 + + + 1 + + + + 2 + 0 + 2 + 2 + 2 + 2 + 2 + 2 + 2 + 2 + 0 + 2 + 2 + 2 + 2 + 2 + 0 + 0 + 2 + 2 + 2 + 2 + 2 + + + + + + + + + + + + embot_app_board_amc2c_test.cpp + 8 + ..\src\app-board-amc2c\embot_app_board_amc2c_test.cpp + + + embot_app_board_amc2c_theCANagentCORE.cpp + 8 + ..\src\app-board-amc2c\embot_app_board_amc2c_theCANagentCORE.cpp + + + + + theMBD + + + embot_app_board_amc2c_theMBD.cpp + 8 + ..\src\app-board-amc2c\embot_app_board_amc2c_theMBD.cpp + + + + + embot::app::bldc + + + embot_app_bldc_theApplication.cpp + 8 + ..\..\..\..\embot\app\bldc\embot_app_bldc_theApplication.cpp + + + embot_app_bldc_theCOMM.cpp + 8 + ..\..\..\..\embot\app\bldc\embot_app_bldc_theCOMM.cpp + + + embot_app_bldc_theCTRL.cpp + 8 + ..\..\..\..\embot\app\bldc\embot_app_bldc_theCTRL.cpp + + + embot_app_bldc_theMSGbroker.cpp + 8 + ..\..\..\..\embot\app\bldc\embot_app_bldc_theMSGbroker.cpp + + + + + embot::app::application + + + embot_app_theLEDmanager.cpp + 8 + ..\..\..\..\embot\app\embot_app_theLEDmanager.cpp + + + embot_app_application_theCANparserCORE.cpp + 8 + ..\..\..\..\embot\app\embot_app_application_theCANparserCORE.cpp + + + embot_app_application_theCANtracer.cpp + 8 + ..\..\..\..\embot\app\embot_app_application_theCANtracer.cpp + + + embot_app_scope.cpp + 8 + ..\..\..\..\embot\app\embot_app_scope.cpp + + + + + stm32hal + + + stm32hal.h7.amc2c.v1A0.lib + 4 + ..\..\..\..\libs\lowlevel\stm32hal\lib\stm32hal.h7.amc2c.v1A0.lib + + + stm32hal.h7.startup.amc.CM4.s + 2 + ..\cfg\stm32hal.h7.startup.amc.CM4.s + + + + + rtos + + + osal.cm4.dbg.lib + 4 + ..\..\..\..\libs\highlevel\abslayer\osal\lib\osal.cm4.dbg.lib + + + eventviewer.c + 1 + ..\..\..\..\libs\midware\eventviewer\src\eventviewer.c + + + + + embot::core + + + embot_core.cpp + 8 + ..\..\..\..\..\..\..\..\icub-firmware-shared\embot\core\embot_core.cpp + + + embot_core_binary.cpp + 8 + ..\..\..\..\..\..\..\..\icub-firmware-shared\embot\core\embot_core_binary.cpp + + + + + embot::tools + + + embot_tools.cpp + 8 + ..\..\..\..\..\..\..\..\icub-firmware-shared\embot\tools\embot_tools.cpp + + + + + embot::hw + + + embot_hw.cpp + 8 + ..\..\..\..\embot\hw\embot_hw.cpp + + + embot_hw_bsp.cpp + 8 + ..\..\..\..\embot\hw\embot_hw_bsp.cpp + + + embot_hw_gpio.cpp + 8 + ..\..\..\..\embot\hw\embot_hw_gpio.cpp + + + embot_hw_led.cpp + 8 + ..\..\..\..\embot\hw\embot_hw_led.cpp + + + embot_hw_sys.cpp + 8 + ..\..\..\..\embot\hw\embot_hw_sys.cpp + + + embot_hw_button.cpp + 8 + ..\..\..\..\embot\hw\embot_hw_button.cpp + + + embot_hw_can.cpp + 8 + ..\..\..\..\embot\hw\embot_hw_can.cpp + + + embot_hw_flash.cpp + 8 + ..\..\..\..\embot\hw\embot_hw_flash.cpp + + + embot_hw_timer.cpp + 8 + ..\..\..\..\embot\hw\embot_hw_timer.cpp + + + embot_hw_types.cpp + 8 + ..\..\..\..\embot\hw\embot_hw_types.cpp + + + embot_hw_motor.cpp + 8 + ..\..\..\..\embot\hw\embot_hw_motor.cpp + + + embot_hw_testpoint.cpp + 8 + ..\..\..\..\embot\hw\embot_hw_testpoint.cpp + + + embot_hw_mtx.cpp + 8 + ..\..\..\..\embot\hw\embot_hw_mtx.cpp + + + embot_hw_icc_sig.cpp + 8 + ..\..\..\..\embot\hw\embot_hw_icc_sig.cpp + + + embot_hw_icc_mem.cpp + 8 + ..\..\..\..\embot\hw\embot_hw_icc_mem.cpp + + + embot_hw_icc_ltr.cpp + 8 + ..\..\..\..\embot\hw\embot_hw_icc_ltr.cpp + + + embot_hw_icc_printer.cpp + 8 + ..\..\..\..\embot\hw\embot_hw_icc_printer.cpp + + + + + embot::hw::lowlevel-0optimized + + + 0 + 0 + 0 + 0 + 0 + 2 + 2 + 2 + 2 + 2 + 11 + + + 1 + + + + 2 + 1 + 2 + 2 + 2 + 2 + 2 + 2 + 2 + 2 + 0 + 2 + 2 + 2 + 2 + 2 + 0 + 0 + 2 + 2 + 2 + 2 + 2 + + + + + + + + + 2 + 2 + 2 + 2 + 2 + 2 + 2 + 2 + 2 + 0 + + + + + + + + + + + + embot_hw_lowlevel.cpp + 8 + ..\..\..\..\embot\hw\embot_hw_lowlevel.cpp + + + 2 + 0 + 0 + 0 + 0 + 2 + 2 + 2 + 2 + 2 + 11 + + + 1 + + + + 2 + 1 + 2 + 2 + 2 + 2 + 2 + 2 + 2 + 2 + 0 + 2 + 2 + 2 + 2 + 2 + 0 + 0 + 2 + 2 + 2 + 2 + 2 + + + + + + + + + + + + + + embot::hw::bsp + + + embot_hw_bsp_amc2c.cpp + 8 + ..\..\bsp\embot_hw_bsp_amc2c.cpp + + + embot_hw_can_bsp_amc2c.cpp + 8 + ..\..\bsp\embot_hw_can_bsp_amc2c.cpp + + + embot_hw_flash_bsp_amc2c.cpp + 8 + ..\..\bsp\embot_hw_flash_bsp_amc2c.cpp + + + embot_hw_timer_bsp_amc2c.cpp + 8 + ..\..\bsp\embot_hw_timer_bsp_amc2c.cpp + + + embot_hw_motor_bsp_amc2c.cpp + 8 + ..\..\bsp\embot_hw_motor_bsp_amc2c.cpp + + + embot_hw_mtx_bsp_amc2c.cpp + 8 + ..\..\bsp\embot_hw_mtx_bsp_amc2c.cpp + + + embot_hw_icc_bsp_amc2c.cpp + 8 + ..\..\bsp\embot_hw_icc_bsp_amc2c.cpp + + + + + embot::os + + + embot_os.cpp + 8 + ..\..\..\..\embot\os\embot_os.cpp + + + embot_os_Action.cpp + 8 + ..\..\..\..\embot\os\embot_os_Action.cpp + + + embot_os_theCallbackManager.cpp + 8 + ..\..\..\..\embot\os\embot_os_theCallbackManager.cpp + + + embot_os_theScheduler.cpp + 8 + ..\..\..\..\embot\os\embot_os_theScheduler.cpp + + + embot_os_theTimerManager.cpp + 8 + ..\..\..\..\embot\os\embot_os_theTimerManager.cpp + + + embot_os_Thread.cpp + 8 + ..\..\..\..\embot\os\embot_os_Thread.cpp + + + embot_os_Timer.cpp + 8 + ..\..\..\..\embot\os\embot_os_Timer.cpp + + + embot_os_rtos.cpp + 8 + ..\..\..\..\embot\os\embot_os_rtos.cpp + + + + + embot::prot::can + + + embot_prot_can.cpp + 8 + ..\..\..\..\embot\prot\can\embot_prot_can.cpp + + + embot_prot_can_analog_periodic.cpp + 8 + ..\..\..\..\embot\prot\can\embot_prot_can_analog_periodic.cpp + + + embot_prot_can_analog_polling.cpp + 8 + ..\..\..\..\embot\prot\can\embot_prot_can_analog_polling.cpp + + + embot_prot_can_bootloader.cpp + 8 + ..\..\..\..\embot\prot\can\embot_prot_can_bootloader.cpp + + + embot_prot_can_inertial_periodic.cpp + 8 + ..\..\..\..\embot\prot\can\embot_prot_can_inertial_periodic.cpp + + + embot_prot_can_motor_periodic.cpp + 8 + ..\..\..\..\embot\prot\can\embot_prot_can_motor_periodic.cpp + + + embot_prot_can_motor_polling.cpp + 8 + ..\..\..\..\embot\prot\can\embot_prot_can_motor_polling.cpp + + + embot_prot_can_skin_periodic.cpp + 8 + ..\..\..\..\embot\prot\can\embot_prot_can_skin_periodic.cpp + + + + + others-cmsis-math + + + arm_cortexM4lf_math.lib + 4 + ..\..\..\..\libs\lowlevel\cmsis\dsp\v160\Lib\ARM\arm_cortexM4lf_math.lib + + + + + others::motorhal + + + motorhal.cpp + 8 + ..\..\bsp\motorhal\motorhal.cpp + + + motorhal_config.c + 8 + ..\..\bsp\motorhal\motorhal_config.c + + + adcm.c + 8 + ..\..\bsp\motorhal\adcm.c + + + enc.c + 8 + ..\..\bsp\motorhal\enc.c + + + hall.c + 8 + ..\..\bsp\motorhal\hall.c + + + pwm.c + 8 + ..\..\bsp\motorhal\pwm.c + + + + + mbd + + + const_params.cpp + 8 + ..\..\..\amcbldc\application\src\model-based-design\sharedutils\const_params.cpp + + + rt_hypotf_snf.cpp + 8 + ..\..\..\amcbldc\application\src\model-based-design\sharedutils\rt_hypotf_snf.cpp + + + rt_nonfinite.cpp + 8 + ..\..\..\amcbldc\application\src\model-based-design\sharedutils\rt_nonfinite.cpp + + + rt_roundd_snf.cpp + 8 + ..\..\..\amcbldc\application\src\model-based-design\sharedutils\rt_roundd_snf.cpp + + + rtGetInf.cpp + 8 + ..\..\..\amcbldc\application\src\model-based-design\sharedutils\rtGetInf.cpp + + + rtGetNaN.cpp + 8 + ..\..\..\amcbldc\application\src\model-based-design\sharedutils\rtGetNaN.cpp + + + uMultiWord2Double.cpp + 8 + ..\..\..\amcbldc\application\src\model-based-design\sharedutils\uMultiWord2Double.cpp + + + uMultiWordShl.cpp + 8 + ..\..\..\amcbldc\application\src\model-based-design\sharedutils\uMultiWordShl.cpp + + + rtw_enable_disable_motors.c + 8 + ..\..\..\amcbldc\application\src\model-based-design\sharedutils\rtw_enable_disable_motors.c + + + rtw_motor_config.c + 8 + ..\..\..\amcbldc\application\src\model-based-design\sharedutils\rtw_motor_config.c + + + + + mbd::can-encoder + + + can_encoder.cpp + 8 + ..\..\..\amcbldc\application\src\model-based-design\can-encoder\can_encoder.cpp + + + + + mbd::can-decoder + + + can_decoder.cpp + 8 + ..\..\..\amcbldc\application\src\model-based-design\can-decoder\can_decoder.cpp + + + + + mbd::supervisor-rx + + + SupervisorFSM_RX.cpp + 8 + ..\..\..\amcbldc\application\src\model-based-design\supervisor-rx\SupervisorFSM_RX.cpp + + + SupervisorFSM_RX_data.cpp + 8 + ..\..\..\amcbldc\application\src\model-based-design\supervisor-rx\SupervisorFSM_RX_data.cpp + + + + + mbd::supervisor-tx + + + SupervisorFSM_TX.cpp + 8 + ..\..\..\amcbldc\application\src\model-based-design\supervisor-tx\SupervisorFSM_TX.cpp + + + + + mbd::control-outer + + + control_outer.cpp + 8 + ..\..\..\amcbldc\application\src\model-based-design\control-outer\control_outer.cpp + + + + + mbd::control-foc + + + control_foc.cpp + 8 + ..\..\..\amcbldc\application\src\model-based-design\control-foc\control_foc.cpp + + + control_foc_data.cpp + 8 + ..\..\..\amcbldc\application\src\model-based-design\control-foc\control_foc_data.cpp + + + FOCInnerLoop.cpp + 8 + ..\..\..\amcbldc\application\src\model-based-design\control-foc\FOCInnerLoop.cpp + + + + + mdb::estimator + + + estimation_velocity.cpp + 8 + ..\..\..\amcbldc\application\src\model-based-design\estimator\estimation_velocity.cpp + + + + + mbd::filter-current + + + filter_current.cpp + 8 + ..\..\..\amcbldc\application\src\model-based-design\filter-current\filter_current.cpp + + + + + mbd::amc-bldc + + + AMC_BLDC.cpp + 8 + ..\..\..\amcbldc\application\src\model-based-design\amc-bldc\AMC_BLDC.cpp + + + + + mbd::thermal-model + + + thermal_model.cpp + 8 + ..\..\..\amcbldc\application\src\model-based-design\thermal-model\thermal_model.cpp + + + + + + + + + + + + + + + + + amc-embot-os + 0 + 1 + + + + + diff --git a/emBODY/eBcode/arch-arm/board/amc2c/test/src/amc2c-main2.cpp b/emBODY/eBcode/arch-arm/board/amc2c/test/src/amc2c-main2.cpp new file mode 100644 index 0000000000..ebfea52224 --- /dev/null +++ b/emBODY/eBcode/arch-arm/board/amc2c/test/src/amc2c-main2.cpp @@ -0,0 +1,31 @@ + +/* + * Copyright (C) 2023 iCub Tech - Istituto Italiano di Tecnologia + * Author: Marco Accame + * email: marco.accame@iit.it +*/ + + +#include "embot_app_bldc_theApplication.h" + +#include "embot_app_board_amc2c_info.h" +#include "embot_app_board_amc2c_test01.h" + +constexpr embot::app::bldc::theApplication::Config cfg +{ + {embot::app::board::amc2c::info::getCodePartition, embot::app::board::amc2c::info::getCANgentCORE}, + {}, // systCfg: the default is typically OK + {}, // CommCfg: the default is typically OK + {embot::app::board::amc2c::test01::Startup, embot::app::board::amc2c::test01::OnTick} // CtrlCfg: the default stack is typically OK +}; + + +int main(void) +{ + embot::app::bldc::theApplication::getInstance().start(cfg); +} + + +// - end-of-file (leave a blank line after)---------------------------------------------------------------------------- + + diff --git a/emBODY/eBcode/arch-arm/board/amc2c/test/src/app-board-amc2c/embot_app_board_amc2c_info.cpp b/emBODY/eBcode/arch-arm/board/amc2c/test/src/app-board-amc2c/embot_app_board_amc2c_info.cpp new file mode 100644 index 0000000000..d991f7f39b --- /dev/null +++ b/emBODY/eBcode/arch-arm/board/amc2c/test/src/app-board-amc2c/embot_app_board_amc2c_info.cpp @@ -0,0 +1,180 @@ + +/* + * Copyright (C) 2023 iCub Tech - Istituto Italiano di Tecnologia + * Author: Marco Accame + * email: marco.accame@iit.it +*/ + + +// -------------------------------------------------------------------------------------------------------------------- +// - public interface +// -------------------------------------------------------------------------------------------------------------------- + + +#include "embot_app_board_amc2c_info.h" + +// -------------------------------------------------------------------------------------------------------------------- +// - external dependencies +// -------------------------------------------------------------------------------------------------------------------- + +#include "embot_app_eth.h" +#include "embot_app_board_amc2c_theCANagentCORE.h" + +// -------------------------------------------------------------------------------------------------------------------- +// - the configurable constants +// -------------------------------------------------------------------------------------------------------------------- + +namespace embot::app::board::amc2c::info { + + constexpr embot::prot::can::applicationInfo applInfo + { + embot::prot::can::versionOfAPPLICATION {100, 100, 0}, + embot::prot::can::versionOfCANPROTOCOL {2, 0} + }; + + constexpr embot::app::eth::Date date + { + 2023, embot::app::eth::Month::Apr, embot::app::eth::Day::fourteen, 16, 54 + }; + + constexpr embot::hw::FLASHpartitionID codePartition + { + embot::hw::FLASHpartitionID::eapplication01 + }; + + constexpr embot::hw::CAN canBus + { + embot::hw::CAN::two + }; + + static const char *info32 + { // 0123456789abcde0123456789abcde + "hi, i am an amc2c can" + }; + +} // embot::app::board::amc2c::info { + + +// -------------------------------------------------------------------------------------------------------------------- +// - all the rest +// -------------------------------------------------------------------------------------------------------------------- + +void force_placement_of_moduleinfo(); + +namespace embot::app::board::amc2c::info { + + embot::hw::FLASHpartitionID getCodePartition() + { + return codePartition; + } + + embot::app::application::CANagentCORE* getCANgentCORE() + { + static bool initted {false}; + if(!initted) + { + force_placement_of_moduleinfo(); + embot::app::board::amc2c::theCANagentCORE::getInstance().initialise({applInfo, canBus, info32}); + initted = true; + } + return &embot::app::board::amc2c::theCANagentCORE::getInstance(); + } + +} // namespace embot::app::board::amc2c::info { + + +// ------------------------------------------------------------------------------------------------------------------ +// - for the partition table we need to place specific infor at a given location +// ------------------------------------------------------------------------------------------------------------------ + +#include "eEsharedServices.h" + + +#define ROMADDR 0x08100000 +#define ROMSIZE 0x20000 + +#define EENV_MODULEINFO_LOADER_AT ".ARM.__at_0x08100400" + + +#define RAMADDR 0x10000000 +#define RAMSIZE 0x00048000 + +constexpr eEmoduleExtendedInfo_t s_cm4app_info_extended __attribute__((section(EENV_MODULEINFO_LOADER_AT))) = +{ + .moduleinfo = + { + .info = + { + .entity = + { + .type = ee_entity_process, + .signature = ee_procOther01, + .version = + { + .major = embot::app::board::amc2c::info::applInfo.version.major, + .minor = embot::app::board::amc2c::info::applInfo.version.minor + }, + .builddate = + { + .year = embot::app::board::amc2c::info::date.year, + .month = embot::app::board::amc2c::info::date.month, + .day = embot::app::board::amc2c::info::date.day, + .hour = embot::app::board::amc2c::info::date.hour, + .min = embot::app::board::amc2c::info::date.minute + } + }, + .rom = + { + .addr = ROMADDR, + .size = ROMSIZE + }, + .ram = + { + .addr = RAMADDR, + .size = RAMSIZE + }, + .storage = + { + .type = ee_strg_none, + .size = 0, + .addr = 0 + }, + .communication = ee_commtype_none, + .name = "eOther01" + }, + .protocols = + { + .udpprotversion = { .major = 0, .minor = 0}, + .can1protversion = { .major = 0, .minor = 0}, + .can2protversion = { .major = 0, .minor = 0}, + .gtwprotversion = { .major = 0, .minor = 0} + }, + .extra = {"EXT"} + }, + .compilationdatetime = __DATE__ " " __TIME__, + .userdefined = {0} +}; + +#include + +eEmoduleExtendedInfo_t ss {}; +void force_placement_of_moduleinfo() +{ + static volatile uint8_t used {0}; + + if(s_cm4app_info_extended.moduleinfo.info.entity.type == ee_entity_process) + { + used = 1; + } + else + { + used = 2; + } + + std::memmove(&ss, &s_cm4app_info_extended, sizeof(ss)); +} + +// - end-of-file (leave a blank line after)---------------------------------------------------------------------------- + + + diff --git a/emBODY/eBcode/arch-arm/board/amc2c/test/src/app-board-amc2c/embot_app_board_amc2c_info.h b/emBODY/eBcode/arch-arm/board/amc2c/test/src/app-board-amc2c/embot_app_board_amc2c_info.h new file mode 100644 index 0000000000..090ee3099d --- /dev/null +++ b/emBODY/eBcode/arch-arm/board/amc2c/test/src/app-board-amc2c/embot_app_board_amc2c_info.h @@ -0,0 +1,31 @@ + +/* + * Copyright (C) 2023 iCub Tech - Istituto Italiano di Tecnologia + * Author: Marco Accame + * email: marco.accame@iit.it +*/ + + +// - include guard ---------------------------------------------------------------------------------------------------- + +#ifndef __EMBOT_APP_BOARD_AMC2C_INFO_H_ +#define __EMBOT_APP_BOARD_AMC2C_INFO_H_ + + +#include "embot_core.h" + +#include "embot_app_bldc_theApplication.h" +#include "embot_app_application_CANagentCORE.h" + +namespace embot::app::board::amc2c::info { + + embot::hw::FLASHpartitionID getCodePartition(); + embot::app::application::CANagentCORE* getCANgentCORE(); + +} // embot::app::board::amc2c::info { + + +#endif // include-guard + + +// - end-of-file (leave a blank line after)---------------------------------------------------------------------------- diff --git a/emBODY/eBcode/arch-arm/board/amc2c/test/src/app-board-amc2c/embot_app_board_amc2c_test.cpp b/emBODY/eBcode/arch-arm/board/amc2c/test/src/app-board-amc2c/embot_app_board_amc2c_test.cpp new file mode 100644 index 0000000000..16cc7d1176 --- /dev/null +++ b/emBODY/eBcode/arch-arm/board/amc2c/test/src/app-board-amc2c/embot_app_board_amc2c_test.cpp @@ -0,0 +1,197 @@ + +/* + * Copyright (C) 2023 iCub Tech - Istituto Italiano di Tecnologia + * Author: Marco Accame + * email: marco.accame@iit.it +*/ + + +// -------------------------------------------------------------------------------------------------------------------- +// - public interface +// -------------------------------------------------------------------------------------------------------------------- + +#include "embot_app_board_amc2c_mbd.h" +#include "embot_app_board_amc2c_theMBD.h" + + +// -------------------------------------------------------------------------------------------------------------------- +// - external dependencies +// -------------------------------------------------------------------------------------------------------------------- + +#include "embot_hw_timer.h" + + +#include "motorhal_config.h" +#include "embot_hw_motor_bsp_amc2c.h" + +extern void HallSetPwm(int32_t pwm); + +using namespace embot::hw; + +#if !defined(MOTORHALCONFIG_enabletests) + #error: MOTORHALCONFIG_enabletests must be defined +#endif + +namespace embot::app::board::amc2c::mbd { + + // forward declaration + result_t hallTest(int32_t set, bool enc_reading_enabled); + + result_t adcTest(); + + + void Startup(embot::prot::can::Address adr) + { + embot::hw::motor::bsp::amc2c::Init_MOTORHAL_testmode(embot::hw::MOTOR::one); + + AdcMotInit(); + EncInit(); + HallInit(); + PwmInit(); + + } + + void OnTick(const std::vector &input, std::vector &output) + { + + size_t numRXframes = input.size(); + + if(numRXframes > 0) + { + embot::prot::can::Frame f = input[0]; + + switch(f.id) + { + // Test Hall Sensors + Motor spinning + quad encoder reading [WIP] + // ID = 1 and PAYLOAD = PWM(%), where -100 < PWM(%) < 100 + case 0x001: + { + int32_t set {0}; + static bool enc_reading_enabled {false}; + + set = 65535 / 100 * f.data[0]; // pwm value to set + enc_reading_enabled = (f.data[1] == 1) ? true : false; + hallTest(set, enc_reading_enabled); + } break; + + // Test PWM Output (ADC currents reading) + case 0x002: + { + // TODO: WIP + adcTest(); + + } break; + + default: + { + } break; + } + } + +// embot::app::board::amc2c::theMBD::getInstance().tick(input, output); + + +// if(numRXframes > 0) +// { +// // just for test: i get the first only and i send it back +// embot::prot::can::Frame f = input[0]; +// output.push_back(f); +// } + } + + int32_t abs_pos {0}; + void qEncReading(embot::os::Thread *t, void *param) + { + EncCaptureIndex(); + abs_pos = EncGetAbsolutePosition(); + } + + result_t hallTest(int32_t set, bool enc_reading_enabled) + { + + // NB: PwmInit, HallInit and EncInit must be called before this test. + // in general they are called in embot::hw::motor::init + result_t res = resOK; + + // setup + static int32_t pwm {0}; + + // do 6-step control + if ((-65536L <= set) && (65535 >= set)) + { + if (set >= pwm) + { + for ( ; pwm < set-100 ; pwm += 100) + { + HallSetPwm(pwm); + embot::core::wait(10 * embot::core::time1millisec); + } + } + else + { + for ( ; pwm > set-100 ; pwm -= 100) + { + HallSetPwm(pwm); + embot::core::wait(10 * embot::core::time1millisec); + } + } + HallSetPwm(pwm = set); + } + else + { + // PWM set value is out of range + res = resNOK; + } + + + + static bool th_qEncReading_started { false }; + + if (enc_reading_enabled) + { + if(false == th_qEncReading_started) + { + // start thread here + + + embot::os::PeriodicThread::Config cfg { + 2048, + embot::os::Priority::abovenorm32, + nullptr, + nullptr, + 100 * embot::core::time1millisec, + qEncReading, + "qEncReadingThreadCfg" + }; + + embot::os::PeriodicThread *periodic_thread_qEnc = new embot::os::PeriodicThread; + periodic_thread_qEnc->start(cfg); + + th_qEncReading_started = true; + } + } + + // TODO: embot::hw::motor::deinit? + + return res; + } + + result_t adcTest() + { + int16_t uI, vI, wI; + uint16_t uV, vV, wV; + do + { + AdcMotGetCurrents(&uI, &vI, &wI); + AdcMotGetVoltages(&uV, &vV, &wV); + embot::core::wait(100 * embot::core::time1millisec); + } while (1); + } + +} // end of namespace + + + +// - end-of-file (leave a blank line after)---------------------------------------------------------------------------- + + diff --git a/emBODY/eBcode/arch-arm/board/amc2c/test/src/app-board-amc2c/embot_app_board_amc2c_test01.cpp b/emBODY/eBcode/arch-arm/board/amc2c/test/src/app-board-amc2c/embot_app_board_amc2c_test01.cpp new file mode 100644 index 0000000000..9c130bd67f --- /dev/null +++ b/emBODY/eBcode/arch-arm/board/amc2c/test/src/app-board-amc2c/embot_app_board_amc2c_test01.cpp @@ -0,0 +1,300 @@ + +/* + * Copyright (C) 2023 iCub Tech - Istituto Italiano di Tecnologia + * Author: Marco Accame + * email: marco.accame@iit.it +*/ + + +// -------------------------------------------------------------------------------------------------------------------- +// - public interface +// -------------------------------------------------------------------------------------------------------------------- + +#include "embot_app_board_amc2c_test01.h" + + + +// -------------------------------------------------------------------------------------------------------------------- +// - external dependencies +// -------------------------------------------------------------------------------------------------------------------- + +#include "embot_hw_motor.h" + + +namespace embot::tools::filter { + + struct MovingAverage + { + struct Config + { + static constexpr uint8_t exponent {10}; + static constexpr size_t size {2 << exponent}; + bool clip {false}; + std::pair limits {-100, +100}; + }; + + Config config {}; + std::array status {}; + size_t index {0}; + int64_t accumulator {0}; + int32_t average {0}; + bool ready {false}; + + int32_t clip(const int32_t &v, bool &clipped) + { + clipped = false; + if(true == config.clip) + { + if(v < config.limits.first) + { + clipped = true; + return config.limits.first; + } + + if(v > config.limits.second) + { + clipped = true; + return config.limits.second; + } + + //return std::clamp(v, config.limits.first, config.limits.second); + } + + return v; + } + + void clear() + { + ready = false; + index = 0; + accumulator = 0; + average = 0; + } + + bool add(const int32_t &v) + { + bool clipped {false}; + int32_t vc = clip(v, clipped); + + if(ready) + { + accumulator -= status[index]; + } + + accumulator += vc; + status[index] = vc; + index = (index+1) % status.size(); + + if(0 == index) + { + ready = true; + } + + if(ready) + { +// average = accumulator << Config::exponent; + average = accumulator / status.size(); + } + + return ready; + } + + int32_t value() const + { + return average; + } + }; + + +} + + + + +namespace embot::app::board::amc2c::test01 { + + embot::prot::can::Address address = 0; + void onCurrents_FOC_innerloop(void *owner, const embot::hw::motor::Currents * const currents); + + void Startup(embot::prot::can::Address adr) + { + address = adr; + + // init motor + embot::hw::motor::init(embot::hw::MOTOR::one, {}); + + // assign the callback to the adc acquistion of currents + embot::hw::motor::setCallbackOnCurrents(embot::hw::MOTOR::one, onCurrents_FOC_innerloop, nullptr); + + // enable the motor + embot::hw::motor::enable(embot::hw::MOTOR::one, true); + } + + struct Shared + { + enum class Mode { movingaverage = 0, raw = 1}; + volatile bool transmit {false}; + volatile embot::core::Time txperiod {0}; + volatile bool imposepwm {false}; + embot::hw::motor::PWMperc pwm {0.0, 0.0, 0.0}; + std::array adcvalues {}; + volatile Mode mode {Mode::movingaverage}; + + std::array mav {}; + + + Shared() = default; + + void clear() + { + transmit = false; + txperiod = 0; + imposepwm = false; + pwm = {0.0, 0.0, 0.0}; + adcvalues = {0, 0, 0}; + mode = Mode::raw; + } + }; + + + Shared _shared {}; + + + + void parse(const std::vector &input); + + void OnTick(const std::vector &input, std::vector &output) + { + output.clear(); + + if(input.size() > 0) + { + parse(input); + } + + if(_shared.transmit) + { + _shared.transmit = false; + + // prepare a frame and add it inside output + embot::prot::can::Frame f + { + static_cast(0xF00 | (address << 4)), + 8, + { + 0x42, + 0x00, + static_cast(_shared.adcvalues[0] & 0xff), static_cast(_shared.adcvalues[0] >> 8), + static_cast(_shared.adcvalues[1] & 0xff), static_cast(_shared.adcvalues[1] >> 8), + static_cast(_shared.adcvalues[2] & 0xff), static_cast(_shared.adcvalues[2] >> 8), + } + }; + + output.push_back(f); + + int16_t i0 = static_cast(_shared.adcvalues[0]); + int16_t i1 = static_cast(_shared.adcvalues[1]); + int16_t i2 = static_cast(_shared.adcvalues[2]); +// constexpr float conv {1000.0}; +// float I0 = conv*33.0*i0 / (256*1024); +// float I1 = conv*33.0*i1 / (256*1024); +// float I2 = conv*33.0*i2 / (256*1024); + embot::core::print("curs = (" + std::to_string(i0) + ", " + std::to_string(i1) + ", " + std::to_string(i2) + ")"); + // embot::core::print("curs [mA] = (" + std::to_string(I0) + ", " + std::to_string(I1) + ", " + std::to_string(I2) + ")"); + } + + + } + + + + + // others + void onCurrents_FOC_innerloop(void *owner, const embot::hw::motor::Currents * const currents) + { + // 1. copy currents straight away, so that we can use them + embot::hw::motor::Currents currs = *currents; + + _shared.mav[0].add(currs.u); + _shared.mav[1].add(currs.v); + _shared.mav[2].add(currs.w); + + // 2. and i filter them with a ... moving average. + static embot::core::Time timelasttx {0}; + + // 3. if time to transmit comes ... set transmit = true; + if(0 != _shared.txperiod) + { + embot::core::Time now = embot::core::now(); + embot::core::Time delta = now - timelasttx; + if(delta >= _shared.txperiod) + { + if(Shared::Mode::movingaverage == _shared.mode) + { + _shared.adcvalues[0] = _shared.mav[0].value(); + _shared.adcvalues[1] = _shared.mav[1].value(); + _shared.adcvalues[2] = _shared.mav[2].value(); + } + else + { + _shared.adcvalues[0] = currs.u; + _shared.adcvalues[1] = currs.v; + _shared.adcvalues[2] = currs.w; + } + + timelasttx = now; + _shared.transmit = true; + } + } + + // 4. if i need to change the pwm ... + if(_shared.imposepwm) + { + _shared.imposepwm = false; + embot::hw::motor::setPWM(embot::hw::MOTOR::one, _shared.pwm); + } + + } + + void parse(const std::vector &input) + { + + for(size_t s=0; s(input[s].data[2] + 256*input[s].data[3]); + _shared.pwm.b = static_cast(input[s].data[4] + 256*input[s].data[5]); + _shared.pwm.c = static_cast(input[s].data[6] + 256*input[s].data[7]); + _shared.imposepwm = true; + } break; + + case 0x41: + { + _shared.txperiod = 1000 * input[s].data[1]; + } break; + + } + } + + } + + + } + +} // end of namespace + + + +// - end-of-file (leave a blank line after)---------------------------------------------------------------------------- + + diff --git a/emBODY/eBcode/arch-arm/board/amc2c/test/src/app-board-amc2c/embot_app_board_amc2c_test01.h b/emBODY/eBcode/arch-arm/board/amc2c/test/src/app-board-amc2c/embot_app_board_amc2c_test01.h new file mode 100644 index 0000000000..ba1c524246 --- /dev/null +++ b/emBODY/eBcode/arch-arm/board/amc2c/test/src/app-board-amc2c/embot_app_board_amc2c_test01.h @@ -0,0 +1,30 @@ + +/* + * Copyright (C) 2023 iCub Tech - Istituto Italiano di Tecnologia + * Author: Marco Accame + * email: marco.accame@iit.it +*/ + + +// - include guard ---------------------------------------------------------------------------------------------------- + +#ifndef __EMBOT_APP_BOARD_AMC2C_TEST01_H_ +#define __EMBOT_APP_BOARD_AMC2C_TEST01_H_ + + + +#include "embot_app_bldc_theApplication.h" + + +namespace embot::app::board::amc2c::test01 { + + void Startup(embot::prot::can::Address adr); + void OnTick(const std::vector &input, std::vector &output); + +} // embot::app::board::amc2c::test1 { + + +#endif // include-guard + + +// - end-of-file (leave a blank line after)---------------------------------------------------------------------------- diff --git a/emBODY/eBcode/arch-arm/board/amc2c/test/src/app-board-amc2c/embot_app_board_amc2c_theCANagentCORE.cpp b/emBODY/eBcode/arch-arm/board/amc2c/test/src/app-board-amc2c/embot_app_board_amc2c_theCANagentCORE.cpp new file mode 100644 index 0000000000..2de3a5ce8d --- /dev/null +++ b/emBODY/eBcode/arch-arm/board/amc2c/test/src/app-board-amc2c/embot_app_board_amc2c_theCANagentCORE.cpp @@ -0,0 +1,281 @@ + +/* + * Copyright (C) 2023 iCub Tech - Istituto Italiano di Tecnologia + * Author: Marco Accame + * email: marco.accame@iit.it +*/ + + + +// -------------------------------------------------------------------------------------------------------------------- +// - public interface +// -------------------------------------------------------------------------------------------------------------------- + +#include "embot_app_board_amc2c_theCANagentCORE.h" + + +// -------------------------------------------------------------------------------------------------------------------- +// - external dependencies +// -------------------------------------------------------------------------------------------------------------------- + +#include "embot_core.h" +#include "embot_core_binary.h" + +#include + + +#include "embot_hw_sys.h" +#include "embot_hw_can.h" + +// -------------------------------------------------------------------------------------------------------------------- +// - pimpl: private implementation (see scott meyers: item 22 of effective modern c++, item 31 of effective c++ +// -------------------------------------------------------------------------------------------------------------------- + + +struct embot::app::board::amc2c::theCANagentCORE::Impl +{ + + Config _config {}; + + static const std::uint32_t validityKey = 0x8888dead; + struct StoredInfo + { // contains the basics stored inside some embot::i2h::Storage + std::uint32_t key {validityKey}; + std::uint8_t canaddress {3}; + std::uint8_t boardtype { embot::core::tointegral(embot::prot::can::Board::amcbldc) }; + std::uint8_t bootloaderVmajor {0}; + std::uint8_t bootloaderVminor {0}; + std::uint8_t info32[32] {"i am a amc2c"}; + std::uint8_t applicationVmajor {0}; + std::uint8_t applicationVminor {0}; + std::uint8_t applicationVbuild {0}; + std::uint8_t protocolVmajor {2}; + std::uint8_t protocolVminor {0}; + + std::uint8_t tobefilled[3]; // to make the size of struct ... multiple of 8. + }; + + StoredInfo _storedinfo = {}; + + embot::prot::can::applicationInfo _applicationinfo {}; + + embot::prot::can::Board _board {embot::prot::can::Board::amcbldc}; + + + Impl() { } + + bool initialise(const Config &config); + + bool setcanaddress(const std::uint8_t adr, const std::uint16_t randominvalidmask); +}; + +bool embot::app::board::amc2c::theCANagentCORE::Impl::initialise(const Config &config) +{ + _config = config; + + // retrieve board type, version of application, can address + // we use the object theCANboardInfo which is based onto FlashStorage which manages + // the flash partition called Partition::ID::sharedstorage + + _storedinfo.applicationVmajor = _config.applicationinfo.version.major; + _storedinfo.applicationVminor = _config.applicationinfo.version.minor; + _storedinfo.applicationVbuild = _config.applicationinfo.version.build; + _storedinfo.protocolVmajor = _config.applicationinfo.protocol.major; + _storedinfo.protocolVminor = _config.applicationinfo.protocol.minor; + + std::memmove(_storedinfo.info32, _config.boardinfo, sizeof(_storedinfo.info32)); + + _board = static_cast(_storedinfo.boardtype); +// _address = _storedinfo.canaddress; + _applicationinfo.version.major = _storedinfo.applicationVmajor; + _applicationinfo.version.minor = _storedinfo.applicationVminor; + _applicationinfo.version.build = _storedinfo.applicationVbuild; + _applicationinfo.protocol.major = _storedinfo.protocolVmajor; + _applicationinfo.protocol.minor = _storedinfo.protocolVminor; + + // note: we could make it even more general by using inside _config a pure interface class which + // does the job of theCANboardInfo, in such a way we just specialise this class ... but maybe later + + return true; +} + + +bool embot::app::board::amc2c::theCANagentCORE::Impl::setcanaddress(const std::uint8_t adr, const std::uint16_t randominvalidmask) +{ + + // i reinforce a reading from storage. just for safety. in here we are dealing w/ can address change and i want to be sure. + std::uint8_t canaddress = _storedinfo.canaddress; + + std::uint8_t target = adr; + + if(0xff == adr) + { + // compute a new random address. use the randoinvalid mask to filter out the undesired values. for sure 0x8001. + std::uint16_t mask = randominvalidmask; + mask |= 0x8001; + if(0xffff == mask) + { // hei, nothing is good for you. + return false; + } + + bool ok = false; + for(std::uint16_t i=0; i<250; i++) + { + target = (embot::hw::sys::random()-embot::hw::sys::minrandom()) & 0xf; + + if(false == embot::core::binary::bit::check(mask, target)) + { + ok = true; + break; + } + } + + if(!ok) + { + return false; + } + } + + // always check that is is not 0 or 0xf + if((0 == target) || (0xf == target)) + { + return false; + } + + + if(canaddress != target) + { + if(embot::hw::resOK != embot::hw::can::setfilters(_config.canbus, target)) + { + return false; + } + _storedinfo.canaddress = target; + canaddress = _storedinfo.canaddress; + } + + return (target == canaddress); +} + + +// -------------------------------------------------------------------------------------------------------------------- +// - the class +// -------------------------------------------------------------------------------------------------------------------- + + +embot::app::board::amc2c::theCANagentCORE& embot::app::board::amc2c::theCANagentCORE::getInstance() +{ + static theCANagentCORE* p = new theCANagentCORE(); + return *p; +} + +embot::app::board::amc2c::theCANagentCORE::theCANagentCORE() +{ + pImpl = std::make_unique(); +} + + +embot::app::board::amc2c::theCANagentCORE::~theCANagentCORE() { } + + +bool embot::app::board::amc2c::theCANagentCORE::initialise(const Config &config) +{ + return pImpl->initialise(config); +} + + +// its interfaces to CANagentCORE + + +const embot::prot::can::applicationInfo & embot::app::board::amc2c::theCANagentCORE::applicationinfo() const +{ + return pImpl->_applicationinfo; +} + +embot::hw::CAN embot::app::board::amc2c::theCANagentCORE::bus() const +{ + return pImpl->_config.canbus; +} + +embot::prot::can::Address embot::app::board::amc2c::theCANagentCORE::address() const +{ + return pImpl->_storedinfo.canaddress; +} + + +bool embot::app::board::amc2c::theCANagentCORE::get(const embot::prot::can::bootloader::Message_BROADCAST::Info &info, embot::prot::can::bootloader::Message_BROADCAST::ReplyInfo &replyinfo) +{ + replyinfo.board = pImpl->_board; + replyinfo.process = embot::prot::can::Process::application; + replyinfo.firmware = {pImpl->_applicationinfo.version.major, pImpl->_applicationinfo.version.minor, pImpl->_applicationinfo.version.build}; + + return true; +} + + +bool embot::app::board::amc2c::theCANagentCORE::get(const embot::prot::can::bootloader::Message_GET_ADDITIONAL_INFO::Info &info, embot::prot::can::bootloader::Message_GET_ADDITIONAL_INFO::ReplyInfo &replyinfo) +{ + std::memmove(replyinfo.info32, pImpl->_storedinfo.info32, sizeof(replyinfo.info32)); + + return true; +} + + +bool embot::app::board::amc2c::theCANagentCORE::get(const embot::prot::can::shared::Message_GET_VERSION::Info &info, embot::prot::can::shared::Message_GET_VERSION::ReplyInfo &replyinfo) +{ + replyinfo.board = pImpl->_board; + replyinfo.firmware = {pImpl->_applicationinfo.version.major, pImpl->_applicationinfo.version.minor, pImpl->_applicationinfo.version.build}; + replyinfo.protocol = pImpl->_applicationinfo.protocol; + + return true; +} + +bool embot::app::board::amc2c::theCANagentCORE::get(const embot::prot::can::bootloader::Message_GET_TIMEOFLIFE::Info &info, embot::prot::can::bootloader::Message_GET_TIMEOFLIFE::ReplyInfo &replyinfo) +{ + replyinfo.timeoflife = embot::core::now(); + + return true; +} + + +bool embot::app::board::amc2c::theCANagentCORE::set(const embot::prot::can::bootloader::Message_BOARD::Info &info) +{ +// // we just restart so we cannot reply +// embot::hw::sys::reset(); + + return false; +} + + +bool embot::app::board::amc2c::theCANagentCORE::set(const embot::prot::can::bootloader::Message_SET_ADDITIONAL_INFO2::Info &info) +{ + if(true == info.valid) + { // we have received all the 8 messages in order (important is that the one with data[1] = 0 is the first) +// embot::app::theCANboardInfo::getInstance().get(pImpl->_storedinfo); + std::memmove(pImpl->_storedinfo.info32, info.info32, sizeof(pImpl->_storedinfo.info32)); +// embot::app::theCANboardInfo::getInstance().set(pImpl->_storedinfo); + } + + return false; +} + + +bool embot::app::board::amc2c::theCANagentCORE::set(const embot::prot::can::shared::Message_SET_ID::Info &info) +{ + pImpl->setcanaddress(info.address, 0x0000); + + return false; +} + + +bool embot::app::board::amc2c::theCANagentCORE::set(const embot::prot::can::bootloader::Message_SETCANADDRESS::Info &info) +{ + pImpl->setcanaddress(info.address, info.randominvalidmask); + + return false; +} + + + +// - end-of-file (leave a blank line after)---------------------------------------------------------------------------- + + diff --git a/emBODY/eBcode/arch-arm/board/amc2c/test/src/app-board-amc2c/embot_app_board_amc2c_theCANagentCORE.h b/emBODY/eBcode/arch-arm/board/amc2c/test/src/app-board-amc2c/embot_app_board_amc2c_theCANagentCORE.h new file mode 100644 index 0000000000..d7696fa7aa --- /dev/null +++ b/emBODY/eBcode/arch-arm/board/amc2c/test/src/app-board-amc2c/embot_app_board_amc2c_theCANagentCORE.h @@ -0,0 +1,69 @@ + +/* + * Copyright (C) 2023 iCub Tech - Istituto Italiano di Tecnologia + * Author: Marco Accame + * email: marco.accame@iit.it +*/ + +// - include guard ---------------------------------------------------------------------------------------------------- + +#ifndef __EMBOT_APP_BOARD_AMC2C_THECANAGENTCORE_H_ +#define __EMBOT_APP_BOARD_AMC2C_THECANAGENTCORE_H_ + +#include +#include + + +#include "embot_app_application_CANagentCORE.h" + + +namespace embot::app::board::amc2c { + + class theCANagentCORE : public embot::app::application::CANagentCORE + { + public: + static theCANagentCORE& getInstance(); + + struct Config + { + embot::prot::can::applicationInfo applicationinfo {{0,0,1}, {2,0}}; + embot::hw::CAN canbus {embot::hw::CAN::one}; + const char *boardinfo {"hello, i am an amc2c"}; + constexpr Config() = default; + constexpr Config(const embot::prot::can::applicationInfo& ai, embot::hw::CAN b, const char *bi) + : applicationinfo(ai), canbus(b), boardinfo(bi) {} + }; + + bool initialise(const Config &config); + + // interface to theCANagentCORE + const embot::prot::can::applicationInfo & applicationinfo() const override; + embot::hw::CAN bus() const override; + embot::prot::can::Address address() const override; + + bool get(const embot::prot::can::bootloader::Message_BROADCAST::Info &info, embot::prot::can::bootloader::Message_BROADCAST::ReplyInfo &replyinfo) override; + bool get(const embot::prot::can::bootloader::Message_GET_ADDITIONAL_INFO::Info &info, embot::prot::can::bootloader::Message_GET_ADDITIONAL_INFO::ReplyInfo &replyinfo) override; + bool get(const embot::prot::can::shared::Message_GET_VERSION::Info &info, embot::prot::can::shared::Message_GET_VERSION::ReplyInfo &replyinfo) override; + bool get(const embot::prot::can::bootloader::Message_GET_TIMEOFLIFE::Info &info, embot::prot::can::bootloader::Message_GET_TIMEOFLIFE::ReplyInfo &replyinfo) override; + + bool set(const embot::prot::can::bootloader::Message_BOARD::Info &info) override; + bool set(const embot::prot::can::bootloader::Message_SET_ADDITIONAL_INFO2::Info &info) override; + bool set(const embot::prot::can::shared::Message_SET_ID::Info &info) override; + bool set(const embot::prot::can::bootloader::Message_SETCANADDRESS::Info &info) override; + + private: + theCANagentCORE(); + ~theCANagentCORE(); + + private: + struct Impl; + std::unique_ptr pImpl; + }; + +} // namespace embot::app::board::amc2c { + + +#endif // include-guard + + +// - end-of-file (leave a blank line after)---------------------------------------------------------------------------- diff --git a/emBODY/eBcode/arch-arm/board/amc2c/test/src/app-board-amc2c/embot_app_board_amc2c_theMBD-debug.cpp b/emBODY/eBcode/arch-arm/board/amc2c/test/src/app-board-amc2c/embot_app_board_amc2c_theMBD-debug.cpp new file mode 100644 index 0000000000..dbb758c3d8 --- /dev/null +++ b/emBODY/eBcode/arch-arm/board/amc2c/test/src/app-board-amc2c/embot_app_board_amc2c_theMBD-debug.cpp @@ -0,0 +1,716 @@ + +/* + * Copyright (C) 2023 iCub Tech - Istituto Italiano di Tecnologia + * Author: Marco Accame + * email: marco.accame@iit.it +*/ + + +// -------------------------------------------------------------------------------------------------------------------- +// - public interface +// -------------------------------------------------------------------------------------------------------------------- + +#include "embot_app_board_amc2c_theMBD.h" + + +// -------------------------------------------------------------------------------------------------------------------- +// - external dependencies +// -------------------------------------------------------------------------------------------------------------------- + +#if defined(STM32HAL_BOARD_AMC2C) +#include "embot_hw_bsp_amc2c.h" +//#warning READ CAREFULLY: we may use the same object for both amcbldc and amc2c +#elif defined(STM32HAL_BOARD_AMCBLDC) +#include "embot_hw_bsp_amcbldc.h" +#else +#error: choose a STM32HAL_BOARD_* +#endif + +#include "embot_hw_button.h" +#include "embot_hw_motor.h" +#include "embot_app_scope.h" +#include "embot_hw_sys.h" +#include "embot_core.h" +#include "embot_app_theLEDmanager.h" +#include + +// some protocol includes +#include "embot_prot_can_motor_polling.h" +#include "embot_prot_can_motor_periodic.h" + + +// mdb components +#include "AMC_BLDC.h" + +// -------------------------------------------------------------------------------------------------------------------- +// - defines +// -------------------------------------------------------------------------------------------------------------------- +//#define TEST_DURATION_FOC + +#define TEST_FOC_logvalues + + +// If the target setup is the KOLLMORGEN motor on RED LEGO platform, +// you need to enable the macro USE_KOLLMORGEN_SETUP, otherwise +// it is implied that motor on wrist mk2 is in use + +//#define EXTFAULT_enabled +#define EXTFAULT_handler_will_disable_motor + +#define EXTFAULT_enabled_polling + +#if defined(EXTFAULT_enabled) +#undef EXTFAULT_enabled_polling +#endif + +// -------------------------------------------------------------------------------------------------------------------- +// - pimpl: private implementation (see scott meyers: item 22 of effective modern c++, item 31 of effective c++ +// -------------------------------------------------------------------------------------------------------------------- + + +namespace embot::app::board::amc2c { + +struct Measure +{ + public: + // interface + virtual void start() = 0; + virtual void stop() = 0; + + protected: + virtual ~Measure() {}; // cannot delete from interface but only from derived object +}; + +// this implementation does nothing +class MeasureDummy: public Measure +{ +public: + + struct Config + { + uint32_t nothing {0}; + Config() = default; + bool isvalid() const { return true; } + }; + + MeasureDummy() {}; + virtual ~MeasureDummy() {}; + + virtual void start() override {}; + virtual void stop() override {}; +}; + +// how to use it: +// 1. create a MeasureHisto w/ a proper MeasureHisto::Config +// 2. call start() and stop() at the beginning and at the end of the piece of code +// of which you want to measure the duration. +// the duration will be added in a histogram whose properties are in +// Config::minduration, Config::maxduration, Config::resolution +// 3. the histogram can be viewed using method MeasureHisto::report(). +// you must place a breakpoint in it and explore the volatile variables {name, beyond, vals} +// but you can also enable a print. you can use Config::name to differentiate amongst +// multiple instances of MeasureHisto +// 4. method MeasureHisto::report() can be called at any time, but if Config::reportfrequency is true, +// is is also called at the end of stop() at the rate specified by Config::reportfrequency + +struct MeasureHisto : public Measure +{ + struct Config + { + const char *name {"dummy"}; + embot::core::relTime minduration {0}; + embot::core::relTime maxduration {100*embot::core::time1microsec}; + embot::core::relTime resolution {100*embot::core::time1microsec}; + bool enablereport {true}; + uint32_t reportfrequency {10000}; + constexpr Config() = default; + constexpr Config(const char *n, embot::core::relTime min, embot::core::relTime max, + embot::core::relTime res = embot::core::time1microsec, + bool er = true, uint32_t rf = 10000) + : name(n), minduration(min), maxduration(max), resolution(res), + enablereport(er), reportfrequency(rf){} + }; + + Config config {}; + + embot::app::scope::SignalHisto *sighisto {nullptr}; + + const embot::tools::Histogram::Values * vv {nullptr}; + + // i use ram ... maybe not best solution. maybe we could also print the histo. + static constexpr size_t nvals {512}; + volatile uint64_t vals[nvals] {0}; + volatile uint64_t beyond {0}; + uint32_t counter {0}; + + MeasureHisto(const Config &cfg) + { + config = cfg; + if(nullptr == config.name) + { + config.name = "dummy"; + } + sighisto = new embot::app::scope::SignalHisto({config.minduration, config.maxduration, config.resolution}); + vv = sighisto->histogram()->getvalues(); + } + + void start() + { + sighisto->on(); + } + + void stop() + { + sighisto->off(); + + if(config.enablereport) + { + if(++counter >= config.reportfrequency) + { + counter = 0; + report(); + } + } + } + +#define PRINT_REPORT + + void report() + { + beyond = vv->beyond; + + for(int i=0; iinside.size(); i++) + { + if(i < nvals) + { + vals[i] = vv->inside[i]; + } + } + // place a breakpoint in here and inspect: beyond and vals[] + beyond = beyond; + // but you can also use embot::core::print() to collect values from the printf window + // the following code can be easily changed according to needs +#if defined(PRINT_REPORT) + std::string str = std::string("name, beyond, histo = " ) + config.name + ", " + std::to_string(beyond); + for(int v=0; v &inpframes, std::vector &outframes); + + // the code called @ 80/3 kHz (aka every 37.5 usec) inside the DMA1_Channel2_IRQHandler() + // it must be static because we use it as a callback + static void onCurrents_FOC_innerloop(void *owner, const embot::hw::motor::Currents * const currents); + + // the objects which measure the duration of the two above functions + Measure *measureFOC {nullptr}; + Measure *measureTick {nullptr}; + + // all the rest, which may or may not be required anymore + Config _config {}; + bool initted {false}; + + embot::hw::motor::Pwm pwm {0}; + embot::prot::can::motor::polling::ControlMode cm {embot::prot::can::motor::polling::ControlMode::Idle}; + bool applychanges {false}; + + embot::hw::BTN buttonEXTfault {embot::hw::BTN::one}; + embot::hw::LED ledEXTfault {embot::hw::LED::two}; + static void onEXTFAULTpressedreleased(void *owner); + volatile bool EXTFAULTisPRESSED {false}; + volatile bool prevEXTFAULTisPRESSED {false}; + volatile embot::core::Time EXTFAULTpressedtime {0}; + volatile embot::core::Time EXTFAULTreleasedtime {0}; + + void onEXTFAULTpolling(); + std::vector faultvalues {}; + static constexpr size_t faultcapacity {5}; + + #ifdef PRINT_HISTO_DEBUG + void printHistogram_rxPkt(size_t cur_size); //cur_size is the currente queue size + static constexpr uint8_t numOfBins {11}; //size of queue of the CAN drive plus 1 (in case any packet is received) + uint32_t bin[numOfBins]{0}; + #endif + +}; + + + +bool embot::app::board::amc2c::theMBD::Impl::initialise(const Config &config) +{ + if(true == initted) + { + return true; + } + + _config = config; + + embot::core::print("embot::app::board::amc2c::theMBD::Impl::initialise()"); + + // create the measure for the foc + if(true == useDUMMYforFOC) + { + measureFOC = new MeasureDummy; + } + else + { + measureFOC = new MeasureHisto({"foc", 0, 64*embot::core::time1microsec, 1, true, 100*1000}); + } + + // create the measure for the tick + if(true == useDUMMYforTICK) + { + measureTick = new MeasureDummy; + } + else + { + measureTick = new MeasureHisto({"tick", 0, 400*embot::core::time1microsec, 1, true, 10*1000}); + } + + // init the external fault. + // we call cbkOnEXTfault.execute() when the button is pressed or released. + // in the callback we set EXTFAULTisPRESSED true or false depending on value of + // embot::hw::button::pressed(buttonEXTfault) + // we also disable the motors if true. + + buttonEXTfault = embot::hw::bsp::amc2c::EXTFAULTbutton(); + ledEXTfault = embot::hw::bsp::amc2c::EXTFAULTled(); + +#if defined(EXTFAULT_enabled) + + embot::core::Callback cbkOnEXTFAULT {onEXTFAULTpressedreleased, this}; + embot::hw::button::init(buttonEXTfault, {embot::hw::button::Mode::TriggeredOnPressAndRelease, cbkOnEXTFAULT, 0}); + prevEXTFAULTisPRESSED = EXTFAULTisPRESSED = embot::hw::button::pressed(buttonEXTfault); + +#elif defined(EXTFAULT_enabled_polling) + + faultvalues.reserve(faultcapacity); + faultvalues.clear(); + + embot::hw::button::init(buttonEXTfault, {embot::hw::button::Mode::Polling, {}, 0}); + for(size_t i=0; i= faultcapacity) + { + faultvalues.erase(faultvalues.begin()); + } + bool pressed = embot::hw::button::pressed(buttonEXTfault); + faultvalues.push_back(pressed); + bool allpressed = std::all_of(faultvalues.cbegin(), faultvalues.cend(), [](auto v){ return v; }); + + EXTFAULTisPRESSED = allpressed; + AMC_BLDC_U.ExternalFlags_p.fault_button = EXTFAULTisPRESSED; + + if(true == EXTFAULTisPRESSED) + { + EXTFAULTpressedtime = embot::core::now(); +#if defined(EXTFAULT_handler_will_disable_motor) + embot::hw::motor::fault(embot::hw::MOTOR::one, true); +#endif + } + else + { + embot::hw::motor::fault(embot::hw::MOTOR::one, false); + EXTFAULTreleasedtime = embot::core::now(); + } + +} + +void embot::app::board::amc2c::theMBD::Impl::onEXTFAULTpressedreleased(void *owner) +{ + Impl * impl = reinterpret_cast(owner); + if(nullptr == impl) + { + return; + } + + impl->EXTFAULTisPRESSED = embot::hw::button::pressed(impl->buttonEXTfault); + AMC_BLDC_U.ExternalFlags_p.fault_button = impl->EXTFAULTisPRESSED; + + if(true == impl->EXTFAULTisPRESSED) + { + impl->EXTFAULTpressedtime = embot::core::now(); +#if defined(EXTFAULT_handler_will_disable_motor) + embot::hw::motor::fault(embot::hw::MOTOR::one, true); +#endif + } + else + { + embot::hw::motor::fault(embot::hw::MOTOR::one, false); + impl->EXTFAULTreleasedtime = embot::core::now(); + } +} + +embot::core::Time lt {0}; + +// Called every 1 ms +bool embot::app::board::amc2c::theMBD::Impl::tick(const std::vector &inpframes, std::vector &outframes) +{ + + constexpr embot::core::Time freq {embot::core::time1second}; + embot::core::Time n = embot::core::now(); + if((n-lt) > freq) + { + lt = n; + embot::core::TimeFormatter tf {n}; +// embot::core::print(tf.to_string() + ": the cm4 is alive"); + } + // in here... + // inpframes.size() is always <= 4. + // outputframes.size() is always = 0 + + measureTick->start(); + + onEXTFAULTpolling(); + + if(prevEXTFAULTisPRESSED != EXTFAULTisPRESSED) + { + prevEXTFAULTisPRESSED = EXTFAULTisPRESSED; + // and manage the transitions [pressed -> unpressed] or vice-versa and use also + // EXTFAULTpressedtime and / or EXTFAULTreleasedtime and + + AMC_BLDC_U.ExternalFlags_p.fault_button = EXTFAULTisPRESSED; + + if(true == EXTFAULTisPRESSED) + { + embot::app::theLEDmanager::getInstance().get(ledEXTfault).on(); + } + else + { + embot::app::theLEDmanager::getInstance().get(ledEXTfault).off(); + } + } + + // add any input can frame into the supervisor input queue + + size_t ninputframes = std::min(inpframes.size(), static_cast(CAN_MAX_NUM_PACKETS)); + + for(uint8_t i=0; istop(); + + + return true; +} + +#if defined(TEST_FOC_logvalues) + +struct dbgFOCvalues +{ + embot::hw::motor::Currents currents {0, 0, 0}; + uint8_t hall {0}; + embot::hw::motor::Position electricalangle {0}; + int32_t position {0}; + float jointangle {0.0}; + std::array pwms {0, 0, 0}; + + dbgFOCvalues() = default; +}; + +dbgFOCvalues dbgFOC {}; + +#endif + +int32_t cURRENT01 {0}; +int32_t cURRENT02 {0}; +int32_t cURRENT03 {0}; + +embot::hw::motor::Currents cURRENTs {}; + +void embot::app::board::amc2c::theMBD::Impl::onCurrents_FOC_innerloop(void *owner, const embot::hw::motor::Currents * const currents) +{ + Impl * impl = reinterpret_cast(owner); + if((nullptr == impl) || (nullptr == currents)) + { + return; + } + + cURRENTs = *currents; + + cURRENT01 = currents->u; + cURRENT02 = currents->v; + cURRENT03 = currents->w; + +return; + + + impl->measureFOC->start(); + + // 1. copy currents straight away, so that we can use them + embot::hw::motor::Currents currs = *currents; + +#if defined(TEST_DURATION_FOC) + embot::hw::sys::delay(25); +#else + + // remember to manage impl->EXTFAULTisPRESSED ............ + + // retrieve the current value of the Hall sensors + uint8_t hall {0}; + embot::hw::motor::gethallstatus(embot::hw::MOTOR::one, hall); + AMC_BLDC_U.SensorsData_p.motorsensors.hallABC = hall; + + // retrieve the current value of the encoder + embot::hw::motor::Position electricalAngle {0}; + embot::hw::motor::getencoder(embot::hw::MOTOR::one, electricalAngle); + + + static int32_T position {0}; + static int32_T electricalAngleOld = static_cast(electricalAngle); + // keep int16_t ... to manage overflow ... + int16_t delta = electricalAngle - electricalAngleOld; + electricalAngleOld = electricalAngle; + + // calculate the current joint position + size_t polepairs = (0 != AMC_BLDC_Y.ConfigurationParameters_p.motorconfig.pole_pairs) ? AMC_BLDC_Y.ConfigurationParameters_p.motorconfig.pole_pairs : 1; + position = position + delta / polepairs; + + AMC_BLDC_U.SensorsData_p.motorsensors.angle = static_cast(electricalAngle)*0.0054931640625f; // (60 interval angle) + + // convert the current from mA to A + AMC_BLDC_U.SensorsData_p.motorsensors.Iabc[0] = 0.001f*currs.u; + AMC_BLDC_U.SensorsData_p.motorsensors.Iabc[1] = 0.001f*currs.v; + AMC_BLDC_U.SensorsData_p.motorsensors.Iabc[2] = 0.001f*currs.w; + + // ----------------------------------------------------------------------------- + // FOC Step Function (~26.6 KHz) + // ----------------------------------------------------------------------------- + + AMC_BLDC_step_FOC(); + + // ----------------------------------------------------------------------------- + + AMC_BLDC_U.SensorsData_p.jointpositions.position = static_cast(position) * 0.0054931640625f; // iCubDegree -> deg + + // Set the voltages + int32_T Vabc0 = static_cast(AMC_BLDC_Y.ControlOutputs_p.Vabc[0] * 163.83F); + int32_T Vabc1 = static_cast(AMC_BLDC_Y.ControlOutputs_p.Vabc[1] * 163.83F); + int32_T Vabc2 = static_cast(AMC_BLDC_Y.ControlOutputs_p.Vabc[2] * 163.83F); + +#if defined(TEST_FOC_logvalues) + + dbgFOC.currents = currs; + dbgFOC.hall = hall; + dbgFOC.electricalangle = electricalAngle; + dbgFOC.position = position; + dbgFOC.jointangle = static_cast(position) * 0.0054931640625f; + dbgFOC.pwms = {Vabc0, Vabc1, Vabc2}; + +#endif + + embot::hw::motor::setpwm(embot::hw::MOTOR::one, Vabc0, Vabc1, Vabc2); + + +//#define DEBUG_PARAMS +#ifdef DEBUG_PARAMS + + static char msg2[64]; + static uint32_t counter; + if(counter % 10 == 0) + { + sprintf(msg2, "%d,%d,%d,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f", \ + Vabc0, \ + Vabc1, \ + Vabc2, \ + AMC_BLDC_U.SensorsData_p.motorsensors.Iabc[0], \ + AMC_BLDC_U.SensorsData_p.motorsensors.Iabc[1], \ + AMC_BLDC_U.SensorsData_p.motorsensors.Iabc[2], \ + AMC_BLDC_U.SensorsData_p.motorsensors.angle, \ + AMC_BLDC_Y.EstimatedData_p.jointvelocities.velocity, \ + AMC_BLDC_B.Targets_n.motorcurrent.current, \ + AMC_BLDC_Y.ControlOutputs_p.Iq_fbk.current, \ + AMC_BLDC_Y.ControlOutputs_p.Vq); + +// sprintf(msg2, "%d,%d,%d,%.3f,%.3f,%.3f", \ +// Vabc0, \ +// Vabc1, \ +// Vabc2, \ +// AMC_BLDC_U.SensorsData_p.motorsensors.Iabc[0], \ +// AMC_BLDC_U.SensorsData_p.motorsensors.Iabc[1], \ +// AMC_BLDC_U.SensorsData_p.motorsensors.Iabc[2]); + embot::core::print(msg2); + counter = 0; + } + counter++; +#endif + +#endif // #if defined(TEST_DURATION_FOC) + + impl->measureFOC->stop(); +} + +#ifdef PRINT_HISTO_DEBUG +void embot::app::board::amc2c::theMBD::Impl::printHistogram_rxPkt(size_t cur_size) +{ + if(cur_size(); +} + + +embot::app::board::amc2c::theMBD::~theMBD() { } + + +bool embot::app::board::amc2c::theMBD::initialise(const Config &config) +{ + return pImpl->initialise(config); +} + +bool embot::app::board::amc2c::theMBD::tick(const std::vector &inpframes, std::vector &outframes) +{ + return pImpl->tick(inpframes, outframes); +} + + + +// - end-of-file (leave a blank line after)---------------------------------------------------------------------------- + + diff --git a/emBODY/eBcode/arch-arm/board/amc2c/test/src/app-board-amc2c/embot_app_board_amc2c_theMBD-test-trace.cpp b/emBODY/eBcode/arch-arm/board/amc2c/test/src/app-board-amc2c/embot_app_board_amc2c_theMBD-test-trace.cpp new file mode 100644 index 0000000000..94d5fe4aba --- /dev/null +++ b/emBODY/eBcode/arch-arm/board/amc2c/test/src/app-board-amc2c/embot_app_board_amc2c_theMBD-test-trace.cpp @@ -0,0 +1,718 @@ + +/* + * Copyright (C) 2023 iCub Tech - Istituto Italiano di Tecnologia + * Author: Marco Accame + * email: marco.accame@iit.it +*/ + + +// -------------------------------------------------------------------------------------------------------------------- +// - public interface +// -------------------------------------------------------------------------------------------------------------------- + +#include "embot_app_board_amc2c_theMBD.h" + + +// -------------------------------------------------------------------------------------------------------------------- +// - external dependencies +// -------------------------------------------------------------------------------------------------------------------- + +#if defined(STM32HAL_BOARD_AMC2C) +#include "embot_hw_bsp_amc2c.h" +//#warning READ CAREFULLY: we may use the same object for both amcbldc and amc2c +#elif defined(STM32HAL_BOARD_AMCBLDC) +#include "embot_hw_bsp_amcbldc.h" +#else +#error: choose a STM32HAL_BOARD_* +#endif + +#include "embot_hw_button.h" +#include "embot_hw_motor.h" +#include "embot_app_scope.h" +#include "embot_hw_sys.h" +#include "embot_core.h" +#include "embot_app_theLEDmanager.h" +#include + +// some protocol includes +#include "embot_prot_can_motor_polling.h" +#include "embot_prot_can_motor_periodic.h" + + +// mdb components +#include "AMC_BLDC.h" + +// -------------------------------------------------------------------------------------------------------------------- +// - defines +// -------------------------------------------------------------------------------------------------------------------- +//#define TEST_DURATION_FOC + +#define TEST_FOC_logvalues + + +// If the target setup is the KOLLMORGEN motor on RED LEGO platform, +// you need to enable the macro USE_KOLLMORGEN_SETUP, otherwise +// it is implied that motor on wrist mk2 is in use + +//#define EXTFAULT_enabled +#define EXTFAULT_handler_will_disable_motor + +#define EXTFAULT_enabled_polling + +#if defined(EXTFAULT_enabled) +#undef EXTFAULT_enabled_polling +#endif + +// -------------------------------------------------------------------------------------------------------------------- +// - pimpl: private implementation (see scott meyers: item 22 of effective modern c++, item 31 of effective c++ +// -------------------------------------------------------------------------------------------------------------------- + + +namespace embot::app::board::amc2c { + +struct Measure +{ + public: + // interface + virtual void start() = 0; + virtual void stop() = 0; + + protected: + virtual ~Measure() {}; // cannot delete from interface but only from derived object +}; + +// this implementation does nothing +class MeasureDummy: public Measure +{ +public: + + struct Config + { + uint32_t nothing {0}; + Config() = default; + bool isvalid() const { return true; } + }; + + MeasureDummy() {}; + virtual ~MeasureDummy() {}; + + virtual void start() override {}; + virtual void stop() override {}; +}; + +// how to use it: +// 1. create a MeasureHisto w/ a proper MeasureHisto::Config +// 2. call start() and stop() at the beginning and at the end of the piece of code +// of which you want to measure the duration. +// the duration will be added in a histogram whose properties are in +// Config::minduration, Config::maxduration, Config::resolution +// 3. the histogram can be viewed using method MeasureHisto::report(). +// you must place a breakpoint in it and explore the volatile variables {name, beyond, vals} +// but you can also enable a print. you can use Config::name to differentiate amongst +// multiple instances of MeasureHisto +// 4. method MeasureHisto::report() can be called at any time, but if Config::reportfrequency is true, +// is is also called at the end of stop() at the rate specified by Config::reportfrequency + +struct MeasureHisto : public Measure +{ + struct Config + { + const char *name {"dummy"}; + embot::core::relTime minduration {0}; + embot::core::relTime maxduration {100*embot::core::time1microsec}; + embot::core::relTime resolution {100*embot::core::time1microsec}; + bool enablereport {true}; + uint32_t reportfrequency {10000}; + constexpr Config() = default; + constexpr Config(const char *n, embot::core::relTime min, embot::core::relTime max, + embot::core::relTime res = embot::core::time1microsec, + bool er = true, uint32_t rf = 10000) + : name(n), minduration(min), maxduration(max), resolution(res), + enablereport(er), reportfrequency(rf){} + }; + + Config config {}; + + embot::app::scope::SignalHisto *sighisto {nullptr}; + + const embot::tools::Histogram::Values * vv {nullptr}; + + // i use ram ... maybe not best solution. maybe we could also print the histo. + static constexpr size_t nvals {512}; + volatile uint64_t vals[nvals] {0}; + volatile uint64_t beyond {0}; + uint32_t counter {0}; + + MeasureHisto(const Config &cfg) + { + config = cfg; + if(nullptr == config.name) + { + config.name = "dummy"; + } + sighisto = new embot::app::scope::SignalHisto({config.minduration, config.maxduration, config.resolution}); + vv = sighisto->histogram()->getvalues(); + } + + void start() + { + sighisto->on(); + } + + void stop() + { + sighisto->off(); + + if(config.enablereport) + { + if(++counter >= config.reportfrequency) + { + counter = 0; + report(); + } + } + } + +#define PRINT_REPORT + + void report() + { + beyond = vv->beyond; + + for(int i=0; iinside.size(); i++) + { + if(i < nvals) + { + vals[i] = vv->inside[i]; + } + } + // place a breakpoint in here and inspect: beyond and vals[] + beyond = beyond; + // but you can also use embot::core::print() to collect values from the printf window + // the following code can be easily changed according to needs +#if defined(PRINT_REPORT) + std::string str = std::string("name, beyond, histo = " ) + config.name + ", " + std::to_string(beyond); + for(int v=0; v &inpframes, std::vector &outframes); + + // the code called @ 80/3 kHz (aka every 37.5 usec) inside the DMA1_Channel2_IRQHandler() + // it must be static because we use it as a callback + static void onCurrents_FOC_innerloop(void *owner, const embot::hw::motor::Currents * const currents); + + // the objects which measure the duration of the two above functions + Measure *measureFOC {nullptr}; + Measure *measureTick {nullptr}; + + // all the rest, which may or may not be required anymore + Config _config {}; + bool initted {false}; + + embot::hw::motor::Pwm pwm {0}; + embot::prot::can::motor::polling::ControlMode cm {embot::prot::can::motor::polling::ControlMode::Idle}; + bool applychanges {false}; + + embot::hw::BTN buttonEXTfault {embot::hw::BTN::one}; + embot::hw::LED ledEXTfault {embot::hw::LED::two}; + static void onEXTFAULTpressedreleased(void *owner); + volatile bool EXTFAULTisPRESSED {false}; + volatile bool prevEXTFAULTisPRESSED {false}; + volatile embot::core::Time EXTFAULTpressedtime {0}; + volatile embot::core::Time EXTFAULTreleasedtime {0}; + + void onEXTFAULTpolling(); + std::vector faultvalues {}; + static constexpr size_t faultcapacity {5}; + + #ifdef PRINT_HISTO_DEBUG + void printHistogram_rxPkt(size_t cur_size); //cur_size is the currente queue size + static constexpr uint8_t numOfBins {11}; //size of queue of the CAN drive plus 1 (in case any packet is received) + uint32_t bin[numOfBins]{0}; + #endif + +}; + + + +bool embot::app::board::amc2c::theMBD::Impl::initialise(const Config &config) +{ + if(true == initted) + { + return true; + } + + _config = config; + + embot::core::print("embot::app::board::amc2c::theMBD::Impl::initialise()"); + + // create the measure for the foc + if(true == useDUMMYforFOC) + { + measureFOC = new MeasureDummy; + } + else + { + measureFOC = new MeasureHisto({"foc", 0, 64*embot::core::time1microsec, 1, true, 100*1000}); + } + + // create the measure for the tick + if(true == useDUMMYforTICK) + { + measureTick = new MeasureDummy; + } + else + { + measureTick = new MeasureHisto({"tick", 0, 400*embot::core::time1microsec, 1, true, 10*1000}); + } + + // init the external fault. + // we call cbkOnEXTfault.execute() when the button is pressed or released. + // in the callback we set EXTFAULTisPRESSED true or false depending on value of + // embot::hw::button::pressed(buttonEXTfault) + // we also disable the motors if true. + + buttonEXTfault = embot::hw::bsp::amc2c::EXTFAULTbutton(); + ledEXTfault = embot::hw::bsp::amc2c::EXTFAULTled(); + +#if defined(EXTFAULT_enabled) + + embot::core::Callback cbkOnEXTFAULT {onEXTFAULTpressedreleased, this}; + embot::hw::button::init(buttonEXTfault, {embot::hw::button::Mode::TriggeredOnPressAndRelease, cbkOnEXTFAULT, 0}); + prevEXTFAULTisPRESSED = EXTFAULTisPRESSED = embot::hw::button::pressed(buttonEXTfault); + +#elif defined(EXTFAULT_enabled_polling) + + faultvalues.reserve(faultcapacity); + faultvalues.clear(); + + embot::hw::button::init(buttonEXTfault, {embot::hw::button::Mode::Polling, {}, 0}); + for(size_t i=0; i= faultcapacity) + { + faultvalues.erase(faultvalues.begin()); + } + bool pressed = embot::hw::button::pressed(buttonEXTfault); + faultvalues.push_back(pressed); + bool allpressed = std::all_of(faultvalues.cbegin(), faultvalues.cend(), [](auto v){ return v; }); + + EXTFAULTisPRESSED = allpressed; + AMC_BLDC_U.ExternalFlags_p.fault_button = EXTFAULTisPRESSED; + + if(true == EXTFAULTisPRESSED) + { + EXTFAULTpressedtime = embot::core::now(); +#if defined(EXTFAULT_handler_will_disable_motor) + embot::hw::motor::fault(embot::hw::MOTOR::one, true); +#endif + } + else + { + embot::hw::motor::fault(embot::hw::MOTOR::one, false); + EXTFAULTreleasedtime = embot::core::now(); + } + +} + +void embot::app::board::amc2c::theMBD::Impl::onEXTFAULTpressedreleased(void *owner) +{ + Impl * impl = reinterpret_cast(owner); + if(nullptr == impl) + { + return; + } + + impl->EXTFAULTisPRESSED = embot::hw::button::pressed(impl->buttonEXTfault); + AMC_BLDC_U.ExternalFlags_p.fault_button = impl->EXTFAULTisPRESSED; + + if(true == impl->EXTFAULTisPRESSED) + { + impl->EXTFAULTpressedtime = embot::core::now(); +#if defined(EXTFAULT_handler_will_disable_motor) + embot::hw::motor::fault(embot::hw::MOTOR::one, true); +#endif + } + else + { + embot::hw::motor::fault(embot::hw::MOTOR::one, false); + impl->EXTFAULTreleasedtime = embot::core::now(); + } +} + +// Called every 1 ms +bool embot::app::board::amc2c::theMBD::Impl::tick(const std::vector &inpframes, std::vector &outframes) +{ + + static uint32_t nn {0}; + static uint32_t s {0}; + nn++; + if(0 == (nn % 10)) + { + s++; + embot::core::print("hi: " + std::to_string(s)); + } + + // in here... + // inpframes.size() is always <= 4. + // outputframes.size() is always = 0 + + measureTick->start(); + + onEXTFAULTpolling(); + + if(prevEXTFAULTisPRESSED != EXTFAULTisPRESSED) + { + prevEXTFAULTisPRESSED = EXTFAULTisPRESSED; + // and manage the transitions [pressed -> unpressed] or vice-versa and use also + // EXTFAULTpressedtime and / or EXTFAULTreleasedtime and + + AMC_BLDC_U.ExternalFlags_p.fault_button = EXTFAULTisPRESSED; + + if(true == EXTFAULTisPRESSED) + { + embot::app::theLEDmanager::getInstance().get(ledEXTfault).on(); + } + else + { + embot::app::theLEDmanager::getInstance().get(ledEXTfault).off(); + } + } + + // add any input can frame into the supervisor input queue + + size_t ninputframes = std::min(inpframes.size(), static_cast(CAN_MAX_NUM_PACKETS)); + + for(uint8_t i=0; istop(); + + + return true; +} + +#if defined(TEST_FOC_logvalues) + +struct dbgFOCvalues +{ + embot::hw::motor::Currents currents {0, 0, 0}; + uint8_t hall {0}; + embot::hw::motor::Position electricalangle {0}; + int32_t position {0}; + float jointangle {0.0}; + std::array pwms {0, 0, 0}; + + dbgFOCvalues() = default; +}; + +dbgFOCvalues dbgFOC {}; + +#endif + +int32_t cURRENT01 = 0; +int32_t cURRENT02 = 0; +int32_t cURRENT03 = 0; +int32_t tenFOCwave = 0; + +// #define TEST_DURATION_FOC + +void embot::app::board::amc2c::theMBD::Impl::onCurrents_FOC_innerloop(void *owner, const embot::hw::motor::Currents * const currents) +{ + Impl * impl = reinterpret_cast(owner); + if((nullptr == impl) || (nullptr == currents)) + { + return; + } + + impl->measureFOC->start(); + + // 1. copy currents straight away, so that we can use them + embot::hw::motor::Currents currs = *currents; + + cURRENT01 = currs.u; + cURRENT02 = currs.v; + cURRENT03 = currs.w; + tenFOCwave++; + if(tenFOCwave >= 10) + { + tenFOCwave = 0; + } + + return; + +#if defined(TEST_DURATION_FOC) + embot::hw::sys::delay(5); +#else + + // remember to manage impl->EXTFAULTisPRESSED ............ + + // retrieve the current value of the Hall sensors + uint8_t hall {0}; + embot::hw::motor::gethallstatus(embot::hw::MOTOR::one, hall); + AMC_BLDC_U.SensorsData_p.motorsensors.hallABC = hall; + + // retrieve the current value of the encoder + embot::hw::motor::Position electricalAngle {0}; + embot::hw::motor::getencoder(embot::hw::MOTOR::one, electricalAngle); + + + static int32_T position {0}; + static int32_T electricalAngleOld = static_cast(electricalAngle); + // keep int16_t ... to manage overflow ... + int16_t delta = electricalAngle - electricalAngleOld; + electricalAngleOld = electricalAngle; + + // calculate the current joint position + size_t polepairs = (0 != AMC_BLDC_Y.ConfigurationParameters_p.motorconfig.pole_pairs) ? AMC_BLDC_Y.ConfigurationParameters_p.motorconfig.pole_pairs : 1; + position = position + delta / polepairs; + + AMC_BLDC_U.SensorsData_p.motorsensors.angle = static_cast(electricalAngle)*0.0054931640625f; // (60 interval angle) + + // convert the current from mA to A + AMC_BLDC_U.SensorsData_p.motorsensors.Iabc[0] = 0.001f*currs.u; + AMC_BLDC_U.SensorsData_p.motorsensors.Iabc[1] = 0.001f*currs.v; + AMC_BLDC_U.SensorsData_p.motorsensors.Iabc[2] = 0.001f*currs.w; + + // ----------------------------------------------------------------------------- + // FOC Step Function (~26.6 KHz) + // ----------------------------------------------------------------------------- + + AMC_BLDC_step_FOC(); + + // ----------------------------------------------------------------------------- + + AMC_BLDC_U.SensorsData_p.jointpositions.position = static_cast(position) * 0.0054931640625f; // iCubDegree -> deg + + // Set the voltages + int32_T Vabc0 = static_cast(AMC_BLDC_Y.ControlOutputs_p.Vabc[0] * 163.83F); + int32_T Vabc1 = static_cast(AMC_BLDC_Y.ControlOutputs_p.Vabc[1] * 163.83F); + int32_T Vabc2 = static_cast(AMC_BLDC_Y.ControlOutputs_p.Vabc[2] * 163.83F); + +#if defined(TEST_FOC_logvalues) + + dbgFOC.currents = currs; + dbgFOC.hall = hall; + dbgFOC.electricalangle = electricalAngle; + dbgFOC.position = position; + dbgFOC.jointangle = static_cast(position) * 0.0054931640625f; + dbgFOC.pwms = {Vabc0, Vabc1, Vabc2}; + +#endif + + embot::hw::motor::setpwm(embot::hw::MOTOR::one, Vabc0, Vabc1, Vabc2); + + +//#define DEBUG_PARAMS +#ifdef DEBUG_PARAMS + + static char msg2[64]; + static uint32_t counter; + if(counter % 10 == 0) + { + sprintf(msg2, "%d,%d,%d,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f", \ + Vabc0, \ + Vabc1, \ + Vabc2, \ + AMC_BLDC_U.SensorsData_p.motorsensors.Iabc[0], \ + AMC_BLDC_U.SensorsData_p.motorsensors.Iabc[1], \ + AMC_BLDC_U.SensorsData_p.motorsensors.Iabc[2], \ + AMC_BLDC_U.SensorsData_p.motorsensors.angle, \ + AMC_BLDC_Y.EstimatedData_p.jointvelocities.velocity, \ + AMC_BLDC_B.Targets_n.motorcurrent.current, \ + AMC_BLDC_Y.ControlOutputs_p.Iq_fbk.current, \ + AMC_BLDC_Y.ControlOutputs_p.Vq); + +// sprintf(msg2, "%d,%d,%d,%.3f,%.3f,%.3f", \ +// Vabc0, \ +// Vabc1, \ +// Vabc2, \ +// AMC_BLDC_U.SensorsData_p.motorsensors.Iabc[0], \ +// AMC_BLDC_U.SensorsData_p.motorsensors.Iabc[1], \ +// AMC_BLDC_U.SensorsData_p.motorsensors.Iabc[2]); + embot::core::print(msg2); + counter = 0; + } + counter++; +#endif + +#endif // #if defined(TEST_DURATION_FOC) + + impl->measureFOC->stop(); +} + +#ifdef PRINT_HISTO_DEBUG +void embot::app::board::amc2c::theMBD::Impl::printHistogram_rxPkt(size_t cur_size) +{ + if(cur_size(); +} + + +embot::app::board::amc2c::theMBD::~theMBD() { } + + +bool embot::app::board::amc2c::theMBD::initialise(const Config &config) +{ + return pImpl->initialise(config); +} + +bool embot::app::board::amc2c::theMBD::tick(const std::vector &inpframes, std::vector &outframes) +{ + return pImpl->tick(inpframes, outframes); +} + + + +// - end-of-file (leave a blank line after)---------------------------------------------------------------------------- + + diff --git a/emBODY/eBcode/arch-arm/board/amc2c/test/src/app-board-amc2c/embot_app_board_amc2c_theMBD.cpp b/emBODY/eBcode/arch-arm/board/amc2c/test/src/app-board-amc2c/embot_app_board_amc2c_theMBD.cpp new file mode 100644 index 0000000000..d19ddc3316 --- /dev/null +++ b/emBODY/eBcode/arch-arm/board/amc2c/test/src/app-board-amc2c/embot_app_board_amc2c_theMBD.cpp @@ -0,0 +1,709 @@ + +/* + * Copyright (C) 2023 iCub Tech - Istituto Italiano di Tecnologia + * Author: Marco Accame + * email: marco.accame@iit.it +*/ + + +// -------------------------------------------------------------------------------------------------------------------- +// - public interface +// -------------------------------------------------------------------------------------------------------------------- + +#include "embot_app_board_amc2c_theMBD.h" + + +// -------------------------------------------------------------------------------------------------------------------- +// - external dependencies +// -------------------------------------------------------------------------------------------------------------------- + +#if defined(STM32HAL_BOARD_AMC2C) +#include "embot_hw_bsp_amc2c.h" +//#warning READ CAREFULLY: we may use the same object for both amcbldc and amc2c +#elif defined(STM32HAL_BOARD_AMCBLDC) +#include "embot_hw_bsp_amcbldc.h" +#else +#error: choose a STM32HAL_BOARD_* +#endif + +#include "embot_hw_button.h" +#include "embot_hw_motor.h" +#include "embot_app_scope.h" +#include "embot_hw_sys.h" +#include "embot_core.h" +#include "embot_app_theLEDmanager.h" +#include "embot_hw_testpoint.h" +#include + +// some protocol includes +#include "embot_prot_can_motor_polling.h" +#include "embot_prot_can_motor_periodic.h" + + +// mdb components +#include "AMC_BLDC.h" + +// -------------------------------------------------------------------------------------------------------------------- +// - defines +// -------------------------------------------------------------------------------------------------------------------- +//#define TEST_DURATION_FOC + +//#define TEST_FOC_logvalues + + +// If the target setup is the KOLLMORGEN motor on RED LEGO platform, +// you need to enable the macro USE_KOLLMORGEN_SETUP, otherwise +// it is implied that motor on wrist mk2 is in use + +//#define EXTFAULT_enabled +#define EXTFAULT_handler_will_disable_motor + +#define EXTFAULT_enabled_polling + +#if defined(EXTFAULT_enabled) +#undef EXTFAULT_enabled_polling +#endif + +// -------------------------------------------------------------------------------------------------------------------- +// - pimpl: private implementation (see scott meyers: item 22 of effective modern c++, item 31 of effective c++ +// -------------------------------------------------------------------------------------------------------------------- + + +namespace embot::app::board::amc2c { + +struct Measure +{ + public: + // interface + virtual void start() = 0; + virtual void stop() = 0; + + protected: + virtual ~Measure() {}; // cannot delete from interface but only from derived object +}; + +// this implementation does nothing +class MeasureDummy: public Measure +{ +public: + + struct Config + { + uint32_t nothing {0}; + Config() = default; + bool isvalid() const { return true; } + }; + + MeasureDummy() {}; + virtual ~MeasureDummy() {}; + + virtual void start() override {}; + virtual void stop() override {}; +}; + +// how to use it: +// 1. create a MeasureHisto w/ a proper MeasureHisto::Config +// 2. call start() and stop() at the beginning and at the end of the piece of code +// of which you want to measure the duration. +// the duration will be added in a histogram whose properties are in +// Config::minduration, Config::maxduration, Config::resolution +// 3. the histogram can be viewed using method MeasureHisto::report(). +// you must place a breakpoint in it and explore the volatile variables {name, beyond, vals} +// but you can also enable a print. you can use Config::name to differentiate amongst +// multiple instances of MeasureHisto +// 4. method MeasureHisto::report() can be called at any time, but if Config::reportfrequency is true, +// is is also called at the end of stop() at the rate specified by Config::reportfrequency + +struct MeasureHisto : public Measure +{ + struct Config + { + const char *name {"dummy"}; + embot::core::relTime minduration {0}; + embot::core::relTime maxduration {100*embot::core::time1microsec}; + embot::core::relTime resolution {100*embot::core::time1microsec}; + bool enablereport {true}; + uint32_t reportfrequency {10000}; + constexpr Config() = default; + constexpr Config(const char *n, embot::core::relTime min, embot::core::relTime max, + embot::core::relTime res = embot::core::time1microsec, + bool er = true, uint32_t rf = 10000) + : name(n), minduration(min), maxduration(max), resolution(res), + enablereport(er), reportfrequency(rf){} + }; + + Config config {}; + + embot::app::scope::SignalHisto *sighisto {nullptr}; + + const embot::tools::Histogram::Values * vv {nullptr}; + + // i use ram ... maybe not best solution. maybe we could also print the histo. + static constexpr size_t nvals {512}; + volatile uint64_t vals[nvals] {0}; + volatile uint64_t beyond {0}; + uint32_t counter {0}; + + MeasureHisto(const Config &cfg) + { + config = cfg; + if(nullptr == config.name) + { + config.name = "dummy"; + } + sighisto = new embot::app::scope::SignalHisto({config.minduration, config.maxduration, config.resolution}); + vv = sighisto->histogram()->getvalues(); + } + + void start() + { + sighisto->on(); + } + + void stop() + { + sighisto->off(); + + if(config.enablereport) + { + if(++counter >= config.reportfrequency) + { + counter = 0; + report(); + } + } + } + +#define PRINT_REPORT + + void report() + { + beyond = vv->beyond; + + for(int i=0; iinside.size(); i++) + { + if(i < nvals) + { + vals[i] = vv->inside[i]; + } + } + // place a breakpoint in here and inspect: beyond and vals[] + beyond = beyond; + // but you can also use embot::core::print() to collect values from the printf window + // the following code can be easily changed according to needs +#if defined(PRINT_REPORT) + std::string str = std::string("name, beyond, histo = " ) + config.name + ", " + std::to_string(beyond); + for(int v=0; v &inpframes, std::vector &outframes); + + // the code called @ 80/3 kHz (aka every 37.5 usec) inside the DMA1_Channel2_IRQHandler() + // it must be static because we use it as a callback + static void onCurrents_FOC_innerloop(void *owner, const embot::hw::motor::Currents * const currents); + + // the objects which measure the duration of the two above functions + Measure *measureFOC {nullptr}; + Measure *measureTick {nullptr}; + + // all the rest, which may or may not be required anymore + Config _config {}; + bool initted {false}; + + embot::hw::motor::Pwm pwm {0}; + embot::prot::can::motor::polling::ControlMode cm {embot::prot::can::motor::polling::ControlMode::Idle}; + bool applychanges {false}; + + embot::hw::BTN buttonEXTfault {embot::hw::BTN::one}; + embot::hw::LED ledEXTfault {embot::hw::LED::two}; + static void onEXTFAULTpressedreleased(void *owner); + volatile bool EXTFAULTisPRESSED {false}; + volatile bool prevEXTFAULTisPRESSED {false}; + volatile embot::core::Time EXTFAULTpressedtime {0}; + volatile embot::core::Time EXTFAULTreleasedtime {0}; + + void onEXTFAULTpolling(); + std::vector faultvalues {}; + static constexpr size_t faultcapacity {5}; + + #ifdef PRINT_HISTO_DEBUG + void printHistogram_rxPkt(size_t cur_size); //cur_size is the currente queue size + static constexpr uint8_t numOfBins {11}; //size of queue of the CAN drive plus 1 (in case any packet is received) + uint32_t bin[numOfBins]{0}; + #endif + + static constexpr embot::hw::TESTPOINT tp1 {embot::hw::TESTPOINT::one}; + static constexpr embot::hw::TESTPOINT tp2 {embot::hw::TESTPOINT::two}; +}; + + + +bool embot::app::board::amc2c::theMBD::Impl::initialise(const Config &config) +{ + if(true == initted) + { + return true; + } + + _config = config; + + embot::core::print("embot::app::board::amc2c::theMBD::Impl::initialise()"); + + // create the measure for the foc + if(true == useDUMMYforFOC) + { + measureFOC = new MeasureDummy; + } + else + { + measureFOC = new MeasureHisto({"foc", 0, 64*embot::core::time1microsec, 1, true, 100*1000}); + } + + // create the measure for the tick + if(true == useDUMMYforTICK) + { + measureTick = new MeasureDummy; + } + else + { + measureTick = new MeasureHisto({"tick", 0, 400*embot::core::time1microsec, 1, true, 10*1000}); + } + + // init the external fault. + // we call cbkOnEXTfault.execute() when the button is pressed or released. + // in the callback we set EXTFAULTisPRESSED true or false depending on value of + // embot::hw::button::pressed(buttonEXTfault) + // we also disable the motors if true. + + buttonEXTfault = embot::hw::bsp::amc2c::EXTFAULTbutton(); + ledEXTfault = embot::hw::bsp::amc2c::EXTFAULTled(); + +#if defined(EXTFAULT_enabled) + + embot::core::Callback cbkOnEXTFAULT {onEXTFAULTpressedreleased, this}; + embot::hw::button::init(buttonEXTfault, {embot::hw::button::Mode::TriggeredOnPressAndRelease, cbkOnEXTFAULT, 0}); + prevEXTFAULTisPRESSED = EXTFAULTisPRESSED = embot::hw::button::pressed(buttonEXTfault); + +#elif defined(EXTFAULT_enabled_polling) + + faultvalues.reserve(faultcapacity); + faultvalues.clear(); + + embot::hw::button::init(buttonEXTfault, {embot::hw::button::Mode::Polling, {}, 0}); + for(size_t i=0; i= faultcapacity) + { + faultvalues.erase(faultvalues.begin()); + } + bool pressed = embot::hw::button::pressed(buttonEXTfault); + faultvalues.push_back(pressed); + bool allpressed = std::all_of(faultvalues.cbegin(), faultvalues.cend(), [](auto v){ return v; }); + + EXTFAULTisPRESSED = allpressed; + AMC_BLDC_U.ExternalFlags_p.fault_button = EXTFAULTisPRESSED; + + if(true == EXTFAULTisPRESSED) + { + EXTFAULTpressedtime = embot::core::now(); +#if defined(EXTFAULT_handler_will_disable_motor) + embot::hw::motor::fault(embot::hw::MOTOR::one, true); +#endif + } + else + { + embot::hw::motor::fault(embot::hw::MOTOR::one, false); + EXTFAULTreleasedtime = embot::core::now(); + } + +} + +void embot::app::board::amc2c::theMBD::Impl::onEXTFAULTpressedreleased(void *owner) +{ + Impl * impl = reinterpret_cast(owner); + if(nullptr == impl) + { + return; + } + + impl->EXTFAULTisPRESSED = embot::hw::button::pressed(impl->buttonEXTfault); + AMC_BLDC_U.ExternalFlags_p.fault_button = impl->EXTFAULTisPRESSED; + + if(true == impl->EXTFAULTisPRESSED) + { + impl->EXTFAULTpressedtime = embot::core::now(); +#if defined(EXTFAULT_handler_will_disable_motor) + embot::hw::motor::fault(embot::hw::MOTOR::one, true); +#endif + } + else + { + embot::hw::motor::fault(embot::hw::MOTOR::one, false); + impl->EXTFAULTreleasedtime = embot::core::now(); + } +} + +// Called every 1 ms +bool embot::app::board::amc2c::theMBD::Impl::tick(const std::vector &inpframes, std::vector &outframes) +{ + + // in here... + // inpframes.size() is always <= 4. + // outputframes.size() is always = 0 + embot::hw::testpoint::on(tp1); + + measureTick->start(); + + onEXTFAULTpolling(); + + if(prevEXTFAULTisPRESSED != EXTFAULTisPRESSED) + { + prevEXTFAULTisPRESSED = EXTFAULTisPRESSED; + // and manage the transitions [pressed -> unpressed] or vice-versa and use also + // EXTFAULTpressedtime and / or EXTFAULTreleasedtime and + + AMC_BLDC_U.ExternalFlags_p.fault_button = EXTFAULTisPRESSED; + + if(true == EXTFAULTisPRESSED) + { + embot::app::theLEDmanager::getInstance().get(ledEXTfault).on(); + } + else + { + embot::app::theLEDmanager::getInstance().get(ledEXTfault).off(); + } + } + + // add any input can frame into the supervisor input queue + + size_t ninputframes = std::min(inpframes.size(), static_cast(CAN_MAX_NUM_PACKETS)); + + for(uint8_t i=0; istop(); + + embot::hw::testpoint::off(tp1); + return true; +} + +#if defined(TEST_FOC_logvalues) + +struct dbgFOCvalues +{ + embot::hw::motor::Currents currents {0, 0, 0}; + uint8_t hall {0}; + embot::hw::motor::Position electricalangle {0}; + int32_t position {0}; + float jointangle {0.0}; + std::array pwms {0, 0, 0}; + + dbgFOCvalues() = default; +}; + +dbgFOCvalues dbgFOC {}; + +#endif + +void embot::app::board::amc2c::theMBD::Impl::onCurrents_FOC_innerloop(void *owner, const embot::hw::motor::Currents * const currents) +{ + embot::hw::testpoint::on(tp2); + + //embot::hw::testpoint::on(tp2); + Impl * impl = reinterpret_cast(owner); + if((nullptr == impl) || (nullptr == currents)) + { + return; + } + + impl->measureFOC->start(); + + // 1. copy currents straight away, so that we can use them + embot::hw::motor::Currents currs = *currents; + +#if defined(TEST_DURATION_FOC) + embot::hw::sys::delay(25); +#else + + // remember to manage impl->EXTFAULTisPRESSED ............ + + // retrieve the current value of the Hall sensors + uint8_t hall {0}; + embot::hw::motor::gethallstatus(embot::hw::MOTOR::one, hall); + AMC_BLDC_U.SensorsData_p.motorsensors.hallABC = hall; + + // retrieve the current value of the encoder + embot::hw::motor::Position electricalAngle {0}; + embot::hw::motor::getencoder(embot::hw::MOTOR::one, electricalAngle); + + + static int32_T position {0}; + static int32_T electricalAngleOld = static_cast(electricalAngle); + // keep int16_t ... to manage overflow ... + int16_t delta = electricalAngle - electricalAngleOld; + electricalAngleOld = electricalAngle; + + // calculate the current joint position + size_t polepairs = (0 != AMC_BLDC_Y.ConfigurationParameters_p.motorconfig.pole_pairs) ? AMC_BLDC_Y.ConfigurationParameters_p.motorconfig.pole_pairs : 1; + position = position + delta / polepairs; + + AMC_BLDC_U.SensorsData_p.motorsensors.angle = static_cast(electricalAngle)*0.0054931640625f; // (60 interval angle) + + // convert the current from mA to A + AMC_BLDC_U.SensorsData_p.motorsensors.Iabc[0] = 0.001f*currs.u; + AMC_BLDC_U.SensorsData_p.motorsensors.Iabc[1] = 0.001f*currs.v; + AMC_BLDC_U.SensorsData_p.motorsensors.Iabc[2] = 0.001f*currs.w; + + // ----------------------------------------------------------------------------- + // FOC Step Function (~26.6 KHz) + // ----------------------------------------------------------------------------- + + AMC_BLDC_step_FOC(); + + // ----------------------------------------------------------------------------- + + AMC_BLDC_U.SensorsData_p.jointpositions.position = static_cast(position) * 0.0054931640625f; // iCubDegree -> deg + //constexpr float coneversion2pwmvalue (163.83F); + constexpr float coneversion2pwmvalue (12.19F); + + // Set the voltages + int32_T Vabc0 = static_cast(AMC_BLDC_Y.ControlOutputs_p.Vabc[0] * coneversion2pwmvalue); + int32_T Vabc1 = static_cast(AMC_BLDC_Y.ControlOutputs_p.Vabc[1] * coneversion2pwmvalue); + int32_T Vabc2 = static_cast(AMC_BLDC_Y.ControlOutputs_p.Vabc[2] * coneversion2pwmvalue); + +#if defined(TEST_FOC_logvalues) + + dbgFOC.currents = currs; + dbgFOC.hall = hall; + dbgFOC.electricalangle = electricalAngle; + dbgFOC.position = position; + dbgFOC.jointangle = static_cast(position) * 0.0054931640625f; + dbgFOC.pwms = {Vabc0, Vabc1, Vabc2}; + +#endif + // TODO: Remove, used only for debug + // 0% PWM value on phase Vabc2 = 0,0,0 + // 10% PWM value on phase Vabc2 = 0,0,122 + // 20% PWM value on phase Vabc2 = 0,0,244 + // 30% PWM value on phase Vabc2 = 0,0,366 + + embot::hw::motor::setpwm(embot::hw::MOTOR::one, 0, 0, 122); + + +//#define DEBUG_PARAMS +#ifdef DEBUG_PARAMS + + static char msg2[64]; + static uint32_t counter; + if(counter % 10 == 0) + { + sprintf(msg2, "%d,%d,%d,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f", \ + Vabc0, \ + Vabc1, \ + Vabc2, \ + AMC_BLDC_U.SensorsData_p.motorsensors.Iabc[0], \ + AMC_BLDC_U.SensorsData_p.motorsensors.Iabc[1], \ + AMC_BLDC_U.SensorsData_p.motorsensors.Iabc[2], \ + AMC_BLDC_U.SensorsData_p.motorsensors.angle, \ + AMC_BLDC_Y.EstimatedData_p.jointvelocities.velocity, \ + AMC_BLDC_B.Targets_n.motorcurrent.current, \ + AMC_BLDC_Y.ControlOutputs_p.Iq_fbk.current, \ + AMC_BLDC_Y.ControlOutputs_p.Vq); + +// sprintf(msg2, "%d,%d,%d,%.3f,%.3f,%.3f", \ +// Vabc0, \ +// Vabc1, \ +// Vabc2, \ +// AMC_BLDC_U.SensorsData_p.motorsensors.Iabc[0], \ +// AMC_BLDC_U.SensorsData_p.motorsensors.Iabc[1], \ +// AMC_BLDC_U.SensorsData_p.motorsensors.Iabc[2]); + embot::core::print(msg2); + counter = 0; + } + counter++; +#endif + +#endif // #if defined(TEST_DURATION_FOC) + + impl->measureFOC->stop(); + embot::hw::testpoint::off(tp2); +} + +#ifdef PRINT_HISTO_DEBUG +void embot::app::board::amc2c::theMBD::Impl::printHistogram_rxPkt(size_t cur_size) +{ + if(cur_size(); +} + + +embot::app::board::amc2c::theMBD::~theMBD() { } + + +bool embot::app::board::amc2c::theMBD::initialise(const Config &config) +{ + return pImpl->initialise(config); +} + +bool embot::app::board::amc2c::theMBD::tick(const std::vector &inpframes, std::vector &outframes) +{ + return pImpl->tick(inpframes, outframes); +} + + + +// - end-of-file (leave a blank line after)---------------------------------------------------------------------------- + + diff --git a/emBODY/eBcode/arch-arm/board/amc2c/test/src/app-board-amc2c/embot_app_board_amc2c_theMBD.h b/emBODY/eBcode/arch-arm/board/amc2c/test/src/app-board-amc2c/embot_app_board_amc2c_theMBD.h new file mode 100644 index 0000000000..8435423acb --- /dev/null +++ b/emBODY/eBcode/arch-arm/board/amc2c/test/src/app-board-amc2c/embot_app_board_amc2c_theMBD.h @@ -0,0 +1,55 @@ + +/* + * Copyright (C) 2023 iCub Tech - Istituto Italiano di Tecnologia + * Author: Marco Accame + * email: marco.accame@iit.it +*/ + + +// - include guard ---------------------------------------------------------------------------------------------------- + +#ifndef _EMBOT_APP_BOARD_AMC_THEMBD_H_ +#define _EMBOT_APP_BOARD_AMC_THEMBD_H_ + + +#include + +#include "embot_core.h" +#include "embot_prot_can.h" + +namespace embot::app::board::amc2c { + + class theMBD + { + public: + static theMBD& getInstance(); + + struct Config + { + embot::prot::can::Address adr {1}; + constexpr Config() = default; + constexpr Config(embot::prot::can::Address a) : adr(a) {} + }; + + bool initialise(const Config &config); + bool tick(const std::vector &inpframes, std::vector &outframes); + + void on(); + void off(); + + private: + theMBD(); + ~theMBD(); + + private: + struct Impl; + std::unique_ptr pImpl; + }; + +} // namespace + + +#endif // include-guard + + +// - end-of-file (leave a blank line after)---------------------------------------------------------------------------- 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 6b3c2d0af5..2703b4595f 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 @@ -120,12 +120,12 @@ 0 ULP2CM3 - -UP0948199 -O206 -S12 -C0 -P00000000 -N00("") -D00(00000000) -L00(0) -TO65555 -TC168000000 -TT10000000 -TP18 -TDX0 -TDD0 -TDS8000 -TDT0 -TDC1F -TIE80000001 -TIP9 -FO7 -FD20000000 -FC1000 -FN1 -FF0STM32G47x-8x_512.FLM -FS08000000 -FL080000 -FP0($$Device:STM32G474QETx$CMSIS\Flash\STM32G47x-8x_512.FLM) + -UP1123199 -O206 -S12 -C0 -P00000000 -N00("ARM CoreSight SW-DP") -D00(2BA01477) -L00(0) -TO65587 -TC168000000 -TT10000000 -TP18 -TDX0 -TDD0 -TDS8000 -TDT0 -TDC1F -TIE80000000 -TIP8 -FO7 -FD20000000 -FC1000 -FN1 -FF0STM32G47x-8x_512.FLM -FS08000000 -FL080000 -FP0($$Device:STM32G474QETx$CMSIS\Flash\STM32G47x-8x_512.FLM) 0 ARMRTXEVENTFLAGS - -L124 -Z14 -C0 -M1 -T1 + -L124 -Z8 -C0 -M1 -T1 0 @@ -140,7 +140,7 @@ 0 DLGUARM - d + 0 @@ -175,7 +175,7 @@ 0 0 - 0 + 1 0 0 0 @@ -660,7 +660,7 @@ stm32hal - 1 + 0 0 0 0 @@ -1216,7 +1216,7 @@ others::motorhal - 1 + 0 0 0 0 @@ -1284,7 +1284,7 @@ others::motorhal2 - 0 + 1 0 0 0 @@ -1520,7 +1520,7 @@ mbd::supervisor-rx - 1 + 0 0 0 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 b31e3e255c..fb86f795b6 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 @@ -16,8 +16,8 @@ STM32G474QETx STMicroelectronics - Keil.STM32G4xx_DFP.1.4.0 - http://www.keil.com/pack/ + Keil.STM32G4xx_DFP.1.5.0 + https://www.keil.com/pack/ IRAM(0x20000000,0x00020000) IROM(0x08000000,0x00080000) CPUTYPE("Cortex-M4") FPU2 CLOCK(12000000) ELITTLE @@ -314,7 +314,7 @@ 1 - 6 + 5 0 0 1 @@ -1363,8 +1363,8 @@ STM32G474QETx STMicroelectronics - Keil.STM32G4xx_DFP.1.4.0 - http://www.keil.com/pack/ + Keil.STM32G4xx_DFP.1.5.0 + https://www.keil.com/pack/ IRAM(0x20000000,0x00020000) IROM(0x08000000,0x00080000) CPUTYPE("Cortex-M4") FPU2 CLOCK(12000000) ELITTLE 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 67c3e14170..4f64040d16 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 @@ -399,6 +399,7 @@ void embot::app::board::amcbldc::theMBD::Impl::onEXTFAULTpressedreleased(void *o } } +float vin {0.0}; // Called every 1 ms bool embot::app::board::amcbldc::theMBD::Impl::tick(const std::vector &inpframes, std::vector &outframes) { @@ -409,6 +410,8 @@ bool embot::app::board::amcbldc::theMBD::Impl::tick(const std::vectorstart(); + vin = embot::hw::bsp::amcbldc::getVIN(); + onEXTFAULTpolling(); if(prevEXTFAULTisPRESSED != EXTFAULTisPRESSED) @@ -498,7 +501,7 @@ void embot::app::board::amcbldc::theMBD::Impl::onCurrents_FOC_innerloop(void *ow return; } - impl->measureFOC->start(); +// impl->measureFOC->start(); // 1. copy currents straight away, so that we can use them embot::hw::motor::Currents currs = *currents; @@ -543,12 +546,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 - // Set the voltages - int32_T Vabc0 = static_cast(AMC_BLDC_Y.ControlOutputs_p.Vabc[0] * 163.83F); - int32_T Vabc1 = static_cast(AMC_BLDC_Y.ControlOutputs_p.Vabc[1] * 163.83F); - int32_T Vabc2 = static_cast(AMC_BLDC_Y.ControlOutputs_p.Vabc[2] * 163.83F); + // Set the voltages - embot::hw::motor::setpwm(embot::hw::MOTOR::one, Vabc0, Vabc1, Vabc2); + 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] + }; + embot::hw::motor::setPWM(embot::hw::MOTOR::one, pwmperc); //#define DEBUG_PARAMS #ifdef DEBUG_PARAMS @@ -585,7 +589,7 @@ void embot::app::board::amcbldc::theMBD::Impl::onCurrents_FOC_innerloop(void *ow #endif // #if defined(TEST_DURATION_FOC) - impl->measureFOC->stop(); +// impl->measureFOC->stop(); } #ifdef PRINT_HISTO_DEBUG diff --git a/emBODY/eBcode/arch-arm/board/amcbldc/application/src/motorhal2/pwm.c b/emBODY/eBcode/arch-arm/board/amcbldc/application/src/motorhal2/pwm.c index db9af6cd7e..fde265fddc 100644 --- a/emBODY/eBcode/arch-arm/board/amcbldc/application/src/motorhal2/pwm.c +++ b/emBODY/eBcode/arch-arm/board/amcbldc/application/src/motorhal2/pwm.c @@ -1211,13 +1211,29 @@ HAL_StatusTypeDef pwm_hallConfig(uint8_t swapBC, uint16_t pwm_hall_offset) return HAL_OK; } -//void pwm_setADC_cb(void (*fn_cb)(void *, int16_t[3], void*, void*), void *owner, void* rtu, void* rty) -//{ -// s_pwm_adc_callback_set.callback_fn = fn_cb; -// s_pwm_adc_callback_set.owner = owner; -// s_pwm_adc_callback_set.rtu = rtu; -// s_pwm_adc_callback_set.rty = rty; -//} +uint16_t s_pwm_map2integer(float x) +{ + static constexpr float maxpwmdiv100 = static_cast(MAX_PWM)/100.0; + uint16_t r = 0; + if((x > 0) && (x < 100.0)) + { + float t = x*maxpwmdiv100; + r = static_cast(t); + } + else if(x >= 100.0) + { + r = MAX_PWM; + } + + return r; +} + +void pwm_SetPerc(float a, float b, float c) +{ + // map a, b, c from range [0.0, 100.0] into [0, MAX_PWM]. + // if negative -> 0, if > MAX_PWM -> MAX_PWM + pwmSet(s_pwm_map2integer(a), s_pwm_map2integer(b), s_pwm_map2integer(c)); +} diff --git a/emBODY/eBcode/arch-arm/board/amcbldc/application/src/motorhal2/pwm.h b/emBODY/eBcode/arch-arm/board/amcbldc/application/src/motorhal2/pwm.h index 5b6357ac8c..dc4dd66c74 100644 --- a/emBODY/eBcode/arch-arm/board/amcbldc/application/src/motorhal2/pwm.h +++ b/emBODY/eBcode/arch-arm/board/amcbldc/application/src/motorhal2/pwm.h @@ -115,6 +115,8 @@ extern void pwmTest(void); HAL_StatusTypeDef pwm_hallDeinit(void); HAL_StatusTypeDef pwm_hallConfig(uint8_t swapBC, uint16_t pwm_hall_offset); +void pwm_SetPerc(float a, float b, float c); + //extern HAL_StatusTypeDef pwm_Deinit(void); diff --git a/emBODY/eBcode/arch-arm/board/amcbldc/bsp/embot_hw_bsp_amcbldc.cpp b/emBODY/eBcode/arch-arm/board/amcbldc/bsp/embot_hw_bsp_amcbldc.cpp index 4a5b5b61fc..ab665af00c 100644 --- a/emBODY/eBcode/arch-arm/board/amcbldc/bsp/embot_hw_bsp_amcbldc.cpp +++ b/emBODY/eBcode/arch-arm/board/amcbldc/bsp/embot_hw_bsp_amcbldc.cpp @@ -763,6 +763,8 @@ namespace embot::hw::motor::bsp { #include "embot_hw_bsp_amcbldc.h" +#include "analog.h" + namespace embot { namespace hw { namespace bsp { namespace amcbldc { #include @@ -831,6 +833,15 @@ std::make_tuple(0x00470027, 0x484E500E, 0x20343356 ) // scheda jointlab camozzi return (Revision::A == revision()) ? embot::hw::BTN::one : embot::hw::BTN::two; } + float getVIN() + { + float v {0.0}; + + v = analogVin() * 0.001; + return v; + + } + }}}} diff --git a/emBODY/eBcode/arch-arm/board/amcbldc/bsp/embot_hw_bsp_amcbldc.h b/emBODY/eBcode/arch-arm/board/amcbldc/bsp/embot_hw_bsp_amcbldc.h index e8c59e4aa5..98c127add0 100644 --- a/emBODY/eBcode/arch-arm/board/amcbldc/bsp/embot_hw_bsp_amcbldc.h +++ b/emBODY/eBcode/arch-arm/board/amcbldc/bsp/embot_hw_bsp_amcbldc.h @@ -19,6 +19,8 @@ namespace embot { namespace hw { namespace bsp { namespace amcbldc { embot::hw::BTN EXTFAULTbutton(); + float getVIN(); + }}}} #endif // include-guard diff --git a/emBODY/eBcode/arch-arm/embot/hw/embot_hw_motor.cpp b/emBODY/eBcode/arch-arm/embot/hw/embot_hw_motor.cpp index 1d86cb03c5..38b600e389 100644 --- a/emBODY/eBcode/arch-arm/embot/hw/embot_hw_motor.cpp +++ b/emBODY/eBcode/arch-arm/embot/hw/embot_hw_motor.cpp @@ -84,7 +84,8 @@ namespace embot { namespace hw { namespace motor { result_t setpwm(MOTOR h, Pwm u, Pwm v, Pwm w) { return resNOK; } result_t setCallbackOnCurrents(MOTOR h, fpOnCurrents callback, void *owner) { return resNOK; } result_t init(MOTOR h, const Config &config) { return resNOK; } - result_t configure(embot::hw::MOTOR h, const Config &config) { return resNOK; } + result_t configure(embot::hw::MOTOR h, const Config &config) { return resNOK; } + result_t setPWM(MOTOR h, const PWMperc &p) { return resNOK; } }}} // namespace embot { namespace hw { namespace MOTOR { @@ -157,6 +158,7 @@ namespace embot { namespace hw { namespace motor { HallStatus s_hw_gethallstatus(MOTOR h); result_t s_hw_setpwmUVW(MOTOR h, Pwm u, Pwm v, Pwm w); + result_t s_hw_setpwmUVWperc(MOTOR h, const PWMperc &p); result_t s_hw_motorEnable(MOTOR h); result_t s_hw_motorDisable(MOTOR h); result_t s_hw_setCallbackOnCurrents(MOTOR h, fpOnCurrents callback, void *owner); @@ -311,6 +313,16 @@ namespace embot { namespace hw { namespace motor { return s_hw_setpwmUVW(h, u, v, w); } + result_t setPWM(MOTOR h, const PWMperc &p) + { + if(false == enabled(h)) + { + return resNOK; + } + + return s_hw_setpwmUVWperc(h, p); + } + result_t getencoder(MOTOR h, Position &position) { @@ -396,7 +408,7 @@ namespace embot { namespace hw { namespace motor { // ok, we start pwm embot::hw::motor::pwm::init({}); - + // now we calibrate adc acquisition embot::hw::motor::adc::calibrate({}); @@ -433,10 +445,7 @@ namespace embot { namespace hw { namespace motor { embot::hw::motor::hall::Mode mode { cfg.pwm_swapBC ? embot::hw::motor::hall::Mode::SWAP::BC : embot::hw::motor::hall::Mode::SWAP::none, cfg.pwm_hall_offset }; embot::hw::motor::hall::start(mode); } - - // if we have both encoder and hall sensors, we may calibrate the encoder w/ the hall values - // we probably need to move the motor to do that. - + return resOK; } @@ -484,7 +493,15 @@ namespace embot { namespace hw { namespace motor { { embot::hw::motor::pwm::set(u, v, w); return resOK; - } + } + + result_t s_hw_setpwmUVWperc(MOTOR h, const PWMperc &p) + { + embot::hw::motor::pwm::setperc(p.a, p.b, p.c); + return resOK; + } + + result_t s_hw_setCallbackOnCurrents(MOTOR h, fpOnCurrents callback, void *owner) { @@ -575,6 +592,12 @@ namespace embot { namespace hw { namespace motor { pwmSet(u, v, w); return resOK; } + + result_t s_hw_setpwmUVWperc(MOTOR h, const PWMperc &p) + { + pwm_SetPerc(p.a, p.b, p.c); + return resOK; + } static_assert(sizeof(pwm_Currents_t) == sizeof(embot::hw::motor::Currents), "embot::hw::motor::Currents and pwm_Currents_t differs"); static_assert(sizeof(pwm_Currents_t::u) == sizeof(embot::hw::motor::Currents::u), "embot::hw::motor::Currents and pwm_Currents_t differs"); diff --git a/emBODY/eBcode/arch-arm/embot/hw/embot_hw_motor.h b/emBODY/eBcode/arch-arm/embot/hw/embot_hw_motor.h index fbded63d67..0e239197f4 100644 --- a/emBODY/eBcode/arch-arm/embot/hw/embot_hw_motor.h +++ b/emBODY/eBcode/arch-arm/embot/hw/embot_hw_motor.h @@ -27,6 +27,7 @@ namespace embot { namespace hw { namespace motor { using HallStatus = std::uint8_t; // so far, we keep int32_t as it is more general. even if lower levels may propagate them as int16_t + // u, v, w are expressed as mA struct Currents { int32_t u {0}; @@ -95,6 +96,16 @@ namespace embot { namespace hw { namespace motor { // imposes a callback on end of measurement of the currents result_t setCallbackOnCurrents(MOTOR h, fpOnCurrents callback, void *owner); + + struct PWMperc + { + float a {0.0}; + float b {0.0}; + float c {0.0}; + constexpr PWMperc() = default; + constexpr PWMperc(float u, float v, float w) : a(u), b(v), c(w) {} + }; + result_t setPWM(MOTOR h, const PWMperc &p); }}} // namespace embot { namespace hw { namespace motor { diff --git a/emBODY/eBdocs/arch-arm/~$D-ICUBUNIT-canprotocol-sensorboards.docx b/emBODY/eBdocs/arch-arm/~$D-ICUBUNIT-canprotocol-sensorboards.docx deleted file mode 100644 index 32805d1106..0000000000 Binary files a/emBODY/eBdocs/arch-arm/~$D-ICUBUNIT-canprotocol-sensorboards.docx and /dev/null differ