From 21c0f7a4cd6017871ef6e02c868f8292847a7e38 Mon Sep 17 00:00:00 2001 From: Robofish <1683502971@qq.com> Date: Mon, 5 Jan 2026 09:33:18 +0800 Subject: [PATCH] =?UTF-8?q?=E8=92=B8?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- CMakeLists.txt | 21 + User/component/component_config.yaml | 24 + User/component/crc16.c | 62 +++ User/component/crc16.h | 30 ++ User/component/crc8.c | 52 ++ User/component/crc8.h | 30 ++ User/component/error_detect.c | 67 +++ User/component/error_detect.h | 82 +++ User/component/filter.c | 185 +++++++ User/component/filter.h | 120 +++++ User/component/freertos_cli.c | 352 +++++++++++++ User/component/freertos_cli.h | 108 ++++ User/component/limiter.c | 107 ++++ User/component/limiter.h | 63 +++ User/component/pid.c | 158 ++++++ User/component/pid.h | 107 ++++ User/device/buzzer.c | 171 ++++++ User/device/buzzer.h | 138 +++++ User/device/device_config.yaml | 13 + User/device/motor_dm.c | 496 ++++++++++++++++++ User/device/motor_dm.h | 98 ++++ User/device/motor_lk.c | 329 ++++++++++++ User/device/motor_lk.h | 119 +++++ User/device/motor_rm.c | 321 ++++++++++++ User/device/motor_rm.h | 132 +++++ .../slprj/_jitprj/s1PQAPIlnTPKm6mZzJoZZuB.l | Bin 0 -> 10288 bytes .../slprj/_jitprj/s1PQAPIlnTPKm6mZzJoZZuB.mat | Bin 0 -> 1249 bytes .../slprj/_jitprj/skZgecjo1NwrHsHAQiPZ4HE.l | Bin 0 -> 10336 bytes .../slprj/_jitprj/skZgecjo1NwrHsHAQiPZ4HE.mat | Bin 0 -> 1279 bytes .../slprj/_jitprj/snB7M5QwQI4hNRLbuP13ztG.l | Bin 0 -> 9128 bytes .../slprj/_jitprj/snB7M5QwQI4hNRLbuP13ztG.mat | Bin 0 -> 1275 bytes .../EMLReport/s1PQAPIlnTPKm6mZzJoZZuB.mat | Bin 0 -> 7840 bytes .../EMLReport/skZgecjo1NwrHsHAQiPZ4HE.mat | Bin 0 -> 19578 bytes .../EMLReport/snB7M5QwQI4hNRLbuP13ztG.mat | Bin 0 -> 2753 bytes .../_sfprj/leg_sim/_self/sfun/info/binfo.mat | Bin 0 -> 834 bytes .../slprj/_sfprj/leg_sim/amsi_serial.mat | Bin 0 -> 227 bytes .../precompile/72vZ1DxF0Nmp1uyeOVki4F.mat | Bin 0 -> 693 bytes .../precompile/8TxxFELrVOZf4nge1lJ5jH.mat | Bin 0 -> 657 bytes .../precompile/JPu9Qdz3VaGiF2LSvQFOuE.mat | Bin 0 -> 693 bytes .../precompile/MkkVTTjQfD4eZeRuuxNw2D.mat | Bin 0 -> 670 bytes .../precompile/N8jHiuL3FACM3BiH3xonGE.mat | Bin 0 -> 670 bytes .../precompile/pz8kySzLQtSuU5A4IQZHYD.mat | Bin 0 -> 633 bytes 42 files changed, 3385 insertions(+) create mode 100644 User/component/crc16.c create mode 100644 User/component/crc16.h create mode 100644 User/component/crc8.c create mode 100644 User/component/crc8.h create mode 100644 User/component/error_detect.c create mode 100644 User/component/error_detect.h create mode 100644 User/component/filter.c create mode 100644 User/component/filter.h create mode 100644 User/component/freertos_cli.c create mode 100644 User/component/freertos_cli.h create mode 100644 User/component/limiter.c create mode 100644 User/component/limiter.h create mode 100644 User/component/pid.c create mode 100644 User/component/pid.h create mode 100644 User/device/buzzer.c create mode 100644 User/device/buzzer.h create mode 100644 User/device/motor_dm.c create mode 100644 User/device/motor_dm.h create mode 100644 User/device/motor_lk.c create mode 100644 User/device/motor_lk.h create mode 100644 User/device/motor_rm.c create mode 100644 User/device/motor_rm.h create mode 100644 utils/Simulation-master/balance/series_legs/slprj/_jitprj/s1PQAPIlnTPKm6mZzJoZZuB.l create mode 100644 utils/Simulation-master/balance/series_legs/slprj/_jitprj/s1PQAPIlnTPKm6mZzJoZZuB.mat create mode 100644 utils/Simulation-master/balance/series_legs/slprj/_jitprj/skZgecjo1NwrHsHAQiPZ4HE.l create mode 100644 utils/Simulation-master/balance/series_legs/slprj/_jitprj/skZgecjo1NwrHsHAQiPZ4HE.mat create mode 100644 utils/Simulation-master/balance/series_legs/slprj/_jitprj/snB7M5QwQI4hNRLbuP13ztG.l create mode 100644 utils/Simulation-master/balance/series_legs/slprj/_jitprj/snB7M5QwQI4hNRLbuP13ztG.mat create mode 100644 utils/Simulation-master/balance/series_legs/slprj/_sfprj/EMLReport/s1PQAPIlnTPKm6mZzJoZZuB.mat create mode 100644 utils/Simulation-master/balance/series_legs/slprj/_sfprj/EMLReport/skZgecjo1NwrHsHAQiPZ4HE.mat create mode 100644 utils/Simulation-master/balance/series_legs/slprj/_sfprj/EMLReport/snB7M5QwQI4hNRLbuP13ztG.mat create mode 100644 utils/Simulation-master/balance/series_legs/slprj/_sfprj/leg_sim/_self/sfun/info/binfo.mat create mode 100644 utils/Simulation-master/balance/series_legs/slprj/_sfprj/leg_sim/amsi_serial.mat create mode 100644 utils/Simulation-master/balance/series_legs/slprj/_sfprj/precompile/72vZ1DxF0Nmp1uyeOVki4F.mat create mode 100644 utils/Simulation-master/balance/series_legs/slprj/_sfprj/precompile/8TxxFELrVOZf4nge1lJ5jH.mat create mode 100644 utils/Simulation-master/balance/series_legs/slprj/_sfprj/precompile/JPu9Qdz3VaGiF2LSvQFOuE.mat create mode 100644 utils/Simulation-master/balance/series_legs/slprj/_sfprj/precompile/MkkVTTjQfD4eZeRuuxNw2D.mat create mode 100644 utils/Simulation-master/balance/series_legs/slprj/_sfprj/precompile/N8jHiuL3FACM3BiH3xonGE.mat create mode 100644 utils/Simulation-master/balance/series_legs/slprj/_sfprj/precompile/pz8kySzLQtSuU5A4IQZHYD.mat diff --git a/CMakeLists.txt b/CMakeLists.txt index cdb3d91..30fb584 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -57,13 +57,34 @@ target_sources(${CMAKE_PROJECT_NAME} PRIVATE # User/component sources User/component/ahrs.c + + # User/component/ahrs sources + User/component/ahrs/ahrs.c + + # User/component sources + User/component/crc16.c + User/component/crc8.c + User/component/error_detect.c + User/component/filter.c + + # User/component/filter sources + User/component/filter/filter.c + + # User/component sources + User/component/freertos_cli.c + User/component/limiter.c + User/component/pid.c User/component/user_math.c # User/device sources User/device/bmi088.c + User/device/buzzer.c User/device/dr16.c User/device/motor.c + User/device/motor_dm.c + User/device/motor_lk.c User/device/motor_lz.c + User/device/motor_rm.c # User/module sources User/module/config.c diff --git a/User/component/component_config.yaml b/User/component/component_config.yaml index 4caf761..1eadd3d 100644 --- a/User/component/component_config.yaml +++ b/User/component/component_config.yaml @@ -2,6 +2,30 @@ ahrs: dependencies: - component/user_math.h enabled: true +crc16: + dependencies: [] + enabled: true +crc8: + dependencies: [] + enabled: true +error_detect: + dependencies: + - bsp/mm + enabled: true +filter: + dependencies: + - component/ahrs + enabled: true +freertos_cli: + dependencies: [] + enabled: true +limiter: + dependencies: [] + enabled: true +pid: + dependencies: + - component/filter + enabled: true user_math: dependencies: [] enabled: true diff --git a/User/component/crc16.c b/User/component/crc16.c new file mode 100644 index 0000000..0d17eb0 --- /dev/null +++ b/User/component/crc16.c @@ -0,0 +1,62 @@ +#include "crc16.h" + +/* USER INCLUDE BEGIN */ + +/* USER INCLUDE END */ + +/* USER DEFINE BEGIN */ + +/* USER DEFINE END */ + +static const uint16_t crc16_tab[256] = { + 0x0000, 0x1189, 0x2312, 0x329b, 0x4624, 0x57ad, 0x6536, 0x74bf, 0x8c48, + 0x9dc1, 0xaf5a, 0xbed3, 0xca6c, 0xdbe5, 0xe97e, 0xf8f7, 0x1081, 0x0108, + 0x3393, 0x221a, 0x56a5, 0x472c, 0x75b7, 0x643e, 0x9cc9, 0x8d40, 0xbfdb, + 0xae52, 0xdaed, 0xcb64, 0xf9ff, 0xe876, 0x2102, 0x308b, 0x0210, 0x1399, + 0x6726, 0x76af, 0x4434, 0x55bd, 0xad4a, 0xbcc3, 0x8e58, 0x9fd1, 0xeb6e, + 0xfae7, 0xc87c, 0xd9f5, 0x3183, 0x200a, 0x1291, 0x0318, 0x77a7, 0x662e, + 0x54b5, 0x453c, 0xbdcb, 0xac42, 0x9ed9, 0x8f50, 0xfbef, 0xea66, 0xd8fd, + 0xc974, 0x4204, 0x538d, 0x6116, 0x709f, 0x0420, 0x15a9, 0x2732, 0x36bb, + 0xce4c, 0xdfc5, 0xed5e, 0xfcd7, 0x8868, 0x99e1, 0xab7a, 0xbaf3, 0x5285, + 0x430c, 0x7197, 0x601e, 0x14a1, 0x0528, 0x37b3, 0x263a, 0xdecd, 0xcf44, + 0xfddf, 0xec56, 0x98e9, 0x8960, 0xbbfb, 0xaa72, 0x6306, 0x728f, 0x4014, + 0x519d, 0x2522, 0x34ab, 0x0630, 0x17b9, 0xef4e, 0xfec7, 0xcc5c, 0xddd5, + 0xa96a, 0xb8e3, 0x8a78, 0x9bf1, 0x7387, 0x620e, 0x5095, 0x411c, 0x35a3, + 0x242a, 0x16b1, 0x0738, 0xffcf, 0xee46, 0xdcdd, 0xcd54, 0xb9eb, 0xa862, + 0x9af9, 0x8b70, 0x8408, 0x9581, 0xa71a, 0xb693, 0xc22c, 0xd3a5, 0xe13e, + 0xf0b7, 0x0840, 0x19c9, 0x2b52, 0x3adb, 0x4e64, 0x5fed, 0x6d76, 0x7cff, + 0x9489, 0x8500, 0xb79b, 0xa612, 0xd2ad, 0xc324, 0xf1bf, 0xe036, 0x18c1, + 0x0948, 0x3bd3, 0x2a5a, 0x5ee5, 0x4f6c, 0x7df7, 0x6c7e, 0xa50a, 0xb483, + 0x8618, 0x9791, 0xe32e, 0xf2a7, 0xc03c, 0xd1b5, 0x2942, 0x38cb, 0x0a50, + 0x1bd9, 0x6f66, 0x7eef, 0x4c74, 0x5dfd, 0xb58b, 0xa402, 0x9699, 0x8710, + 0xf3af, 0xe226, 0xd0bd, 0xc134, 0x39c3, 0x284a, 0x1ad1, 0x0b58, 0x7fe7, + 0x6e6e, 0x5cf5, 0x4d7c, 0xc60c, 0xd785, 0xe51e, 0xf497, 0x8028, 0x91a1, + 0xa33a, 0xb2b3, 0x4a44, 0x5bcd, 0x6956, 0x78df, 0x0c60, 0x1de9, 0x2f72, + 0x3efb, 0xd68d, 0xc704, 0xf59f, 0xe416, 0x90a9, 0x8120, 0xb3bb, 0xa232, + 0x5ac5, 0x4b4c, 0x79d7, 0x685e, 0x1ce1, 0x0d68, 0x3ff3, 0x2e7a, 0xe70e, + 0xf687, 0xc41c, 0xd595, 0xa12a, 0xb0a3, 0x8238, 0x93b1, 0x6b46, 0x7acf, + 0x4854, 0x59dd, 0x2d62, 0x3ceb, 0x0e70, 0x1ff9, 0xf78f, 0xe606, 0xd49d, + 0xc514, 0xb1ab, 0xa022, 0x92b9, 0x8330, 0x7bc7, 0x6a4e, 0x58d5, 0x495c, + 0x3de3, 0x2c6a, 0x1ef1, 0x0f78}; + +static inline uint16_t CRC16_Byte(uint16_t crc, const uint8_t data) { + return (crc >> 8) ^ crc16_tab[(crc ^ data) & 0xff]; +} + +uint16_t CRC16_Calc(const uint8_t *buf, size_t len, uint16_t crc) { + while (len--) crc = CRC16_Byte(crc, *buf++); + return crc; +} + +bool CRC16_Verify(const uint8_t *buf, size_t len) { + if (len < 2) return false; + + uint16_t expected = CRC16_Calc(buf, len - sizeof(uint16_t), CRC16_INIT); + return expected == + ((const uint16_t *)((const uint8_t *)buf + + (len % 2)))[len / sizeof(uint16_t) - 1]; +} + +/* USER FUNCTION BEGIN */ + +/* USER FUNCTION END */ diff --git a/User/component/crc16.h b/User/component/crc16.h new file mode 100644 index 0000000..68b0a87 --- /dev/null +++ b/User/component/crc16.h @@ -0,0 +1,30 @@ +#pragma once + +#ifdef __cplusplus +extern "C" { +#endif + +#include + +#include "user_math.h" + +/* USER INCLUDE BEGIN */ + +/* USER INCLUDE END */ + +/* USER DEFINE BEGIN */ + +/* USER DEFINE END */ + +#define CRC16_INIT 0XFFFF + +uint16_t CRC16_Calc(const uint8_t *buf, size_t len, uint16_t crc); +bool CRC16_Verify(const uint8_t *buf, size_t len); + +/* USER FUNCTION BEGIN */ + +/* USER FUNCTION END */ + +#ifdef __cplusplus +} +#endif diff --git a/User/component/crc8.c b/User/component/crc8.c new file mode 100644 index 0000000..66f4ad2 --- /dev/null +++ b/User/component/crc8.c @@ -0,0 +1,52 @@ +#include "crc8.h" + +/* USER INCLUDE BEGIN */ + +/* USER INCLUDE END */ + +/* USER DEFINE BEGIN */ + +/* USER DEFINE END */ + +static const uint8_t crc8_tab[256] = { + 0x00, 0x5e, 0xbc, 0xe2, 0x61, 0x3f, 0xdd, 0x83, 0xc2, 0x9c, 0x7e, 0x20, + 0xa3, 0xfd, 0x1f, 0x41, 0x9d, 0xc3, 0x21, 0x7f, 0xfc, 0xa2, 0x40, 0x1e, + 0x5f, 0x01, 0xe3, 0xbd, 0x3e, 0x60, 0x82, 0xdc, 0x23, 0x7d, 0x9f, 0xc1, + 0x42, 0x1c, 0xfe, 0xa0, 0xe1, 0xbf, 0x5d, 0x03, 0x80, 0xde, 0x3c, 0x62, + 0xbe, 0xe0, 0x02, 0x5c, 0xdf, 0x81, 0x63, 0x3d, 0x7c, 0x22, 0xc0, 0x9e, + 0x1d, 0x43, 0xa1, 0xff, 0x46, 0x18, 0xfa, 0xa4, 0x27, 0x79, 0x9b, 0xc5, + 0x84, 0xda, 0x38, 0x66, 0xe5, 0xbb, 0x59, 0x07, 0xdb, 0x85, 0x67, 0x39, + 0xba, 0xe4, 0x06, 0x58, 0x19, 0x47, 0xa5, 0xfb, 0x78, 0x26, 0xc4, 0x9a, + 0x65, 0x3b, 0xd9, 0x87, 0x04, 0x5a, 0xb8, 0xe6, 0xa7, 0xf9, 0x1b, 0x45, + 0xc6, 0x98, 0x7a, 0x24, 0xf8, 0xa6, 0x44, 0x1a, 0x99, 0xc7, 0x25, 0x7b, + 0x3a, 0x64, 0x86, 0xd8, 0x5b, 0x05, 0xe7, 0xb9, 0x8c, 0xd2, 0x30, 0x6e, + 0xed, 0xb3, 0x51, 0x0f, 0x4e, 0x10, 0xf2, 0xac, 0x2f, 0x71, 0x93, 0xcd, + 0x11, 0x4f, 0xad, 0xf3, 0x70, 0x2e, 0xcc, 0x92, 0xd3, 0x8d, 0x6f, 0x31, + 0xb2, 0xec, 0x0e, 0x50, 0xaf, 0xf1, 0x13, 0x4d, 0xce, 0x90, 0x72, 0x2c, + 0x6d, 0x33, 0xd1, 0x8f, 0x0c, 0x52, 0xb0, 0xee, 0x32, 0x6c, 0x8e, 0xd0, + 0x53, 0x0d, 0xef, 0xb1, 0xf0, 0xae, 0x4c, 0x12, 0x91, 0xcf, 0x2d, 0x73, + 0xca, 0x94, 0x76, 0x28, 0xab, 0xf5, 0x17, 0x49, 0x08, 0x56, 0xb4, 0xea, + 0x69, 0x37, 0xd5, 0x8b, 0x57, 0x09, 0xeb, 0xb5, 0x36, 0x68, 0x8a, 0xd4, + 0x95, 0xcb, 0x29, 0x77, 0xf4, 0xaa, 0x48, 0x16, 0xe9, 0xb7, 0x55, 0x0b, + 0x88, 0xd6, 0x34, 0x6a, 0x2b, 0x75, 0x97, 0xc9, 0x4a, 0x14, 0xf6, 0xa8, + 0x74, 0x2a, 0xc8, 0x96, 0x15, 0x4b, 0xa9, 0xf7, 0xb6, 0xe8, 0x0a, 0x54, + 0xd7, 0x89, 0x6b, 0x35, +}; + +uint8_t CRC8_Calc(const uint8_t *buf, size_t len, uint8_t crc) { + /* loop over the buffer data */ + while (len-- > 0) crc = crc8_tab[(crc ^ *buf++) & 0xff]; + + return crc; +} + +bool CRC8_Verify(const uint8_t *buf, size_t len) { + if (len < 2) return false; + + uint8_t expected = CRC8_Calc(buf, len - sizeof(uint8_t), CRC8_INIT); + return expected == buf[len - sizeof(uint8_t)]; +} + +/* USER FUNCTION BEGIN */ + +/* USER FUNCTION END */ diff --git a/User/component/crc8.h b/User/component/crc8.h new file mode 100644 index 0000000..a376c71 --- /dev/null +++ b/User/component/crc8.h @@ -0,0 +1,30 @@ +#pragma once + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include +#include + +/* USER INCLUDE BEGIN */ + +/* USER INCLUDE END */ + +/* USER DEFINE BEGIN */ + +/* USER DEFINE END */ + +#define CRC8_INIT 0xFF + +uint8_t CRC8_Calc(const uint8_t *buf, size_t len, uint8_t crc); +bool CRC8_Verify(const uint8_t *buf, size_t len); + +/* USER FUNCTION BEGIN */ + +/* USER FUNCTION END */ + +#ifdef __cplusplus +} +#endif diff --git a/User/component/error_detect.c b/User/component/error_detect.c new file mode 100644 index 0000000..b29e591 --- /dev/null +++ b/User/component/error_detect.c @@ -0,0 +1,67 @@ +/* + 错误检测。 +*/ + +#include "error_detect.h" + +#include +#include + +#include "bsp/mm.h" + +static ErrorDetect_t ged; +static bool inited = false; + +int8_t ErrorDetect_Init(void) { + if (inited) return -1; + + memset(&ged, 0x00, sizeof(ged)); + + for (uint8_t i = 0; i < ERROR_DETECT_UNIT_NUM; i++) { + ged.error[i].enable = true; + ged.error[i].priority = i; + ged.error[i].patient_lost = 500; + ged.error[i].patient_work = 500; + } + return 0; +} + +void ErrorDetect_Processing(uint32_t sys_time) { + for (uint8_t i = 0; i < ERROR_DETECT_UNIT_NUM; i++) { + if (!ged.error[i].enable) continue; + + if (sys_time - ged.error[i].showup > ged.error[i].patient_lost) { + ged.error[i].is_lost = true; + ged.error[i].found_lost = sys_time; + } else if (sys_time - ged.error[i].showup > ged.error[i].patient_lost) { + } else { + ged.error[i].cycle_time = ged.error[i].showup - ged.error[i].showup_last; + } + } +} + +bool ErrorDetect_ErrorExist(ErrorDetect_Unit_t unit) { + if (unit == ERROR_DETECT_UNIT_NO_DEV) { + for (uint8_t i = ERROR_DETECT_UNIT_NUM; i > 0; i--) { + if (ged.error[i].error_exist) return true; + } + return false; + } else { + return ged.error[unit].error_exist; + } +} + +ErrorDetect_Unit_t ErrorDetect_GetErrorUnit(void) { + for (uint8_t i = ERROR_DETECT_UNIT_NUM; i > 0; i--) { + if (ged.error[i].error_exist) return i; + } + return ERROR_DETECT_UNIT_NO_DEV; +} + +const ErrorDetect_Error_t *ErrorDetect_GetDetail(ErrorDetect_Unit_t unit) { + return &ged.error[unit]; +} + +void ErrorDetect_Update(ErrorDetect_Unit_t unit, uint32_t time_current) { + ged.error[unit].showup = time_current; +} diff --git a/User/component/error_detect.h b/User/component/error_detect.h new file mode 100644 index 0000000..3dba7a2 --- /dev/null +++ b/User/component/error_detect.h @@ -0,0 +1,82 @@ +/* + 错误检测。 +*/ + +#pragma once + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include + +/* USER INCLUDE BEGIN */ + +/* USER INCLUDE END */ + +/* USER DEFINE BEGIN */ + +/* USER DEFINE END */ + +typedef enum { + /* Low priority */ + ERROR_DETECT_UNIT_NO_DEV = 0, + ERROR_DETECT_UNIT_REFEREE, + ERROR_DETECT_UNIT_CHASSIS_M1, + ERROR_DETECT_UNIT_CHASSIS_M2, + ERROR_DETECT_UNIT_CHASSIS_M3, + ERROR_DETECT_UNIT_CHASSIS_M4, + ERROR_DETECT_UNIT_TRIGGER, + ERROR_DETECT_UNIT_FEED, + ERROR_DETECT_UNIT_GIMBAL_YAW, + ERROR_DETECT_UNIT_GIMBAL_PIT, + ERROR_DETECT_UNIT_GYRO, + ERROR_DETECT_UNIT_ACCL, + ERROR_DETECT_UNIT_MAGN, + ERROR_DETECT_UNIT_DBUS, + ERROR_DETECT_UNIT_NUM, + /* High priority */ +} ErrorDetect_Unit_t; + +typedef struct { + bool enable; + uint8_t priority; + uint32_t patient_lost; + uint32_t patient_work; + + uint32_t showup; + uint32_t showup_last; + uint32_t cycle_time; + uint32_t duration_lost; + uint32_t duration_work; + uint32_t found_lost; + bool error_exist; + bool is_lost; + uint8_t data_is_error; + +} ErrorDetect_Error_t; + +typedef struct { + ErrorDetect_Error_t error[ERROR_DETECT_UNIT_NUM]; +} ErrorDetect_t; + +/* USER STRUCT BEGIN */ + +/* USER STRUCT END */ + +int8_t ErrorDetect_Init(void); +void ErrorDetect_Processing(uint32_t sys_time); +bool ErrorDetect_ErrorExist(ErrorDetect_Unit_t unit); +ErrorDetect_Unit_t ErrorDetect_GetErrorUnit(void); +const ErrorDetect_Error_t *ErrorDetect_GetDetail(ErrorDetect_Unit_t unit); + +void ErrorDetect_Update(ErrorDetect_Unit_t unit, uint32_t time_current); + +/* USER FUNCTION BEGIN */ + +/* USER FUNCTION END */ + +#ifdef __cplusplus +} +#endif diff --git a/User/component/filter.c b/User/component/filter.c new file mode 100644 index 0000000..5375b8e --- /dev/null +++ b/User/component/filter.c @@ -0,0 +1,185 @@ +/* + 各类滤波器。 +*/ + +#include "filter.h" + +#include "user_math.h" + +/** + * @brief 初始化滤波器 + * + * @param f 滤波器 + * @param sample_freq 采样频率 + * @param cutoff_freq 截止频率 + */ +void LowPassFilter2p_Init(LowPassFilter2p_t *f, float sample_freq, + float cutoff_freq) { + if (f == NULL) return; + + f->cutoff_freq = cutoff_freq; + + f->delay_element_1 = 0.0f; + f->delay_element_2 = 0.0f; + + if (f->cutoff_freq <= 0.0f) { + /* no filtering */ + f->b0 = 1.0f; + f->b1 = 0.0f; + f->b2 = 0.0f; + + f->a1 = 0.0f; + f->a2 = 0.0f; + + return; + } + const float fr = sample_freq / f->cutoff_freq; + const float ohm = tanf(M_PI / fr); + const float c = 1.0f + 2.0f * cosf(M_PI / 4.0f) * ohm + ohm * ohm; + + f->b0 = ohm * ohm / c; + f->b1 = 2.0f * f->b0; + f->b2 = f->b0; + + f->a1 = 2.0f * (ohm * ohm - 1.0f) / c; + f->a2 = (1.0f - 2.0f * cosf(M_PI / 4.0f) * ohm + ohm * ohm) / c; +} + +/** + * @brief 施加一次滤波计算 + * + * @param f 滤波器 + * @param sample 采样的值 + * @return float 滤波后的值 + */ +float LowPassFilter2p_Apply(LowPassFilter2p_t *f, float sample) { + if (f == NULL) return 0.0f; + + /* do the filtering */ + float delay_element_0 = + sample - f->delay_element_1 * f->a1 - f->delay_element_2 * f->a2; + + if (isinf(delay_element_0)) { + /* don't allow bad values to propagate via the filter */ + delay_element_0 = sample; + } + + const float output = delay_element_0 * f->b0 + f->delay_element_1 * f->b1 + + f->delay_element_2 * f->b2; + + f->delay_element_2 = f->delay_element_1; + f->delay_element_1 = delay_element_0; + + /* return the value. Should be no need to check limits */ + return output; +} + +/** + * @brief 重置滤波器 + * + * @param f 滤波器 + * @param sample 采样的值 + * @return float 滤波后的值 + */ +float LowPassFilter2p_Reset(LowPassFilter2p_t *f, float sample) { + if (f == NULL) return 0.0f; + + const float dval = sample / (f->b0 + f->b1 + f->b2); + + if (isfinite(dval)) { + f->delay_element_1 = dval; + f->delay_element_2 = dval; + + } else { + f->delay_element_1 = sample; + f->delay_element_2 = sample; + } + + return LowPassFilter2p_Apply(f, sample); +} + +/** + * @brief 初始化滤波器 + * + * @param f 滤波器 + * @param sample_freq 采样频率 + * @param notch_freq 中心频率 + * @param bandwidth 带宽 + */ +void NotchFilter_Init(NotchFilter_t *f, float sample_freq, float notch_freq, + float bandwidth) { + if (f == NULL) return; + + f->notch_freq = notch_freq; + f->bandwidth = bandwidth; + + f->delay_element_1 = 0.0f; + f->delay_element_2 = 0.0f; + + if (notch_freq <= 0.0f) { + /* no filtering */ + f->b0 = 1.0f; + f->b1 = 0.0f; + f->b2 = 0.0f; + + f->a1 = 0.0f; + f->a2 = 0.0f; + + return; + } + + const float alpha = tanf(M_PI * bandwidth / sample_freq); + const float beta = -cosf(M_2PI * notch_freq / sample_freq); + const float a0_inv = 1.0f / (alpha + 1.0f); + + f->b0 = a0_inv; + f->b1 = 2.0f * beta * a0_inv; + f->b2 = a0_inv; + + f->a1 = f->b1; + f->a2 = (1.0f - alpha) * a0_inv; +} + +/** + * @brief 施加一次滤波计算 + * + * @param f 滤波器 + * @param sample 采样的值 + * @return float 滤波后的值 + */ +inline float NotchFilter_Apply(NotchFilter_t *f, float sample) { + if (f == NULL) return 0.0f; + + /* Direct Form II implementation */ + const float delay_element_0 = + sample - f->delay_element_1 * f->a1 - f->delay_element_2 * f->a2; + const float output = delay_element_0 * f->b0 + f->delay_element_1 * f->b1 + + f->delay_element_2 * f->b2; + + f->delay_element_2 = f->delay_element_1; + f->delay_element_1 = delay_element_0; + + return output; +} + +/** + * @brief 重置滤波器 + * + * @param f 滤波器 + * @param sample 采样的值 + * @return float 滤波后的值 + */ +float NotchFilter_Reset(NotchFilter_t *f, float sample) { + if (f == NULL) return 0.0f; + + float dval = sample; + + if (fabsf(f->b0 + f->b1 + f->b2) > FLT_EPSILON) { + dval = dval / (f->b0 + f->b1 + f->b2); + } + + f->delay_element_1 = dval; + f->delay_element_2 = dval; + + return NotchFilter_Apply(f, sample); +} diff --git a/User/component/filter.h b/User/component/filter.h new file mode 100644 index 0000000..ae2b072 --- /dev/null +++ b/User/component/filter.h @@ -0,0 +1,120 @@ +/* + 各类滤波器。 +*/ + +#pragma once + +#ifdef __cplusplus +extern "C" { +#endif + +#include "user_math.h" + +/* USER INCLUDE BEGIN */ + +/* USER INCLUDE END */ + +/* USER DEFINE BEGIN */ + +/* USER DEFINE END */ + +/* 二阶低通滤波器 */ +typedef struct { + float cutoff_freq; /* 截止频率 */ + + float a1; + float a2; + + float b0; + float b1; + float b2; + + float delay_element_1; + float delay_element_2; + +} LowPassFilter2p_t; + +/* 带阻滤波器 */ +typedef struct { + float notch_freq; /* 阻止频率 */ + float bandwidth; /* 带宽 */ + + float a1; + float a2; + + float b0; + float b1; + float b2; + float delay_element_1; + float delay_element_2; + +} NotchFilter_t; + +/* USER STRUCT BEGIN */ + +/* USER STRUCT END */ + +/** + * @brief 初始化滤波器 + * + * @param f 滤波器 + * @param sample_freq 采样频率 + * @param cutoff_freq 截止频率 + */ +void LowPassFilter2p_Init(LowPassFilter2p_t *f, float sample_freq, + float cutoff_freq); + +/** + * @brief 施加一次滤波计算 + * + * @param f 滤波器 + * @param sample 采样的值 + * @return float 滤波后的值 + */ +float LowPassFilter2p_Apply(LowPassFilter2p_t *f, float sample); + +/** + * @brief 重置滤波器 + * + * @param f 滤波器 + * @param sample 采样的值 + * @return float 滤波后的值 + */ +float LowPassFilter2p_Reset(LowPassFilter2p_t *f, float sample); + +/** + * @brief 初始化滤波器 + * + * @param f 滤波器 + * @param sample_freq 采样频率 + * @param notch_freq 中心频率 + * @param bandwidth 带宽 + */ +void NotchFilter_Init(NotchFilter_t *f, float sample_freq, float notch_freq, + float bandwidth); + +/** + * @brief 施加一次滤波计算 + * + * @param f 滤波器 + * @param sample 采样的值 + * @return float 滤波后的值 + */ +float NotchFilter_Apply(NotchFilter_t *f, float sample); + +/** + * @brief 重置滤波器 + * + * @param f 滤波器 + * @param sample 采样的值 + * @return float 滤波后的值 + */ +float NotchFilter_Reset(NotchFilter_t *f, float sample); + +/* USER FUNCTION BEGIN */ + +/* USER FUNCTION END */ + +#ifdef __cplusplus +} +#endif diff --git a/User/component/freertos_cli.c b/User/component/freertos_cli.c new file mode 100644 index 0000000..32421de --- /dev/null +++ b/User/component/freertos_cli.c @@ -0,0 +1,352 @@ +/* + * FreeRTOS+CLI V1.0.4 + * Copyright (C) 2017 Amazon.com, Inc. or its affiliates. All Rights Reserved. + * + * Permission is hereby granted, free of charge, to any person obtaining a copy of + * this software and associated documentation files (the "Software"), to deal in + * the Software without restriction, including without limitation the rights to + * use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of + * the Software, and to permit persons to whom the Software is furnished to do so, + * subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS + * FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR + * COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER + * IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN + * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. + * + * http://www.FreeRTOS.org + * http://aws.amazon.com/freertos + * + * 1 tab == 4 spaces! + */ + +/* Standard includes. */ +#include +#include + +/* FreeRTOS includes. */ +#include "FreeRTOS.h" +#include "task.h" + +/* Utils includes. */ +#include "FreeRTOS_CLI.h" + +/* If the application writer needs to place the buffer used by the CLI at a +fixed address then set configAPPLICATION_PROVIDES_cOutputBuffer to 1 in +FreeRTOSConfig.h, then declare an array with the following name and size in +one of the application files: + char cOutputBuffer[ configCOMMAND_INT_MAX_OUTPUT_SIZE ]; +*/ +#ifndef configAPPLICATION_PROVIDES_cOutputBuffer + #define configAPPLICATION_PROVIDES_cOutputBuffer 0 +#endif + +typedef struct xCOMMAND_INPUT_LIST +{ + const CLI_Command_Definition_t *pxCommandLineDefinition; + struct xCOMMAND_INPUT_LIST *pxNext; +} CLI_Definition_List_Item_t; + +/* + * The callback function that is executed when "help" is entered. This is the + * only default command that is always present. + */ +static BaseType_t prvHelpCommand( char *pcWriteBuffer, size_t xWriteBufferLen, const char *pcCommandString ); + +/* + * Return the number of parameters that follow the command name. + */ +static int8_t prvGetNumberOfParameters( const char *pcCommandString ); + +/* The definition of the "help" command. This command is always at the front +of the list of registered commands. */ +static const CLI_Command_Definition_t xHelpCommand = +{ + "help", + "\r\nhelp:\r\n Lists all the registered commands\r\n\r\n", + prvHelpCommand, + 0 +}; + +/* The definition of the list of commands. Commands that are registered are +added to this list. */ +static CLI_Definition_List_Item_t xRegisteredCommands = +{ + &xHelpCommand, /* The first command in the list is always the help command, defined in this file. */ + NULL /* The next pointer is initialised to NULL, as there are no other registered commands yet. */ +}; + +/* A buffer into which command outputs can be written is declared here, rather +than in the command console implementation, to allow multiple command consoles +to share the same buffer. For example, an application may allow access to the +command interpreter by UART and by Ethernet. Sharing a buffer is done purely +to save RAM. Note, however, that the command console itself is not re-entrant, +so only one command interpreter interface can be used at any one time. For that +reason, no attempt at providing mutual exclusion to the cOutputBuffer array is +attempted. + +configAPPLICATION_PROVIDES_cOutputBuffer is provided to allow the application +writer to provide their own cOutputBuffer declaration in cases where the +buffer needs to be placed at a fixed address (rather than by the linker). */ + +#if( configAPPLICATION_PROVIDES_cOutputBuffer == 0 ) + static char cOutputBuffer[ configCOMMAND_INT_MAX_OUTPUT_SIZE ]; +#else + extern char cOutputBuffer[ configCOMMAND_INT_MAX_OUTPUT_SIZE ]; +#endif + + +/*---------------------------------------------------------- */ + +BaseType_t FreeRTOS_CLIRegisterCommand( const CLI_Command_Definition_t * const pxCommandToRegister ) +{ +static CLI_Definition_List_Item_t *pxLastCommandInList = &xRegisteredCommands; +CLI_Definition_List_Item_t *pxNewListItem; +BaseType_t xReturn = pdFAIL; + + /* Check the parameter is not NULL. */ + configASSERT( pxCommandToRegister ); + + /* Create a new list item that will reference the command being registered. */ + pxNewListItem = ( CLI_Definition_List_Item_t * ) pvPortMalloc( sizeof( CLI_Definition_List_Item_t ) ); + configASSERT( pxNewListItem ); + + if( pxNewListItem != NULL ) + { + taskENTER_CRITICAL(); + { + /* Reference the command being registered from the newly created + list item. */ + pxNewListItem->pxCommandLineDefinition = pxCommandToRegister; + + /* The new list item will get added to the end of the list, so + pxNext has nowhere to point. */ + pxNewListItem->pxNext = NULL; + + /* Add the newly created list item to the end of the already existing + list. */ + pxLastCommandInList->pxNext = pxNewListItem; + + /* Set the end of list marker to the new list item. */ + pxLastCommandInList = pxNewListItem; + } + taskEXIT_CRITICAL(); + + xReturn = pdPASS; + } + + return xReturn; +} +/*---------------------------------------------------------- */ + +BaseType_t FreeRTOS_CLIProcessCommand( const char * const pcCommandInput, char * pcWriteBuffer, size_t xWriteBufferLen ) +{ +static const CLI_Definition_List_Item_t *pxCommand = NULL; +BaseType_t xReturn = pdTRUE; +const char *pcRegisteredCommandString; +size_t xCommandStringLength; + + /* Note: This function is not re-entrant. It must not be called from more + thank one task. */ + + if( pxCommand == NULL ) + { + /* Search for the command string in the list of registered commands. */ + for( pxCommand = &xRegisteredCommands; pxCommand != NULL; pxCommand = pxCommand->pxNext ) + { + pcRegisteredCommandString = pxCommand->pxCommandLineDefinition->pcCommand; + xCommandStringLength = strlen( pcRegisteredCommandString ); + + /* To ensure the string lengths match exactly, so as not to pick up + a sub-string of a longer command, check the byte after the expected + end of the string is either the end of the string or a space before + a parameter. */ + if( ( pcCommandInput[ xCommandStringLength ] == ' ' ) || ( pcCommandInput[ xCommandStringLength ] == 0x00 ) ) + { + if( strncmp( pcCommandInput, pcRegisteredCommandString, xCommandStringLength ) == 0 ) + { + /* The command has been found. Check it has the expected + number of parameters. If cExpectedNumberOfParameters is -1, + then there could be a variable number of parameters and no + check is made. */ + if( pxCommand->pxCommandLineDefinition->cExpectedNumberOfParameters >= 0 ) + { + if( prvGetNumberOfParameters( pcCommandInput ) != pxCommand->pxCommandLineDefinition->cExpectedNumberOfParameters ) + { + xReturn = pdFALSE; + } + } + + break; + } + } + } + } + + if( ( pxCommand != NULL ) && ( xReturn == pdFALSE ) ) + { + /* The command was found, but the number of parameters with the command + was incorrect. */ + strncpy( pcWriteBuffer, "Incorrect command parameter(s). Enter \"help\" to view a list of available commands.\r\n\r\n", xWriteBufferLen ); + pxCommand = NULL; + } + else if( pxCommand != NULL ) + { + /* Call the callback function that is registered to this command. */ + xReturn = pxCommand->pxCommandLineDefinition->pxCommandInterpreter( pcWriteBuffer, xWriteBufferLen, pcCommandInput ); + + /* If xReturn is pdFALSE, then no further strings will be returned + after this one, and pxCommand can be reset to NULL ready to search + for the next entered command. */ + if( xReturn == pdFALSE ) + { + pxCommand = NULL; + } + } + else + { + /* pxCommand was NULL, the command was not found. */ + strncpy( pcWriteBuffer, "Command not recognised. Enter 'help' to view a list of available commands.\r\n\r\n", xWriteBufferLen ); + xReturn = pdFALSE; + } + + return xReturn; +} +/*---------------------------------------------------------- */ + +char *FreeRTOS_CLIGetOutputBuffer( void ) +{ + return cOutputBuffer; +} +/*---------------------------------------------------------- */ + +const char *FreeRTOS_CLIGetParameter( const char *pcCommandString, UBaseType_t uxWantedParameter, BaseType_t *pxParameterStringLength ) +{ +UBaseType_t uxParametersFound = 0; +const char *pcReturn = NULL; + + *pxParameterStringLength = 0; + + while( uxParametersFound < uxWantedParameter ) + { + /* Index the character pointer past the current word. If this is the start + of the command string then the first word is the command itself. */ + while( ( ( *pcCommandString ) != 0x00 ) && ( ( *pcCommandString ) != ' ' ) ) + { + pcCommandString++; + } + + /* Find the start of the next string. */ + while( ( ( *pcCommandString ) != 0x00 ) && ( ( *pcCommandString ) == ' ' ) ) + { + pcCommandString++; + } + + /* Was a string found? */ + if( *pcCommandString != 0x00 ) + { + /* Is this the start of the required parameter? */ + uxParametersFound++; + + if( uxParametersFound == uxWantedParameter ) + { + /* How long is the parameter? */ + pcReturn = pcCommandString; + while( ( ( *pcCommandString ) != 0x00 ) && ( ( *pcCommandString ) != ' ' ) ) + { + ( *pxParameterStringLength )++; + pcCommandString++; + } + + if( *pxParameterStringLength == 0 ) + { + pcReturn = NULL; + } + + break; + } + } + else + { + break; + } + } + + return pcReturn; +} +/*---------------------------------------------------------- */ + +static BaseType_t prvHelpCommand( char *pcWriteBuffer, size_t xWriteBufferLen, const char *pcCommandString ) +{ +static const CLI_Definition_List_Item_t * pxCommand = NULL; +BaseType_t xReturn; + + ( void ) pcCommandString; + + if( pxCommand == NULL ) + { + /* Reset the pxCommand pointer back to the start of the list. */ + pxCommand = &xRegisteredCommands; + } + + /* Return the next command help string, before moving the pointer on to + the next command in the list. */ + strncpy( pcWriteBuffer, pxCommand->pxCommandLineDefinition->pcHelpString, xWriteBufferLen ); + pxCommand = pxCommand->pxNext; + + if( pxCommand == NULL ) + { + /* There are no more commands in the list, so there will be no more + strings to return after this one and pdFALSE should be returned. */ + xReturn = pdFALSE; + } + else + { + xReturn = pdTRUE; + } + + return xReturn; +} +/*---------------------------------------------------------- */ + +static int8_t prvGetNumberOfParameters( const char *pcCommandString ) +{ +int8_t cParameters = 0; +BaseType_t xLastCharacterWasSpace = pdFALSE; + + /* Count the number of space delimited words in pcCommandString. */ + while( *pcCommandString != 0x00 ) + { + if( ( *pcCommandString ) == ' ' ) + { + if( xLastCharacterWasSpace != pdTRUE ) + { + cParameters++; + xLastCharacterWasSpace = pdTRUE; + } + } + else + { + xLastCharacterWasSpace = pdFALSE; + } + + pcCommandString++; + } + + /* If the command string ended with spaces, then there will have been too + many parameters counted. */ + if( xLastCharacterWasSpace == pdTRUE ) + { + cParameters--; + } + + /* The value returned is one less than the number of space delimited words, + as the first word should be the command itself. */ + return cParameters; +} + diff --git a/User/component/freertos_cli.h b/User/component/freertos_cli.h new file mode 100644 index 0000000..e6d8266 --- /dev/null +++ b/User/component/freertos_cli.h @@ -0,0 +1,108 @@ +/* + * FreeRTOS+CLI V1.0.4 + * Copyright (C) 2017 Amazon.com, Inc. or its affiliates. All Rights Reserved. + * + * Permission is hereby granted, free of charge, to any person obtaining a copy of + * this software and associated documentation files (the "Software"), to deal in + * the Software without restriction, including without limitation the rights to + * use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of + * the Software, and to permit persons to whom the Software is furnished to do so, + * subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS + * FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR + * COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER + * IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN + * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. + * + * http://www.FreeRTOS.org + * http://aws.amazon.com/freertos + * + * 1 tab == 4 spaces! + */ + +#ifndef COMMAND_INTERPRETER_H +#define COMMAND_INTERPRETER_H + +/* This config should be defined in FreeRTOSConfig.h. But due to the limition of CubeMX I put it here. */ +#define configCOMMAND_INT_MAX_OUTPUT_SIZE 512 + +/* The prototype to which callback functions used to process command line +commands must comply. pcWriteBuffer is a buffer into which the output from +executing the command can be written, xWriteBufferLen is the length, in bytes of +the pcWriteBuffer buffer, and pcCommandString is the entire string as input by +the user (from which parameters can be extracted).*/ +typedef BaseType_t (*pdCOMMAND_LINE_CALLBACK)( char *pcWriteBuffer, size_t xWriteBufferLen, const char *pcCommandString ); + +/* The structure that defines command line commands. A command line command +should be defined by declaring a const structure of this type. */ +typedef struct xCOMMAND_LINE_INPUT +{ + const char * const pcCommand; /* The command that causes pxCommandInterpreter to be executed. For example "help". Must be all lower case. */ + const char * const pcHelpString; /* String that describes how to use the command. Should start with the command itself, and end with "\r\n". For example "help: Returns a list of all the commands\r\n". */ + const pdCOMMAND_LINE_CALLBACK pxCommandInterpreter; /* A pointer to the callback function that will return the output generated by the command. */ + int8_t cExpectedNumberOfParameters; /* Commands expect a fixed number of parameters, which may be zero. */ +} CLI_Command_Definition_t; + +/* For backward compatibility. */ +#define xCommandLineInput CLI_Command_Definition_t + +/* + * Register the command passed in using the pxCommandToRegister parameter. + * Registering a command adds the command to the list of commands that are + * handled by the command interpreter. Once a command has been registered it + * can be executed from the command line. + */ +BaseType_t FreeRTOS_CLIRegisterCommand( const CLI_Command_Definition_t * const pxCommandToRegister ); + +/* + * Runs the command interpreter for the command string "pcCommandInput". Any + * output generated by running the command will be placed into pcWriteBuffer. + * xWriteBufferLen must indicate the size, in bytes, of the buffer pointed to + * by pcWriteBuffer. + * + * FreeRTOS_CLIProcessCommand should be called repeatedly until it returns pdFALSE. + * + * pcCmdIntProcessCommand is not reentrant. It must not be called from more + * than one task - or at least - by more than one task at a time. + */ +BaseType_t FreeRTOS_CLIProcessCommand( const char * const pcCommandInput, char * pcWriteBuffer, size_t xWriteBufferLen ); + +/*---------------------------------------------------------- */ + +/* + * A buffer into which command outputs can be written is declared in the + * main command interpreter, rather than in the command console implementation, + * to allow application that provide access to the command console via multiple + * interfaces to share a buffer, and therefore save RAM. Note, however, that + * the command interpreter itself is not re-entrant, so only one command + * console interface can be used at any one time. For that reason, no attempt + * is made to provide any mutual exclusion mechanism on the output buffer. + * + * FreeRTOS_CLIGetOutputBuffer() returns the address of the output buffer. + */ +char *FreeRTOS_CLIGetOutputBuffer( void ); + +/* + * Return a pointer to the xParameterNumber'th word in pcCommandString. + */ +const char *FreeRTOS_CLIGetParameter( const char *pcCommandString, UBaseType_t uxWantedParameter, BaseType_t *pxParameterStringLength ); + +#endif /* COMMAND_INTERPRETER_H */ + + + + + + + + + + + + + diff --git a/User/component/limiter.c b/User/component/limiter.c new file mode 100644 index 0000000..71e4bf1 --- /dev/null +++ b/User/component/limiter.c @@ -0,0 +1,107 @@ +/* + 限制器 +*/ + +#include "limiter.h" + +#include +#include + +#define POWER_BUFF_THRESHOLD 20 +#define CHASSIS_POWER_CHECK_FREQ 10 +#define CHASSIS_POWER_FACTOR_PASS 0.9f +#define CHASSIS_POWER_FACTOR_NO_PASS 1.5f + +#define CHASSIS_MOTOR_CIRCUMFERENCE 0.12f + +/** + * @brief 限制底盘功率不超过power_limit + * + * @param power_limit 最大功率 + * @param motor_out 电机输出值 + * @param speed 电机转速 + * @param len 电机数量 + * @return int8_t 0对应没有错误 + */ +int8_t PowerLimit_ChassicOutput(float power_limit, float *motor_out, + float *speed, uint32_t len) { + /* power_limit小于0时不进行限制 */ + if (motor_out == NULL || speed == NULL || power_limit < 0) return -1; + + float sum_motor_out = 0.0f; + for (uint32_t i = 0; i < len; i++) { + /* 总功率计算 P=F(由转矩电流表示)*V(由转速表示) */ + sum_motor_out += + fabsf(motor_out[i]) * fabsf(speed[i]) * CHASSIS_MOTOR_CIRCUMFERENCE; + } + + /* 保持每个电机输出值缩小时比例不变 */ + if (sum_motor_out > power_limit) { + for (uint32_t i = 0; i < len; i++) { + motor_out[i] *= power_limit / sum_motor_out; + } + } + + return 0; +} + +/** + * @brief 电容输入功率计算 + * + * @param power_in 底盘当前功率 + * @param power_limit 裁判系统功率限制值 + * @param power_buffer 缓冲能量 + * @return float 裁判系统输出最大值 + */ +float PowerLimit_CapInput(float power_in, float power_limit, + float power_buffer) { + float target_power = 0.0f; + + /* 计算下一个检测周期的剩余缓冲能量 */ + float heat_buff = power_buffer - (float)(power_in - power_limit) / + (float)CHASSIS_POWER_CHECK_FREQ; + if (heat_buff < POWER_BUFF_THRESHOLD) { /* 功率限制 */ + target_power = power_limit * CHASSIS_POWER_FACTOR_PASS; + } else { + target_power = power_limit * CHASSIS_POWER_FACTOR_NO_PASS; + } + + return target_power; +} + +/** + * @brief 使用缓冲能量计算底盘最大功率 + * + * @param power_limit 裁判系统功率限制值 + * @param power_buffer 缓冲能量 + * @return float 底盘输出最大值 + */ +float PowerLimit_TargetPower(float power_limit, float power_buffer) { + float target_power = 0.0f; + + /* 根据剩余缓冲能量计算输出功率 */ + target_power = power_limit * (power_buffer - 10.0f) / 20.0f; + if (target_power < 0.0f) target_power = 0.0f; + + return target_power; +} + +/** + * @brief 射击频率控制 + * + * @param heat 当前热量 + * @param heat_limit 热量上限 + * @param cooling_rate 冷却速率 + * @param heat_increase 冷却增加 + * @param shoot_freq 经过热量限制后的射击频率 + * @return float 射击频率 + */ +float HeatLimit_ShootFreq(float heat, float heat_limit, float cooling_rate, + float heat_increase, bool is_big) { + float heat_percent = heat / heat_limit; + float stable_freq = cooling_rate / heat_increase; + if (is_big) + return stable_freq; + else + return (heat_percent > 0.7f) ? stable_freq : 3.0f * stable_freq; +} diff --git a/User/component/limiter.h b/User/component/limiter.h new file mode 100644 index 0000000..d0aa92a --- /dev/null +++ b/User/component/limiter.h @@ -0,0 +1,63 @@ +/* + 限制器 +*/ + +#pragma once + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include + +/* USER INCLUDE BEGIN */ + +/* USER INCLUDE END */ + +/* USER DEFINE BEGIN */ + +/* USER DEFINE END */ + +/** + * @brief 限制底盘功率不超过power_limit + * + * @param power_limit 最大功率 + * @param motor_out 电机输出值 + * @param speed 电机转速 + * @param len 电机数量 + * @return int8_t 0对应没有错误 + */ +int8_t PowerLimit_ChassicOutput(float power_limit, float *motor_out, + float *speed, uint32_t len); +/** + * @brief 电容输入功率计算 + * + * @param power_in 底盘当前功率 + * @param power_limit 裁判系统功率限制值 + * @param power_buffer 缓冲能量 + * @return float 裁判系统输出最大值 + */ +float PowerLimit_CapInput(float power_in, float power_limit, + float power_buffer); +/** + * @brief 使用缓冲能量计算底盘最大功率 + * + * @param power_limit 裁判系统功率限制值 + * @param power_buffer 缓冲能量 + * @return float 底盘输出最大值 + */ +float PowerLimit_TargetPower(float power_limit, float power_buffer); + +/** + * @brief 射击频率控制 + * + * @param heat 当前热量 + * @param heat_limit 热量上限 + * @param cooling_rate 冷却速率 + * @param heat_increase 冷却增加 + * @param shoot_freq 经过热量限制后的射击频率 + * @return float 射击频率 + */ +float HeatLimit_ShootFreq(float heat, float heat_limit, float cooling_rate, + float heat_increase, bool is_big); diff --git a/User/component/pid.c b/User/component/pid.c new file mode 100644 index 0000000..0a3c7d4 --- /dev/null +++ b/User/component/pid.c @@ -0,0 +1,158 @@ +/* + Modified from + https://github.com/PX4/Firmware/blob/master/src/lib/pid/pid.cpp + + 参考资料： + https://github.com/PX4/Firmware/issues/12362 + https://dev.px4.io/master/en/flight_stack/controller_diagrams.html + https://docs.px4.io/master/en/config_mc/pid_tuning_guide_multicopter.html#standard_form + https://www.controleng.com/articles/not-all-pid-controllers-are-the-same/ + https://en.wikipedia.org/wiki/PID_controller + http://brettbeauregard.com/blog/2011/04/improving-the-beginner%E2%80%99s-pid-derivative-kick/ +*/ + +#include "pid.h" + +#define SIGMA 0.000001f + +/** + * @brief 初始化PID + * + * @param pid PID结构体 + * @param mode PID模式 + * @param sample_freq 采样频率 + * @param param PID参数 + * @return int8_t 0对应没有错误 + */ +int8_t PID_Init(KPID_t *pid, KPID_Mode_t mode, float sample_freq, + const KPID_Params_t *param) { + if (pid == NULL) return -1; + + if (!isfinite(param->p)) return -1; + if (!isfinite(param->i)) return -1; + if (!isfinite(param->d)) return -1; + if (!isfinite(param->i_limit)) return -1; + if (!isfinite(param->out_limit)) return -1; + pid->param = param; + + float dt_min = 1.0f / sample_freq; + if (isfinite(dt_min)) + pid->dt_min = dt_min; + else + return -1; + + LowPassFilter2p_Init(&(pid->dfilter), sample_freq, pid->param->d_cutoff_freq); + + pid->mode = mode; + PID_Reset(pid); + return 0; +} + +/** + * @brief PID计算 + * + * @param pid PID结构体 + * @param sp 设定值 + * @param fb 反馈值 + * @param fb_dot 反馈值微分 + * @param dt 间隔时间 + * @return float 计算的输出 + */ +float PID_Calc(KPID_t *pid, float sp, float fb, float fb_dot, float dt) { + if (!isfinite(sp) || !isfinite(fb) || !isfinite(fb_dot) || !isfinite(dt)) { + return pid->last.out; + } + + /* 计算误差值 */ + const float err = CircleError(sp, fb, pid->param->range); + + /* 计算P项 */ + const float k_err = err * pid->param->k; + + /* 计算D项 */ + const float k_fb = pid->param->k * fb; + const float filtered_k_fb = LowPassFilter2p_Apply(&(pid->dfilter), k_fb); + + float d; + switch (pid->mode) { + case KPID_MODE_CALC_D: + /* 通过fb计算D，避免了由于sp变化导致err突变的问题 */ + /* 当sp不变时，err的微分等于负的fb的微分 */ + d = (filtered_k_fb - pid->last.k_fb) / fmaxf(dt, pid->dt_min); + break; + + case KPID_MODE_SET_D: + d = fb_dot; + break; + + case KPID_MODE_NO_D: + d = 0.0f; + break; + } + pid->last.err = err; + pid->last.k_fb = filtered_k_fb; + + if (!isfinite(d)) d = 0.0f; + + /* 计算PD输出 */ + float output = (k_err * pid->param->p) - (d * pid->param->d); + + /* 计算I项 */ + const float i = pid->i + (k_err * dt); + const float i_out = i * pid->param->i; + + if (pid->param->i > SIGMA) { + /* 检查是否饱和 */ + if (isfinite(i)) { + if ((fabsf(output + i_out) <= pid->param->out_limit) && + (fabsf(i) <= pid->param->i_limit)) { + /* 未饱和，使用新积分 */ + pid->i = i; + } + } + } + + /* 计算PID输出 */ + output += i_out; + + /* 限制输出 */ + if (isfinite(output)) { + if (pid->param->out_limit > SIGMA) { + output = AbsClip(output, pid->param->out_limit); + } + pid->last.out = output; + } + return pid->last.out; +} + +/** + * @brief 重置微分项 + * + * @param pid PID结构体 + * @return int8_t 0对应没有错误 + */ +int8_t PID_ResetIntegral(KPID_t *pid) { + if (pid == NULL) return -1; + + pid->i = 0.0f; + + return 0; +} + +/** + * @brief 重置PID + * + * @param pid PID结构体 + * @return int8_t 0对应没有错误 + */ +int8_t PID_Reset(KPID_t *pid) { + if (pid == NULL) return -1; + + pid->i = 0.0f; + pid->last.err = 0.0f; + pid->last.k_fb = 0.0f; + pid->last.out = 0.0f; + LowPassFilter2p_Reset(&(pid->dfilter), 0.0f); + + return 0; +} diff --git a/User/component/pid.h b/User/component/pid.h new file mode 100644 index 0000000..4b451eb --- /dev/null +++ b/User/component/pid.h @@ -0,0 +1,107 @@ +/* + Modified from + https://github.com/PX4/Firmware/blob/master/src/lib/pid/pid.h +*/ + +#pragma once + +#ifdef __cplusplus +extern "C" { +#endif + +#include + +#include "filter.h" +#include "user_math.h" + +/* USER INCLUDE BEGIN */ + +/* USER INCLUDE END */ + +/* USER DEFINE BEGIN */ + +/* USER DEFINE END */ + +/* PID模式 */ +typedef enum { + KPID_MODE_NO_D = 0, /* 不使用微分项，PI控制器 */ + KPID_MODE_CALC_D, /* 根据反馈的值计算离散微分，忽略PID_Calc中的fb_dot */ + KPID_MODE_SET_D /* 直接提供微分值，PID_Calc中的fb_dot将被使用，(Gyros) */ +} KPID_Mode_t; + +/* PID参数 */ +typedef struct { + float k; /* 控制器增益，设置为1用于并行模式 */ + float p; /* 比例项增益，设置为1用于标准形式 */ + float i; /* 积分项增益 */ + float d; /* 微分项增益 */ + float i_limit; /* 积分项上限 */ + float out_limit; /* 输出绝对值限制 */ + float d_cutoff_freq; /* D项低通截止频率 */ + float range; /* 计算循环误差时使用，大于0时启用 */ +} KPID_Params_t; + +/* PID主结构体 */ +typedef struct { + KPID_Mode_t mode; + const KPID_Params_t *param; + + float dt_min; /* 最小PID_Calc调用间隔 */ + float i; /* 积分 */ + + struct { + float err; /* 上次误差 */ + float k_fb; /* 上次反馈值 */ + float out; /* 上次输出 */ + } last; + + LowPassFilter2p_t dfilter; /* D项低通滤波器 */ +} KPID_t; + +/** + * @brief 初始化PID + * + * @param pid PID结构体 + * @param mode PID模式 + * @param sample_freq 采样频率 + * @param param PID参数 + * @return int8_t 0对应没有错误 + */ +int8_t PID_Init(KPID_t *pid, KPID_Mode_t mode, float sample_freq, + const KPID_Params_t *param); + +/** + * @brief PID计算 + * + * @param pid PID结构体 + * @param sp 设定值 + * @param fb 反馈值 + * @param fb_dot 反馈值微分 + * @param dt 间隔时间 + * @return float 计算的输出 + */ +float PID_Calc(KPID_t *pid, float sp, float fb, float fb_dot, float dt); + +/** + * @brief 重置微分项 + * + * @param pid PID结构体 + * @return int8_t 0对应没有错误 + */ +int8_t PID_ResetIntegral(KPID_t *pid); + +/** + * @brief 重置PID + * + * @param pid PID结构体 + * @return int8_t 0对应没有错误 + */ +int8_t PID_Reset(KPID_t *pid); + +/* USER FUNCTION BEGIN */ + +/* USER FUNCTION END */ + +#ifdef __cplusplus +} +#endif diff --git a/User/device/buzzer.c b/User/device/buzzer.c new file mode 100644 index 0000000..f5cc820 --- /dev/null +++ b/User/device/buzzer.c @@ -0,0 +1,171 @@ +#include "device/buzzer.h" +#include "bsp/time.h" +#include + +/* USER INCLUDE BEGIN */ + +/* USER INCLUDE END */ + +/* USER DEFINE BEGIN */ + +/* USER DEFINE END */ + +#define MUSIC_DEFAULT_VOLUME 0.5f +#define MUSIC_A4_FREQ 440.0f // A4音符频率 + +/* USER MUSIC MENU BEGIN */ +// RM音乐 +const Tone_t RM[] = { + {NOTE_B, 5, 200}, + {NOTE_G, 4, 200}, + {NOTE_B, 5, 400}, + {NOTE_G, 4, 200}, + {NOTE_B, 5, 400}, + {NOTE_G, 4, 200}, + {NOTE_D, 5, 400}, + {NOTE_G, 4, 200}, + {NOTE_C, 5, 200}, + {NOTE_C, 5, 200}, + {NOTE_G, 4, 200}, + {NOTE_B, 5, 200}, + {NOTE_C, 5, 200} +}; + +// Nokia 经典铃声音符 +const Tone_t NOKIA[] = { + {NOTE_E, 5, 125}, {NOTE_D, 5, 125}, {NOTE_FS, 4, 250}, {NOTE_GS, 4, 250}, + {NOTE_CS, 5, 125}, {NOTE_B, 4, 125}, {NOTE_D, 4, 250}, {NOTE_E, 4, 250}, + {NOTE_B, 4, 125}, {NOTE_A, 4, 125}, {NOTE_CS, 4, 250}, {NOTE_E, 4, 250}, + {NOTE_A, 4, 500} +}; +/* USER MUSIC MENU END */ + +static void BUZZER_Update(BUZZER_t *buzzer){ + buzzer->header.online = true; + buzzer->header.last_online_time = BSP_TIME_Get_ms(); +} + +// 根据音符和八度计算频率的辅助函数 +static float BUZZER_CalcFreq(NOTE_t note, uint8_t octave) { + if (note == NOTE_REST) { + return 0.0f; // 休止符返回0频率 + } + + // 将音符和八度转换为MIDI音符编号 + int midi_num = (int)note + (int)((octave + 1) * 12); + + // 使用A4 (440Hz) 作为参考，计算频率 + // 公式: freq = 440 * 2^((midi_num - 69)/12) + float freq = 440.0f * powf(2.0f, ((float)midi_num - 69.0f) / 12.0f); + + return freq; +} + +// 播放单个音符 +static int8_t BUZZER_PlayTone(BUZZER_t *buzzer, NOTE_t note, uint8_t octave, uint16_t duration_ms) { + if (buzzer == NULL || !buzzer->header.online) + return DEVICE_ERR; + + float freq = BUZZER_CalcFreq(note, octave); + + if (freq > 0.0f) { + // 播放音符 + if (BUZZER_Set(buzzer, freq, MUSIC_DEFAULT_VOLUME) != DEVICE_OK) + return DEVICE_ERR; + + if (BUZZER_Start(buzzer) != DEVICE_OK) + return DEVICE_ERR; + } else { + // 休止符，停止播放 + BUZZER_Stop(buzzer); + } + + // 等待指定时间 + BSP_TIME_Delay_ms(duration_ms); + + // 停止当前音符，为下一个音符做准备 + BUZZER_Stop(buzzer); + BSP_TIME_Delay_ms(20); // 短暂间隔 + + return DEVICE_OK; +} + +int8_t BUZZER_Init(BUZZER_t *buzzer, BSP_PWM_Channel_t channel) { + if (buzzer == NULL) return DEVICE_ERR; + + buzzer->channel = channel; + buzzer->header.online = true; + + BUZZER_Stop(buzzer); + + return DEVICE_OK ; +} + +int8_t BUZZER_Start(BUZZER_t *buzzer) { + if (buzzer == NULL || !buzzer->header.online) + return DEVICE_ERR; + BUZZER_Update(buzzer); + return (BSP_PWM_Start(buzzer->channel) == BSP_OK) ? + DEVICE_OK : DEVICE_ERR; +} + +int8_t BUZZER_Stop(BUZZER_t *buzzer) { + if (buzzer == NULL || !buzzer->header.online) + return DEVICE_ERR; + BUZZER_Update(buzzer); + return (BSP_PWM_Stop(buzzer->channel) == BSP_OK) ? + DEVICE_OK : DEVICE_ERR; +} + +int8_t BUZZER_Set(BUZZER_t *buzzer, float freq, float duty_cycle) { + if (buzzer == NULL || !buzzer->header.online) + return DEVICE_ERR; + + int result = DEVICE_OK ; + BUZZER_Update(buzzer); + if (BSP_PWM_SetFreq(buzzer->channel, freq) != BSP_OK) + result = DEVICE_ERR; + + if (BSP_PWM_SetComp(buzzer->channel, duty_cycle) != BSP_OK) + result = DEVICE_ERR; + + return result; +} + +int8_t BUZZER_PlayMusic(BUZZER_t *buzzer, MUSIC_t music) { + if (buzzer == NULL || !buzzer->header.online) + return DEVICE_ERR; + + const Tone_t *melody = NULL; + size_t melody_length = 0; + + // 根据音乐类型选择对应的音符数组 + switch (music) { + case MUSIC_RM: + melody = RM; + melody_length = sizeof(RM) / sizeof(Tone_t); + break; + case MUSIC_NOKIA: + melody = NOKIA; + melody_length = sizeof(NOKIA) / sizeof(Tone_t); + break; + default: + return DEVICE_ERR; + } + + // 播放整首音乐 + for (size_t i = 0; i < melody_length; i++) { + if (BUZZER_PlayTone(buzzer, melody[i].note, melody[i].octave, melody[i].duration_ms) != DEVICE_OK) { + BUZZER_Stop(buzzer); // 出错时停止播放 + return DEVICE_ERR; + } + } + + // 音乐播放完成后停止 + BUZZER_Stop(buzzer); + return DEVICE_OK; +} + +/* USER FUNCTION BEGIN */ + +/* USER FUNCTION END */ diff --git a/User/device/buzzer.h b/User/device/buzzer.h new file mode 100644 index 0000000..4ef7e43 --- /dev/null +++ b/User/device/buzzer.h @@ -0,0 +1,138 @@ +/** + * @file buzzer.h + * @brief 蜂鸣器设备驱动头文件 + * @details 提供蜂鸣器音频播放功能，支持单音符播放和预设音乐播放 + * @author Generated by STM32CubeMX + * @date 2025年10月23日 + */ + +#pragma once + +#ifdef __cplusplus +extern "C" { +#endif + +/* Includes ----------------------------------------------------------------- */ +#include "bsp/pwm.h" // PWM底层硬件抽象层 +#include "device.h" // 设备通用头文件 +#include // 标准定义 +#include // 标准整型定义 + +/* USER INCLUDE BEGIN */ + +/* USER INCLUDE END */ + +/* Exported constants ------------------------------------------------------- */ + +/* USER DEFINE BEGIN */ + +/* USER DEFINE END */ + +/* Exported types ----------------------------------------------------------- */ + +/** + * @brief 音符枚举类型 + * @details 定义标准十二平均律音符，用于音乐播放 + */ +typedef enum { + NOTE_C = 0, ///< Do音符 + NOTE_CS = 1, ///< Do#音符 (升Do) + NOTE_D = 2, ///< Re音符 + NOTE_DS = 3, ///< Re#音符 (升Re) + NOTE_E = 4, ///< Mi音符 + NOTE_F = 5, ///< Fa音符 + NOTE_FS = 6, ///< Fa#音符 (升Fa) + NOTE_G = 7, ///< Sol音符 + NOTE_GS = 8, ///< Sol#音符 (升Sol) + NOTE_A = 9, ///< La音符 + NOTE_AS = 10, ///< La#音符 (升La) + NOTE_B = 11, ///< Si音符 + NOTE_REST = 255 ///< 休止符 (无声音) +} NOTE_t; + +/** + * @brief 音调结构体 + * @details 定义一个完整的音调信息，包括音符、八度和持续时间 + */ +typedef struct { + NOTE_t note; ///< 音符名称 (使用NOTE_t枚举) + uint8_t octave; ///< 八度 (0-8，通常使用3-7) + uint16_t duration_ms; ///< 持续时间，单位毫秒 +} Tone_t; + +/** + * @brief 预设音乐枚举类型 + * @details 定义可播放的预设音乐类型 + */ +typedef enum { + /* USER MUSIC MENU BEGIN */ + MUSIC_RM, ///< RM战队音乐 + MUSIC_NOKIA, ///< 诺基亚经典铃声 + /* USER MUSIC MENU END */ +} MUSIC_t; + +/** + * @brief 蜂鸣器设备结构体 + * @details 蜂鸣器设备的完整描述，包含设备头信息和PWM通道 + */ +typedef struct { + DEVICE_Header_t header; ///< 设备通用头信息 (在线状态、时间戳等) + BSP_PWM_Channel_t channel; ///< PWM输出通道 +} BUZZER_t; + +/* USER STRUCT BEGIN */ + +/* USER STRUCT END */ + +/* Exported functions prototypes -------------------------------------------- */ + +/** + * @brief 初始化蜂鸣器设备 + * @param buzzer 蜂鸣器设备结构体指针 + * @param channel PWM输出通道 + * @return int8_t 返回值：DEVICE_OK(0) 成功，DEVICE_ERR(-1) 失败 + * @note 初始化后蜂鸣器处于停止状态 + */ +int8_t BUZZER_Init(BUZZER_t *buzzer, BSP_PWM_Channel_t channel); + +/** + * @brief 启动蜂鸣器播放 + * @param buzzer 蜂鸣器设备结构体指针 + * @return int8_t 返回值：DEVICE_OK(0) 成功，DEVICE_ERR(-1) 失败 + * @note 需要先调用BUZZER_Set设置频率和占空比 + */ +int8_t BUZZER_Start(BUZZER_t *buzzer); + +/** + * @brief 停止蜂鸣器播放 + * @param buzzer 蜂鸣器设备结构体指针 + * @return int8_t 返回值：DEVICE_OK(0) 成功，DEVICE_ERR(-1) 失败 + */ +int8_t BUZZER_Stop(BUZZER_t *buzzer); + +/** + * @brief 设置蜂鸣器频率和占空比 + * @param buzzer 蜂鸣器设备结构体指针 + * @param freq 频率 (Hz)，通常范围20Hz-20kHz + * @param duty_cycle 占空比 (0.0-1.0)，影响音量大小 + * @return int8_t 返回值：DEVICE_OK(0) 成功，DEVICE_ERR(-1) 失败 + * @note 设置后需要调用BUZZER_Start才能听到声音 + */ +int8_t BUZZER_Set(BUZZER_t *buzzer, float freq, float duty_cycle); + +/** + * @brief 播放预设音乐 + * @param buzzer 蜂鸣器设备结构体指针 + * @param music 音乐类型 (使用MUSIC_t枚举) + * @return int8_t 返回值：DEVICE_OK(0) 成功，DEVICE_ERR(-1) 失败 + * @note 这是一个阻塞函数，会播放完整首音乐后返回 + */ +int8_t BUZZER_PlayMusic(BUZZER_t *buzzer, MUSIC_t music); + +/* USER FUNCTION BEGIN */ + +/* USER FUNCTION END */ + +#ifdef __cplusplus +} +#endif diff --git a/User/device/device_config.yaml b/User/device/device_config.yaml index e187851..62f5e40 100644 --- a/User/device/device_config.yaml +++ b/User/device/device_config.yaml @@ -6,6 +6,10 @@ bmi088: BSP_GPIO_GYRO_INT: BSP_GPIO_ACC_INT BSP_SPI_BMI088: BSP_SPI_BMI088 enabled: true +buzzer: + bsp_config: + BSP_PWM_BUZZER: BSP_PWM_TIM12_CH2 + enabled: true dr16: bsp_config: BSP_UART_DR16: BSP_UART_DR16 @@ -13,6 +17,15 @@ dr16: motor: bsp_config: {} enabled: true +motor_dm: + bsp_config: {} + enabled: true +motor_lk: + bsp_config: {} + enabled: true motor_lz: bsp_config: {} enabled: true +motor_rm: + bsp_config: {} + enabled: true diff --git a/User/device/motor_dm.c b/User/device/motor_dm.c new file mode 100644 index 0000000..a77656f --- /dev/null +++ b/User/device/motor_dm.c @@ -0,0 +1,496 @@ +#define MOTOR_DM_FLOAT_TO_INT_SIGNED(x, x_min, x_max, bits) \ + ((int32_t)roundf(((x) / ((x_max) - (x_min))) * (1 << (bits)) + (1 << ((bits) - 1)))) + +#define MOTOR_DM_INT_TO_FLOAT_SIGNED(x_int, x_min, x_max, bits) \ + (((float)((int32_t)(x_int) - (1 << ((bits) - 1))) * ((x_max) - (x_min)) / (float)(1 << (bits)))) +/* Includes ----------------------------------------------------------------- */ +#include "device/motor_dm.h" +#include "bsp/mm.h" +#include "bsp/time.h" +#include "component/user_math.h" +#include "string.h" +#include "math.h" + +/* Private constants -------------------------------------------------------- */ +/* DM电机数据映射范围 */ +#define DM_P_MIN (-12.56637f) +#define DM_P_MAX (12.56637f) +#define DM_V_MIN (-30.0f) +#define DM_V_MAX (30.0f) +#define DM_T_MIN (-12.0f) +#define DM_T_MAX (12.0f) +#define DM_KP_MIN (0.0f) +#define DM_KP_MAX (500.0f) +#define DM_KD_MIN (0.0f) +#define DM_KD_MAX (5.0f) + +/* CAN ID偏移量 */ +#define DM_CAN_ID_OFFSET_POS_VEL 0x100 +#define DM_CAN_ID_OFFSET_VEL 0x200 + +/* Private macro ------------------------------------------------------------ */ +#define FLOAT_TO_UINT(x, x_min, x_max, bits) \ + (uint32_t)((x - x_min) * ((1 << bits) - 1) / (x_max - x_min)) + +#define UINT_TO_FLOAT(x_int, x_min, x_max, bits) \ + ((float)(x_int) * (x_max - x_min) / ((1 << bits) - 1) + x_min) + + +/* Private variables -------------------------------------------------------- */ +static MOTOR_DM_CANManager_t *can_managers[BSP_CAN_NUM] = {NULL}; + +static int float_to_uint(float x_float, float x_min, float x_max, int bits) +{ + /* Converts a float to an unsigned int, given range and number of bits */ + float span = x_max - x_min; + float offset = x_min; + return (int) ((x_float-offset)*((float)((1<motor.feedback.rotor_abs_angle = uint_to_float(p_int, DM_P_MIN, DM_P_MAX, 16); // (-12.5,12.5) + uint16_t v_int=(data[3]<<4)|(data[4]>>4); + motor->motor.feedback.rotor_speed = uint_to_float(v_int, DM_V_MIN, DM_V_MAX, 12); // (-30.0,30.0) + uint16_t t_int=((data[4]&0xF)<<8)|data[5]; + motor->motor.feedback.torque_current = uint_to_float(t_int, DM_T_MIN, DM_T_MAX, 12); // (-12.0,12.0) + motor->motor.feedback.temp = (float)(data[6]); + + while (motor->motor.feedback.rotor_abs_angle < 0) { + motor->motor.feedback.rotor_abs_angle += M_2PI; + } + while (motor->motor.feedback.rotor_abs_angle >= M_2PI) { + motor->motor.feedback.rotor_abs_angle -= M_2PI; + } + + if (motor->param.reverse) { + motor->motor.feedback.rotor_abs_angle = M_2PI - motor->motor.feedback.rotor_abs_angle; + motor->motor.feedback.rotor_speed = -motor->motor.feedback.rotor_speed; + motor->motor.feedback.torque_current = -motor->motor.feedback.torque_current; + } + return DEVICE_OK; +} + +/** + * @brief 发送MIT模式控制命令 + * @param motor 电机实例 + * @param output MIT控制参数 + * @return 发送结果 + */ +static int8_t MOTOR_DM_SendMITCmd(MOTOR_DM_t *motor, MOTOR_MIT_Output_t *output) { + if (motor == NULL || output == NULL) { + return DEVICE_ERR_NULL; + } + + uint8_t data[8]; + uint16_t pos_tmp,vel_tmp,kp_tmp,kd_tmp,tor_tmp; + uint16_t id = motor->param.can_id; + + pos_tmp = float_to_uint(output->angle, DM_P_MIN , DM_P_MAX, 16); + vel_tmp = float_to_uint(output->velocity, DM_V_MIN , DM_V_MAX, 12); + kp_tmp = float_to_uint(output->kp, DM_KP_MIN, DM_KP_MAX, 12); + kd_tmp = float_to_uint(output->kd, DM_KD_MIN, DM_KD_MAX, 12); + tor_tmp = float_to_uint(output->torque, DM_T_MIN , DM_T_MAX, 12); + + /* 打包数据 */ + data[0] = (pos_tmp >> 8); + data[1] = pos_tmp; + data[2] = (vel_tmp >> 4); + data[3] = ((vel_tmp&0xF)<<4)|(kp_tmp>>8); + data[4] = kp_tmp; + data[5] = (kd_tmp >> 4); + data[6] = ((kd_tmp&0xF)<<4)|(tor_tmp>>8); + data[7] = tor_tmp; + + /* 发送CAN消息 */ + BSP_CAN_StdDataFrame_t frame; + frame.id = motor->param.can_id; + frame.dlc = 8; + memcpy(frame.data, data, 8); + + + return BSP_CAN_TransmitStdDataFrame(motor->param.can, &frame) == BSP_OK ? DEVICE_OK : DEVICE_ERR; +} + +/** + * @brief 发送位置速度模式控制命令 + * @param motor 电机实例 + * @param pos 目标位置 + * @param vel 目标速度 + * @return 发送结果 + */ +static int8_t MOTOR_DM_SendPosVelCmd(MOTOR_DM_t *motor, float pos, float vel) { + if (motor == NULL) { + return DEVICE_ERR_NULL; + } + + uint8_t data[8] = {0}; + + + /* 直接发送浮点数数据 */ + memcpy(&data[0], &pos, 4); // 位置，低位在前 + memcpy(&data[4], &vel, 4); // 速度，低位在前 + + /* 发送CAN消息，ID为原ID+0x100 */ + uint32_t can_id = DM_CAN_ID_OFFSET_POS_VEL + motor->param.can_id; + BSP_CAN_StdDataFrame_t frame; + frame.id = can_id; + frame.dlc = 8; + memcpy(frame.data, data, 8); + + return BSP_CAN_TransmitStdDataFrame(motor->param.can, &frame) == BSP_OK ? DEVICE_OK : DEVICE_ERR; +} + +/** + * @brief 发送速度模式控制命令 + * @param motor 电机实例 + * @param vel 目标速度 + * @return 发送结果 + */ +static int8_t MOTOR_DM_SendVelCmd(MOTOR_DM_t *motor, float vel) { + if (motor == NULL) { + return DEVICE_ERR_NULL; + } + + uint8_t data[4] = {0}; + + /* 直接发送浮点数数据 */ + memcpy(&data[0], &vel, 4); // 速度，低位在前 + + /* 发送CAN消息，ID为原ID+0x200 */ + uint32_t can_id = DM_CAN_ID_OFFSET_VEL + motor->param.can_id; + BSP_CAN_StdDataFrame_t frame; + frame.id = can_id; + frame.dlc = 4; + memcpy(frame.data, data, 4); + + return BSP_CAN_TransmitStdDataFrame(motor->param.can, &frame) == BSP_OK ? DEVICE_OK : DEVICE_ERR; +} + +/** + * @brief 获取指定CAN总线的管理器 + * @param can CAN总线 + * @return CAN管理器指针 + */ +static MOTOR_DM_CANManager_t* MOTOR_DM_GetCANManager(BSP_CAN_t can) { + if (can >= BSP_CAN_NUM) { + return NULL; + } + + return can_managers[can]; +} + +/** + * @brief 创建CAN管理器 + * @param can CAN总线 + * @return 创建结果 + */ +static int8_t MOTOR_DM_CreateCANManager(BSP_CAN_t can) { + if (can >= BSP_CAN_NUM) return DEVICE_ERR; + if (can_managers[can] != NULL) return DEVICE_OK; + + can_managers[can] = (MOTOR_DM_CANManager_t*)BSP_Malloc(sizeof(MOTOR_DM_CANManager_t)); + if (can_managers[can] == NULL) return DEVICE_ERR; + + memset(can_managers[can], 0, sizeof(MOTOR_DM_CANManager_t)); + can_managers[can]->can = can; + return DEVICE_OK; +} + +/* Exported functions ------------------------------------------------------- */ + +/** + * @brief 注册一个DM电机 + * @param param 电机参数 + * @return 注册结果 + */ +int8_t MOTOR_DM_Register(MOTOR_DM_Param_t *param) { + if (param == NULL) { + return DEVICE_ERR_NULL; + } + + /* 创建CAN管理器 */ + if (MOTOR_DM_CreateCANManager(param->can) != DEVICE_OK) { + return DEVICE_ERR; + } + + /* 获取CAN管理器 */ + MOTOR_DM_CANManager_t *manager = MOTOR_DM_GetCANManager(param->can); + if (manager == NULL) { + return DEVICE_ERR; + } + + /* 检查是否已注册 */ + for (int i = 0; i < manager->motor_count; i++) { + if (manager->motors[i] && manager->motors[i]->param.master_id == param->master_id) { + return DEVICE_ERR_INITED; + } + } + + /* 检查是否已达到最大数量 */ + if (manager->motor_count >= MOTOR_DM_MAX_MOTORS) { + return DEVICE_ERR; + } + + /* 分配内存 */ + MOTOR_DM_t *motor = (MOTOR_DM_t *)BSP_Malloc(sizeof(MOTOR_DM_t)); + if (motor == NULL) { + return DEVICE_ERR; + } + + /* 初始化电机 */ + memset(motor, 0, sizeof(MOTOR_DM_t)); + memcpy(&motor->param, param, sizeof(MOTOR_DM_Param_t)); + motor->motor.header.online = false; + motor->motor.reverse = param->reverse; + + /* 注册CAN接收ID - DM电机使用Master ID接收反馈 */ + uint16_t feedback_id = param->master_id; + if (BSP_CAN_RegisterId(param->can, feedback_id, 3) != BSP_OK) { + BSP_Free(motor); + return DEVICE_ERR; + } + + /* 添加到管理器 */ + manager->motors[manager->motor_count] = motor; + manager->motor_count++; + + return DEVICE_OK; +} + +/** + * @brief 更新指定电机数据 + * @param param 电机参数 + * @return 更新结果 + */ +int8_t MOTOR_DM_Update(MOTOR_DM_Param_t *param) { + if (param == NULL) { + return DEVICE_ERR_NULL; + } + + MOTOR_DM_CANManager_t *manager = MOTOR_DM_GetCANManager(param->can); + if (manager == NULL) { + return DEVICE_ERR_NO_DEV; + } + + /* 查找电机 */ + MOTOR_DM_t *motor = NULL; + for (int i = 0; i < manager->motor_count; i++) { + if (manager->motors[i] && manager->motors[i]->param.master_id == param->master_id) { + motor = manager->motors[i]; + break; + } + } + + if (motor == NULL) { + return DEVICE_ERR_NO_DEV; + } + + /* 主动接收CAN消息 */ + uint16_t feedback_id = param->master_id; + BSP_CAN_Message_t rx_msg; + if (BSP_CAN_GetMessage(param->can, feedback_id, &rx_msg, BSP_CAN_TIMEOUT_IMMEDIATE) != BSP_OK) { + uint64_t now_time = BSP_TIME_Get(); + if (now_time - motor->motor.header.last_online_time > 100000) { // 100ms超时，单位微秒 + motor->motor.header.online = false; + } + return DEVICE_ERR; + } + + motor->motor.header.online = true; + motor->motor.header.last_online_time = BSP_TIME_Get(); + MOTOR_DM_ParseFeedbackFrame(motor, rx_msg.data); + + return DEVICE_OK; +} + +/** + * @brief 更新所有电机数据 + * @return 更新结果 + */ +int8_t MOTOR_DM_UpdateAll(void) { + int8_t ret = DEVICE_OK; + for (int can = 0; can < BSP_CAN_NUM; can++) { + MOTOR_DM_CANManager_t *manager = MOTOR_DM_GetCANManager((BSP_CAN_t)can); + if (manager == NULL) continue; + + for (int i = 0; i < manager->motor_count; i++) { + MOTOR_DM_t *motor = manager->motors[i]; + if (motor != NULL) { + if (MOTOR_DM_Update(&motor->param) != DEVICE_OK) { + ret = DEVICE_ERR; + } + } + } + } + return ret; +} + +/** + * @brief MIT模式控制 + * @param param 电机参数 + * @param output MIT控制参数 + * @return 控制结果 + */ +int8_t MOTOR_DM_MITCtrl(MOTOR_DM_Param_t *param, MOTOR_MIT_Output_t *output) { + if (param == NULL || output == NULL) { + return DEVICE_ERR_NULL; + } + + MOTOR_DM_t *motor = MOTOR_DM_GetMotor(param); + if (motor == NULL) { + return DEVICE_ERR_NO_DEV; + } + + return MOTOR_DM_SendMITCmd(motor, output); +} + +/** + * @brief 位置速度模式控制 + * @param param 电机参数 + * @param target_pos 目标位置 + * @param target_vel 目标速度 + * @return 控制结果 + */ +int8_t MOTOR_DM_PosVelCtrl(MOTOR_DM_Param_t *param, float target_pos, float target_vel) { + if (param == NULL) { + return DEVICE_ERR_NULL; + } + + MOTOR_DM_t *motor = MOTOR_DM_GetMotor(param); + if (motor == NULL) { + return DEVICE_ERR_NO_DEV; + } + + return MOTOR_DM_SendPosVelCmd(motor, target_pos, target_vel); +} + +/** + * @brief 速度模式控制 + * @param param 电机参数 + * @param target_vel 目标速度 + * @return 控制结果 + */ +int8_t MOTOR_DM_VelCtrl(MOTOR_DM_Param_t *param, float target_vel) { + if (param == NULL) { + return DEVICE_ERR_NULL; + } + + MOTOR_DM_t *motor = MOTOR_DM_GetMotor(param); + if (motor == NULL) { + return DEVICE_ERR_NO_DEV; + } + + return MOTOR_DM_SendVelCmd(motor, target_vel); +} + +/** + * @brief 获取指定电机的实例指针 + * @param param 电机参数 + * @return 电机实例指针 + */ +MOTOR_DM_t* MOTOR_DM_GetMotor(MOTOR_DM_Param_t *param) { + if (param == NULL) { + return NULL; + } + + MOTOR_DM_CANManager_t *manager = MOTOR_DM_GetCANManager(param->can); + if (manager == NULL) { + return NULL; + } + + /* 查找对应的电机 */ + for (int i = 0; i < manager->motor_count; i++) { + MOTOR_DM_t *motor = manager->motors[i]; + if (motor && motor->param.can == param->can && + motor->param.master_id == param->master_id) { + return motor; + } + } + + return NULL; +} + + +int8_t MOTOR_DM_Enable(MOTOR_DM_Param_t *param){ + if (param == NULL) { + return DEVICE_ERR_NULL; + } + + MOTOR_DM_t *motor = MOTOR_DM_GetMotor(param); + if (motor == NULL) { + return DEVICE_ERR_NO_DEV; + } + + BSP_CAN_StdDataFrame_t frame; + frame.id = motor->param.can_id; + frame.dlc = 8; + frame.data[0] = 0XFF; + frame.data[1] = 0xFF; + frame.data[2] = 0xFF; + frame.data[3] = 0xFF; + frame.data[4] = 0xFF; + frame.data[5] = 0xFF; + frame.data[6] = 0xFF; + frame.data[7] = 0xFC; + + return BSP_CAN_TransmitStdDataFrame(motor->param.can, &frame) == BSP_OK ? DEVICE_OK : DEVICE_ERR; +} + +/** + * @brief 使电机松弛（设置输出为0） + * @param param 电机参数 + * @return 操作结果 + */ +int8_t MOTOR_DM_Relax(MOTOR_DM_Param_t *param) { + if (param == NULL) { + return DEVICE_ERR_NULL; + } + + MOTOR_MIT_Output_t output = {0}; + return MOTOR_DM_MITCtrl(param, &output); +} + +/** + * @brief 使电机离线（设置在线状态为false） + * @param param 电机参数 + * @return 操作结果 + */ +int8_t MOTOR_DM_Offine(MOTOR_DM_Param_t *param) { + if (param == NULL) { + return DEVICE_ERR_NULL; + } + + MOTOR_DM_t *motor = MOTOR_DM_GetMotor(param); + if (motor == NULL) { + return DEVICE_ERR_NO_DEV; + } + + motor->motor.header.online = false; + return DEVICE_OK; +} diff --git a/User/device/motor_dm.h b/User/device/motor_dm.h new file mode 100644 index 0000000..20e91d4 --- /dev/null +++ b/User/device/motor_dm.h @@ -0,0 +1,98 @@ +#pragma once + +#ifdef __cplusplus +extern "C" { +#endif + +/* Includes ----------------------------------------------------------------- */ +#include "device/device.h" +#include "device/motor.h" +#include "bsp/can.h" + +/* Exported constants ------------------------------------------------------- */ +#define MOTOR_DM_MAX_MOTORS 32 + +/* Exported macro ----------------------------------------------------------- */ +/* Exported types ----------------------------------------------------------- */ +typedef enum { + MOTOR_DM_J4310, +} MOTOR_DM_Module_t; + +/*每个电机需要的参数*/ +typedef struct { + BSP_CAN_t can; + uint16_t master_id; /* 主站ID，用于发送控制命令 */ + uint16_t can_id; /* 反馈ID，用于接收电机反馈 */ + MOTOR_DM_Module_t module; + bool reverse; +} MOTOR_DM_Param_t; + +/*电机实例*/ +typedef struct{ + MOTOR_DM_Param_t param; + MOTOR_t motor; +} MOTOR_DM_t; + +/*CAN管理器，管理一个CAN总线上所有的电机*/ +typedef struct { + BSP_CAN_t can; + MOTOR_DM_t *motors[MOTOR_DM_MAX_MOTORS]; + uint8_t motor_count; +} MOTOR_DM_CANManager_t; + +/* Exported functions prototypes -------------------------------------------- */ + +/** + * @brief 注册一个LK电机 + * @param param 电机参数 + * @return + */ +int8_t MOTOR_DM_Register(MOTOR_DM_Param_t *param); + +/** + * @brief 更新指定电机数据 + * @param param 电机参数 + * @return + */ +int8_t MOTOR_DM_Update(MOTOR_DM_Param_t *param); + +/** + * @brief 更新所有电机数据 + * @return + */ +int8_t MOTOR_DM_UpdateAll(void); + +int8_t MOTOR_DM_MITCtrl(MOTOR_DM_Param_t *param, MOTOR_MIT_Output_t *output); + +int8_t MOTOR_DM_PosVelCtrl(MOTOR_DM_Param_t *param, float target_pos, float target_vel); + +int8_t MOTOR_DM_VelCtrl(MOTOR_DM_Param_t *param, float target_vel); + +/** + * @brief 获取指定电机的实例指针 + * @param param 电机参数 + * @return + */ +MOTOR_DM_t* MOTOR_DM_GetMotor(MOTOR_DM_Param_t *param); + +int8_t MOTOR_DM_Enable(MOTOR_DM_Param_t *param); + +/** + * @brief 使电机松弛（设置输出为0） + * @param param + * @return + */ +int8_t MOTOR_DM_Relax(MOTOR_DM_Param_t *param); + +/** + * @brief 使电机离线（设置在线状态为false） + * @param param + * @return + */ +int8_t MOTOR_DM_Offine(MOTOR_DM_Param_t *param); + + + +#ifdef __cplusplus +} +#endif \ No newline at end of file diff --git a/User/device/motor_lk.c b/User/device/motor_lk.c new file mode 100644 index 0000000..c26878e --- /dev/null +++ b/User/device/motor_lk.c @@ -0,0 +1,329 @@ +/* + LK电机驱动 +*/ +/* Includes ----------------------------------------------------------------- */ +#include "motor_lk.h" +#include +#include +#include "bsp/can.h" +#include "bsp/mm.h" +#include "bsp/time.h" +#include "component/user_math.h" + +/* USER INCLUDE BEGIN */ + +/* USER INCLUDE END */ + +/* Private define ----------------------------------------------------------- */ +#define LK_CTRL_ID_BASE (0x140) +#define LK_FB_ID_BASE (0x240) + +// LK电机命令字节定义 +#define LK_CMD_FEEDBACK (0x9C) // 反馈命令字节 +#define LK_CMD_MOTOR_OFF (0x80) // 电机关闭命令 +#define LK_CMD_MOTOR_ON (0x88) // 电机运行命令 +#define LK_CMD_TORQUE_CTRL (0xA1) // 转矩闭环控制命令 + +// LK电机参数定义 +#define LK_CURR_LSB_MF (33.0f / 4096.0f) // MF电机转矩电流分辨率 A/LSB +#define LK_CURR_LSB_MG (66.0f / 4096.0f) // MG电机转矩电流分辨率 A/LSB +#define LK_POWER_RANGE_MS (1000) // MS电机功率范围 ±1000 +#define LK_TORQUE_RANGE (2048) // 转矩控制值范围 ±2048 +#define LK_TORQUE_CURRENT_MF (16.5f) // MF电机最大转矩电流 A +#define LK_TORQUE_CURRENT_MG (33.0f) // MG电机最大转矩电流 A + +#define MOTOR_TX_BUF_SIZE (8) +#define MOTOR_RX_BUF_SIZE (8) + +// 编码器分辨率定义 +#define LK_ENC_14BIT_MAX (16383) // 14位编码器最大值 +#define LK_ENC_15BIT_MAX (32767) // 15位编码器最大值 +#define LK_ENC_16BIT_MAX (65535) // 16位编码器最大值 + +/* USER DEFINE BEGIN */ + +/* USER DEFINE END */ + +/* Private macro ------------------------------------------------------------ */ +/* Private typedef ---------------------------------------------------------- */ +/* USER STRUCT BEGIN */ + +/* USER STRUCT END */ + +/* Private variables -------------------------------------------------------- */ +static MOTOR_LK_CANManager_t *can_managers[BSP_CAN_NUM] = {NULL}; + +/* Private functions -------------------------------------------------------- */ +/* USER FUNCTION BEGIN */ + +/* USER FUNCTION END */ + +static float MOTOR_LK_GetCurrentLSB(MOTOR_LK_Module_t module) { + switch (module) { + case MOTOR_LK_MF9025: + case MOTOR_LK_MF9035: + return LK_CURR_LSB_MF; + default: + return LK_CURR_LSB_MG; // 默认使用MG的分辨率 + } +} + +static uint16_t MOTOR_LK_GetEncoderMax(MOTOR_LK_Module_t module) { + // 根据电机型号返回编码器最大值，这里假设都使用16位编码器 + // 实际使用时需要根据具体电机型号配置 + return LK_ENC_16BIT_MAX; +} + +static MOTOR_LK_CANManager_t* MOTOR_LK_GetCANManager(BSP_CAN_t can) { + if (can >= BSP_CAN_NUM) return NULL; + return can_managers[can]; +} + +static int8_t MOTOR_LK_CreateCANManager(BSP_CAN_t can) { + if (can >= BSP_CAN_NUM) return DEVICE_ERR; + if (can_managers[can] != NULL) return DEVICE_OK; + + can_managers[can] = (MOTOR_LK_CANManager_t*)BSP_Malloc(sizeof(MOTOR_LK_CANManager_t)); + if (can_managers[can] == NULL) return DEVICE_ERR; + + memset(can_managers[can], 0, sizeof(MOTOR_LK_CANManager_t)); + can_managers[can]->can = can; + return DEVICE_OK; +} + +static void MOTOR_LK_Decode(MOTOR_LK_t *motor, BSP_CAN_Message_t *msg) { + + // 检查命令字节是否为反馈命令 + if (msg->data[0] != LK_CMD_FEEDBACK) { + // 如果不是标准反馈命令，可能是其他格式的数据 + // 临时跳过命令字节检查，直接解析数据 + // return; + } + + // 解析温度 (DATA[1]) + motor->motor.feedback.temp = (int8_t)msg->data[1]; + + // 解析转矩电流值或功率值 (DATA[2], DATA[3]) + int16_t raw_current_or_power = (int16_t)((msg->data[3] << 8) | msg->data[2]); + + // 根据电机类型解析电流或功率 + switch (motor->param.module) { + case MOTOR_LK_MF9025: + case MOTOR_LK_MF9035: + motor->motor.feedback.torque_current = raw_current_or_power * MOTOR_LK_GetCurrentLSB(motor->param.module); + break; + default: + motor->motor.feedback.torque_current = (float)raw_current_or_power; + break; + } + + // 解析转速 (DATA[4], DATA[5]) - 单位：1dps/LSB + int16_t raw_speed = (int16_t)((msg->data[5] << 8) | msg->data[4]); + motor->motor.feedback.rotor_speed = motor->param.reverse ? -raw_speed : raw_speed; + + // 解析编码器值 (DATA[6], DATA[7]) + uint16_t raw_encoder = (uint16_t)((msg->data[7] << 8) | msg->data[6]); + uint16_t encoder_max = MOTOR_LK_GetEncoderMax(motor->param.module); + + // 将编码器值转换为弧度 (0 ~ 2π) + float angle = (float)raw_encoder / (float)encoder_max * M_2PI; + motor->motor.feedback.rotor_abs_angle = motor->param.reverse ? (M_2PI - angle) : angle; +} + +/* Exported functions ------------------------------------------------------- */ + +int8_t MOTOR_LK_Register(MOTOR_LK_Param_t *param) { + if (param == NULL) return DEVICE_ERR_NULL; + + if (MOTOR_LK_CreateCANManager(param->can) != DEVICE_OK) return DEVICE_ERR; + MOTOR_LK_CANManager_t *manager = MOTOR_LK_GetCANManager(param->can); + if (manager == NULL) return DEVICE_ERR; + + // 检查是否已注册 + for (int i = 0; i < manager->motor_count; i++) { + if (manager->motors[i] && manager->motors[i]->param.id == param->id) { + return DEVICE_ERR_INITED; + } + } + + // 检查数量 + if (manager->motor_count >= MOTOR_LK_MAX_MOTORS) return DEVICE_ERR; + + // 创建新电机实例 + MOTOR_LK_t *new_motor = (MOTOR_LK_t*)BSP_Malloc(sizeof(MOTOR_LK_t)); + if (new_motor == NULL) return DEVICE_ERR; + + memcpy(&new_motor->param, param, sizeof(MOTOR_LK_Param_t)); + memset(&new_motor->motor, 0, sizeof(MOTOR_t)); + new_motor->motor.reverse = param->reverse; + + // 对于某些LK电机，反馈数据可能通过命令ID发送 + // 根据实际测试，使用命令ID接收反馈数据 + uint16_t feedback_id = param->id; // 使用命令ID作为反馈ID + + // 注册CAN接收ID + if (BSP_CAN_RegisterId(param->can, feedback_id, 3) != BSP_OK) { + BSP_Free(new_motor); + return DEVICE_ERR; + } + + manager->motors[manager->motor_count] = new_motor; + manager->motor_count++; + return DEVICE_OK; +} + +int8_t MOTOR_LK_Update(MOTOR_LK_Param_t *param) { + if (param == NULL) return DEVICE_ERR_NULL; + + MOTOR_LK_CANManager_t *manager = MOTOR_LK_GetCANManager(param->can); + if (manager == NULL) return DEVICE_ERR_NO_DEV; + + for (int i = 0; i < manager->motor_count; i++) { + MOTOR_LK_t *motor = manager->motors[i]; + if (motor && motor->param.id == param->id) { + // 对于某些LK电机，反馈数据通过命令ID发送 + uint16_t feedback_id = param->id; + + BSP_CAN_Message_t rx_msg; + if (BSP_CAN_GetMessage(param->can, feedback_id, &rx_msg, BSP_CAN_TIMEOUT_IMMEDIATE) != BSP_OK) { + uint64_t now_time = BSP_TIME_Get(); + if (now_time - motor->motor.header.last_online_time > 1000) { + motor->motor.header.online = false; + return DEVICE_ERR_NO_DEV; + } + return DEVICE_ERR; + } + + motor->motor.header.online = true; + motor->motor.header.last_online_time = BSP_TIME_Get(); + MOTOR_LK_Decode(motor, &rx_msg); + return DEVICE_OK; + } + } + return DEVICE_ERR_NO_DEV; +} + +int8_t MOTOR_LK_UpdateAll(void) { + int8_t ret = DEVICE_OK; + for (int can = 0; can < BSP_CAN_NUM; can++) { + MOTOR_LK_CANManager_t *manager = MOTOR_LK_GetCANManager((BSP_CAN_t)can); + if (manager == NULL) continue; + + for (int i = 0; i < manager->motor_count; i++) { + MOTOR_LK_t *motor = manager->motors[i]; + if (motor != NULL) { + if (MOTOR_LK_Update(&motor->param) != DEVICE_OK) { + ret = DEVICE_ERR; + } + } + } + } + return ret; +} + +int8_t MOTOR_LK_SetOutput(MOTOR_LK_Param_t *param, float value) { + if (param == NULL) return DEVICE_ERR_NULL; + + MOTOR_LK_CANManager_t *manager = MOTOR_LK_GetCANManager(param->can); + if (manager == NULL) return DEVICE_ERR_NO_DEV; + + // 限制输出值范围 + if (value > 1.0f) value = 1.0f; + if (value < -1.0f) value = -1.0f; + + MOTOR_LK_t *motor = MOTOR_LK_GetMotor(param); + if (motor == NULL) return DEVICE_ERR_NO_DEV; + + // 根据反转参数调整输出 + float output = param->reverse ? -value : value; + + // 转矩闭环控制命令 - 将输出值转换为转矩控制值 + int16_t torque_control = (int16_t)(output * (float)LK_TORQUE_RANGE); + + // 构建CAN帧 + BSP_CAN_StdDataFrame_t tx_frame; + tx_frame.id = param->id; + tx_frame.dlc = MOTOR_TX_BUF_SIZE; + + tx_frame.data[0] = LK_CMD_TORQUE_CTRL; + tx_frame.data[1] = 0x00; + tx_frame.data[2] = 0x00; + tx_frame.data[3] = 0x00; + tx_frame.data[4] = (uint8_t)(torque_control & 0xFF); + tx_frame.data[5] = (uint8_t)((torque_control >> 8) & 0xFF); + tx_frame.data[6] = 0x00; + tx_frame.data[7] = 0x00; + return BSP_CAN_TransmitStdDataFrame(param->can, &tx_frame) == BSP_OK ? DEVICE_OK : DEVICE_ERR; +} + +int8_t MOTOR_LK_Ctrl(MOTOR_LK_Param_t *param) { + // 对于LK电机，每次设置输出时就直接发送控制命令 + // 这个函数可以用于发送其他控制命令，如电机开启/关闭 + return DEVICE_OK; +} + +int8_t MOTOR_LK_MotorOn(MOTOR_LK_Param_t *param) { + if (param == NULL) return DEVICE_ERR_NULL; + + BSP_CAN_StdDataFrame_t tx_frame; + tx_frame.id = param->id; + tx_frame.dlc = MOTOR_TX_BUF_SIZE; + + // 电机运行命令 + tx_frame.data[0] = LK_CMD_MOTOR_ON; // 命令字节 + tx_frame.data[1] = 0x00; + tx_frame.data[2] = 0x00; + tx_frame.data[3] = 0x00; + tx_frame.data[4] = 0x00; + tx_frame.data[5] = 0x00; + tx_frame.data[6] = 0x00; + tx_frame.data[7] = 0x00; + return BSP_CAN_TransmitStdDataFrame(param->can, &tx_frame) == BSP_OK ? DEVICE_OK : DEVICE_ERR; +} + +int8_t MOTOR_LK_MotorOff(MOTOR_LK_Param_t *param) { + if (param == NULL) return DEVICE_ERR_NULL; + + BSP_CAN_StdDataFrame_t tx_frame; + tx_frame.id = param->id; + tx_frame.dlc = MOTOR_TX_BUF_SIZE; + + // 电机关闭命令 + tx_frame.data[0] = LK_CMD_MOTOR_OFF; // 命令字节 + tx_frame.data[1] = 0x00; + tx_frame.data[2] = 0x00; + tx_frame.data[3] = 0x00; + tx_frame.data[4] = 0x00; + tx_frame.data[5] = 0x00; + tx_frame.data[6] = 0x00; + tx_frame.data[7] = 0x00; + return BSP_CAN_TransmitStdDataFrame(param->can, &tx_frame) == BSP_OK ? DEVICE_OK : DEVICE_ERR; +} + +MOTOR_LK_t* MOTOR_LK_GetMotor(MOTOR_LK_Param_t *param) { + if (param == NULL) return NULL; + + MOTOR_LK_CANManager_t *manager = MOTOR_LK_GetCANManager(param->can); + if (manager == NULL) return NULL; + + for (int i = 0; i < manager->motor_count; i++) { + MOTOR_LK_t *motor = manager->motors[i]; + if (motor && motor->param.id == param->id) { + return motor; + } + } + return NULL; +} + +int8_t MOTOR_LK_Relax(MOTOR_LK_Param_t *param) { + return MOTOR_LK_SetOutput(param, 0.0f); +} + +int8_t MOTOR_LK_Offine(MOTOR_LK_Param_t *param) { + MOTOR_LK_t *motor = MOTOR_LK_GetMotor(param); + if (motor) { + motor->motor.header.online = false; + return DEVICE_OK; + } + return DEVICE_ERR_NO_DEV; +} \ No newline at end of file diff --git a/User/device/motor_lk.h b/User/device/motor_lk.h new file mode 100644 index 0000000..f17dde0 --- /dev/null +++ b/User/device/motor_lk.h @@ -0,0 +1,119 @@ +#pragma once + +#ifdef __cplusplus +extern "C" { +#endif + +/* Includes ----------------------------------------------------------------- */ +#include "device/device.h" +#include "device/motor.h" +#include "bsp/can.h" + +/* Exported constants ------------------------------------------------------- */ +#define MOTOR_LK_MAX_MOTORS 32 + +/* Exported macro ----------------------------------------------------------- */ +/* Exported types ----------------------------------------------------------- */ +typedef enum { + MOTOR_LK_MF9025, + MOTOR_LK_MF9035, +} MOTOR_LK_Module_t; + + +/*每个电机需要的参数*/ +typedef struct { + BSP_CAN_t can; + uint16_t id; + MOTOR_LK_Module_t module; + bool reverse; +} MOTOR_LK_Param_t; + +/*电机实例*/ +typedef struct{ + MOTOR_LK_Param_t param; + MOTOR_t motor; +} MOTOR_LK_t; + +/*CAN管理器，管理一个CAN总线上所有的电机*/ +typedef struct { + BSP_CAN_t can; + MOTOR_LK_t *motors[MOTOR_LK_MAX_MOTORS]; + uint8_t motor_count; +} MOTOR_LK_CANManager_t; + +/* Exported functions prototypes -------------------------------------------- */ + +/** + * @brief 注册一个LK电机 + * @param param 电机参数 + * @return + */ +int8_t MOTOR_LK_Register(MOTOR_LK_Param_t *param); + +/** + * @brief 更新指定电机数据 + * @param param 电机参数 + * @return + */ +int8_t MOTOR_LK_Update(MOTOR_LK_Param_t *param); + +/** + * @brief 设置一个电机的输出 + * @param param 电机参数 + * @param value 输出值，范围[-1.0, 1.0] + * @return + */ +int8_t MOTOR_LK_SetOutput(MOTOR_LK_Param_t *param, float value); + +/** + * @brief 发送控制命令到电机，注意一个CAN可以控制多个电机，所以只需要发送一次即可 + * @param param 电机参数 + * @return + */ +int8_t MOTOR_LK_Ctrl(MOTOR_LK_Param_t *param); + +/** + * @brief 发送电机开启命令 + * @param param 电机参数 + * @return + */ +int8_t MOTOR_LK_MotorOn(MOTOR_LK_Param_t *param); + +/** + * @brief 发送电机关闭命令 + * @param param 电机参数 + * @return + */ +int8_t MOTOR_LK_MotorOff(MOTOR_LK_Param_t *param); + +/** + * @brief 获取指定电机的实例指针 + * @param param 电机参数 + * @return + */ +MOTOR_LK_t* MOTOR_LK_GetMotor(MOTOR_LK_Param_t *param); + +/** + * @brief 使电机松弛（设置输出为0） + * @param param + * @return + */ +int8_t MOTOR_LK_Relax(MOTOR_LK_Param_t *param); + +/** + * @brief 使电机离线（设置在线状态为false） + * @param param + * @return + */ +int8_t MOTOR_LK_Offine(MOTOR_LK_Param_t *param); + +/** + * @brief + * @param + * @return + */ +int8_t MOTOR_LK_UpdateAll(void); + +#ifdef __cplusplus +} +#endif \ No newline at end of file diff --git a/User/device/motor_rm.c b/User/device/motor_rm.c new file mode 100644 index 0000000..88c1ef4 --- /dev/null +++ b/User/device/motor_rm.c @@ -0,0 +1,321 @@ +/* + RM电机驱动 +*/ +/* Includes ----------------------------------------------------------------- */ +#include "motor_rm.h" +#include +#include +#include "bsp/can.h" +#include "bsp/mm.h" +#include "bsp/time.h" +#include "component/user_math.h" + +/* USER INCLUDE BEGIN */ + +/* USER INCLUDE END */ + +/* Private define ----------------------------------------------------------- */ +#define GM6020_FB_ID_BASE (0x205) +#define GM6020_CTRL_ID_BASE (0x1ff) +#define GM6020_CTRL_ID_EXTAND (0x2ff) + +#define M3508_M2006_FB_ID_BASE (0x201) +#define M3508_M2006_CTRL_ID_BASE (0x200) +#define M3508_M2006_CTRL_ID_EXTAND (0x1ff) +#define M3508_M2006_ID_SETTING_ID (0x700) + +#define GM6020_MAX_ABS_LSB (30000) +#define M3508_MAX_ABS_LSB (16384) +#define M2006_MAX_ABS_LSB (10000) + +#define MOTOR_TX_BUF_SIZE (8) +#define MOTOR_RX_BUF_SIZE (8) + +#define MOTOR_ENC_RES (8192) /* 电机编码器分辨率 */ +#define MOTOR_CUR_RES (16384) /* 电机转矩电流分辨率 */ + +/* USER DEFINE BEGIN */ + +/* USER DEFINE END */ + +/* Private macro ------------------------------------------------------------ */ +/* Private typedef ---------------------------------------------------------- */ +/* USER STRUCT BEGIN */ + +/* USER STRUCT END */ + +/* Private variables -------------------------------------------------------- */ +static MOTOR_RM_CANManager_t *can_managers[BSP_CAN_NUM] = {NULL}; + +/* Private function -------------------------------------------------------- */ +/* USER FUNCTION BEGIN */ + +/* USER FUNCTION END */ + +static int8_t MOTOR_RM_GetLogicalIndex(uint16_t can_id, MOTOR_RM_Module_t module) { + switch (module) { + case MOTOR_M2006: + case MOTOR_M3508: + if (can_id >= M3508_M2006_FB_ID_BASE && can_id <= M3508_M2006_FB_ID_BASE + 7) { + return can_id - M3508_M2006_FB_ID_BASE; + } + break; + case MOTOR_GM6020: + if (can_id >= GM6020_FB_ID_BASE && can_id <= GM6020_FB_ID_BASE + 6) { + return can_id - GM6020_FB_ID_BASE + 4; + } + break; + default: + break; + } + return DEVICE_ERR; +} + +static float MOTOR_RM_GetRatio(MOTOR_RM_Module_t module) { + switch (module) { + case MOTOR_M2006: return 36.0f; + case MOTOR_M3508: return 3591.0f / 187.0f; + case MOTOR_GM6020: return 1.0f; + default: return 1.0f; + } +} + +static int16_t MOTOR_RM_GetLSB(MOTOR_RM_Module_t module) { + switch (module) { + case MOTOR_M2006: return M2006_MAX_ABS_LSB; + case MOTOR_M3508: return M3508_MAX_ABS_LSB; + case MOTOR_GM6020: return GM6020_MAX_ABS_LSB; + default: return DEVICE_ERR; + } +} + +static MOTOR_RM_CANManager_t* MOTOR_RM_GetCANManager(BSP_CAN_t can) { + if (can >= BSP_CAN_NUM) return NULL; + return can_managers[can]; +} + +static int8_t MOTOR_RM_CreateCANManager(BSP_CAN_t can) { + if (can >= BSP_CAN_NUM) return DEVICE_ERR; + if (can_managers[can] != NULL) return DEVICE_OK; + can_managers[can] = (MOTOR_RM_CANManager_t*)BSP_Malloc(sizeof(MOTOR_RM_CANManager_t)); + if (can_managers[can] == NULL) return DEVICE_ERR; + memset(can_managers[can], 0, sizeof(MOTOR_RM_CANManager_t)); + can_managers[can]->can = can; + return DEVICE_OK; +} + +static void Motor_RM_Decode(MOTOR_RM_t *motor, BSP_CAN_Message_t *msg) { + uint16_t raw_angle = (uint16_t)((msg->data[0] << 8) | msg->data[1]); + int16_t raw_speed = (int16_t)((msg->data[2] << 8) | msg->data[3]); + int16_t raw_current = (int16_t)((msg->data[4] << 8) | msg->data[5]); + int16_t lsb = MOTOR_RM_GetLSB(motor->param.module); + float ratio = MOTOR_RM_GetRatio(motor->param.module); + + float rotor_angle = raw_angle / (float)MOTOR_ENC_RES * M_2PI; + float rotor_speed = raw_speed; + float torque_current = raw_current * lsb / (float)MOTOR_CUR_RES; + + if (motor->param.gear) { + // 多圈累加 + int32_t delta = (int32_t)raw_angle - (int32_t)motor->last_raw_angle; + if (delta > (MOTOR_ENC_RES / 2)) { + motor->gearbox_round_count--; + } else if (delta < -(MOTOR_ENC_RES / 2)) { + motor->gearbox_round_count++; + } + motor->last_raw_angle = raw_angle; + float single_turn = rotor_angle; + motor->gearbox_total_angle = (motor->gearbox_round_count * M_2PI + single_turn) / ratio; + // 输出轴多圈绝对值 + motor->feedback.rotor_abs_angle = motor->gearbox_total_angle; + motor->feedback.rotor_speed = rotor_speed / ratio; + motor->feedback.torque_current = torque_current * ratio; + } else { + // 非gear模式，直接用转子单圈 + motor->gearbox_round_count = 0; + motor->last_raw_angle = raw_angle; + motor->gearbox_total_angle = 0.0f; + motor->feedback.rotor_abs_angle = rotor_angle; + motor->feedback.rotor_speed = rotor_speed; + motor->feedback.torque_current = torque_current; + } + while (motor->feedback.rotor_abs_angle < 0) { + motor->feedback.rotor_abs_angle += M_2PI; + } + while (motor->feedback.rotor_abs_angle >= M_2PI) { + motor->feedback.rotor_abs_angle -= M_2PI; + } + if (motor->motor.reverse) { + motor->feedback.rotor_abs_angle = M_2PI - motor->feedback.rotor_abs_angle; + motor->feedback.rotor_speed = -motor->feedback.rotor_speed; + motor->feedback.torque_current = -motor->feedback.torque_current; + } + motor->feedback.temp = msg->data[6]; +} + +/* Exported functions ------------------------------------------------------- */ + +int8_t MOTOR_RM_Register(MOTOR_RM_Param_t *param) { + if (param == NULL) return DEVICE_ERR_NULL; + if (MOTOR_RM_CreateCANManager(param->can) != DEVICE_OK) return DEVICE_ERR; + MOTOR_RM_CANManager_t *manager = MOTOR_RM_GetCANManager(param->can); + if (manager == NULL) return DEVICE_ERR; + // 检查是否已注册 + for (int i = 0; i < manager->motor_count; i++) { + if (manager->motors[i] && manager->motors[i]->param.id == param->id) { + return DEVICE_ERR_INITED; + } + } + // 检查数量 + if (manager->motor_count >= MOTOR_RM_MAX_MOTORS) return DEVICE_ERR; + // 创建新电机实例 + MOTOR_RM_t *new_motor = (MOTOR_RM_t*)BSP_Malloc(sizeof(MOTOR_RM_t)); + if (new_motor == NULL) return DEVICE_ERR; + memcpy(&new_motor->param, param, sizeof(MOTOR_RM_Param_t)); + memset(&new_motor->motor, 0, sizeof(MOTOR_t)); + new_motor->motor.reverse = param->reverse; + // 注册CAN接收ID + if (BSP_CAN_RegisterId(param->can, param->id, 3) != BSP_OK) { + BSP_Free(new_motor); + return DEVICE_ERR; + } + manager->motors[manager->motor_count] = new_motor; + manager->motor_count++; + return DEVICE_OK; +} + +int8_t MOTOR_RM_Update(MOTOR_RM_Param_t *param) { + if (param == NULL) return DEVICE_ERR_NULL; + MOTOR_RM_CANManager_t *manager = MOTOR_RM_GetCANManager(param->can); + if (manager == NULL) return DEVICE_ERR_NO_DEV; + for (int i = 0; i < manager->motor_count; i++) { + MOTOR_RM_t *motor = manager->motors[i]; + if (motor && motor->param.id == param->id) { + BSP_CAN_Message_t rx_msg; + if (BSP_CAN_GetMessage(param->can, param->id, &rx_msg, BSP_CAN_TIMEOUT_IMMEDIATE) != BSP_OK) { + uint64_t now_time = BSP_TIME_Get(); + if (now_time - motor->motor.header.last_online_time > 1000) { + motor->motor.header.online = false; + return DEVICE_ERR_NO_DEV; + } + return DEVICE_ERR; + } + motor->motor.header.online = true; + motor->motor.header.last_online_time = BSP_TIME_Get(); + Motor_RM_Decode(motor, &rx_msg); + motor->motor.feedback = motor->feedback; + return DEVICE_OK; + } + } + return DEVICE_ERR_NO_DEV; +} + +int8_t MOTOR_RM_UpdateAll(void) { + int8_t ret = DEVICE_OK; + for (int can = 0; can < BSP_CAN_NUM; can++) { + MOTOR_RM_CANManager_t *manager = MOTOR_RM_GetCANManager((BSP_CAN_t)can); + if (manager == NULL) continue; + for (int i = 0; i < manager->motor_count; i++) { + MOTOR_RM_t *motor = manager->motors[i]; + if (motor != NULL) { + if (MOTOR_RM_Update(&motor->param) != DEVICE_OK) { + ret = DEVICE_ERR; + } + } + } + } + return ret; +} + +int8_t MOTOR_RM_SetOutput(MOTOR_RM_Param_t *param, float value) { + if (param == NULL) return DEVICE_ERR_NULL; + MOTOR_RM_CANManager_t *manager = MOTOR_RM_GetCANManager(param->can); + if (manager == NULL) return DEVICE_ERR_NO_DEV; + if (value > 1.0f) value = 1.0f; + if (value < -1.0f) value = -1.0f; + if (param->reverse){ + value = -value; + } + MOTOR_RM_t *motor = MOTOR_RM_GetMotor(param); + if (motor == NULL) return DEVICE_ERR_NO_DEV; + int8_t logical_index = MOTOR_RM_GetLogicalIndex(param->id, param->module); + if (logical_index < 0) return DEVICE_ERR; + MOTOR_RM_MsgOutput_t *output_msg = &manager->output_msg; + int16_t output_value = (int16_t)(value * (float)MOTOR_RM_GetLSB(param->module)); + output_msg->output[logical_index] = output_value; + return DEVICE_OK; +} + +int8_t MOTOR_RM_Ctrl(MOTOR_RM_Param_t *param) { + if (param == NULL) return DEVICE_ERR_NULL; + MOTOR_RM_CANManager_t *manager = MOTOR_RM_GetCANManager(param->can); + if (manager == NULL) return DEVICE_ERR_NO_DEV; + MOTOR_RM_MsgOutput_t *output_msg = &manager->output_msg; + BSP_CAN_StdDataFrame_t tx_frame; + uint16_t id = param->id; + switch (id) { + case M3508_M2006_FB_ID_BASE: + case M3508_M2006_FB_ID_BASE+1: + case M3508_M2006_FB_ID_BASE+2: + case M3508_M2006_FB_ID_BASE+3: + tx_frame.id = M3508_M2006_CTRL_ID_BASE; + tx_frame.dlc = MOTOR_TX_BUF_SIZE; + for (int i = 0; i < 4; i++) { + tx_frame.data[i*2] = (uint8_t)((output_msg->output[i] >> 8) & 0xFF); + tx_frame.data[i*2+1] = (uint8_t)(output_msg->output[i] & 0xFF); + } + break; + case M3508_M2006_FB_ID_BASE+4: + case M3508_M2006_FB_ID_BASE+5: + case M3508_M2006_FB_ID_BASE+6: + case M3508_M2006_FB_ID_BASE+7: + tx_frame.id = M3508_M2006_CTRL_ID_EXTAND; + tx_frame.dlc = MOTOR_TX_BUF_SIZE; + for (int i = 4; i < 8; i++) { + tx_frame.data[(i-4)*2] = (uint8_t)((output_msg->output[i] >> 8) & 0xFF); + tx_frame.data[(i-4)*2+1] = (uint8_t)(output_msg->output[i] & 0xFF); + } + break; + case GM6020_FB_ID_BASE+4: + case GM6020_FB_ID_BASE+5: + case GM6020_FB_ID_BASE+6: + tx_frame.id = GM6020_CTRL_ID_EXTAND; + tx_frame.dlc = MOTOR_TX_BUF_SIZE; + for (int i = 8; i < 11; i++) { + tx_frame.data[(i-8)*2] = (uint8_t)((output_msg->output[i] >> 8) & 0xFF); + tx_frame.data[(i-8)*2+1] = (uint8_t)(output_msg->output[i] & 0xFF); + } + tx_frame.data[6] = 0; + tx_frame.data[7] = 0; + break; + default: + return DEVICE_ERR; + } + return BSP_CAN_TransmitStdDataFrame(param->can, &tx_frame) == BSP_OK ? DEVICE_OK : DEVICE_ERR; +} + +MOTOR_RM_t* MOTOR_RM_GetMotor(MOTOR_RM_Param_t *param) { + if (param == NULL) return NULL; + MOTOR_RM_CANManager_t *manager = MOTOR_RM_GetCANManager(param->can); + if (manager == NULL) return NULL; + for (int i = 0; i < manager->motor_count; i++) { + MOTOR_RM_t *motor = manager->motors[i]; + if (motor && motor->param.id == param->id) { + return motor; + } + } + return NULL; +} + +int8_t MOTOR_RM_Relax(MOTOR_RM_Param_t *param) { + return MOTOR_RM_SetOutput(param, 0.0f); +} + +int8_t MOTOR_RM_Offine(MOTOR_RM_Param_t *param) { + MOTOR_RM_t *motor = MOTOR_RM_GetMotor(param); + if (motor) { + motor->motor.header.online = false; + return DEVICE_OK; + } + return DEVICE_ERR_NO_DEV; +} \ No newline at end of file diff --git a/User/device/motor_rm.h b/User/device/motor_rm.h new file mode 100644 index 0000000..670b427 --- /dev/null +++ b/User/device/motor_rm.h @@ -0,0 +1,132 @@ +#pragma once + +#include "motor.h" +#ifdef __cplusplus +extern "C" { +#endif + +/* Includes ----------------------------------------------------------------- */ +#include "device/device.h" +#include "device/motor.h" +#include "bsp/can.h" + +/* Exported constants ------------------------------------------------------- */ +#define MOTOR_RM_MAX_MOTORS 11 + +/* Exported macro ----------------------------------------------------------- */ +/* Exported types ----------------------------------------------------------- */ +typedef enum { + MOTOR_M2006, + MOTOR_M3508, + MOTOR_GM6020, +} MOTOR_RM_Module_t; + +/*一个can最多控制11个电机*/ +typedef union { + int16_t output[MOTOR_RM_MAX_MOTORS]; + struct { + int16_t m3508_m2006_id201; + int16_t m3508_m2006_id202; + int16_t m3508_m2006_id203; + int16_t m3508_m2006_id204; + int16_t m3508_m2006_gm6020_id205; + int16_t m3508_m2006_gm6020_id206; + int16_t m3508_m2006_gm6020_id207; + int16_t m3508_m2006_gm6020_id208; + int16_t gm6020_id209; + int16_t gm6020_id20A; + int16_t gm6020_id20B; + } named; +} MOTOR_RM_MsgOutput_t; + +/*每个电机需要的参数*/ +typedef struct { + BSP_CAN_t can; + uint16_t id; + MOTOR_RM_Module_t module; + bool reverse; + bool gear; +} MOTOR_RM_Param_t; + +typedef MOTOR_Feedback_t MOTOR_RM_Feedback_t; + +typedef struct { + MOTOR_RM_Param_t param; + MOTOR_RM_Feedback_t feedback; + MOTOR_t motor; + // 多圈相关变量，仅gear模式下有效 + uint16_t last_raw_angle; + int32_t gearbox_round_count; + float gearbox_total_angle; +} MOTOR_RM_t; + +/*CAN管理器，管理一个CAN总线上所有的电机*/ +typedef struct { + BSP_CAN_t can; + MOTOR_RM_MsgOutput_t output_msg; + MOTOR_RM_t *motors[MOTOR_RM_MAX_MOTORS]; + uint8_t motor_count; +} MOTOR_RM_CANManager_t; + +/* Exported functions prototypes -------------------------------------------- */ + +/** + * @brief 注册一个RM电机 + * @param param 电机参数 + * @return + */ +int8_t MOTOR_RM_Register(MOTOR_RM_Param_t *param); + +/** + * @brief 更新指定电机数据 + * @param param 电机参数 + * @return + */ +int8_t MOTOR_RM_Update(MOTOR_RM_Param_t *param); + +/** + * @brief 设置一个电机的输出 + * @param param 电机参数 + * @param value 输出值，范围[-1.0, 1.0] + * @return + */ +int8_t MOTOR_RM_SetOutput(MOTOR_RM_Param_t *param, float value); + +/** + * @brief 发送控制命令到电机，注意一个CAN可以控制多个电机，所以只需要发送一次即可 + * @param param 电机参数 + * @return + */ +int8_t MOTOR_RM_Ctrl(MOTOR_RM_Param_t *param); + +/** + * @brief 获取指定电机的实例指针 + * @param param 电机参数 + * @return + */ +MOTOR_RM_t* MOTOR_RM_GetMotor(MOTOR_RM_Param_t *param); + +/** + * @brief 使电机松弛（设置输出为0） + * @param param + * @return + */ +int8_t MOTOR_RM_Relax(MOTOR_RM_Param_t *param); + +/** + * @brief 使电机离线（设置在线状态为false） + * @param param + * @return + */ +int8_t MOTOR_RM_Offine(MOTOR_RM_Param_t *param); + +/** + * @brief + * @param + * @return + */ +int8_t MOTOR_RM_UpdateAll(void); + +#ifdef __cplusplus +} +#endif \ No newline at end of file diff --git a/utils/Simulation-master/balance/series_legs/slprj/_jitprj/s1PQAPIlnTPKm6mZzJoZZuB.l b/utils/Simulation-master/balance/series_legs/slprj/_jitprj/s1PQAPIlnTPKm6mZzJoZZuB.l new file mode 100644 index 0000000000000000000000000000000000000000..0784ce8ae32378cba77144745657adeff78dab4f GIT binary patch literal 10288 zcmcIp3s_TEw%#WR$pJzLFAWm#L{QP8V|b__a7jqOSg{7Aj^5fP0Yjxih$JfZ#R-W9 zEsoe>EY?2Iacsx-8#7bu*iNSeLLJ6pMQkfouLZ@|l(t&2(+(nc?UO{4sPnkr-2LU` zWdD2Zwbx$j-)rx4lCN-{n;nf%5JISoFHL%Q`G*yM8a;R*w3sd;0F&?$%Hbgt%90=! z)F;5jvm|bpcXJkSg=twY5;p$SATleag^=IPGJs= z+u0^Q-4;JG5PzDAAEpH(PQjJ7us(@k2!)SO`1`ao{*ugtS1@=>f^bTbjUPLMV6C!A zQ&J=P4wkc!j<#|TqF_uIw?CC|O1tF17MLRfe7F_Sq9F?CgslR6T@h0AP0j9B1bFr| zB<0Rixm#(3==dlkR>UU(0If2FoIejn!VUo!04Nkf+COv9W+RWy)bT(LFhlPQFe7a! zz>L@tA2T+UqEFrZJ>SmekhBB}Tj(vKlI2Ta20lfxKZBY=`2a{%@-8tA%)DEi&HJNY z3&xYZQ#O?z?0rGwk8l)`I}m679{K#i7@O;ht7}?cd;X7?4^%WC<%hJF#1<_L z{bQ%`hZk-?|ISA>x86Ei(cWKq{jK(Q|MeSCntMuHBRQdnp z?t9})!=#v_P5l>dtbfh$l_66UyE=zgHa<5#PHWpcccgIXv^Nxg`~JwZ?>D`%q^qLz za^cA56Lwr$nl$}y)8;RT)Rotm_EyQ2+oMkWezUx2I9jq*-jl$WETH9h(!YaT^d8*P zNm|@BD6WVmavK$yHdj+V&D&7NGqtYaUqazu3k4&zAf2C|(Ag3H$!>9lG>8$i#`Ums zyA`=j?A)HZCbN|44yUJb*-?ZJ>Y&U9o>ymlv zeh31!>uI}&w677_Yg_FbS}iHxToZgVAh_ui3`^N%-Mhs@yTv`s_gS6zs6^ali9f32 ze%`Fe>Q-d6GT(a=#V$Zl@4 z$9}RT{2LVhomw!|EVv*GAEX4sFbgU<{%l7=r#zugB0g(LI6EL78jSxdkx(Hj&U7f| zy<*MoQ73sOqP4HDVAME~*7Lq=_AU@`RzmiT#7| z=fD6}Y_`Lid(awZb7eLuvU*%OhdZ*mT`Ae3v91B6vkI zxq^B%`vXPpVRlxFHLGXY2M~Rj?9*VU%D&!ckNx5{-TXTz?3-9vV`c z3>@M;z0BAOcH<$ldR+~d#7B0AU6EsoLmzi+CS+XD3NEW5Y%>fcO zyKutNQE4`$cu66YySPdx2T5dC zE(?o_XW8k7O<|4H1QtC3i=^y-Nnyky(ojMsz*=^Gc1+TH5VSa1^b{;|`0ZVa-ZdaX zgGe+MnSAP)1hEbMG-;NgieT3Jv_qaLL@8ij;q2TcBXKdpF;(IsS)#Ds@W{#Zg7#wK z#I}q+P6q4xmxnU^1lpfNpC=#6>!VH{7xrk#V~aDE*uMB|r07IVsca;1YX-4ZcI*Av z1;?Yu3WJn+eX6Klva3eruk+MC|3SwMkNXSUm?#rYOk4h`zuHVNmnk|5&gJmS8w>1b z&W(xr zH=>S3>y9M)$+X`r@|%uEgXAl^J-eokg0Vrb*A{#rQ~uIP%2K1TSvdY5xow({dXy-7 za&xDHh!A526!Jq5r$hU)mFD8GHJ3wtHX({lCUUu`L96~k?&u|K&kKl?{5WBU5bZ^} zE+>)+`TKK7@qg33$0al-kBFt+LG>Cbw@(iF&ySnrXb%n>*2JCg_vN9~MSgQS@L03I z0B=ANmNpjIsSfQ+&O3k0DOK8%#+2y=t+8Rl^}8N%zTD@${bP`@Yh%$Se}d!*@U)rr z5)bz%j+|L9J~8t6L+RrWA<;G{V-5-d_J5w}X#Y{=XKFo)1NP;qdcye%rK)dR#zrP+ z`=T7oz@D-3(Z?GTd{DtL!R-)_i$3va@y0_MMW~;bg}|yYa-2mR*^k5w?DYufFadXn z(iKB<2C{@uO`>Q-Hgsh0IvV9{ni6SEQqD*4M72k)+o)sjAz9k4HXNosSu$8_Gv8*` zS`v(XL_v-&-dUVDJkC}s(>J>`XSPd*^KPEmw~`#)Hg@^g934jyCyLHrd#o6ge7{SLRRu=z^LM;r+69|HPd!83mCB9q zI~==%iPH+Wdwqpz9o>Y4kFpX`f$WaYZn}N;Fpk-$Zl5I+*k|>O&)OKDjWIs6M1s$r zl0l_MxeR1kVX`ZKl_nnr+LJ`@XO?BHOvY<_OVN=6?!LxBGh|c{iP_D#ZC@@~$QQ!O z82Br@jBL@d^MmhrGlF`u zoL-P;m`AM6KH;0}bn`*%q(yc9+L>nO?axm_RQSqtNl?CW$H?8^_0=Wd%Kq1PZBHD| zOA{WmMUxR@n@VvA3zPg4$Lz4-8n?#_Uho(&hMf*YMz`1>X?quHD0TF+6UIn*W!bt6`+tc~SOBJauDJbYCGUkMiQbw#aOXqnN= zg4ANU0eKR_LEZ6iw$`1NsQ0ZEp0t8yfSm!`K)oRcIMlbY&_8j1Imb+n&RSGo4ZdA; z*4MY!$2)K5!KwNW0Tdr2hil~E&%Tk#^z?|6vm`yU7CDu-I&aUiU0+BtTwe#vWAWJG z;F!+aOWKKmt)+lHdWP98m~4A5pV?r$m(QH0JNq&CmdVE+-->;FEA#pm*1sIDe`W(` zf&qvn0=c+d;QvMo0luuNq^U$M57M>s*F;1zG#NQ425;R-Awe=x>M|tcSPfw+$+8{# z<%N6!(3)OtO*E1OO9IWA(9Av%#+eQaz@w%^9;tkXLrj(RQ+Urx2K$e=^TX`;&f5!5 zz7ENEQsm$KHr#f{_;E}Bcy$1sWvtOS$T000W9|%d2)nR+oT%fbLT10!$0_QXH9Vd# zYPU)AP83B{$ou>vb|!MlaQ(IN@#=sepTa?&e5=2n;H;70Jrx%nh0y<3zE4=QDDMKl zYw-%(wb0Ra;SnGSK6dPU#EDmpdu$=ZKZ>Jd_8R<-4##{pOU)`!iwlZ#Hmdo0bqK`2 zd3GW7{&Ok#^L1WRL{HZ0Pw;0nyqrUk4fxYN{;`N~|0KT~mlo2$`pLulzn;8rimG=W z40m_`coeVA7WdjL1^zG_#NFnFKFv~wUheIs7wFYB$+OqccPeJI`*lUzqG+`HfTE)s z@*>5NY3zH>z}atrq(2ncPZcBEhXPXcEYpVMz@B7Gt^?t$E%+=zv48sug_^6!Y2!&D zQWBug$bCY?Bg1;NaI&zWwor0R%$hA@htM)h%qn(%p4}PNPqQescH>W2@1>LH8`9e# z8SooWo7B;1RM1?`R!hvLxP}a&fn2vkB5Z)|jem|ph5SiCE=8!jK zgb*5Nl3v~*cUgd2X1FTm_@(-KgEk-&PFF`er$p@7;U_jeYq);FSC)fa)vU&^Sl5O&A-Da}`fB3V(7DNdZ4x~=*N$gXplvv>^71q)BsSlv?J z9$Jq#yqndl`tMP7@Lj#1J>#LHn!Z0Rg6tKV_q_(C7DVf6y1H`Sl|wetv>yXpQ~&rG z9lC!`D`hr^$IeQI$8VlGxMl{I9rDCKv@Fl z3$S@<;E6JiH98r^NnPJ9>{9-NDJ<^=rw03zttKwuSqtXndxZHElPZn{ej3mv`8U?lq7I_kWvnGX9(1=TQ6_IKY3f2F8h1Jq|c@C{Oww zI`Y_~r!ECNa9`yNGmnv=Mcw9jwcfU^=TfL#xu z8#DfN(D#4w2~B#R7kS){6ghs(W!^*F>mMdZJO+F@6tD=H2i_=FimK4>SnSW5HQ*yW z=QM$EiY;_YP=D_eba*p2Q&&w#-{dwD&(g84k%#5mXsRz*6!|(m)yiL0dz7C3F~70z z6ME*a!w%=4r60N)cICw@pamg4{B@BI2{4k~(gw>4vEe!l6v6Mvir~VzaT#2=;5&X# z4{xpf%7-BD3;DKVd0aX{2HTAbey=tT1;NAeNx*p?43CL%67bW2!=_0G&v=eabKd(K{d+@yh_!hvi zUk(6{^O0BnRS(|bgX4HP0LOAC0mppA@)+*~9CK<2aQrOh#lHd^=LA0RYnl%p2_(e! zrvdJ@AICQta6I4nfcwr@4mg(60FLA7)xXq(n|yGrXEorMhjzfdJj8Z30*=T172w`+ zG5#9hI8NSz_0wR$TMxhwKLG#e0eIg7@Gl>L4+DgdV82u|v)n2_|Sw&6xTuqrqk5y}`b+x8dVBi9SsRHb()2v); z^!%!D_w>`7H825<$*eQ$H6T!>e-Z)Ig z#)8#4b8Tgvr?bvvf|+_MZW&L-LbsGXeFvDC~t@o;g znOCn>)~qe7(l3EH>1yThzw63MlMVo%o_a$7P`$EBQx3s2tTk$C*BY=&h_0z-ZEd;U zVeXYRhNb#)b4{%YcQxy4t1Auueq~gebr3rrUxWV{H&mLy z6VMCTfg@iDx2N?Qqpnt0?Q51#+g&B8bAKxR=^QfzS`$cNjhn*^9zbFZX0e;^_o1;S zfXAADNMv{G1DNa{Baq723HNdthV_!!`_s*4Z*8TU&bW6VpK-kIOI2@Q52G~?qO?~o zkkek=O=@q=E#<9wXpMt-UtVLmdx_1U9%g$r;fQJO<+kQta%=8kx8`4^HzWAZ`3*MR zhu}=-`!U?DHGtwwNCG*|aNvV1G4QomZNXQTOFn*5(7 literal 0 HcmV?d00001 diff --git a/utils/Simulation-master/balance/series_legs/slprj/_jitprj/s1PQAPIlnTPKm6mZzJoZZuB.mat b/utils/Simulation-master/balance/series_legs/slprj/_jitprj/s1PQAPIlnTPKm6mZzJoZZuB.mat new file mode 100644 index 0000000000000000000000000000000000000000..d1eaf63428f97b476292cc34fde6eb99df737b66 GIT binary patch literal 1249 zcmV<71RncMK~zjZLLfCRFd$7qR4ry{Y-KDUP;6mzW^ZzBIv`C!LqRq)EFeR2Wnpw> zWFT*DIv`VZZXilwZXh5uATc#MF*G_dGaxcBGBzL}ARr(hARr(hARr(hARr(hARr(h zARr(hARr(hARr(hARr(hARr(B00000000000ZB~{000141ONbdoYh#}ZsSB0o>Xnc zE^O7k+C@UJmk0@Ip%N?-LfhsC0xb>ebXV$4HL*|PA>*+-KWS55fd}CsxZn-ol1Jf^ zGmf3QY1+(qg<34R?%4Cqcjoi?bG8T}y{Ck1;`JRo*U0SHD8`M-`3TRSnQn^_6VMgI zG@rLlwu|S_@XS)Bn`s0i5c!NW(=1^ZRI^m5LqWE1&TmiL_(-w-*TwkPhlGSNmAc^e zJ;Iq?ff)|jxhGbbq(>!9R1IE16LgcV%Ok6YhY*m9$=+d)K6biX`{e1l8j)d1`2F56%liP!;=M6o^XG}*s9gq-*@D@j_nE!a!5mP&V7YSDw5PQ*^*B8dcKy zE}d5ZQ5#Cs(1_MqD#xg?Z<4*#%0_}88ln-jKtrA|+UVV{DL0NbCkiv1b&n&38Ox6s zu{M5cfkPU`5ZQXdI2?eo3%I^wWlPa6?smfTv;``ZEOU({qaeaUVpZx-GdL1sk)DZ; zOp~DK2yVtk0GKtX6j&Dauq?@G2aR{LTEc1HdRCqGETNjyf%T1VL11OEd$pC2h8R_C zfvp=F^fuUlU0`V!iYbll)U=()Kt#aC-YW==R7G#*m*8zb`?TTL&ps~3JEV-ouIfvC z(-h0Nz)ov-kT;9aX9?st0*XD!Y*^uaPA+*LtliGLhW|Bc;_C91xR({&`VxH}R&d+J zIQ;U2!{`KCy5;+Q&p~Gw9Jz+OpZxOI`k!yZ|A(g^Zx!PYNYU5LE8pZ=aj{xx)`)Rk= z<)YvFA^9RXzW6>pJ~n&#dfT4%miM3GsWCCy`$~r0h|Ipx?3kL_x5J{2n=978DZX5A z8wKvg3Vk*g^s(bCW}lzzT=m!co)_*a58WlsC0{Mm<-f$$wezm_=bMNB@AIoVE#?EV zFn{|V?_=>=w;tcOef?dT?^1t0D#qWC@*BGuGafDJ@{7mVh!2-Ny{CHk&i}_#zp_7Q zTm6b336FPn1?5w0>+@;l?Vhy1Pt6}mplbQI(_Pes^17K~U?e1HRCtY|ovn5uKu{_qAqk3YS8fO; z*kYxQcJyNw{n@VeZCCmZoon4oAk;C7f@mufrl8i^jdfVjZmpI*=O)qIh5A_=>qJANkMH-+kNuUica+j{r>0M<|zzP&iAD zSkRscC)W}Gf~tqJB2<)|zK5veKM=x;j^#X8L`CJYa`uGhbc>QDX}qXaVaY`eT1&S) zUNha*C{pcVmGto5myqgZ4dfq)6{D9niVlyF*k;kx<| z^ejR)PN6OoRoA1HVwlA`BY{SC?EQR4*!u_pj*g?u)$*B$w}6G**E9PH8bYe6IE_-g z|1JI+$&67Vl$A^#d8Y8CEy?Nc&zEX4g=jcOu`2c5cLpVb*|F%zvA3wJ%V(FdW;>LB z0OyQ5qD(4uIRG^kXtsS0v7OXQ5GW1}L_wE}d) zs3@{d*&^ac^7ktQb6+MCJavUH>lFD0X(Wq_V!B$T=UQbGBeHX(Y>X02xCK{RBL?Jx z%Sbpu;@@*_*%hS^FQxJ3SW(61JggeE7mF!vq47hVp^7S!=5qFyS zhJSG7XeAX06>4+CWgJc*rhdD6ge39g{72NC#eX?*OuI|u*Z6V5$%TKJ|=J7Z2X&d0lldclm!V3S6*4-Q4{ z{%v76kIqsbUcc_GflW#T>Fqxh2i|@$l+AMc+u(Egi$~I;MSbGBiB$W(=qShYFZzlq6ayLD0-XFmTW&OH$;wzMa-4qAD`o&ImP~dLm1qV*6 zeLS!im}FF!^)n!M)1P;rYz;I3&n|tvB*@`6=LgR&w_ttn?ArG~L?Xdm{ZVUgtUr6V z33tGO-@F6GB;c64geN(lj!+K^TQLXDm&A{qm#twzZrT zJf}Av{_U4fpSWcG;NSD>a!!yhMoVfNtG;g8`}Bv#bqOg&KkvJ+Fw;yqt47ac1G{GWLqKk$t~QaTU=K7rCgRK zm!*)qErGl3?;)UeJLS^zT$NESYl~|~izD&Nalw}(f*WqZn1Wr@vrl??pR||$9X3dh z%cb28*>OYYr%meg9(8&P{d>fd)~rrzU}rRY7QvvmYBSETGuk}--

88(db4)4qjU zUFfn$x$Ld3=#vG)FOl#oo#1kl;F3}}N(#nc7G#2KxHGm(6+0l84m)CpN2HfWWq&4O zOQY7LwW%L^&6&}wo9mlMp=*0Jx4LCb-b9lSdm#4l*y9H2Rk`eZr|fAW78@*=jT=JO zJ>yL4^<=cEv%n#@x-u`TGa8`jY5XV*X6o7w_VBn*Byg=Qj!lo7Saobv;Ev-}jq|%tlXUvnQ=z{SeTp7d*z-%4g&lV!_mtowBtK2|5T| z{~>TaJj|K^9O6E)!O{YD;~~@gJ+)V)$M!{BRbh%lA8%}Crd}!(e6E9#3oqlq3qRKh zCfXxhAb6Rwe*oJ3s8@Ve97Og(@!eLUqfi8@`PDr?q6OE^S4@<@x(t4mVS?3MOJ^h;D1+^OfRvuGk$1|xPB{_3 zE0x%#y!AoIiayEhWg(jE0c~VIuiLC<>OA}KV#c78rVUJi>$8-iS;^6-nQGI(T>3DO zuqTE)e=Kl(CYotq7l5zJl^iwFp*pdXg;a{dVY1}`UaAyUK)eTB7ReZz+qA3b+vVLk zX!^Aj&h=S2*CS6z45jf5nT|i@F{Y!}Bh{uJ-`2!HBD4wg`fb5W*hC|0R*EIS29`hm zVABfJt3iFTrmi+ZJQp)y89xkhI^8cjsZboYnI(f1uO5+XUVLaMsx3r+RJr?!x`l$y zOZ*s7rwAQDhHf`fiukRWytqfH{*UA>`yuR6EJ{i?MnxfBEpdiR2ZLqm;+Z*+iJ{8W(bBta~V#k$&bl*zjDg`-ETk9 z1bUFx5v z%=mAMZ|fMHG!MqUSvHz+oT} z`Uzb9KEvrFX+-@6tay~GyyLr{d40DM`);+@ckpfrzS}|ju9f!PW!iU+>EJu#I5c`S z8jxj0D6f8|NH`AZ#UuJZc5G;Y+z{~r)*8GH7S0M$ z!f>oO7OW8Lr9kbCL&8wNVef?$-$xImofrPjg)9?Q7W{9Z!nLL=OH>OX+)gM!T zPNlGPtXv(B(+JOC|KY1g^!nqgM{@(}kusnjsc=2=cUZ}$!N)@?j0y*#^CxyqVW!$bAI#}3f zHAlW2nbrAgAlyF9PO<|i zmKQ)V1#psH1idSwlH=PTDN~%@-SOTxYSc&trJM}$?vAyh5M?VF79Sb{G-*4evgpXm zXy0>cYP|$GY7@M5sB;OobG>6&>~14eA%t=U-W}Qo15*nBOtDB<{0TAoHOEoUCK|wZSD2|^6TFqW zFz+{0vT{pzcjnP)n02YS?vmZu8jOmA3yk_VxU3n9dSfBm%+26hB(^+MD>`#p0CSj~T$6!@OK8)2) z1icw;2Jpk3xbNA%+czp!4o%Vb4A6r<3?xGFaOclC0`7h9JyY&^@4RCuFV*_@`|yPq z1=TOS2mXvdrjL1^S$bes$Zd*f;Bdm3Yaty#JT^WIB>b-cuKydG#L%Bhi6cab-LM*W zh>k6i_u}`;N0_7?+~xl6rIfasiZo`)1bYAE&KnuF2evd|&-Z^Xz~5%4@X0;z1!gz@ z<3)*_eqG<3H7Dv6-E+iy)VVDAZHQjD;nwsnyxVO#MZZ>qMz3#{z}4>?de4R54d(~$ zx#(rqx95t&?}b`iB}US|S<2z%gy5PIHqas8@RE1EYd!4b`8PbVpgYL!n<>oXtw?8m zJ9N_h-JxsDcUbcrWsJh})v9hyR*hzHWS6K$qZQ1URyV<@^lt|3`C;RFclYssx&BNN zPxZFliWDbl*?CM3MBhtF>{^<4|D#-Jb$*{*nBzVMvCdbfGJ_jL3A{|=y*B=^;Ij$G zAGkLOxCsjl=*M$x75NyCoD6jm)sr-xC)67Vq`RgK+ip?R(dURwpYCcH;D+ z_sXo^>73aDyqCn&y4SWTYX#RWk4TogYfFNnm+w9Y>;FFZ-jc$rr{+CoX64;wu7HWmLYU;EW@>VebzEk%7)!mlD)28O90 zd==3X_XTJ{$jC?NL73=tcC#WH+H5$-A<^)+JiJTBKG*;!KJXQPpKQXD3pj3qd>|C~ z9LwY58<4?v;{$*Hn}h?+g>RiOApSof#_*UJ=K($)a9mgL|7x)x{qpkw$NyJ?53eQ@IEe)-FQW^T{^{$YcG-0LT3g z0`BJn)-wb+=JW3W_wyOc{{wLB=W~Du`dI|YhU4l#u$>eTL3ST}!F}+h_rW#y!Pnde z|H*xDY~N!*h`l(D?B>7VoLsqLoxaReX4h93EA*wsreZ^Rxw#mlR%5x*U^Dt3lv<5O zTF_QjQB_`M+Nv)z>kTC(R-?^kE4A8}8|}Fj9wgoI)VX~EiVb%JQDvR%~ ztil3g`C+5YZZ+?~UGM2=wCiCCdYj!~H|nuOxp6Z_d|fvi?Zw9Oa=83Uv0|$c`eVro zgWXzI?dxr@*)A5~JQ&Z7dENeluv?ZrBlwf+hAc2v3#9Tvb$V zEPxOhtZK832^5r>f`Ce^or$RI22*lLnau!N0tYdgf{}`na(yv`-&AGMTdPcXc;JK0 zTxBgb+5)>`L73TpC1xuvFxpk7GVoQI+4Qin*lxDka9_L8T2W>SiUh6HZh&|Ph6iFs zJFd(IzJq?iDa@BLxIAIhTMSl11;ey}hXHDmmM&B*Tw(`)=!2=R_i~@cgUGMP?Dyu$ z6!g~z@n8Qh6TtiQJ{jO0BRB|8un#`o=I;=i68`WA{c?B2!iOW&`*&T z`g`+2|Gg7~mi!)>0rQz6HR#?`=7v{qaB|Rr49*UkOZQEWz^4KEk*C+|t4tQUe={0~<>M8%ycs@t zWFT*DIv`VZZXilwZXh5uATc#MF*G_dG$1lCGBzL}ARr(hARr(hARr(hARr(hARr(h zARr(hARr(hARr(hARr(hARr(B00000000000ZB~{0001Y1ONbdoYh!OZ{tK5o(|nFyX>P&cnu(QV!lF1RbF?MHMy@<9>ZjH8k~QX%At_FVIPxaz=%_))7@cr05+Q z&e9P7o*IqnIr1W3p9!iH$td??-n1U>i0yK;^90?RI~$uHO2@hdeKW*Z9$Iy!i9zTr zPPg;=gJv=YNr!N{b@tUGi(l_Dd%-imbi`r|cC1@gia`@k-%15=!Rw7tp_2F+Z#xi8 z%6S<$VtYw?3}fIKe~W5b0XyX{0al8Iz2q5{HVR5ck`e5sfgIaRM9AFV@OS~0CEO>N zJRRUHwk`!B$~-fDw$K)AV3bZ`TeVup$Bul<*pAR32Q&cZ+$SkhfCguNRgyPkiuSoh zqe?p4rSl3PDnp4H7}2^&+8p0v8Ktq-=JoN5olp9BzV}%*cy2qKq zj8%>ou{M4xfdd*u5ZZdeIGlj83%I^wWlPZ^?sm-dqy;LFEOm_}qaegWVpZx*QaBSM zkz9zjOkzLp2-agm0L&Ux3M>ncu`J0+8;y6fTEfeo^{guGSxhyjed`oKO{vg zc2rN|o2FRC1$J78{j6Dp9*ZHn;Zy8Mro%bjee#C)-TG$UI{sIyh^xw1;J%*2HCE_z zcMi9gkAs7Q=NyL5v8P+f|GtCHE;zD;BQsoa{d5jrTfo=m)^O3OzN8Z>Ra@5S-U3f! z0k@ND_1j-R{_*E8KYzCS?~8l4Xzc##}DxsXTd=?XPUT_`lEJln2Pd{JmSekHu@%zTvj*>+iYwE_MCmeEbzDehX+u zjE5_}M)Jq_!7y0$^p@&TcK+*<`W1bQXZ6c}%shX1C@7!c=SMcJxZRcZceL3f2~@5A p#=D8SP+T98_3>TH|DTUe>g#sT50SpMA{IhF2+7KXADuh6SV%x2W zph3lo)|sWP^9?$-JN9dLqQ$P=u}k<5fqL+?PDbi`wbV zH}~V^z59FTo_p>&zkBYzk93{;ixo120uVx_B6U>G)BjoV;oXkk1{K(I3BXh$gc|q= z1#wh}1NHfE2^^98wSBy7J{gyEfM^mu8Xydd;O!~3%Q89X2ZGX%l5w&`p?q~OKH+g0rORrjeow>|Hp!PoFKdl`$+dnz^-&+_-9;g^1n!F~c;t!n z?b*^o_}O3Eov#mkWJ%_kfb_y8!j#t|H7BT}nrNae677hy7iOW|7EPEKWulT-kmxLO zFXTjLa*_RQ1#&;L>D{%>Tolbk?V^%*(-kN^6QND-o(!yYb22x!3xhv~ zwXl-2(4kqyM|M$&W({#DL%llTwF4?~M!YH@J!2yR(@=pNp-{MoH&dv$gsT$TX=yRa7P&j8P$zEM2|&?&`2y{ zS0B}puF(uP|S7ZxmR`;cMQgA=$^fp`si-KD+EV#PXyUjYgZRPGFOKu(Xrm{ zIu=r*-?3mf2Zl3ObjyY|MoE zSObBGEG>IuUf)x@EOT9;c9aPEKC-VP_R|+v{(c$Sh$-ToEFL%)-M%Lw2H^n60=%zZBjg$FODaBm0ze7BPwOL zF``1fEBSO|@}MrMgPU@?F{O!{+&?MXTq1aJqu_~+&N}$DJ1wnFONn4}v|#gh0ibq^ z-DwayYvs zT~ZtUd#We#pf0hQn|!cmIdnR`J^2zh+0`TZuS%z7owLr;Xxk`gC~?~4PFuS(>_U;` zTO|2&zIePTyaEP8C8srDgT>@sE`*Vx^zz-YE16W zkMa(r#JQzG(9l+pJJl+|9*B58;;d0|L#6zpOZhSpfeltECyo5#9gT_oJ;^RzDmY}i zJ7rv#+zi#8mX88qq|Pm1k5KSJw4kodvHlthn&p2QYiQh!g%sjj5H9vEXvzrLeZC;+Q!!GY_XVVBQ{CrTuL8s$$?lCRW~ zuPDi#e6g7nUsH>rdTW&b&8T=hU;Ldq^2-UMqT8Vy8Cx)+QeIandsRw^;0@KH3hI~1 zr*)~PxJd^alls@4hUmj!UjRGx&Mg*a*txI8|D%?Sjf%g~NWN)@UU~?w^)~_-i=hpu zf<0^-gitz~v`M2q;JQ?ZRKKRTU3vDfa#S1fMVF!?s!S~&!||1XlCRV{Rb#zeVAVSf z?ar6%(DwL2BBiA#9u!y?4IC1@ zu+Gv3cH=IS279(&SDZNnw+Q5<7_Gqm8KtdpMhb+=?q>iYt_2+=zc~I3l`@iowH*K93WPT?8yUlCgXV z%&VYg&tL)u62bHlAZQP`G8rNV^Qw=V3?`!?NFX@(3i?xyHYOhe;&M~l8&iP9tvxv1 z7%5ou;U3CMkHNVH%NFOjLplbRIi!daiFy>_ZvMH2G0($Xm%5MekE%;c5GCZFS*SSU zB2+v=y_#%A-TW~Pl0aKQ$==etk->2@qOR#3gVP!DnQb5F2V^r@0U2q-`p`jPZ;kq5 zd}e0>F|;pXn3ur0`RM`;xgd^7eMSoQJ)noN2jd-(KB4ae(07%Fr$hOuyID)vDLGX^ z_J{BmiXoHXy}{0q)S{{IdE|^fGEWmp3R=OGRlVsbX+TYP?xWWyRuI-|5vWrCJcCYhV!g(SkWlQDli>F7_%IR));1Q{7$&3#B;B7K@ z2vms(jTlt%L6!TGRx`v+|5Rtm>)~gpc_^D|k;{>AJMn8lS7s>gq#~H^m!Tit zm#~8-#Lf^GbVnB>g%Dm|nRw%MwIip5W&^~!+tdFl+<#Cw9cu-l4kco$G=UHk3(uX) zbGeA6IMz}i$9dc!bO^sruxkPsP8`*$O9$}`TC-53Js!RLNOW-rj7z3PEpizS=mCCT zj8>Bw6yC%o=so#cg~AmxMFRIDD`o^~T`@yb@tP>5`$Y5kH3>ycVS6JlP0b?dQ0|wL zpU%Gsl4+WZgjW-%mz2S?p!2*uJ#TiTCYspvgfsu*w#lo0p1Ka6`d9%nmufBt-cwbw ze3&&!!!M{VP3@aG)rGZPq9cfBoF`rz;Li9AJ4=>@HvZf#)JRD=J*~la!yez|VBf9w z_%53ET{Z2ycG`CxwC@}t;JZKI2H-n*onRlltByVk(?LiK-qpnP!&5Xo<*U!ZQwpI8 z6dtw1k9W%Tf_q^DDCA#Ikh*q=MGlEPuk5|rc*s@5bgo(sc0TnUcxa7a@vfd4KZ0M~ z%mrsha))yD4sytFSp<%`nt$%kxkEJ?@M7AXuLbQO5ZWJ-m&VN)CSvxC(5ddTd(K`u z}tiIP!dxOep;f=H;CLvUf*r)-5oV zu?Oc_T#OV#R=6|1XZYFID%yaJoZ7C8kRb-cVaU=qjyhy$f zlGtdUR?9oAkwLa1HWwhAElEP{5$aPGd&6|av0^p3F_A=g0n3Vabylxnr=P2D#j`TX zHghWX?jHq{*}YS%g|}9Rs->Oz7iZJukdWvk6xW=8u`^-jV6zA-Sx((}76 zJh6tSF7ILE6;_C5QQFINBK(M+Qob?$XP=ld^Jy?q#eADIHLHw;z}(zUa!h!T3DAzJ!&Oj#Tit~fC?`BMiw@z zolF_H)go6wldf{Uo-E*S9)*zOy4J+sE&~Sb3Prv28b$`dn;3XQyvcu=EMB{IZ6sd+ zJWwCkhOVEo(~BFf=O1@i0%Q)gr~8ZHDUOw^$w4ljs7Wy1PuSs3GpFprnXGKKTI-pN zbgC=s9HPM6-TP+lyvL%)?D9)fzlvzCPP*(nC2!7x@HdW2kpKNM;z}Dtr)<`&=G+aJ zF1|FiJTI*#9eV7XEbBC{fF6NM@8bxTbyh#YX6C2&EsQVmV`oCTr-ymDyV!ks# zJ0CZZ+hlf)Ht3&twHwG@2ke>-IpnZ8^rRI}4k?9kEMadzwwaUJQM)=nlSPq6yT3pAzZOKY&PB zp{xXri)6<>#&3U><9_zu$-_u7XP_Uy2Zqnc{{0Ik`Tefx?sODeF zu&CgCle%}2^nfmtqnd$4A7q?{ER0X1hj|n9jtct4#Yp|5a_;@-jYaV>vl$tpVHA23 z=Fjq`o3J~e`poa4CFnJ>b}uy>QL*;@JLY0bc<1 zBXHsOjGgs>2SNQ5T!CI3kIN1?{yg3d7vASE{k{S?wr>C~{GP#o2{`WW4{%|BF#1mb zz7Xm+;lkr)@P4lz_(a6gbJ;78PsP6Sw*klgiUk~xgXuRIoQv(y1MX{QxEEjRhc5*j z+g}Q}ul>N?`}SK1IPPy3;J$vV1RN-deg!!8A7f8F;CTFR1MbU*U4Y~Ihk)a? zneoFRz?VUN0C3C|20sIMG}ONX9M5$I{}ym;$0Fbh6JH!J9*_;&p8&Y8{Skm;`8*$a z%!dTPar;uhnRsD6*??odZ3dj-8jehd}Ll2;TM(9Jk*9 z4z0%i;T|-FLp>+E*idP%v>B>R)rN{PbD6QKs-_I1b*3tl(Q0B|RMeSFw4k-Jy1uH? zyva~mV=$JN*O{zVYek)HmC2S_T~%k(*&3`?sH~~CS?X>1HFdV4%ID0+D#MmKqs3yX zqhDs5ANY>eUv8~t^nzs-HD$4CL#e@pbsMUUb=HkwVYb;?0fsdg%IhuOr^;#z^u@p? ztF5kPD{gvULzB$_LoirvMw`iiC8|u%VZ__?Ig_o-R8W_jYw#sT#d8O4@TE#NiWcEj@ z%c~4!5I1wZ#ZXso#_b@G)|&ddGLzL;F%|@q{+5`vqR3>^nk!+Dl{Mz4Ol7v3IxB8# zGu2gBn*CfzE43LRP`-WzXVW9Dw8EgF9gqPBx)N?Lm<$$Uow1r_n$JrgHL*`Tu6}%_ z4FYTM=d8g)S{nCbtpU^4L+}Ui*5F56!w)jo^Y$Us_4MJ-UD^-#lNb7B*vmY6=*!f~ zJ^ZDc`x6+4?ZMP!T6-yMcqoS%L4Oi6xQE3|%_GIsygbICd@zx*-~*UU!(J*I9>8V8 z{bV-$Xg1T5KZ4F+!~^(Dw|*d@J$n5aO?%&;(lh}c%4y%XKBUey7!37hvk_*uA=bAN L=Ua*QCjS2e6$=l? literal 0 HcmV?d00001 diff --git a/utils/Simulation-master/balance/series_legs/slprj/_jitprj/snB7M5QwQI4hNRLbuP13ztG.mat b/utils/Simulation-master/balance/series_legs/slprj/_jitprj/snB7M5QwQI4hNRLbuP13ztG.mat new file mode 100644 index 0000000000000000000000000000000000000000..2dd2a8360efd1c450e1730b9fc8c11df2ef84b8e GIT binary patch literal 1275 zcmV zWFT*DIv`VZZXilwZXh5uATc#MF*G_dG$1lCGBzL}ARr(hARr(hARr(hARr(hARr(h zARr(hARr(hARr(hARr(hARr(B00000000000ZB~{0001U1ONbdoYh#%Zrer_9SMrj zG>zdR2%1HKW|>WaxNgxbilVker2_0&G36BKCK!=d;?%<-GY{GFH+0u+|Dv1zMHk)l zBf99Ki!6JGA{ANBAq~VX0h%6i&YU}k_eE_JLf$?iWDBn!;8`b=<4!)V&7Ak~{E2B# z44HtA7$n)e2HDA^&UhvUAVCDUK$=KhidV7Na3;>$ zMg*DuKSV5u{3tOzJirBMcr5sM6>n;E7a3bjLZI6Kp^ju?hEbc-;d&LMK5s@4oGBBR zvNy9Cv~NhKAcf@u%+IfU-&7Fg1#AIEDx=^(!?Ni z7N?te{XsJsfuut?-Z=Z}k;Sifk-gxlUpitj20PX*E5)FRCvT+!xZw3hs8C7#l(!v- zCgr>g9I?G5J%kbPjK4)St$>~ImjElp!d~)eDuDwUL=f6~!Z;j(vJ1GrVr5IwKJIqJ^tc5okSukLB%>h2LSj|wjZ-)k zXCgTlZJES=-Vvb^SinywL1@5~UTw{qo z_hxWA`8e2p`kKS=HTHB%`9E^d*#$=yaAb-ru1{w0wK;rkW(^me>I*udQnf{$?$7Zw z=5SlNR)0MBXaASKy+0r9KKWJr_VC~9_(fDN=f~Um_*0Vib<@f>S||>7?)PN9AACZt z`hguQaq#ugS1-?BcD{^WoE-K|cX#hnKhM_teM8*U$yIxc`;YO|m>BJSC4){#Cf{gw zOwHumVP40r4SKAKFBjaMCAek!G{~$z4bqL+?yR4_FMID*xA%)x_TI+)byNBBCUGu# zwf>%QRqec~{gtg3|M&Up@&K8ezjKTCF@LSvH{7;;{WUY+rLKRLkH00wZvoAS@o>r4 zNd6c<7zWFp-cmiv&VO4{zoL)vtbX~AnLqCB3(Cj%`H@X4Zg-{q9c}tZ0#(bu@ou6n l6xU~Db$r+I|7YXdj!%2-&!1J~FY4}f@{ntN_zw$R75_rTQhNXZ literal 0 HcmV?d00001 diff --git a/utils/Simulation-master/balance/series_legs/slprj/_sfprj/EMLReport/s1PQAPIlnTPKm6mZzJoZZuB.mat b/utils/Simulation-master/balance/series_legs/slprj/_sfprj/EMLReport/s1PQAPIlnTPKm6mZzJoZZuB.mat new file mode 100644 index 0000000000000000000000000000000000000000..8c7ae55172f6e3cabbb0a71be7c62170ffe2e793 GIT binary patch literal 7840 zcma)f_dguY^S=ZU5iJDKOZ1*ZZ&AVvL9{5*+a-E;O$iC2muS&NZijH2{2#V1d>pGZBC<^KPr znOh|Pk8bH2(B2@U8y=X~W-@AcTbfQ{SFcAaa)NZ+p=3Hfg!sVt(_QdNgH@JQ<-69> z$+YK~{IAKFiL66g&n({EzDmJdXbzz~KgVYtNy(Eaqfw3Ywj^`;j#Pm$E{_SZ0Q6d~ zQasPk>d9GooMf6f(}jgnDA$5^m`0a zZ-Kc?OJ0_v1B>1I{CgQjP{k9`mJWf(!VyBc}do#Y=B|=MZZ6+G|oHEc3;$R7K!^ zvdr*Wd#rJkHD~EhCej4?`>DKKHG-^;I|+$e10TvdOyjCj8OOlXPr7Sm4XrRa+7~ zQS044o6Ej?Uod$BX?s-g#DI!+}uj!j`F5kWjqV zle!@efBTCz%#0PZO$T0VlhvWHomyp;KM7b1#9a0e3#r!w%v)^tSMA2yuu8opyOI)J zL|k*6eT$u0d*z|EGkBFq#&A4w-7m{?o%t<9e*R)j)l}%i zY8g}je~+;CmK*Q4{`QYm(2I3O>9l}I?p=xT;nleW>_biRd>d~h)zb5k7LOIrtusf0 z(E5f&*XFvroX3a9xZ!#j|4RLa7TBOcqdLW}WY~zQVSf=kA#Sf){n-xe)j619lkv@Z z1oTLf{f($1;d$B6(~`wRW8OWu(3#yYi9VzTTDUMXII6=cwJvciS&x^a)~ueTwsKgP z7s6A1S(_E&@3h~t38--0M4F<~uxEA(C6@P6pA`%WR+L1OJ>7R-Da(CmTIRb-S#7Oh zCAAMB)Mvu-?r%WnzLu=E5?*C`XLLonw+KRVQeAPDr_$z@R;gKW(R>{M1#!i^;5BZo znDy(&QG||TW_ZV&p%&xe5^I$b0(an1&QT~$`80bUT~Voct<=ymg+shVxK5_-# zed`wYKoxB55R-P=<*)UNF~$A&qEUcFCw0Tr(S_G7&Bh>#jMmCUgLOVLRh{))BKGA+ zDP^|sCKD@_aM%mH&6iv-C+(*pTvappn<9qVVT5f(%I67tfJbyUoSX*tT7INCHW8f# z_K50+in4F)`DTZ7IJQ&M;Qm8hj_ss0xP0GiY-Bf7j(x-S2EFfwO0sWUEK8wjV+gn8 zM`*hoQPeR6o*UG?LV#-R_ls|yb~uJH(%_tZv$>JE0QrM_&I2R4>#xKQ+6oM><;3_`?>u^-PLFPB8 z(=m*L1{bm{#TDIsX)8bC)8Tl{$-W_XGnPDt;3YqzbEEp$4ZTl;^OGOJ-WcAB0X?9> zwfbhGqq>#JV?eAlYC7rug+KDtZVaBWZ(zF}RY_wAV(c3MzS-m5D<_fzDtxkP*wYoM zaFQ4tSC`}UT^d}!Z?@X|?n`0z4X&H$+>IeTl^@}dAIVyl8t-r{_}G0qmjTiBY%d)4 z&4zWXTyFYiLwvIl5#5)X@*_8XVO@>|G2L*I7!W-TE_qq%#yRb>6zz(XWT)fx14D+x zWaEA)b4ab`uYx&yzMr=#C#>df>9keCeHLZvzaqoV+ccUqn`%hw z{qghm8OLBE*IZcsw7ZLN^W;`@5xkEDbddm8&1*q@1G}(4XpT&HzVuOaB0P-GCyU=F zBoVX$-EUai`;;o@wsy=4f{24I+((}FJ0tk9&l8ldC4Pe_#6e5aN974{Z$4}aKh`$^ z{*DheIiTb;xLYL+8y!$`A5?nnZ1?qM_Oy_aKIH(Yk@b;b??7%{+Kk7b5(l48v0nJN z<yPpB=A?l`ZBzaDIL8Ej?elN9}n z;Ia?{X-Xeu)-+x5K~GG zcHvc6#kOmc@wyvyYTtJKRYx4;EWQ86A_MNHkIHA+Yt61) zYhnd4WWrq%L2eiO)<^rj18gs63l|uVMnEO)e82zeQlTeW*eWtuo;|I z+wd-^q`+9ksNmC(vY(b~ftb~~0@v5V>5aiier#9*9Gd{I&_b`+ zTWZ#mqXO=6c8g>CSWb)McWf5D$G^UyLmdNLZO|;M=TXx2YOJg0={FtQ>N(ZCYkTp*Yx|TblD+e9T6tPV=2n zh9^?JE|Q`SLm%2unI<=U^rDya6~n88cF9+V8t_-faV2JC%9u~a@RE{P%*STqSMUSv zABHn{Gh)bP$Y^t~N4a}2pu9C4cl;gwS3s~DNcFW%VLqs_ka16Zf^!xZr@~qJx`z4$ zXx5ljWLOibmIHj+IShhJFMnLl*MOUnA10DY(xTq=C~?5y{T`=1a`C_)ouWXvVX|Bd z5ODWgX>Jnz^ixP?$L=VOu*YjB2Ylb3%d7|^L)lLhSZJ%Xz>Pi7bPR2WM2$$dgQ)fM|IL=mKRUvcM`F14;l1$Y}Y&( zg+1&6kL+_FpLWK!B60MYZut{ zVK{Q6DaJG3#qQpTJf@n*Msog~RulkBOc8)w&AsJvOD;4$3{eY!b8Z|G-g0>*L%xRA zs)W7A2Q(EG^7r=d(PQf9ftt!FYfK#ja9A1jwFmNYxsC)z4)6bTs+G^#H=_v`Bu9xH zoRDCJF)HE*NDv0n2mY00Z|2RBd+{yE6O|qWzx6licipN{#+&{^;pQYY3vZ6_;zh-4 zla`>%jKtxd&v?hIdX9AMb$=0L4OzY2qn(E)t0!p=i_pUOtU+b?ZM&_*`HpUE8|(pA z<{*yKhZlIJf=C9!sLqvoC(&}Tr{zOo^U;@J{22>Z3b|I+mc+qSPqOA z6VUZQWVuZre)?as#Cst2%Vlb8pHAN&A3*wIL?Bni095+;v5rG{UR*#8_gHE{Sh&+_ z5A?XnpD@%jcw}ze0-c8Cg?cU@nX9)#AG&A3;^vQQZlL)e3456xd5ICt{!VKZ>lWqD zM$=S9u*qV(Y}RHUvek`Y3-r97rcZ7MPNlz@0jus6uM`j ztf3Hb^+ipR$b&*eUeiR#k-~3b%y;p=m!Gc@6+u6<#>77{(@6>*ltYgEQDA}Qra2E+ z4kE`4yW9)O)jhu-NyLpb7Pn#bR;ITW?}O|9{fVNhlw;i4%c+*Tzq3bv9KAaa)X|b; zNb@tTA4UCkd%Q*{T@pIb(Oi-i4(B?MIdd)^ZAZ!lDeg60m1OCj0K-jGX@+RI*C{jz z>ZNe%p#@z6PzZa&YZAyzROyFkdDin^sR~=sMJ+7L{-X8{@)ihvsnC@#*WEfOw*dka zxVl2x9L8n-9v(w4YfTP)Fr~Hyja0?9y#-uP_bN4$)x^BjsORO%<^{ddPd1=h&|fkU z4QtIwhvjZPrh2{3V}Ov5(e3>&ei}!_K|Jttq;z>bA} z=V+Boa^_c5^q0HH2}0y`U93+nz771bf68@Jz~9_?77K=5&*}30A2Pn^RdP2=h z?aC5mXI=&B{<0z!pF}6s;fts^-a*BUI$LkmqXfqzB4QO zy(bxl0hA~JOYfVx&th*fBfYvj`uxr7X?Yx&-Vf>sTP=kGq;WF58}Se>LkKz^dS{h^ zke7(1kb|vZlt=*iX{V^Y6+xUunc@%psq$om5bp*()s)=Io7<4kOnT8+b1m}k z(Uv-%+_Lm^h0NV$x zbv0M@Oa)Mmj=a!bJWSkW_7o?aSI%`CBe+?OI#B%;v3Fu|32(wbUzMyg z*bhGB`u-628aosh3}E?J{Yp7$BIES8`FddL=2}?N+sLQHuyeO#C?Aj<`)q79{0f4% zAKT_NG=}lE4-(Emu8q;%+da&9o1C>{;_d$(h3s9E$>)sy&1K}qQ;OY!{j@oLfpH15 zOmt*5!n{ANONT}wpT0j2%mwLh$eEvyT&j8x(MvgnHKaxaKAtwjGlk-{IlwlVGV_(ybz%Kq4MSy1WUsa! zVG`+D(Rb-p@R9f#?@*2KPGv-G3<|Wyj6C(7dh`4GdKNFydbz|qw%H$c8gE3jJ|UAL z-OrIORlhS5sUStWA4T5ffxl&Wdw7}0;bNU0cwZ}pRCJlrlH9116?o^Uh@>Ejx#%v^ zmat|tD7c?GUMkMzkudw0-|IxI?U#PiY#0%7Ar*Tjj#&D(X;CPx6DG|&XsuEy|94mH zyhqwFZ+FC{&T)&{nL?>gsqn{NHpHeUEct`5s7MIf=&IO$4B<@(nE4@QJa0tUOFg;p zj%?g-Wh|7vxL}zj;Xk+szmw}g<(b>w)jq5vci}xKh}f;a%of{LR6%L=>NPb*i?GF& zGfh3nSc_}64asxn^h15?9c%{_wk}iqsnSjeqSW`syEj5+$72rQ93!}&7DOY8E9J11 zlPe+cK=eW0fg&Bt#uJ%|}yv&qo@&@)S41?lSuO(WJT>SsYBSj;{rW1w7e^EH* zcSG!aHc!(V)wl$8cL|o5$qfSVA^UA|C^?#h+&rR&d?JFToiuoV`c_TL9eLgrNEYs2 zwY_{N*yJg|QzBx)ffY`sROB?jQ%Wlk8^G~sx24X>`^7iZ-bj!WpvyI8Q10D&k&Q*Q zK2^N1C>E_6>@K175+z4UkJsckEqK_VA#C6A*y^Q$Eu++1#oRBt&xjzmHrcXsnO%LqCXo=KGkG6 zl{Kd6iDp6lgy|^L7~W@7x<#`N?bfk}j*Jg1A^J}*BQ7HXNwX`?(wZc4%Y+|v;AaxU zirK6;^UsYvt}0#S>c$j(V7MeNAM@-jSk*Y!3gLDE?FvCf*$9uH$BvvC@3X0eKXI13 z3l$-a#uKeI!Yxbs{eJ9TEbdMGKGymTT~bloX@@KCHYyfQ-Z{Q<+X7t<#pimC)x264!*y~49*>{O_9 zusz}K=4x1pOVL#aMfvY9L0=G-U!57Vc#hhjnJ0h~-~to}-Id-y>nR^!8~9YZSvL_0 z?0$}KuhjQdTKxkFJs|TbUJxofWtYn~=$89~>^4>dRLdWBj7U?KmpGdi``bbjEV4vN zJ!eBfiasB%Ztu12tYux73uG7m2Iium=rg-q)me!{Rdsk+(J1W4FI&MQ=~UF37Y_JW;dGeFpW&K|4n6Q@N$TB$OH z!(h99Oczzhvr_RLt()O z`ogjP7(%7kEMV1pu~D(+@y1RC>Nx&*GAgUzCwN2WkIIi`$+?I@eU2an+=7S^)hk|> zw~$s0}AL{*&Pl_)^!W2R!o_AA8Uvo^Dt4nR1oM?lU+0x6rNJDhnii%%fD3Kl|M@ysbZMy1Fk@qY&Z7#!mWyg4wewZ_-U*EicnIM@s}K_5CsP~Wvsv38*8R|Ozpzd^yr30756SzYLc(v5bCl8(scIMkMHgsk9D0RFMu4XCL zpUlO0lYQ{XS2;eUCOF$XdB_}0Czp~?ZWr^%QTpBEsr--Ncnw=Kwp(7}5Qdo7EyBh- z?m{H@DUx!A#&{C9=`TO%?xP4Y?Z+8KLzPA>Ahw5`)HZVTOT=v%mW5Km#+tH`cLcK z5#e*JMNP40>j8G=-@N5B;E zB6zkt?9^)?n{tU~ie6U=F(<_nyd-EnxVN$u<{tnJ#RejOv{rS~%{(I9Nm6n+c7s8t zRFuxnXS*m{i=Q}=v`KRI__H`NzhE?dLEXtoPPbD%DAGb9mKXUd#f6(5|7b*G$T3@p z`)j2xP_sDic7OU05XZ|+!uXB@CwRQElvL!IaL#px``<+`vlR@F=<;$dhN`kcOcMIPt12~@B38^=ZEwf57b z06K6_B94N;$~rEN&a`xX-tpUrRHR>L=VX{*5)Sm~7(O#9Gs8)ef0-kfIH0>^M_8{x zLPOZ!qFX0f#U6`|X8FCWk+#6~!$vx-tB~l03KQWx<)@FL@_rBg6GY)m>>@OFczF0} zTkQ|Vi?>y0m{Jj6?tal1%jQ;E5V_{oUD@iDeJ&tjb@vZEowUZFQ>fJVzZg*dreZummEBpC=r0_J(?@)`P%AA=%KE*Ru`}KVK^C?U zsh%VeA3jqFG$CFe0HujAE@r~&+05Us{J2AT+pv~|(Gt-PyhQImQ&|Y5()H}Kr4L;u zNoKEw+js7e?oZGuePi|3_GS(B?oxk5XcyE(qF0~YbsqyR`#0%nu^J52)K(>^rI3GiZ8YE=&0DR9_+!5-VPs|yHWQS99oTh>e231@%( z&gX*t{%7Zmg*V%G-f5G6FZw(9tWk-Wb~~PDH935|xBEw$+I8od3nrQC%de<$qvX*` zQ=s=t8-taWkDE$BR2QzTSeU3rB>wMbwr4r&&L&NlnV2uYf5rb<`8llu6L|Z3r|v*x ze)cS7%R^U-pXSI`?X-AXJt9?T0w?4z-x~e-?T@M&T*ZIt8<8Tpv#s<$N$Ce_a`Jt& zSrgqF>mz=dgY4JadM@IiaVM|-*vTv7=t)iqXCK!n2|=XxnX@L*b`u6re$D+PNBiGl zv%M1dy4i4r-fPnC_9CV34p;g6cE=9OemaX|-Pg(m201$XGRgORAt^Tf-mZg4`!yT9 z&ccU|?+-83h8duld1K5+7hl>S(UoN8AQ1d3IsE`=W;;^$j zAnnW?rowVm1G_DNBfN%y{LZr-a;Y;-uTuD>=|y8VkEY8}oR+zgHRFYxT%Q60tH!!F7@nwfu?2v}YQF|@+dJ@c%< zf*+W;0t9IFb;5B~xv$4j%iD0K{7tyui__w_Hsj8r#Cu^Dfe}NU)(x4u3Ptzt)K}U` zf2BWHK%}==xBtd`onBn2~AWbRZwME8wiRw|4 z`j2PqHUvn!;UdHbnaIzHUh6K_-i`y8hXzSw_uJJs&W2(YbXXZB2e+%Ud?gVnrKjE3i3tEEPi>HP$Me3Q}HNp52eOEFJxUyn!JX1;DAo`4`f zCEPhJ4SCr|+*&~%ZXq7-+yH+iZi`TVZUZ-eZf;p_DLEx6StV&%ZfQwrdG7z;1A=$& z{|>yT#xyu)ifaSQ`pu^e2&8CDARV^D=CfLr8o!0F@INInHL8m_N7!w=?ld<3Dv)a> z0z}N}p>`*)#06L9n=|T#2)JZm{4%G;If6XZ9lH8!D^wY89xe9nbkP50qna~f@8Zfa z=7Mz$IU3Y@EpCbsQHjKTGMW3taocpq%gLg{RRZpinnd2BQtsi;zmdukR{ph6;n`%|hI=*-1q@(1h zOos&9{oj)b^NSmA+TPHaGg_#Al&YOs($93XC+Dpx+D`vu0y^$;2?2}t?qKWs=K6P; zBur5;lJV|298PNcq*b}>F{+Q>Dw$y2p2qTBU6&IaEZqK;r~&y6x}d-D>{U3QbHLi} zJ*1u~RBzl-uWDh)-}kjTZgJj203+82NSLCjGH8-T(lq}IYpKD1VGUU#$+NV13 zv5frTIfFiWMo)GBiI!}4>({_ihbIDfTva;;wBPD(fU zU>ud(dFB{3|I**r!Z1*6Ijch`CEA$Dg>wLs4{f)f`}2Fz4!U!gwjN#U-H(9;e?3OOMcSfx$cTZ`HClZYF7pu#syPG?~sWRn|KdknX^xB;$^l zGP~o8L1J<4M>Z^eP?sGwG}4mSIT>WW+|POH+i z&(x4#Wvg*}-a9hDJHuag7Wid)rv?1`Nw6?SCe)T4X` zgbweEB-V_g70P|s{yuOg8QGx5B>Cp0{mDv5;x1$i3l+T`sOpjy8*YwzP^0C!>3W}} z#zV|4hp_+H7Zaf_BlZ-5q|1eFtUCtUxUFnd2(!2w+qf`Ze(TF%fA#kDn8t(rPGQRG zmCsVSKCe!NazYX@*K2Os-t78%62b#VpGq^k5jB1GNzP*N^*|*GZplne&b`mkvT;asLwj) z5ce4EsRbJJBD|J7P4G^%D&OWg0A91UW_Tzc!iFRkZ zcqn~n;jvWrv(>cFP2D9qI++q}#CBmPec0}?wDY4CobUFZK28uviPmJh(2+jO@K`GR z(JGPe<{~&c;FuhpM2UXkzQiRudiR*^Vq&t@g?sdlknKX~S8Ge2+n`x8h8NfP*{W`r zjCsy>VI_TN`ra+}3nf}W`mn)c3GxGpShrVLw6ep>vR%~S)I~-IOp-CuY!zUczb_lV z-W5w97EZRd6uQM0P(~(DqKVlqvZN1jA8^6?xqndAs^wkZZ~oDkB+AR!x0I3Xl#v;f zkqwlQrIe91R+SICotQjI)Y!%$n^&)MKe7 z#|?&a$tf9fuBmYIrIE&cDeOn9m-|wr`;r#hMY;50zw}|W^x=ETmaszhNTYu{S2))i zDK8O)Zs4D-J043E(uWcSZrL773~U#c$IDMXplS+-N8;`{0|eAMTz$D z=*Ax4bRrjEbcOMr4K7}+|)%z@4Rsig1ZB!ETQ04_a#8# z`duZia~UO?L;5fbcP#>=F}O&mN*`YSYK?VH#^3_wlINx_IC>|*2E|JB1@`{BX~7xA z{g4k%ROmKnnv7wWK7{4DVGAhHT^>t=e4}?xY!`^1t+6<-NN|=WTh(!yh8vho=|ds+ zB~x6c;~aQTiN?i0mUr~dg6*Obmv+LVckd|CV2>r!$yTgpG7RgouX^F)jYNRi*qX5& z>|PBIE)}E?2XWV1PKic%EMak0_1G@jr4M6s)KUBv!q#abj|T%-@N1#YppQGLdCf&J0S0+_0@>g|BGLUz@!mwlQP4nr5&FQjm2 zvnIx<98jSjA5ca1pX2UF9O7~KghLJvUvWss;R_BWIP{(;W1?{VDRD5u;SmnLI6T0? z5r_Y5{U^uAK@$gF9G>GqkApc5(l`X+K#oH!Zpz1E^Olx_nRFP5k!oQ-js59!D3XQA zcn$TrdGIv#HAe;##b~(?g+-1jjzvX8$WzhH-SGHy^v!PRzXX~Bz#X6U_49lsi@Lh) zt$E1zVh6d<(w4f|kjoJ8Un6!!1!bim{|ztyylK4f$h?j?P(pXs%nn`G>~++Jyb^-n zzv@&RM_iu#t*t*f7Ixs|s6OG`kYBOW|Es%`k-EAYB^AhCeo2p0p#M*S52sKw<`)7r z#mNb9MD4$hlmB&0#aT{CTisQu*yMTF!tHNSbCN$6s^p^MSa-VcAJM{*j{k@rj-2BN z>`LZNP(;WUIZ4c0s5vK3(_WspZR4K_O%-kDPVnT(z4 zUtU|QvZ>epXa8cW4&K>%xP8~taa6AZeHw0Han=A@4T-XW_V8Nn1LcST)Xt&q zAs-oF3jV1A3-E|dRWX)hsMHI2)-HzHCTN+1bC6BBuppUPK~>a{DM*JH z;hb{0`W$}ldA1c{hYuRatZXMnOX_IuC|!^+#p2s>gBPeK^4WbEjWL&a?F zX%{|Fl&a$Hs1VrjrfUUWPBsO*4{wqKdL=lumqrY?TngX7x?i8r+E#_+n_)4LLxT8772$?Q!Og9gpi;g=1)u7xw@y z!)QC8@yIF(2*ak@6H9Xs@nLu_mO>E919$R9#!yVbtcH|*kufwtn@H0Eh3?rJ1l0yX zzk>dvQ`DXD$Xi&40D|f`a4$09D!d4!&<)1PkHh|JvK4|9fV7#Xo>DW8u4QtcB>2a} zDUkkhQ>*iD!q}1i3TF~AT^SWxdU0WTx~3Nh6Zo13G9t-_5xNG=J=o$YY|aIZ9`MiP zhMu$*O%6ErnpA}CDTNQ$9Bk>}WQDjB&aZJNh%!%$)_ao|fAS4Q&cm(np)ZD%{ky48 zuK8kVRH#8hl$cyF*YcoHT-XSl3IE{uAzalD`HBptZiQ+9|)V~Z_zkGXi^{-J4k6= zjvS$aRQtL6^Ddek1g5NjvZsX&M+&S*ABzv%RkOqjp>HVMVn5$p=YOo6!{5AHxwL?` z#02eOzU&G@Tkc5ytE7!fULm+jN`hRD?th0*FH3`uPfq$gja-EJkcdL~^*1=3O(CY%_CD!YNr8h@R?CN`sNMV@r=_FI30gUi{i8f zW)LFiHdMbLSKw>LgaA&|cHNoez(ne0_lV(Jze80GkPs8pooAq~%tnr*hiT2hRVhd~ zq%t=cQqZuo9#e*p7+Lz3q6Sl!$3Y4QIUHngP*8&na$};N&YC{Vb`gY%lAHe8Od5-1 z0+}I?@3jy^ea0f4=wNG_Q2<(maZ(}O$(Gnge-rW-T<*;!H419H=N5&aLqsP*?m4r8 zXi_gf;;PFA`bYR<@PKSgg-&qVbbBM}l+p)|1XX&4ZtmEfxu}5ShrsQU$kRvnH-Qx% z32(atob0Hh>sbez6VrA|G_j6M6_Y~HRh#rn!RghOpO2#moOO}O@r48cDbxUwgbROHPBwBb2E&1azR9_uU4y262&`hwhHVJM88XFx2iD}N+|I3J_h zC5IXp0X;96=%PY(T7d@D;2rT@9H>rHT-qqp0`!t>2wI3h<*8l41~i+iNx(i45FT7E zL18Amy9H|e%;k4RMcTo|aa`9WsnSGG9WNl+J-nRF^hTAZCIt~Fv2oOdOeTYBZyZ%2 zbxAR6g^hzGqc`kL!M+$V_Q5+&`~$!@@$Uz?mFQgs{sH21>zzsR`K7FFtf>%2#d6lv zyI&;}m?8?Qhz}z~y|n~(Q9z%KDci5F5jpb#Cuk8%xEO~$K)tmAUcRxxLlAD1*CRbC zQ03YuK=Q4?kimx2zR&L_fH4XTCs#^8HV=5R0dG_W=uWN#Ss>W(7@Mb{E}32_i5dRQ z0A?W3aPACZMi8n9XCg9H8dNR>?mWB^Er*qxf+!ddNo|pplg*hMJ zUi=NdJghJd()#!g)%+Y`uoS-3mwx!7Tpg&c2+ao|rq7e{Z5>|jut*mhHJ2k;K;dHz zHo(u4m)i^gJfJS}%aCD0*E?VaBWi&hB_;@^Pg1Z$7SIFOfLTNcB4k8-R~b-rD1!p3 zgrtY}Yl8CAG5!RA$H0cvu9ex*+a?~UeNq<(=p(#|4;qmKnL=L4+k(4zFQ9^e)Rm4q zOi_8Hqu|8MDX4)S)E)izkZO77K@NFEu;bMrBzTS`i@Jbvg+8I5va5NZx*Z zm#tvAbRGT`E0%uKV2r{H8h{{-*AHka@DQOJ{U4EnB;^$v!M z$alaLMrW^KWg`$Dd5b!*2hJe?mBL*^SQ*eo1T;JnGsp{tkOSOH1}iJLp$g=H9C)-L z2*C(txe-5*z&J~-S~!!EbPfux>UoZ6aP48GSM`#O zf9u_6ID1cr!?WOUM=6FeE8($xn!taooHl<61OI6{lBJ501S z4@yBPBwuADUQux0MWtdb`FFvh+;;`Ev7)J1`_w`D4S=6`EQYa26k^Cb=8~N>CTb28 zh_^8U(I?r60F6^^z96^X*ib^P#`qQ2OMORKQ>NwPAsD*1dO~v)rC-fFkgi6uuf8yMKuJ-xhxGf{NEK#x9=S}F5 zo71|$fpcbX%WlqXCz|UV$f4MB0T&>mIgp}uCfNujYD&`q=#k*X+v zGhh=TLU&`W8Y!Z6d#bJjk_D0tAMgX6QXy9BM{i0V619|`Z@||McdswMo-eEsUWjdP zJO)^Gqlb(&%evl2+Z2mls)oKFIv{eU*;qT?UGMp;0jeMe@Bul7`*MK`BGeX>Die%M6}l7dqSLY>}n-f?XD zZ%{o0&J7(9LsiGv_jlJ1{yA@~)jiHGOgv@P1+AxFS;r~hgV1RTBeOpE&SEHC5l}fi z6c50N(p8QCa?;QOGB;FAD0ZX@fHWeYOt`8xD1`}~r6FNW1?3xyoSz*<;BBT}7LE8& zI2$6z836HBDLd%LC_#~#Dy4Do3wo5G57d+hK#r;~I}E!Ayo&AmJQn$9Rum6#A5~!v zd@e*qDo)ObC#^gHCNR^uQ-zbT{y(GOGL0|b%ujKEsYwFF1y z$$+KytX-uY;dwhsN3MCr!lg?PYxvT{H6b7lzeU>|rWxNQtjoFBRRkA%2G7Trpa#~& z7YaACR62OtMz&V+#H`CW@_$eqEdO9T_%~n)vsB416`Oj!H=;guyEvl${PE@YXWLzL zcT(G340nOg3LozO8?|5iDP!4Q!F3T#hHlgR$i_<#pYKs$OUaeUXa^&SUekoC~2Hd0=6-bVa$OBvkn8QjVM=s*UQCvv7j zevRuYhaaWBPdPv02lg=nk6|U#tH#I+VdTpFkmyJ<=Op}={=}|qxRnSp;wcbK2E`+C z29O~>z)?Fs$wkV;+e)O^YPWuyAS~mlGG0O{S?}BjWrE&zU0Lh{_ORJ9SY?wUO$o(# z%WcL`OMFM6X_|wA&Sq}niPt^`EB^+xSJA5G;zn5W7XfNvfzv}>=RcaJxPsO9- zMI~Gy%RvoC*uJZ>he7D?k4dkOFz3FXX5KZJsLf1+*$XNa0+0buD>ZdxQ`ln=l6RkO zjh%!JZEQ2!gB#DV&zO@KCiE5wQ2E->nS_W+w2_fPfWk@)ZR49N3cq*ZVzYkj+E(*Y_bS+ zaSg)6uLRsM&(fJ(OiR) z3qt3Ip(yC;CHJL-?Rk&JoN6s-4LhPhesM_masH@aJ5Kr{Wcn zRMo>Ou1>CgF8Huv8fAs!NmC?6!ZHsmEbC;aXXrB_InwL;n5w12^@%`(N@DsOH z;X?9%f0zck>q7T2$NT#!{z-+GTA1jp_kAtC4axom&UQO;R_c|3UsNi+eQxTjZja@c z@cLY74s+-0toP;&!Jl6hQB7*?n_q?QHGQXuR8P25hbve?@(Hc>e-+3@gG_B%sDJ#) z)e0bdJ52VN$Sy$lB*uFIlhg7H_O4E?Ghb5!9hsKfz}W8LB(`=x!UXJ-9%e~TA^A9F>&-7!T%NRWs?zHZqR(KPrGjnk{l;N<`@EuB&BpW zlYRb%Hr8i#=)4Ssan6UKDnkrvYi!Ldg=Kyg7Jn>np9-Qbr=&ZNW9$(SyCEZUPM$5% zRVDn&%?CX|pt3h>^zI9BLNY1l3eWZo&6Dz<*GL?n7N6`Hz8|ODRKG1vXDvi!1*1zm z9z#66nkLF+{UtvD5^vF2g9-=o>MG#`u5^#$JX-OsH8>mNLR<0VhnbhCcPp`)u}a$5 z`f8Zy)2GmCelU9>^%=t-w7S!d^%7zaLlr)KEHo{mx3WH+<~vd3o^t7{H}B{yKJqr^ z1T2GacR z7#3e5K*R4ni02D<6!N+Ipx||SpU0YSF~^C@4)5XaVmJHcaak}DXM%2HX5o1D zp%*~5r6D|~0MWD0=%JZAalbP)q{jT(ZS04&jfP)bM@RqjB@0TT7B7UPbRH4~5+JGv zAa^3izfMMO*{QB3(2dXcyku(V`zmFM{8)_EKrd^en{L$5g#VWsU_*8d#tD}aZK{`T zA6QVk!&Q1TOhK(R{bcdL5Ib_>#kKBbMpYZb*lV<2gY~TSci4(`8`cP07?ZM0B~vQr z;7bn)C028X=7g(zdcr_SE+mflqN$D9~11r@o(>` z4@(4)HAh`otO)=M9QKUr^MZ=xA6b!sZYTV!%z?HePD-o{S)3z>ouy8brc1#L7y-;7V^4 z=e?Su8#&N$R$lh+oGb8V#%lWe+|2Y&W=8lSDFoKy|0=c5zo`6|$abr}5|(3)d)x44 z%w{Jod%0KE2pxJYAlBw`+Y1|SY4qMQs-j_wRWvn23sg!NcZxL>M6rg7Mpq8BTn@Hk zo9V(&B~Nstve#2jKOQ8hnuB2-I|Fd~d1z|DzBhQqk%fP2(wj|;mS zg43KUuQn>KHds(Kp9@%j6_eS?)z=yrXf9U%oAS51Z~0Q`U2wYj)C45uM}zC>t0AEQ zYSXAGU0u7+R{9N@DtUV!!@uzVDJiQf$#sLAuG(oq5zI4yopwI7)t24W$)WtE&Chr9 zGxRs3En$BqxB@FgW-PvAo&U{EzH2z>k7IDTcQa#VOhM4`2Ys{ z;C~efmro;4Sd+b0K7II9U*Eksptt#xf=usi1xSHkkl!}5b6owPh4%8i+(@s^R{MpN zN0FfEiuVp03fR+RHMr8nV7MROQ{^UmgpC^EvcQ%URiY;T*)J#uc1J(`YO1g#6y@Ls z-uAd&&(}~>__#_P+jPR9rW51q*rRgs0CtBg1t!ucR)833lmYQRW+V!CirZaFyv>$c zj=qkke=WeOU%}P47yRlE`F2zMlD8(_9ojDlJdMBcbqwqVEiUv1&gCDCg#6o)O{mgL zb;^tId;;cY0lumN!a5ry+z`V-(~4oMj&gnA1{PlM`)ANWg6_xuD zUoK70Z!IAQXLozB{+oek8ZC1NY>*wZODTI*(GOnlzdAL&p2OM<9>q$HHRqyRB4VB$ zG|g=v$P>{w3YFUIq;MU`AF2%!g~2&h-sm9-D~2h6M1VwgGuL|zw%o&K8WQ^PjJpIF z*L&6fEveWfDRqa0fBbJ%^@Q3e{-Gq5=g^}ff>Ul9<{@5N{7F}$#(243K5hS-or#rY zZCmKQoh49I?+=(dk3D~#7v7TfX^$2eoSrLfK@i`LR%Y&KHt0IuZ~lCWO7-c>L!94i zVh_gGqtVp=8(`aM|5aVug0jc@iFbI@A9uauhUh%z=xfHB`1Ew$C;Tmcq{F)d;&=lf ze+-_3c~eNETM^ow%E{9=sD0${^fxcz_~P9ZcqU2) zEmNt_-%KD7*9^p>(D&>o=1o8(gK{5ypdj|(<0}wk93VclLzRtmGEThC!3#PUV5pIb zYM;OCg5CPdS~-Wi4#9;xsN;mKP@^SI^s?sl~BQf)t(EBpig zZ;lUrPeh+o-is#xMjk>_Px_5GgsYzLTSbVUhBD{(B`Njsx21T>w+I7QCxa&yUQ8-tIm;UELbBzCO(9dT-mMD-4@3i?G9N5H zaK**o->~hnd~k`2A-G}KmH03icZc^&&}r=@BtaVlQ`b+#y!~bYZ75Lq@f!T{aj!Sy z4Bt+9>H}ZcMq@PhnSY>_=v7Q_MP}h?q&9V@Lj})P!j5HG5d;OwU?H;gD7LMFz_~? zRJ}9UV{#)R;ghM(OxDG)zlLbz??)YtWiLu@e;@AH7VdP%6djnS%#A(}Niy4tI$v%U zVmWFR9Aol%_M6(5X5&>{O4Mfq^;~--ZMU~P+jS<}R6%FXmzb9p=#V#4map&}LEiRt zozUaQ{IP)Z;~xPHx}qqFKb-J7 zWP9VtwwPiy?g-ES22M8xHGES0!8@t=i6`v|@#BeS+>PYg9RxCe*n-gVJm63_nP0=U zEJQC=s6}Ug7;Ay6eNV_n zW6M?MfZdmECNa}|i2Ac9dML{zflLpcW#8A5++MnwKg8b}aJbozE84yrb^I-}bs`pL z891l2(WHiV$)y*N?KdQ^9?d(tFDP~rby{gPBIh81lzT_B7WX`q0)b^!o$!RCGi}^ zjYdz`m);Q(?!RoAq)gGT$#Gz*7P8GP+cini&LJh z#Mbyo^VJBCG-endLfa!LzvNA3UB~O}hr6GVp>A#S%D!7S9YV%uMTzG7Ua;8k^SmhrUcTh%!uGOq^5gKm%jzZm&0;>>{hR z+k7exe|y#T7pycFTwZj(lDts)a=bKO>M`Fl{drT8V5qA=`bJBNK>fb)VhihAeAJ7t zq))*l)`F_{gQ;2h=9TS?^FtDvhE?mRk~&z4ChooKd_4Mh8A7y5IF*)8w~JyT1&c7V z;uCNQ%Q8H4n8!j(NqT8es`^lFk07#EM`rKHrMeu z`Jk9Qq;od;qM^bxyivou&!>jycexJ)iet^HTSEs4d874{b7V<_T!qOo4)y-K&)WeHDc{??B8RW7-<~2S$u6kj( zGNDt{?7@#%(}4lo(dHFQ^emAIZyFX#V-@c4&7l1Qgidg{@qg1bfgR}^&I1#7H=e4q zcbjw%wQC@cRnObwYAeF+D)*ON&+uGF+J~>YlF3?#3nvR9&V~ICitU_Y`opC=#j*d} zQ=PK&1Mqdxy^c4`C4V_fgE9N?M1$Mx+gFIFovP4h7`$-zdDTvP-~g|V`q&44gXhM9 zU0c7LIq<8v&=xm?H!L>rnEJTvMq)Wyl zy2b@JN+un?JkDUhT4g8rFG5)Je&o|RKv^O|?duIm z#(v4=VguEzPiEhJ193 z_AA$^S;bF74fIs0)`NngZy#x73uEwGeSCZdlXn;^T!v1GR3sJ;cVraq2rFQ{fZN;g ztUbRUBzgw3%c2^aMNh@`e5esFqR1HG_4V#h!V9FDj&jK4&FPI_-P&^P%U6?%jYMzd zz{zcm9q&sPznBXbvEp`3I2U&Y1)>8~LevE4x(CSnth)^89nDTubB);k{jAyOlJ(?w z%C~PaAvM#H`L4HF6tdy~dl&XXcg9=gj>l4AMab(b*ZjWACu2V12YX-1#ET&xHr~z{ z+3MImI2$%@+u1!^MIM*ksakH;RsM+fp4NLbFdjh3H$UzZB5*`TJi=tUy)DZ<_2*pK zu`Om$A#Ng`(fo5j>MISh^ceie;8{muxcz5d^k&y(7 ziNWpLKbU#jKduB#miN&M!#3sH3|gD1nZ>C^yv0*IjazT#-c#~e=()QlG9>?z>)AvV zNe&AdjayDrYXAC7@aEvMF8AT1M{^y$#ouX4+^o*azB=HZE67IIKX;fGB(swHjDElK zq`hmKC2mI%okQ|k5MN@kzC_5_!44a;|unqG0`!_T4LluO^K z#M-}CIhPnM?LVKinE*6X7kHWXloa?xd#joI{+REW?)`g0K+i?r#e{pgvQX{}Zs5rx z+-mwmgASfee9t+siSLVS0@a6Y=Hz)Y&xy>liN;v1jdwrH2E0V($BQNX`ReizKRMse zkuk8q`o`e`x_-sX$YcY=BHuAPy1|gb=uJ4 z$OyEqOol5ot`peLtv}`#lqJahCD-(odk2&_=l&`uC4RR3>Ei{v96}dygo08X zC%$)ab+&-NWbbKkIZfK6>A*ey763=4#me0rvqop`p*MyA>MFQ2DZn$4$ ze`{AM`=~$vs-i@u!lidw``@2V6K9S0vr+uZL$+MK%nyN-QL~ZgGX!QJ7`a)cxPBo2PUBbcgO%K8luV{`WHN!8^~7HUCg1fU zz4X}sv=nMJdy>(sT-#Et8@*;qcj(-#b$lpkhO(p1nkyTPrAkj-uQUI5|%%pAue|v+q6tSslLccT4Yk zIPh3*_t9&z=ITtZXRMLLe-!@qYy}YGUB)rlzf4F^^B^l7j=1p5@U?CJ@J}$<*i229 zlVH(qL4ygez&9>qyjCgx5z@X{xn$?NhML6hpHEmDOAVwgrqk2gZTqDc8?~*Jd0aA2 z(%p%-wNK4Oy(Av_&4BX&+Yq&HgND|~^1PeAT%n7w_)B|+`xGlaQh2HwD|0mwE-^Q) zx9U;$dD)xU?Ja2RnmZ&-ajV$3Kqfwgo}rAQeEa$8vw$1oo1zeFnI3tgfcH|^$*n?*vjcH-hT|Gs;tVb2t2RCVzp2P1kg zt+y}$%fHRXlNoBAqpAOfPl?NuC$#o2Zv*aYeS58Ux8MTqLzqYmMDBmz7dW*0Sn`su z$}}{S7e2p|0Pmp7O@FxpjWhi%q$2nzt$se0(w3CyKoLJ)Od8JM=@|lVR<<|_7_EHe zd2Aue(8yKlOgKZzZ~5Ep`SaCt@UR7`IN|$`e`Nu<_t(Bi8`nP%c#}ggOg!wUUup0s z!5Y8#{(a2ilQ?dmo1q+qI~kAS-a99C{mQyPUo%5(R^14;jGu8YUf4@9h=>Ea29ka) zgkJ|^uj{|ObW$vT)YHfk<%fA*lZNlwy z2+I^SLGhka-h;E|S+r<+nH>yhQc2Rj8o!)ngsmSV~309#LO{w95Epm5Q@ z&#tCXOkcg<_qlkU*o^_2ZWB}JOuJE+O|%d7_hc8C1U`mHxxnec#Zz`7I~irhM1~!7 zMpK^ejOxp#zXc`+D&(dOu{fm(o#xDIvd)H9^1U{pJax6Me?!u5^7y-Yd29KPnwKfP zKiYW*OTuRCdD}E+p2#!TvU?{fCo!pe64}+XKlQC(x+}BnkxUm_BdYGF%jVtt<<9FnPtxqoxrES+@I`4t~djqqGfM?Lg~c+k?Q+nfY+*S}3=D}3r!=6aetcz%b% z{Dg^?A6pW>7J{AM-C%x_t#vAe6S{a+<+Ti@2-r{4nMmf-zi(wGYh%_bJxfNX@_R~=nvAY^zPw+_KeUxMpY4!P2cH~=i1R+etRA?;tbr0>VG;;sX zL+$T82a3zDh?Z%FgS+?V?F?LAp9Ign)svIR`zIsynKD2>_58&!*CTv<l$<3e9{#ajrEjYUQK>hx!USZpLsb%9KWF}T~6I!!+Rv6 z7yt(yvQNH?PR`|u0*=a-c_+UOW7!m%4e5r((ZVbB6v4)FWF0$cRmtnIXMzy_US)ba zQ6tVM-795H(pVAjbePEATC)VrkzvKDHaI2WwXGoJ^VH{7@8IG$^LtB9K`U7yY8TS! z-Tk_o&R4L-w15{^0t)VdJ zXIo!yIPiI8k90S;cuf)EtxKk4kY+wBnyRPF2NJv4A8<3Gcp3AH6~Mr=B^!7Yj))vyhtXmSuL0|3Eez ze-gI(S}c?^zb`gAVINZo0DyJ)Q&A!Gl)UM6eJ~WJ)YXA?k+u<`4L4{-FR{ zxb4y7>EK1t1)$!K+K||f1@)QfATZm9-1I=T3*y($Uk7?WUTu9RleK^}(yV_}osym| z`n@23{()!1eU*(E0yu4_n(Jp3ur9S9= zfm44SVicK>dY(=~%FC83AMG&m#PJ7kz!k5SvZ(skZxcl!+l%9{Ng5!=#ane zRcV~1DuT4^sONps<8EM;Y+K#c^!attw~BfXzx}r{KXetLTS)Kv89A+ZX4XM&nkj zt*g^NT`i?26I@Rwsw(upV}h+0bX~qUzrOZYR!sK%qWOBUm)+^;8I2uU#ZvJ;^VUZr z`^6M;kB}U~?l7hMR~J6lC!|y@QQe1mVqg;&6;(fzQP#&XCVv(a?pi{|U%4n`CuBsO|;wbid%+c8Q%la}inL;U_`|sXo!57iQeo25Lyg=yzdzI7kirkWEMoco9<-o%IUq~R{V{jh{&C0H!6*ic) zrtQMUyR8=QDZ=X)1iyQFIFQTeue!k9&pB?!YvKLb8KFW19A+lpU*mgIi~lg6+y~7M zXD`6sbwj@GB*N)D|(485Fx=ljwLw4%%PN=X=J{R(S>Cas=g4U(D6kvRBk`q(OR?>|Pt)2?owxybvl8jg%t^KHqeiMzVq|{z;K@5$ZDoJC zKr$}IaK*T8;dA{xx?_GL>aOrHEjOKz>gl9(>ow-bs2)1m7+z;H2p^iPQk|^I?>7Y= z8}*OF0xH+L*zaV8VO6NnYV6!Fb00D3?Q+HouewRG-rR;*lL!au`tX}=jT+{Ce<{T% z)JEOcDu8qK7Zje(CN*X@#j7VD#%nGS7z6yyeejCkvBwv_Ko=t++ef^sy(Log;o-H< ztYvJq0+sTBb@y`q^BVzk-^hl%=cB7Bq=2&irPu<5a)$o!5U4uc35>{lC!5j=M$7Y8le?`8nH6)$mdzmj?f!=yY!PKi?jWAur(gYX0pk%O6|P^T}Ew=5sBz zi0w-N-aSA#r`Sc~;=hZLJ1>VO!P$hEM-Uk!GDE}rT{MUClaAhq^EokWi~IomAMulI z6FE@v>9suq7U?c4+|fW$C6dGcXZcshI6coILe{k)sOFYUOS|M>1NW=NiG>yE`Zw;3 z$I_TnSPH0htOk?Tkbc}R{}$vYADcXpv&&6Ru=3(Zy+!{G^EuiPUtv+A$@e|^0i;hn zZ!MWL@Cd@G&-+Qq<4wBjJ63nR=@`-%U00uu94^BK)2_Mx?}@bCn*ZiF(}h21Pg8#d z%Gl3Q9iY9C=l)RmUBqUSfMK8YzWNfiIX`J4k77=3t!EM2pW06E*-14`3H0;mBsDEC z+g<7>{>jzQRw8@6_U(5&QNefHWistN7V$S(Pul1#v~0*Z?4`*Qgb%ZQBt)XL9rvUb zNI&0i|NW)nb+f6`+N&l__|98i4eB2snO56N4C9!bi_)CVuc;bVh=fP)&69DmxPATl zHg4x^O!HCc3bmQEi;UKQRJ!_`KzBRM|U{7PoS9OlKj9^dWQGi#Y**=>~DM`n@v zS4#S0SEiYZ)c&-2wZndJcJdIj3G3`F)tvn7pDDGp#9F=!vK|kEP0b+fOvi^3jT!Fd ztVm@MP3fFpFEz)FBz3ZYvV_tfsQ6z(fU*<`u9+@fa0u|6C~K{h)eGU z$e=B^Rs)P3azqt`Ze~{!1z-&@)n`PrP@A}n9PKI|gt}2VyZWkob#ytnxW0Pe+CCHG zaAM_tYkr2y4vt#D4=Ung8y*Y5@T$`)p%>TPMLxvnv`bXTw_AiF^^CS#Mnvu$sk?;K zU5AwCr`(<7rO||Z`YFNL7obe9mhR)%Sy)mNjb;;@9Sf99n4M3B2Qi9~%#t0f-_Uy; zk9h>b7Q-mr!1|4|iQZYHU$NyE>D?mz4xz4OBXV#+_DH?{lCU^Hp-Xx|q;wi(hL&M4 z;E)}q7~jX`JaC!r62n85$+`L*as=j5HX3o4L2A9@zE2#65sz7-MZn1>{UTw?B?E9I zh%=!YoN1hoS-C~b_f+AvRxQ8RM+k7OB4!!)feJP9!w<&ye>q4)4S}6^0j5P@XAYc% z0VJ&p8ct3l9rlf>2Lg*<4`O(6Gmu6jvE5;qd>`oEOwcKJyR%n3W?n~l9p67kYw3<+ z{qG~{o1A^tvy*&>++fsu*E}FsC+A-eFFM!9!~P#GI%mD%_3_|p*c+Sq~ieO8+QT{w*u;tE*{(Was`Z{<4JqCrH)%kfZ->b9WPE2Piv8 zNjFh;fwGH~PHnD8-i19t^7?woLC$62S5s1ZS@_kIj=dy&QF3s6_P5+irmdVKy&0w< zeC7hG!c^+e6os^JvzCOeWlf5;BxTKZtYv|I+^#GDaWu4oLsKH+mL0ciPTIAknkp7XN*E1=*ILerIqwVj-d_8? zofL6|cnSZu4K^fpT(4C)3Z$>viA}Q|onS^nN#D0x`@*Z`xh=KIjgdW>wPBVANh*IH zCos@1A?W?$aebD%Y=)iH`EZ`(&M&S9r-N@g*Msx3Jk_o3bCEp)`+aU2(nZ+&bM>6F z^%k6$3!Lbv#|`Bz{QVThA&d`UJcqHpmHeKi^CvJqO8?^T=joj5*Fn@57};Ki^Y;xn ztb~K}W+w=I?wSLfZYHPO;bnWT{Tte#LC_4P^c@*D^@PHyr8y2F!EcA7usR@$BL z@#KWTk+HcTCs^z;?6ryiS&1L#6#B?^YNx+$;J+yGU%Z8#*wi;;cgZH&r9M?nBS>S4 za**x&<@nbJi*n#R-{T6K8^p3^b_}QML?m*AcNeeccNZtzf%-KguTnnP>yR0bWH3|U zqmZ=s_S*IKUhB6|Mj1JU7Vc}QZnRAX!fjM`euZj4b5L1%8b7A~C_9a#JfQe+E)(Vz zQ=S=gYDmuE#9;1LTpB=NE_(m7L*qU>W!N|=I9~Le{JT{_{AA$)S=6L{DxN=Hd4*W6-U0-8;^QqR> zY&SM{0|EESCu4J^gyvKpzvUjY0&cf!b^?&xC_pjfT2Nud=ID_;&uYfM#Dqt=GQ;;2 z0Qu9-oog;ez=sFxim@We4P30n4eI*@~;~YFLfyWO6k2iy_ z3dUUH*c^V>>xj(glH&4)?29F`dYP{D{S8xcjhF@MHPQmv!|KbXd>3!v@0Iv_Z<+6W z?lh+~^r_QiKJ1Vz$8BZ#_UZlWfBAHAeVqSDd9J;;Aiw|_U{e#ixMx8Pqc;4r}`8)~$+Kx+e^}a=IQL`(ozyb_M`<|K9)r0RR8(SJ7_MFcfvGwybDF15Icjz{4Jqc;R*GHZ4-S zmZlp8p1juUjMAjCUDP5zfY0EOM?PVnzz-nt348)}VkeH5jwV=bC(V*u-rpZq32sJ9P2w$--m57#6I+_#%tBMP2PVuAm6>CvBwL%V1kd_qGb%lvD9n!IH> z`X4`jSM&L?C*?2vN7WHMpPIErwDy!Cl02Y7*YF^v=|mjqhUn^C1G@u)CoG+Y9oi#2 zc}^ffK8tmSQx=Fkig{WcNL^YE=in94#SBr6FDt8O<@BghG=l*YjzkPCL@6Al=s@op zlVOMR&71g=K!W*Whbmk%7>)SSgX{|7_>(D1v4!!wtpuBQ&dm9$8~UEh;)i#@W)LofGV7 z$vv`&uPwsjoFa08DZW&8>&_GRWxwQ^@3i3kQ_R;0_{R#Gh0ns;EbUzUIbK;YKAQV* z%xmnrE+A4~~s8w6FHnm$TMk!SdMJQ2G zjZv*pG&N#{#)_nZ5F=mS^Zf(gJ?Eb1-rs%hIrm&EgIksch9C{F3dqXf*0qP;z8*@T zo4)RkA0h+&bl41Tpb1d|!2>L z3RiJ4T$l>rBS{wiLE%(@V6eyhiwMLfY32f7!p$JA@^&IY2?%kkD~%4!||bI&RDZ&Ph)AUD)rKs)Z6-_IO?aK3G+dw<%OGF$WkMV#1_vN_3G{gkE? z@MlY6Y0~@gkEk_FrioU-8{{T`W;OHcD(uRtn?Q6l8Rw`0V^}C42T=qlL(quas^8ao zt!Dm!f)_|V^4t!7ZxO7i$Z$Y!hhjCf=ANcN#aSFDz9(V+ed{96zi-`m>4cOr5{bQ> z928KfUG&;)+PN^-!pR(NSndsMhAjPXOc7Spk{SG2s*}3m;u9);-PAP6;TVv4_@8Kg z<-D+^EH}Wn$?H8bHV?-%Znl(5dN-4cOXwv*47f7xP5ul?@mKemi?hQA` zd3Mj_u+V7#M$C!W}bFr7uie$EiM>pt#@_&J3w!Q1=cBlY8T#SD4T3mfe^k zokK^LZARwgjsx^+#hdl+9&Y#t61$x0NxGx?LIk7Hm^06xfj59#gD+O{H@Z?jk-`^@ zd#*jpApa%+tCwz1eefy9!+$-R;Ez{}!6+{gv#H~7m8prf_rhDT%S6u^hpVKPtJ9?K z{#wmPD%qMJop=T0Pq<$CWoQ@nq{ww3f1wqtHmCcZeg%t_OxJ0s;{(5rtalAIw{Dw# zH;n+CRy8z68a2irwUBieRGpB|KS}c`(m)N0SDnUhpRf5M^z>wcuvnJ*^zLAWPpU1y zbeFS8)j6b&P1JO#+>K5lIq%A)->n{U-Tdfq6YMvPl>0iM?OK}QdAiK^M9zD24qEBhG)K4kS{^ z@ohcprN4MDJBl&XCrltw&SP&hqU^_zK1^vVH@#QA$v@w$(u%*U>>rMD4--XFV;pX+ z8_E((4T=y{V_D)XJko33h^>jxcKuXndnc;p82mPbBkJ)1fVr}3usuq-&QADB<@7bS z_OR7t#H2N|A`{V-1Z|Hx%{FSYjTY>IawQwavmuZTFR-Bkdt+>6ta1{)*y;Km`jm0Y zj;hCma_k3nR;KPD&~y#g>rmmj3gvK-#pm18GVw}S%4o-n$m@-d{oBqqyMPp z!n&D%9p?_T0Qc34g{gig;GD2^oox*1B7m(8%uo~ zK-#|}_ly3vDJiI7Nxdnd*RX^HlHGde3ob^Bt>NLGJ#-%o^EKDd{m+u)+Dw*@L?U&Y zyxpaz&)UBn9zI&z6GyrH%>vVu$7wZ}h_*Pa694js>?O9Aq1c1Mw~Y^r6l}^`m@@dH zSsqxhF)6rFzVWTHy39^ff+ivCF@Pup@ z9qS#>p)wuxQRIW=?J)Fu5bG^@CvvV>hH5pb1rvrHMcmIXIh@_IP8k+Oo(cL|CY!eU zPk6(gQY2tsFenL6=JxO9^(>@Gb9!d^SMzu#`HS&*#t2Jr4$W4pGD5m?j)4FGi$%G! z@Mu~^&>nlysU6XL(>TU2j$(33(MkPw-`Ber{c6v9!*lXKOTg3fZuB&!*QY($;NXr( zKAIVZ8p^sWtNfDR5V|y(EnjDC7jfRK9Efipiwga*`q(q)9N;eR9;KC=n<5qiEsJ`# zdyW$7J?tNCU#^~g=wo&qk``&5?Kc0oN3%m(5l1!KJ2(j)Yy_gDK192R{$PHYeGoO6 z4OM*;aC(e^Na)&BIKJ0B*88lQ5(2s-K+rHB##SVb+E|V=R~Ckf($gIbhxd8(=S20> z5Fz#4-I#yE-26y8p4)Hs%4wLtP#wot9nSSu!|)Ryfqs@LL+JfOjT12C_lT6_dw|!z zI3u@(-Bq9a)q0;;XYM&qwP|i_)+U1&$)Z?ayNRNO-H7?Eh%2PN7FJ*L z97iQR*6YI&M$;K8+vGY4E0jb3ex;-zCw0i@+KY-^U|6JU^~RihKl}2y2|v(F5Bwb% zY22+G$h=4i9+%euy2xqbBs?Cpm=>voP4w#%Do?%YZJN|$x<&jw$FKm zZwS@|G>plEw@m`=j$Z$i9bIAyCl)Q!&z%6*yKPM^Yy0tllRMpbsZC*S_k*#EEua0X zNGf8IsOQD7e0kf99!>&I4AQrTHX}W}emrFGA*h4|IT#x0gpW7E=# z8zdXeNoc7E1^-^3O#s1X3tWtoeVQVos*^7P@n#KE&T3WmNLA`ybZt=%#ml-MW z>+ucYHoX|@X_PnNy0`OGkf{_Q=hxg^?wqI-UAQeq4_MD#=gLuevd6bJV#3AO zd_eyNkDJrWFIkB*_cs>o&TCQh0$dg^CUSr2dZTiYskahNu|_ev?-eBP{uT5Xc5M>`!1LNk)ePO3*aRn0r)QsyMMC) literal 0 HcmV?d00001 diff --git a/utils/Simulation-master/balance/series_legs/slprj/_sfprj/leg_sim/_self/sfun/info/binfo.mat b/utils/Simulation-master/balance/series_legs/slprj/_sfprj/leg_sim/_self/sfun/info/binfo.mat new file mode 100644 index 0000000000000000000000000000000000000000..a218a1adde980eae7eee70192a71ee1fa43a3544 GIT binary patch literal 834 zcmeZu4DoSvQZUssQ1EpO(M`+DN!3vZ$Vn_o%P-2c0*X01nwjV*I2WZRmZYXAo6bqDJbClz(w#>Q-gcgpoLRZjAkoB8g_lRQ z)Ai*_)fHTgep+Xpg_9CI&YtD;^3^(f#`B}Eu4jp-jwj2{9XHgCS88lrA#`)@%%ge^ ztuI&HYq|Aq=Jd%kr_Y!&xvOB0y1|tRp-iGFyPv8o6IbS*mhvo#O;%nY+Cbq^JFixs zZkyZDz4;QX@Ymta=O@exwFkmp8F=h@xHxB*-ZgGcops&e*}9qKZ+JW23Dj~u zGJY~=^9_f_ot*P|r5ztGm=M8QXg+zuPf>xtQ7PYbH|!McIWpB-pipdwMaMZ$-+ujb zXIht73!N2g{xr8?hsIV5`{g>ON7tI@?_`OH5^dgGB)DWsLEF@@2{k-|x{kuF3{EfQ z*#utBFRE=h#T#Z)n_y>fSu<&)c=W&YB!_eVeR%4Aw%qNOyXWH|rri2X)Zk%%d>Y%8 zAEJ$)avLl*ZaKs$GWCRNNl{gSXA-B^O`eeON%Ln;pBObIa?*_HbGX9o1U_1=$}f0v z+2PXh4p;diEA`!T*4&=5<#1Nx-=@grmABXIyJ4*JVDIlX`=fHt^cXTbzdir}U6N+d literal 0 HcmV?d00001 diff --git a/utils/Simulation-master/balance/series_legs/slprj/_sfprj/leg_sim/amsi_serial.mat b/utils/Simulation-master/balance/series_legs/slprj/_sfprj/leg_sim/amsi_serial.mat new file mode 100644 index 0000000000000000000000000000000000000000..6ab35cb3da512900ce39e10d408b30259f98ffb9 GIT binary patch literal 227 zcmeZu4DoSvQZUssQ1EpO(M`+DN!3vZ$Vn_o%P-2c0*X01nwjV*I2WZRmZYXAsHBRZtN5B7TDI=y(eVL+VhbIjY1-F&YTQ- z%d*5M>1AJSgxIGgPgZUExAxogC2mH?SdQNEF-dNnC2*JBz|6XzlROV4{YrEwR|z7G$3wsm%I{G4Mpf)dde zuRIeM>71o|WP4_Xj#Htg4_7u8y)4Pte7Po3W{a{ab&d9-!C^Y&em zQ9mP!k3Dz|pre>2~GRG0KLI3jM7Q({eAmq(#yhLgnn zuxC>XQs(h&7w<`v_`ATU+0>|PPx+gSbJ>5&-tJia{pjHZwtnTEx3}}fZ99^jaJ;_v HakvHmDS|$F literal 0 HcmV?d00001 diff --git a/utils/Simulation-master/balance/series_legs/slprj/_sfprj/precompile/8TxxFELrVOZf4nge1lJ5jH.mat b/utils/Simulation-master/balance/series_legs/slprj/_sfprj/precompile/8TxxFELrVOZf4nge1lJ5jH.mat new file mode 100644 index 0000000000000000000000000000000000000000..61873011b874fdea560c9fe885ce48bbaeeea710 GIT binary patch literal 657 zcmeZu4DoSvQZUssQ1EpO(M`+DN!3vZ$Vn_o%P-2c0*X01nwjV*I2WZRmZYXAwB?t7bieBYs2iySs4YRoDtznLbJxGkl_DD8dbHw)`;hTk}>{k)z;{A7$-zBgwB zL!{c$ekJ?B7Cy!YZv-A3=+XD#Pq=x^PpA=OeW=vjs zkMl&znN6}~5po~5JlR$DaqS!LOTse~x{`9$<{2-2&+&3KXG&Q8pw zS~|~bwsZE436&EQQ(t8Nx%=h)0_AlF4*#vZc7A`k-#!D|q%HI8=IB+np4|Gx>9$F; zlN1S@K`@pXAkr1PxY7k5B%ACOL+S&{TJfjCr+*uPLmd2uIbOJ=C@_%Pf6ER z&$$gBojLD&D&^tCwopY@uDi30!jIhvj&!@3b#w8fKmLV_Z43Lqeir|7dUmq<{2B8$ zXYD#sKV5FlI~Nu2i97E66tAe~SeLo`)!6|4_9}}jhZoAtI}pJC{ic4wj{6t-zXz+& zKm2KuX79;V&7`wS6+XB5oy-2+HSdP{*KbWHWgh2;Jlk+>jl8^#{jI;ZcGTL>d-eCP Z(TiswF@ literal 0 HcmV?d00001 diff --git a/utils/Simulation-master/balance/series_legs/slprj/_sfprj/precompile/JPu9Qdz3VaGiF2LSvQFOuE.mat b/utils/Simulation-master/balance/series_legs/slprj/_sfprj/precompile/JPu9Qdz3VaGiF2LSvQFOuE.mat new file mode 100644 index 0000000000000000000000000000000000000000..0ed5fbaa66b64d811912a66f806dc5f2ae95f824 GIT binary patch literal 693 zcmeZu4DoSvQZUssQ1EpO(M`+DN!3vZ$Vn_o%P-2c0*X01nwjV*I2WZRmZYXAsHBRZtN5B7TDI=y(eVL+VhbIjY1-F&YTQ- z%d*5M>1AJSgxIGgPgZUExAxogC2mH?SdQNEF-dNnC2*JBz|6XzlROV4{YrEwR|z7G$3wsm%I{G4Mpf)dde zuRIeM>71o|WP4_Xj#Htg4_7u8y)4Pte7Po3W{a{ab&d9-!C^Y&em zQ9mP!k3Dz|pre>2~GRG0KLI3jM7Q({eAmq(#yhLgnn zuxC>XQs(h&7w<`v_`ATU+0>|PPx+gSbJ>5&-tJia{pjHZwtnTEx3}}fZ99^jaJ;_v HakvHmDS|$F literal 0 HcmV?d00001 diff --git a/utils/Simulation-master/balance/series_legs/slprj/_sfprj/precompile/MkkVTTjQfD4eZeRuuxNw2D.mat b/utils/Simulation-master/balance/series_legs/slprj/_sfprj/precompile/MkkVTTjQfD4eZeRuuxNw2D.mat new file mode 100644 index 0000000000000000000000000000000000000000..823d3099631d15c4d4dd086c34d375669794fef1 GIT binary patch literal 670 zcmeZu4DoSvQZUssQ1EpO(M`+DN!3vZ$Vn_o%P-2c0*X01nwjV*I2WZRmZYXA8z&st# zg$-Ui4{Y6A_(X8QO07_ZkgdGs`DL3VH)gNTS%2f{$qJTvEFU>!veoac+s^r9x$Tyz z?ozpFZ_a=F^CS3)EyItWYzfC}=1cY_=rN~%x7qsb_+|a)%WIU_F0?N{w?BQ##IH=# zH=n=rJ@0Jk^M7`GO26eV=2G3*Q#P!sz3B=_XN+^`{o3t zbI)G>{lc@A^~(OeCY871g|8Ry6T8lyyD6hkzU+VebdFu6l@H%IFFrNp2=C3OvsSpq zr9{kB`K!Hdu4tjF+`IRa&;EM0b#s63F`v@k%~v=>XUySzbZeWS&kC(A_Rg7}Hnx)& zebCzc>W}$(!I|;rw;zdpFPa^ z-qlV8Pjg+gR{(XB~ug>GyAO359NPKYkKs&p@4}%ZB4`#P_an}^r zzrK3HXy47U{q`Fo@A1#AW%=x_W~!#HDxWPB(<`FpzO|N5=GOLuM>!8`_U&J3)ZTaI nUg+N9Ex)%G{(in=_4lK9GydA#WqtRwYX{rAYS!ty{T2ZL;0QBh literal 0 HcmV?d00001 diff --git a/utils/Simulation-master/balance/series_legs/slprj/_sfprj/precompile/N8jHiuL3FACM3BiH3xonGE.mat b/utils/Simulation-master/balance/series_legs/slprj/_sfprj/precompile/N8jHiuL3FACM3BiH3xonGE.mat new file mode 100644 index 0000000000000000000000000000000000000000..4741eb0fe75a82e2ded7b84c51872c3f34183f74 GIT binary patch literal 670 zcmeZu4DoSvQZUssQ1EpO(M`+DN!3vZ$Vn_o%P-2c0*X01nwjV*I2WZRmZYXA8z&st# zg$-Ui4{Y6A_(X8QO07_ZkgdGs`DL3VH)gNTS%2f{$qJTvEFU>!veoac+s^r9x$Tyz z?ozpFZ_a=F^CS3)EyItWYzfC}=1cY_=rN~%x7qsb_+|a)%WIU_F0?N{w?BQ##IH=# zH=n=rJ@0Jk^M7`GO26eV=2G3*Q#P!sz3B=_XN+^`{o3t zbI)G>{lc@A^~(OeCY871g|8Ry6T8lyyD6hkzU+VebdFu6l@H%IFFrNp2=C3OvsSpq zr9{kB`K!Hdu4tjF+`IRa&;EM0b#s63F`v@k%~v=>XUySzbZeWS&kC(A_Rg7}Hnx)& zebCzc>W}$(!I|;rw;zdpFPa^ z-qlV8Pjg+gR{(XB~ug>GyAO359NPKYkKs&p@4}%ZB4`#P_an}^r zzrK3HXy47U{q`Fo@A1#AW%=x_W~!#HDxWPB(<`FpzO|N5=GOLuM>!8`_U&J3)ZTaI nUg+N9Ex)%G{(in=_4lK9GydA#WqtRwYX{rAYS!ty{T2ZL;$bso literal 0 HcmV?d00001 diff --git a/utils/Simulation-master/balance/series_legs/slprj/_sfprj/precompile/pz8kySzLQtSuU5A4IQZHYD.mat b/utils/Simulation-master/balance/series_legs/slprj/_sfprj/precompile/pz8kySzLQtSuU5A4IQZHYD.mat new file mode 100644 index 0000000000000000000000000000000000000000..87ab1e680d779be4db5e52ec62066a6890ce0bfd GIT binary patch literal 633 zcmeZu4DoSvQZUssQ1EpO(M`+DN!3vZ$Vn_o%P-2c0*X01nwjV*I2WZRmZYXA5tFJ4@iEWXd#vSS= zrD{A+^1O7v`#q)x`}qvl&VJwfkYU-C@}l#}+VXSe{k3?ie6d~Np5@eX7pT*DhiLH|&y!IQ|CC!Qrv((C7X5uaItfL|>!*uz%nJJQ4Q#N}qy5K)^ zdD4kXKNWPkH8efK`66YbHEab8)lU3veK^Cjc4@$069%`d=4n4&*VpCJ+pMSa``t5Mb7tZWRq1tZx|?i{UJ`j}8S|RE zfOX#eyHUIU*jfKPQE=o+PW=R{WkQdO9b;~rH3?4L`Av7qv?G&?HH+S-h&}yp(SP7i z?!F7}6ZbEWeV3~q%)9xZ>#HWAzKz1U{B`d0UVL=v>GtQ2tnVoq~GpiHd zE!0@_`e(uu8H}uQ0?H