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 0000000..0784ce8 Binary files /dev/null and b/utils/Simulation-master/balance/series_legs/slprj/_jitprj/s1PQAPIlnTPKm6mZzJoZZuB.l differ 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 0000000..d1eaf63 Binary files /dev/null and b/utils/Simulation-master/balance/series_legs/slprj/_jitprj/s1PQAPIlnTPKm6mZzJoZZuB.mat differ diff --git a/utils/Simulation-master/balance/series_legs/slprj/_jitprj/skZgecjo1NwrHsHAQiPZ4HE.l b/utils/Simulation-master/balance/series_legs/slprj/_jitprj/skZgecjo1NwrHsHAQiPZ4HE.l new file mode 100644 index 0000000..4918c02 Binary files /dev/null and b/utils/Simulation-master/balance/series_legs/slprj/_jitprj/skZgecjo1NwrHsHAQiPZ4HE.l differ diff --git a/utils/Simulation-master/balance/series_legs/slprj/_jitprj/skZgecjo1NwrHsHAQiPZ4HE.mat b/utils/Simulation-master/balance/series_legs/slprj/_jitprj/skZgecjo1NwrHsHAQiPZ4HE.mat new file mode 100644 index 0000000..0df35f0 Binary files /dev/null and b/utils/Simulation-master/balance/series_legs/slprj/_jitprj/skZgecjo1NwrHsHAQiPZ4HE.mat differ diff --git a/utils/Simulation-master/balance/series_legs/slprj/_jitprj/snB7M5QwQI4hNRLbuP13ztG.l b/utils/Simulation-master/balance/series_legs/slprj/_jitprj/snB7M5QwQI4hNRLbuP13ztG.l new file mode 100644 index 0000000..ec52173 Binary files /dev/null and b/utils/Simulation-master/balance/series_legs/slprj/_jitprj/snB7M5QwQI4hNRLbuP13ztG.l differ 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 0000000..2dd2a83 Binary files /dev/null and b/utils/Simulation-master/balance/series_legs/slprj/_jitprj/snB7M5QwQI4hNRLbuP13ztG.mat differ 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 0000000..8c7ae55 Binary files /dev/null and b/utils/Simulation-master/balance/series_legs/slprj/_sfprj/EMLReport/s1PQAPIlnTPKm6mZzJoZZuB.mat differ diff --git a/utils/Simulation-master/balance/series_legs/slprj/_sfprj/EMLReport/skZgecjo1NwrHsHAQiPZ4HE.mat b/utils/Simulation-master/balance/series_legs/slprj/_sfprj/EMLReport/skZgecjo1NwrHsHAQiPZ4HE.mat new file mode 100644 index 0000000..3078668 Binary files /dev/null and b/utils/Simulation-master/balance/series_legs/slprj/_sfprj/EMLReport/skZgecjo1NwrHsHAQiPZ4HE.mat differ diff --git a/utils/Simulation-master/balance/series_legs/slprj/_sfprj/EMLReport/snB7M5QwQI4hNRLbuP13ztG.mat b/utils/Simulation-master/balance/series_legs/slprj/_sfprj/EMLReport/snB7M5QwQI4hNRLbuP13ztG.mat new file mode 100644 index 0000000..895fb3c Binary files /dev/null and b/utils/Simulation-master/balance/series_legs/slprj/_sfprj/EMLReport/snB7M5QwQI4hNRLbuP13ztG.mat differ 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 0000000..a218a1a Binary files /dev/null and b/utils/Simulation-master/balance/series_legs/slprj/_sfprj/leg_sim/_self/sfun/info/binfo.mat differ 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 0000000..6ab35cb Binary files /dev/null and b/utils/Simulation-master/balance/series_legs/slprj/_sfprj/leg_sim/amsi_serial.mat differ diff --git a/utils/Simulation-master/balance/series_legs/slprj/_sfprj/precompile/72vZ1DxF0Nmp1uyeOVki4F.mat b/utils/Simulation-master/balance/series_legs/slprj/_sfprj/precompile/72vZ1DxF0Nmp1uyeOVki4F.mat new file mode 100644 index 0000000..0ed5fba Binary files /dev/null and b/utils/Simulation-master/balance/series_legs/slprj/_sfprj/precompile/72vZ1DxF0Nmp1uyeOVki4F.mat differ 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 0000000..6187301 Binary files /dev/null and b/utils/Simulation-master/balance/series_legs/slprj/_sfprj/precompile/8TxxFELrVOZf4nge1lJ5jH.mat differ 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 0000000..0ed5fba Binary files /dev/null and b/utils/Simulation-master/balance/series_legs/slprj/_sfprj/precompile/JPu9Qdz3VaGiF2LSvQFOuE.mat differ 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 0000000..823d309 Binary files /dev/null and b/utils/Simulation-master/balance/series_legs/slprj/_sfprj/precompile/MkkVTTjQfD4eZeRuuxNw2D.mat differ 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 0000000..4741eb0 Binary files /dev/null and b/utils/Simulation-master/balance/series_legs/slprj/_sfprj/precompile/N8jHiuL3FACM3BiH3xonGE.mat differ 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 0000000..87ab1e6 Binary files /dev/null and b/utils/Simulation-master/balance/series_legs/slprj/_sfprj/precompile/pz8kySzLQtSuU5A4IQZHYD.mat differ