From 6bac98da0abf33e150966e657a2eafed40e70439 Mon Sep 17 00:00:00 2001 From: bmy <2583236812@qq.com> Date: Fri, 26 Apr 2024 16:13:07 +0800 Subject: [PATCH] =?UTF-8?q?feat:=20=E5=A2=9E=E5=8A=A0=E9=80=9F=E5=BA=A6?= =?UTF-8?q?=E7=8E=AF?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- 3rd-part/PID-Library/pid.c | 15 ++-- 3rd-part/PID-Library/pid.h | 127 +++++++++++++++---------------- BC2D.ATWP | 2 +- BC2D.code-workspace | 3 +- app/by_can.c | 19 +++-- app/by_can.h | 3 +- app/by_motion.c | 104 ++++++++++++++++++++++--- app/by_motion.h | 26 ++++++- app/by_utils.h | 15 ++++ project/inc/at32f413_int.h | 1 + project/src/at32f413_int.c | 110 ++++++++++++++++---------- project/src/at32f413_wk_config.c | 14 +++- 12 files changed, 303 insertions(+), 136 deletions(-) diff --git a/3rd-part/PID-Library/pid.c b/3rd-part/PID-Library/pid.c index d65326c..1eda413 100644 --- a/3rd-part/PID-Library/pid.c +++ b/3rd-part/PID-Library/pid.c @@ -36,10 +36,11 @@ void PID_Init(PID_TypeDef *uPID) { uPID->OutputSum = uPID->OutMin; } - + else { } } -void PID(PID_TypeDef *uPID, float *Input, float *Output, float *Setpoint, float Kp, float Ki, float Kd, PIDPON_TypeDef POn, PIDCD_TypeDef ControllerDirection) + +void PID(PID_TypeDef *uPID, const float *Input, float *Output, const float *Setpoint, float Kp, float Ki, float Kd, PIDPON_TypeDef POn, PIDCD_TypeDef ControllerDirection) { /* ~~~~~~~~~~ Set parameter ~~~~~~~~~~ */ uPID->MyOutput = Output; @@ -56,7 +57,7 @@ void PID(PID_TypeDef *uPID, float *Input, float *Output, float *Setpoint, float } -void PID2(PID_TypeDef *uPID, float *Input, float *Output, float *Setpoint, float Kp, float Ki, float Kd, PIDCD_TypeDef ControllerDirection) +void PID2(PID_TypeDef *uPID, const float *Input, float *Output, const float *Setpoint, float Kp, float Ki, float Kd, PIDCD_TypeDef ControllerDirection) { PID(uPID, Input, Output, Setpoint, Kp, Ki, Kd, _PID_P_ON_E, ControllerDirection); } @@ -65,10 +66,10 @@ void PID2(PID_TypeDef *uPID, float *Input, float *Output, float *Setpoint, float uint8_t PID_Compute(PID_TypeDef *uPID) { - float input=0; - float error=0; - float dInput=0; - float output=0; + float input; + float error; + float dInput; + float output; /* ~~~~~~~~~~ Check PID mode ~~~~~~~~~~ */ if (!uPID->InAuto) diff --git a/3rd-part/PID-Library/pid.h b/3rd-part/PID-Library/pid.h index ebfddc6..374bc53 100644 --- a/3rd-part/PID-Library/pid.h +++ b/3rd-part/PID-Library/pid.h @@ -1,4 +1,4 @@ - /* +/* ------------------------------------------------------------------------------ ~ File : pid.h ~ Author : Majid Derhambakhsh @@ -6,9 +6,9 @@ ~ Created: 02/11/2021 03:43:00 AM ~ Brief : ~ Support: - E-Mail : Majid.do16@gmail.com (subject : Embedded Library Support) + E-Mail : Majid.do16@gmail.com (subject : Embedded Library Support) - Github : https://github.com/Majid-Derhambakhsh + Github : https://github.com/Majid-Derhambakhsh ------------------------------------------------------------------------------ ~ Description: @@ -25,11 +25,9 @@ #include #include -// #include "zf_common_headfile.h" - /* ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ Defines ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ */ /* ------------------------ Library ------------------------ */ -#define _PID_LIBRARY_VERSION 1.0.0 +#define _PID_LIBRARY_VERSION 1.0.0 /* ------------------------ Public ------------------------- */ #define _PID_8BIT_PWM_MAX UINT8_MAX @@ -37,37 +35,36 @@ #ifndef _FALSE - #define _FALSE 0 +#define _FALSE 0 #endif #ifndef _TRUE - #define _TRUE 1 +#define _TRUE 1 #endif /* ---------------------- By compiler ---------------------- */ #ifndef GetTime - /* ---------------------- By compiler ---------------------- */ +/* ---------------------- By compiler ---------------------- */ - #ifdef __CODEVISIONAVR__ /* Check compiler */ +#ifdef __CODEVISIONAVR__ /* Check compiler */ - #define GetTime() 0 +#define GetTime() 0 - /* ------------------------------------------------------------------ */ +/* ------------------------------------------------------------------ */ - #elif defined(__GNUC__) /* Check compiler */ +#elif defined(__GNUC__) /* Check compiler */ - #define GetTime() 0 +#define GetTime() 0 - /* ------------------------------------------------------------------ */ +/* ------------------------------------------------------------------ */ - - #else - #endif /* __CODEVISIONAVR__ */ - /* ------------------------------------------------------------------ */ +#else +#endif /* __CODEVISIONAVR__ */ + /* ------------------------------------------------------------------ */ #endif @@ -75,64 +72,61 @@ /* ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ Types ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ */ /* PID Mode */ -typedef enum -{ - - _PID_MODE_MANUAL = 0, - _PID_MODE_AUTOMATIC = 1 - -}PIDMode_TypeDef; +typedef enum { + + _PID_MODE_MANUAL = 0, + _PID_MODE_AUTOMATIC = 1 + +} PIDMode_TypeDef; /* PID P On x */ -typedef enum -{ - - _PID_P_ON_M = 0, /* Proportional on Measurement */ - _PID_P_ON_E = 1 - -}PIDPON_TypeDef; +typedef enum { + + _PID_P_ON_M = 0, /* Proportional on Measurement */ + _PID_P_ON_E = 1 + +} PIDPON_TypeDef; /* PID Control direction */ -typedef enum -{ - - _PID_CD_DIRECT = 0, - _PID_CD_REVERSE = 1 - -}PIDCD_TypeDef; +typedef enum { + + _PID_CD_DIRECT = 0, + _PID_CD_REVERSE = 1 + +} PIDCD_TypeDef; /* PID Structure */ typedef struct { - - PIDPON_TypeDef POnE; - PIDMode_TypeDef InAuto; - PIDPON_TypeDef POn; - PIDCD_TypeDef ControllerDirection; + PIDPON_TypeDef POnE; + PIDMode_TypeDef InAuto; - uint32_t LastTime; - uint32_t SampleTime; + PIDPON_TypeDef POn; + PIDCD_TypeDef ControllerDirection; - float DispKp; - float DispKi; - float DispKd; + uint32_t LastTime; + uint32_t SampleTime; - float Kp; - float Ki; - float Kd; + float DispKp; + float DispKi; + float DispKd; - float *MyInput; - float *MyOutput; - float *MySetpoint; + float Kp; + float Ki; + float Kd; - float OutputSum; - float LastInput; + float *MyInput; + float *MyOutput; + float *MySetpoint; - float OutMin; - float OutMax; - -}PID_TypeDef; + float OutputSum; + float LastInput; + + float OutMin; + float OutMax; + +} PID_TypeDef; /* ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ Variables ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ */ /* ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ Enum ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ */ @@ -141,14 +135,15 @@ typedef struct /* ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ Prototype ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ */ /* :::::::::::::: Init ::::::::::::: */ void PID_Init(PID_TypeDef *uPID); -void PID(PID_TypeDef *uPID, float *Input, float *Output, float *Setpoint, float Kp, float Ki, float Kd, PIDPON_TypeDef POn, PIDCD_TypeDef ControllerDirection); -void PID2(PID_TypeDef *uPID, float *Input, float *Output, float *Setpoint, float Kp, float Ki, float Kd, PIDCD_TypeDef ControllerDirection); + +void PID(PID_TypeDef *uPID, const float *Input, float *Output, const float *Setpoint, float Kp, float Ki, float Kd, PIDPON_TypeDef POn, PIDCD_TypeDef ControllerDirection); +void PID2(PID_TypeDef *uPID, const float *Input, float *Output, const float *Setpoint, float Kp, float Ki, float Kd, PIDCD_TypeDef ControllerDirection); /* ::::::::::: Computing ::::::::::: */ uint8_t PID_Compute(PID_TypeDef *uPID); /* ::::::::::: PID Mode :::::::::::: */ -void PID_SetMode(PID_TypeDef *uPID, PIDMode_TypeDef Mode); +void PID_SetMode(PID_TypeDef *uPID, PIDMode_TypeDef Mode); PIDMode_TypeDef PID_GetMode(PID_TypeDef *uPID); /* :::::::::: PID Limits ::::::::::: */ @@ -159,7 +154,7 @@ void PID_SetTunings(PID_TypeDef *uPID, float Kp, float Ki, float Kd); void PID_SetTunings2(PID_TypeDef *uPID, float Kp, float Ki, float Kd, PIDPON_TypeDef POn); /* ::::::::: PID Direction ::::::::: */ -void PID_SetControllerDirection(PID_TypeDef *uPID, PIDCD_TypeDef Direction); +void PID_SetControllerDirection(PID_TypeDef *uPID, PIDCD_TypeDef Direction); PIDCD_TypeDef PID_GetDirection(PID_TypeDef *uPID); /* ::::::::: PID Sampling :::::::::: */ diff --git a/BC2D.ATWP b/BC2D.ATWP index e4f44cc..f17d0ca 100644 --- a/BC2D.ATWP +++ b/BC2D.ATWP @@ -81,7 +81,7 @@ 0;0;0 0;0;0 0;0;0 - 0;0;0 + 1;1;0 0;0;0 0;0;0 0;0;0 diff --git a/BC2D.code-workspace b/BC2D.code-workspace index f333bcb..0829fea 100644 --- a/BC2D.code-workspace +++ b/BC2D.code-workspace @@ -30,7 +30,8 @@ "titleBar.activeBackground": "#1D18AE", "titleBar.activeForeground": "#FAFAFE" }, - "EIDE.OpenOCD.ExePath": "D:/Program Files (x86)/at32_OpenOCD_V2.0.2/bin/openocd.exe" + "EIDE.OpenOCD.ExePath": "D:/Program Files (x86)/at32_OpenOCD_V2.0.2/bin/openocd.exe", + "cortex-debug.variableUseNaturalFormat": true }, "extensions": { } diff --git a/app/by_can.c b/app/by_can.c index 18814a2..cb1b32a 100644 --- a/app/by_can.c +++ b/app/by_can.c @@ -2,8 +2,11 @@ #include #include +#include "dwt_delay.h" +#include "by_utils.h" +#include "by_debug.h" -uint8_t by_can_send_stdd(uint32_t id, const uint8_t *data, uint8_t len) +by_error_status by_can_send_stdd(uint32_t id, const uint8_t *data, uint8_t len, uint16_t timeout) { assert(id < 0x7FF); @@ -23,9 +26,15 @@ uint8_t by_can_send_stdd(uint32_t id, const uint8_t *data, uint8_t len) transmit_mailbox = can_message_transmit(CAN1, &tx_message_struct); /* 将以上待发送报文写入发送邮箱并请求发送 */ /* 等待该邮箱发送成功—对应邮箱发送成功标志置起 */ - while (can_transmit_status_get(CAN1, (can_tx_mailbox_num_type)transmit_mailbox) != - CAN_TX_STATUS_SUCCESSFUL) - ; + while (can_transmit_status_get(CAN1, (can_tx_mailbox_num_type)transmit_mailbox) != CAN_TX_STATUS_SUCCESSFUL) { + // LOGD("CAN#SEND: timeout=%d", timeout); + if (0 == timeout--) { + LOGW("CAN#TIMEOUT: ID=0x%x", id); - return 0; + return BY_ERROR; + } + DWT_Delay(10); + } + + return BY_SUCCESS; } diff --git a/app/by_can.h b/app/by_can.h index 6c5491d..f7f72ff 100644 --- a/app/by_can.h +++ b/app/by_can.h @@ -2,7 +2,8 @@ #define _BY_CAN_H__ #include "at32f413.h" +#include "by_utils.h" -uint8_t by_can_send_stdd(uint32_t id, const uint8_t *data, uint8_t len); +by_error_status by_can_send_stdd(uint32_t id, const uint8_t *data, uint8_t len, uint16_t timeout); #endif \ No newline at end of file diff --git a/app/by_motion.c b/app/by_motion.c index eae0444..b29e6d6 100644 --- a/app/by_motion.c +++ b/app/by_motion.c @@ -1,15 +1,20 @@ #include "by_motion.h" +#include "pid.h" #include "dwt_delay.h" #include "by_utils.h" +#include "by_debug.h" #define DRV_ENABLE() gpio_bits_set(GPIOB, GPIO_PINS_15) #define DRV_DISABLE() gpio_bits_reset(GPIOB, GPIO_PINS_15) -int16_t speed_m1; -int16_t speed_m2; +by_motor_param param_m1; +by_motor_param param_m2; +PID_TypeDef pid_m1; +PID_TypeDef pid_m2; +uint8_t motion_enable_flag; -void by_motion_pwm_m1(int32_t pwm_duty) +void by_motion_set_pwm_m1(int32_t pwm_duty) { pwm_duty = clip_s32(pwm_duty, -449, 449); // 不可以拉满哦 pwm_duty += 499; @@ -18,7 +23,7 @@ void by_motion_pwm_m1(int32_t pwm_duty) tmr_channel_value_set(TMR1, TMR_SELECT_CHANNEL_1, pwm_duty); } -void by_motion_pwm_m2(int32_t pwm_duty) +void by_motion_set_pwm_m2(int32_t pwm_duty) { pwm_duty = clip_s32(pwm_duty, -449, 449); // 不可以拉满哦 pwm_duty += 499; @@ -29,21 +34,100 @@ void by_motion_pwm_m2(int32_t pwm_duty) int16_t by_motion_get_speed_m1(void) { - speed_m1 = (int16_t)tmr_counter_value_get(TMR2); +#define alpha (0.1f) + static float last_speed = 0.0f; + param_m1.real_speed = alpha * last_speed + (1.0f - alpha) * (float)(-1 * (int16_t)tmr_counter_value_get(TMR2)); + last_speed = param_m1.real_speed; tmr_counter_value_set(TMR2, 0); - return speed_m1; + return (int16_t)param_m1.real_speed; +#undef alpha } int16_t by_motion_get_speed_m2(void) { - speed_m2 = (int16_t)tmr_counter_value_get(TMR3); +#define alpha (0.1f) + static float last_speed = 0.0f; + param_m2.real_speed = alpha * last_speed + (1.0f - alpha) * (float)(-1 * (int16_t)tmr_counter_value_get(TMR3)); + last_speed = param_m2.real_speed; tmr_counter_value_set(TMR3, 0); - return speed_m2; + return (int16_t)param_m2.real_speed; +#undef alpha +} + +void by_motion_set_speed_m1(int16_t speed) +{ + param_m1.target_speed = speed; +} + +void by_motion_set_speed_m2(int16_t speed) +{ + param_m2.target_speed = speed; } void by_motion_init(void) { + motion_enable_flag = 0; + + by_motion_set_pwm_m1(0); + by_motion_set_pwm_m2(0); + + /* set default parameters */ + param_m1.Kp = BY_MOTION_DEFAULT_KP_M1; + param_m1.Ki = BY_MOTION_DEFAULT_KI_M1; + param_m1.Kd = BY_MOTION_DEFAULT_KD_M1; + param_m2.Kp = BY_MOTION_DEFAULT_KP_M2; + param_m2.Ki = BY_MOTION_DEFAULT_KI_M2; + param_m2.Kd = BY_MOTION_DEFAULT_KD_M2; + + /* load parameters form ee */ + // TODO + + PID(&pid_m1, ¶m_m1.real_speed, ¶m_m1.out_pwm, ¶m_m1.target_speed, param_m1.Kp, param_m1.Ki, param_m1.Kd, _PID_P_ON_E, _PID_CD_DIRECT); + PID_SetMode(&pid_m1, _PID_MODE_AUTOMATIC); + PID_SetSampleTime(&pid_m1, 10); + PID_SetOutputLimits(&pid_m1, -400, 400); + + PID(&pid_m2, ¶m_m2.real_speed, ¶m_m2.out_pwm, ¶m_m2.target_speed, param_m2.Kp, param_m2.Ki, param_m2.Kd, _PID_P_ON_E, _PID_CD_DIRECT); + PID_SetMode(&pid_m2, _PID_MODE_AUTOMATIC); + PID_SetSampleTime(&pid_m2, 10); + PID_SetOutputLimits(&pid_m2, -400, 400); + + motion_enable_flag = 1; DRV_ENABLE(); - by_motion_pwm_m1(-125); - by_motion_pwm_m2(0); } + +void by_motion_run(void) +{ + if (motion_enable_flag) { + by_motion_get_speed_m1(); + by_motion_get_speed_m2(); + + PID_Compute(&pid_m1); + PID_Compute(&pid_m2); + + by_motion_set_pwm_m1((int32_t)param_m1.out_pwm); + by_motion_set_pwm_m2((int32_t)param_m2.out_pwm); + } +} + +void by_motion_can_handle(uint16_t stdd_id, const uint8_t *data, uint8_t len) +{ +#define BC2D_MODEL2 + + if (0x01 == stdd_id) { +#if defined(BC2D_MODEL1) + int16_t speed_m1_temp = (int16_t)data[0] | ((int16_t)data[1] << 8); + int16_t speed_m2_temp = (int16_t)data[2] | ((int16_t)data[3] << 8); + by_motion_set_speed_m1(speed_m1_temp); + by_motion_set_speed_m2(speed_m2_temp); +#elif defined(BC2D_MODEL2) + int16_t speed_m1_temp = (int16_t)(data[4] | (data[5] << 8)); + int16_t speed_m2_temp = (int16_t)(data[6] | (data[7] << 8)); + by_motion_set_speed_m1(speed_m1_temp); + by_motion_set_speed_m2(speed_m2_temp); +#endif + } + + LOGD("m1:%f,%f,%f", param_m1.real_speed, param_m1.target_speed, param_m1.out_pwm); + LOGD("m2:%f,%f,%f", param_m2.real_speed, param_m2.target_speed, param_m2.out_pwm); +} \ No newline at end of file diff --git a/app/by_motion.h b/app/by_motion.h index 192699d..fdf4fe0 100644 --- a/app/by_motion.h +++ b/app/by_motion.h @@ -3,11 +3,33 @@ #include "at32f413.h" +typedef struct by_motor_param { + float Kp; + float Ki; + float Kd; + float real_speed; + float target_speed; + float out_pwm; +} by_motor_param; + +#define BY_MOTION_DEFAULT_KP_M1 (5.0f) +#define BY_MOTION_DEFAULT_KI_M1 (100.0f) +#define BY_MOTION_DEFAULT_KD_M1 (0.0f) +#define BY_MOTION_DEFAULT_KP_M2 (5.0f) +#define BY_MOTION_DEFAULT_KI_M2 (100.0f) +#define BY_MOTION_DEFAULT_KD_M2 (0.0f) + extern void by_motion_init(void); +extern void by_motion_run(void); +extern void by_motion_set_pwm_m1(int32_t pwm_duty); +extern void by_motion_set_pwm_m2(int32_t pwm_duty); extern int16_t by_motion_get_speed_m1(void); extern int16_t by_motion_get_speed_m2(void); +extern void by_motion_set_speed_m1(int16_t speed); +extern void by_motion_set_speed_m2(int16_t speed); +extern void by_motion_can_handle(uint16_t stdd_id, const uint8_t *data, uint8_t len); -extern int16_t speed_m1; -extern int16_t speed_m2; +extern by_motor_param param_m1; +extern by_motor_param param_m2; #endif diff --git a/app/by_utils.h b/app/by_utils.h index 0ab18f0..bd18d44 100644 --- a/app/by_utils.h +++ b/app/by_utils.h @@ -3,6 +3,21 @@ #include "at32f413.h" +typedef enum { + BY_ERROR = 0, + BY_SUCCESS = !BY_ERROR +} by_error_status; + +typedef enum { + T_U8 = 0, + T_U16, + T_U32, + T_S8, + T_S16, + T_S32, + T_F32 +} by_data_type; + int32_t clip_s32(int32_t x, int32_t low, int32_t up); #endif \ No newline at end of file diff --git a/project/inc/at32f413_int.h b/project/inc/at32f413_int.h index 9e3e1f5..73de982 100644 --- a/project/inc/at32f413_int.h +++ b/project/inc/at32f413_int.h @@ -65,6 +65,7 @@ void SVC_Handler(void); void DebugMon_Handler(void); void PendSV_Handler(void); +void USBFS_L_CAN1_RX0_IRQHandler(void); void TMR4_GLOBAL_IRQHandler(void); /* add user code begin exported functions */ diff --git a/project/src/at32f413_int.c b/project/src/at32f413_int.c index 4b45be9..2f57a73 100644 --- a/project/src/at32f413_int.c +++ b/project/src/at32f413_int.c @@ -29,6 +29,7 @@ /* private includes ----------------------------------------------------------*/ /* add user code begin private includes */ +#include "by_debug.h" #include "by_motion.h" /* add user code end private includes */ @@ -68,10 +69,10 @@ /* add user code end external variables */ /** - * @brief this function handles nmi exception. - * @param none - * @retval none - */ + * @brief this function handles nmi exception. + * @param none + * @retval none + */ void NMI_Handler(void) { /* add user code begin NonMaskableInt_IRQ 0 */ @@ -84,17 +85,18 @@ void NMI_Handler(void) } /** - * @brief this function handles hard fault exception. - * @param none - * @retval none - */ + * @brief this function handles hard fault exception. + * @param none + * @retval none + */ void HardFault_Handler(void) { /* add user code begin HardFault_IRQ 0 */ /* add user code end HardFault_IRQ 0 */ /* go to infinite loop when hard fault exception occurs */ - while (1) { + while (1) + { /* add user code begin W1_HardFault_IRQ 0 */ /* add user code end W1_HardFault_IRQ 0 */ @@ -102,17 +104,18 @@ void HardFault_Handler(void) } /** - * @brief this function handles memory manage exception. - * @param none - * @retval none - */ + * @brief this function handles memory manage exception. + * @param none + * @retval none + */ void MemManage_Handler(void) { /* add user code begin MemoryManagement_IRQ 0 */ /* add user code end MemoryManagement_IRQ 0 */ /* go to infinite loop when memory manage exception occurs */ - while (1) { + while (1) + { /* add user code begin W1_MemoryManagement_IRQ 0 */ /* add user code end W1_MemoryManagement_IRQ 0 */ @@ -120,17 +123,18 @@ void MemManage_Handler(void) } /** - * @brief this function handles bus fault exception. - * @param none - * @retval none - */ + * @brief this function handles bus fault exception. + * @param none + * @retval none + */ void BusFault_Handler(void) { /* add user code begin BusFault_IRQ 0 */ /* add user code end BusFault_IRQ 0 */ /* go to infinite loop when bus fault exception occurs */ - while (1) { + while (1) + { /* add user code begin W1_BusFault_IRQ 0 */ /* add user code end W1_BusFault_IRQ 0 */ @@ -138,17 +142,18 @@ void BusFault_Handler(void) } /** - * @brief this function handles usage fault exception. - * @param none - * @retval none - */ + * @brief this function handles usage fault exception. + * @param none + * @retval none + */ void UsageFault_Handler(void) { /* add user code begin UsageFault_IRQ 0 */ /* add user code end UsageFault_IRQ 0 */ /* go to infinite loop when usage fault exception occurs */ - while (1) { + while (1) + { /* add user code begin W1_UsageFault_IRQ 0 */ /* add user code end W1_UsageFault_IRQ 0 */ @@ -156,10 +161,10 @@ void UsageFault_Handler(void) } /** - * @brief this function handles svcall exception. - * @param none - * @retval none - */ + * @brief this function handles svcall exception. + * @param none + * @retval none + */ void SVC_Handler(void) { /* add user code begin SVCall_IRQ 0 */ @@ -171,10 +176,10 @@ void SVC_Handler(void) } /** - * @brief this function handles debug monitor exception. - * @param none - * @retval none - */ + * @brief this function handles debug monitor exception. + * @param none + * @retval none + */ void DebugMon_Handler(void) { /* add user code begin DebugMonitor_IRQ 0 */ @@ -186,10 +191,10 @@ void DebugMon_Handler(void) } /** - * @brief this function handles pendsv_handler exception. - * @param none - * @retval none - */ + * @brief this function handles pendsv_handler exception. + * @param none + * @retval none + */ void PendSV_Handler(void) { /* add user code begin PendSV_IRQ 0 */ @@ -201,16 +206,37 @@ void PendSV_Handler(void) } /** - * @brief this function handles TMR4 handler. - * @param none - * @retval none - */ + * @brief this function handles USB Low Priority or CAN1 RX0 handler. + * @param none + * @retval none + */ +void USBFS_L_CAN1_RX0_IRQHandler(void) +{ + /* add user code begin USBFS_L_CAN1_RX0_IRQ 0 */ + if (SET == can_flag_get(CAN1, CAN_RF0MN_FLAG)) { + + can_rx_message_type can_rx_message; + can_message_receive(CAN1, CAN_RX_FIFO0, &can_rx_message); + by_motion_can_handle((uint16_t)can_rx_message.standard_id, can_rx_message.data, can_rx_message.dlc); + + can_flag_clear(CAN1, CAN_RF0MN_FLAG); + } + /* add user code end USBFS_L_CAN1_RX0_IRQ 0 */ + /* add user code begin USBFS_L_CAN1_RX0_IRQ 1 */ + + /* add user code end USBFS_L_CAN1_RX0_IRQ 1 */ +} + +/** + * @brief this function handles TMR4 handler. + * @param none + * @retval none + */ void TMR4_GLOBAL_IRQHandler(void) { /* add user code begin TMR4_GLOBAL_IRQ 0 */ if (SET == tmr_interrupt_flag_get(TMR4, TMR_OVF_FLAG)) { - by_motion_get_speed_m1(); - by_motion_get_speed_m2(); + by_motion_run(); tmr_flag_clear(TMR4, TMR_OVF_FLAG); } /* add user code end TMR4_GLOBAL_IRQ 0 */ diff --git a/project/src/at32f413_wk_config.c b/project/src/at32f413_wk_config.c index 72a98d7..5f0a3c0 100644 --- a/project/src/at32f413_wk_config.c +++ b/project/src/at32f413_wk_config.c @@ -204,6 +204,7 @@ void wk_nvic_config(void) { nvic_priority_group_config(NVIC_PRIORITY_GROUP_4); + nvic_irq_enable(USBFS_L_CAN1_RX0_IRQn, 1, 0); nvic_irq_enable(TMR4_GLOBAL_IRQn, 0, 0); } @@ -660,8 +661,19 @@ void wk_can1_init(void) can_filter_init(CAN1, &can_filter_init_struct); - /* add user code begin can1_init 2 */ + /** + * Users need to configure CAN1 interrupt functions according to the actual application. + * 1. Call the below function to enable the corresponding CAN1 interrupt. + * --can_interrupt_enable(...) + * 2. Add the user's interrupt handler code into the below function in the at32f413_int.c file. + * --void USBFS_L_CAN1_RX0_IRQHandler(void) + */ + /*can1 rx0 interrupt config--------------------------------------------------------*/ + //can_interrupt_enable(CAN1, CAN_RF0MIEN_INT, TRUE); + + /* add user code begin can1_init 2 */ + can_interrupt_enable(CAN1, CAN_RF0MIEN_INT, TRUE); /* add user code end can1_init 2 */ }