From 5d14731af5bd94ca8ed2c8aace6d4ff015761bfb Mon Sep 17 00:00:00 2001 From: cbb <2625274797@qq.com> Date: Wed, 29 Oct 2025 21:22:10 +0800 Subject: [PATCH] =?UTF-8?q?feat(FOC):=20=E6=B7=BB=E5=8A=A0=E5=BC=80?= =?UTF-8?q?=E5=85=B3=E6=9C=BA=E5=8A=9F=E8=83=BD=E5=B9=B6=E4=BC=98=E5=8C=96?= =?UTF-8?q?=E5=AE=9A=E6=97=B6=E5=99=A8=E9=85=8D=E7=BD=AE?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- Robot_V2/Algorithm/DengFOC.cpp | 28 ++++++--- Robot_V2/Algorithm/DengFOC.h | 3 + Robot_V2/device/Can.cpp | 17 +++-- Robot_V2/device/Timer.cpp | 109 +++++++++++++++++++++++++++++---- Robot_V2/device/Timer.h | 2 + Robot_V2/sys/initial.cpp | 3 + 6 files changed, 139 insertions(+), 23 deletions(-) diff --git a/Robot_V2/Algorithm/DengFOC.cpp b/Robot_V2/Algorithm/DengFOC.cpp index 5b36b5d..d08b43d 100644 --- a/Robot_V2/Algorithm/DengFOC.cpp +++ b/Robot_V2/Algorithm/DengFOC.cpp @@ -3,8 +3,10 @@ #include "systick.h" #include #include +#include #include + constexpr double PI = 3.1415926535897932384626433832795f; constexpr float _3PI_2 = 4.71238898038f; constexpr u8 PP = 11, DIR = 1; @@ -15,7 +17,6 @@ constexpr float sqrt3 = 1.73205080757f; // sqrt(3)的预计算值 #define constrain(value, min, max, target_min, target_max) \ ((value - min) / (max - min) * (target_max - target_min)) - inline float _electricalAngle(float shaft_angle) { return shaft_angle * PP; } /** @@ -81,17 +82,17 @@ void FOC_Controlroller::setPhaseVoltage(float Uq, float Ud, float angle_el) { */ float FOC_Controlroller::velocity_open_loop(float target_velocity) { // 通过时间间隔和目标速度计算需要转动的机械角度，存储在shaft_angle中，在此之前，还需要对轴角度归一化 - // 预计算常量 1e-4 = 100微秒控制周期 / 1秒 = 100e-6 + // 预计算常量 1e-4 = 100微秒控制周期 shaft_angle = _normalizeAngle(shaft_angle + target_velocity * 1e-4f); - + // 直接计算电气角度，避免函数调用开销 float angle_el = shaft_angle * PP + zero_electric_angle; angle_el = _normalizeAngle(angle_el); - + // 因为在开环控制中，Ud始终为0，只控制Uq const float Uq = limit_voltage; const float Ud = 0.0f; - + // 帕克逆变换 (直接计算，避免函数调用开销) const float sin_el = sin(angle_el); const float cos_el = cos(angle_el); @@ -103,13 +104,16 @@ float FOC_Controlroller::velocity_open_loop(float target_velocity) { Ua = Ualpha + v_half; Ub = (sqrt3 * Ubeta - Ualpha) * 0.5f + v_half; Uc = (-Ualpha - sqrt3 * Ubeta) * 0.5f + v_half; - + // 计算占空比 + Ua = constrain(Ua, 0.0f, voltage_power_supply, 0.0f, limit_voltage); + Ub = constrain(Ub, 0.0f, voltage_power_supply, 0.0f, limit_voltage); + Uc = constrain(Uc, 0.0f, voltage_power_supply, 0.0f, limit_voltage); const float dc_a = _constrain(Ua / voltage_power_supply, 0.0f, 1.0f); const float dc_b = _constrain(Ub / voltage_power_supply, 0.0f, 1.0f); const float dc_c = _constrain(Uc / voltage_power_supply, 0.0f, 1.0f); Set_3Pwm(dc_a, dc_b, dc_c); - + return limit_voltage; } @@ -128,3 +132,13 @@ void FOC_Controlroller::DFOC_alignSensor(int _PP, int _DIR) { setPhaseVoltage(0, 0, _3PI_2); std::printf("0电角度:%f", zero_electric_angle); } + +void FOC_Controlroller::shut_down() { + is_shut_down = 100; + gpio_bit_reset(GPIOB, GPIO_PIN_12); +} + +void FOC_Controlroller::resume() { + is_shut_down = 0; + gpio_bit_set(GPIOB, GPIO_PIN_12); +} \ No newline at end of file diff --git a/Robot_V2/Algorithm/DengFOC.h b/Robot_V2/Algorithm/DengFOC.h index 43f04ed..bd08c8a 100644 --- a/Robot_V2/Algorithm/DengFOC.h +++ b/Robot_V2/Algorithm/DengFOC.h @@ -17,9 +17,12 @@ class FOC_Controlroller { float limit_velocity = 0.0f; float target_angle = 0.0f; float target_velocity = 0.0f; + u8 is_shut_down = 0; float velocity_open_loop(float target_velocity); void DFOC_alignSensor(int _PP, int _DIR); + void shut_down(void); + void resume(void); private: std::u64 last_time_us = 0; diff --git a/Robot_V2/device/Can.cpp b/Robot_V2/device/Can.cpp index 3ec259f..db283da 100644 --- a/Robot_V2/device/Can.cpp +++ b/Robot_V2/device/Can.cpp @@ -596,10 +596,19 @@ void CAN_transmiter::communication_check() { // } if (receive_message.rx_efid == 0x18ff0200) { FOC.control_mode = receive_message.rx_data[0]; - FOC.limit_voltage = - float(unpack_bytes_to_u16(receive_message.rx_data, 1)) / 1000; - FOC.limit_velocity = - unpack_bytes_to_float(receive_message.rx_data, 3).f; + if (float(unpack_bytes_to_u16(receive_message.rx_data, 1)) / 1000 != + FOC.limit_voltage) { + FOC.limit_voltage = + float(unpack_bytes_to_u16(receive_message.rx_data, 1)) / + 1000; + FOC.is_shut_down = 100; // 1s滤除启动电流 + } + if (unpack_bytes_to_float(receive_message.rx_data, 3).f != + FOC.limit_velocity) { + FOC.limit_velocity = + unpack_bytes_to_float(receive_message.rx_data, 3).f; + FOC.is_shut_down = 100; // 1s滤除启动电流 + } FOC.target_angle = unpack_bytes_to_float(receive_message.rx_data, 7).f; FOC.target_velocity = diff --git a/Robot_V2/device/Timer.cpp b/Robot_V2/device/Timer.cpp index 7c5bf8b..da16973 100644 --- a/Robot_V2/device/Timer.cpp +++ b/Robot_V2/device/Timer.cpp @@ -5,8 +5,10 @@ #include "adc.h" #include "gd32c10x.h" #include "gd32c10x_gpio.h" +#include "gd32c10x_misc.h" #include "gd32c10x_rcu.h" #include "gd32c10x_timer.h" +#include "systick.h" #include #include #include @@ -18,6 +20,8 @@ // 然后设置周期为99，即计数100次，1MHz/100 = 10kHz = 100微秒 constexpr int TIMER2_PRESCALER = 119; // 120MHz / 120 = 1MHz constexpr int TIMER2_PERIOD = 99; // 1MHz / 100 = 100us +constexpr int TIMER3_PRESCALER = 119; // 120MHz / 120 = 1MHz +constexpr int TIMER3_PERIOD = 9999; // 1MHz / 10000 = 100Hz = 10ms constexpr int TIMER1_PRESCALER = 0; // 不分频，计数器时钟为120MHz constexpr int TIMER1_PERIOD = 5999; // 120MHz / (0+1) / (5999+1) = 20kHz @@ -47,13 +51,14 @@ void nvic_config(void) { nvic_priority_group_set(NVIC_PRIGROUP_PRE2_SUB2); /* 使能TIMER2中断，设置抢占优先级为1，子优先级为1 */ nvic_irq_enable(TIMER2_IRQn, 1, 1); + nvic_irq_enable(TIMER3_IRQn, 1, 2); } /** - * @brief 配置定时器1 + * @brief 配置定时器2 * @return void * - *配置定时器1 + *配置定时器2，用于定时执行FOC控制、采样、CAN发送等任务 */ void TIMER2_config(void) { /* ---------------------------------------------------------------------------- @@ -90,6 +95,47 @@ void TIMER2_config(void) { timer_enable(TIMER2); } +/** + * @brief 配置定时器3 + * @return void + * + *配置定时器3，用于诊断恢复 + */ +void TIMER3_config(void) { + /* ---------------------------------------------------------------------------- + TIMER3 Configuration: + TIMER3CLK = SystemCoreClock/120 = 1MHz, the period is 10ms(10000/1000000 = + 10ms). + ---------------------------------------------------------------------------- + */ + timer_parameter_struct timer_struct; + + /* 使能TIMER3时钟 */ + rcu_periph_clock_enable(RCU_TIMER3); + + /* 复位TIMER3外设到默认状态 */ + timer_deinit(TIMER3); + /* 初始化TIMER初始化参数结构体 */ + timer_struct_para_init(&timer_struct); + /* TIMER3配置参数设置 */ + timer_struct.prescaler = + TIMER3_PRESCALER; /* 预分频值：120分频(从0开始计数) */ + timer_struct.alignedmode = TIMER_COUNTER_EDGE; /* 边沿对齐模式 */ + timer_struct.counterdirection = TIMER_COUNTER_UP; /* 向上计数模式 */ + timer_struct.period = + TIMER3_PERIOD; /* 自动重装载值：10000次计数(从0开始计数) */ + timer_struct.clockdivision = TIMER_CKDIV_DIV1; /* 时钟分频值：1分频 */ + timer_init(TIMER3, &timer_struct); + + /* 清除更新中断标志位 */ + timer_interrupt_flag_clear(TIMER3, TIMER_INT_FLAG_UP); + /* 使能更新中断 */ + timer_interrupt_enable(TIMER3, TIMER_INT_UP); + + /* 使能TIMER3 */ + timer_enable(TIMER3); +} + /** * @brief 配置TIMER2中断 * @return void @@ -118,45 +164,52 @@ void TIMER2_IRQHandler(void) { if (timer_interrupt_flag_get(TIMER2, TIMER_INT_UP) != RESET) { // 清除更新中断标志 timer_interrupt_flag_clear(TIMER2, TIMER_INT_UP); - + // 增加总计数器 (使用更高效的位运算代替取模运算) total_us100_counter++; system_runtime_us += 100; - + // 在定时器中断中执行FOC控制，确保控制频率稳定 (最高频操作放在最前) FOC.velocity_open_loop(FOC.target_velocity); - + // 每100us执行一次的操作 sampler.update_current(); // 每1ms执行的代码 (100us * 10 = 1ms) if (total_us100_counter >= 10) { total_us100_counter = 0; // 重置计数器 - + // 设置CAN发送标志 CAN0_transmiter.can0_send_flag = SET; - + // 更新各种传感器数据 sampler.update_mcu_temp(); sampler.update_drv_temp(); sampler.update_vbus_voltage(); // 每10ms执行的代码 (1ms * 10 = 10ms) - static uint8_t ms10_counter = 0; + static u8 ms10_counter = 0; ms10_counter++; if (ms10_counter >= 10) { ms10_counter = 0; // 可以添加每10ms执行的代码 - + // 每100ms执行的代码 (10ms * 10 = 100ms) - static uint8_t ms100_counter = 0; + static u8 ms100_counter = 0; ms100_counter++; if (ms100_counter >= 10) { ms100_counter = 0; // 可以添加每100ms执行的代码 - + // if (sampler.get_current('U') > 1.0 || + // sampler.get_current('V') > 1.0 || + // sampler.get_current('W') > 1.0) { + // if (FOC.is_shut_down == 0) { + // FOC.shut_down(); + // } + // } + // 每1s执行的代码 (100ms * 10 = 1s) - static uint8_t sec_counter = 0; + static u8 sec_counter = 0; sec_counter++; if (sec_counter >= 10) { sec_counter = 0; @@ -168,6 +221,38 @@ void TIMER2_IRQHandler(void) { } } +/** + * @brief TIMER3中断处理函数 + * @return void + */ +void TIMER3_IRQHandler(void) { + // 检查更新中断标志并处理 + if (timer_interrupt_flag_get(TIMER3, TIMER_INT_UP) != RESET) { + // 清除更新中断标志 + timer_interrupt_flag_clear(TIMER3, TIMER_INT_UP); + static u8 count = 0; + count++; + if (FOC.is_shut_down > 0) + FOC.is_shut_down--; + else + FOC.resume(); + + // 10ms测试代码 + if (count >= 10) { + count = 0; + // 100ms测试代码 + + static u8 count_10 = 0; + count_10++; + if (count_10 >= 10) { + count_10 = 0; + + // 1s测试代码 + } + } + } +} + /** * @brief TIMER1 GPIO配置函数 */ diff --git a/Robot_V2/device/Timer.h b/Robot_V2/device/Timer.h index 5cf00ef..91b8116 100644 --- a/Robot_V2/device/Timer.h +++ b/Robot_V2/device/Timer.h @@ -11,6 +11,8 @@ void User_timer_init(void); void Set_Pwm(uint16_t channel, float duty_cycle); void Set_3Pwm(float duty_cycle0, float duty_cycle1,float duty_cycle2); void TIMER2_IRQHandler(void); +void TIMER3_config(void); +void TIMER3_IRQHandler(void); void timer1_pwm_config(void); void timer1_gpio_config(void); diff --git a/Robot_V2/sys/initial.cpp b/Robot_V2/sys/initial.cpp index cd28de7..f76fc7d 100644 --- a/Robot_V2/sys/initial.cpp +++ b/Robot_V2/sys/initial.cpp @@ -1,4 +1,5 @@ #include "initial.h" +#include "gd32c10x_timer.h" #include "systick.h" #include #include "Timer.h" @@ -124,6 +125,8 @@ u8 watchdog_init(u32 time) { void driver_init(void) { timer1_gpio_config(); timer1_pwm_config(); + TIMER3_config(); + } int fputc(int ch, FILE *f) throw() {