feat(FOC): 增加闭环速度控制算法

- 重构setPhaseVoltage函数实现，提高执行效率
- 增加update_power函数，动态更新电源电压
- 优化velocity_open_loop函数，调用、setPhaseVoltage避免重复代码
- 实现angle_control和velocity_control闭环控制函数
- 调整PID控制器参数初始化及增加参数设置接口
- 改进电流和编码器速度卡尔曼滤波器实现
- 更新CAN通信数据打包逻辑，增加测试字段
- 在定时器中断中集成多种控制模式切换逻辑
- 完善角度传感器数据处理，改进速度计算方法
- 减小速度滤波数组大小，提升响应速度

Robot_V2/device/Timer.cpp

@@ -1,6 +1,7 @@
 #include "Timer.h"
 #include "Can.h"
 #include "DengFOC.h"
+#include "Filter.h"
 #include "Timer.h"
 #include "adc.h"
 #include "ang_sensor.h"
@@ -9,6 +10,7 @@
 #include "gd32c10x_misc.h"
 #include "gd32c10x_rcu.h"
 #include "gd32c10x_timer.h"
+#include <cstdio>
 #include <stdbool.h>
 #include <stdint.h>
 #include <stdio.h>
@@ -35,6 +37,7 @@ extern FOC_Controlroller FOC;
 // 声明外部采样器对象
 extern sampler sampler;
 extern angle_sensor KTH7823;
+extern EncoderVelocityKalman Vel_kalman;
 
 /**
  * @brief 设置引脚中断优先级
@@ -156,6 +159,7 @@ uint64_t get_system_runtime_us(void) { return system_runtime_us; }
  * @return void
  */
 void TIMER2_IRQHandler(void) {
+	static u8 mode = 0;
 	// 检查更新中断标志并处理
 	if (timer_interrupt_flag_get(TIMER2, TIMER_INT_UP) != RESET) {
 		// 清除更新中断标志
@@ -168,9 +172,15 @@ void TIMER2_IRQHandler(void) {
 		// 每100us执行一次的操作
 		sampler.update_current(); // 更新电流采样值
 		KTH7823.update();		  // 更新角度传感器值
-								  // FOC.velocity_open_loop(); // 速度开环控制
-		// FOC.velocity_control(FOC.target_velocity); // 速度闭环控制
-		FOC.angle_control(FOC.target_angle);
+		FOC.rt_velocity = Vel_kalman.update(KTH7823.getVelocity());
+
+		if (FOC.control_mode == 1) {
+			FOC.angle_control(FOC.target_angle); // 角度闭环控制
+		} else if (FOC.control_mode == 2) {
+			FOC.velocity_control(FOC.target_velocity);// 速度闭环控制
+		} else if (FOC.control_mode == 4) {
+			FOC.velocity_open_loop(); // 速度开环控制
+		}
 
 		if (total_us100_counter >= 10) {
 			// 每1ms执行的代码
@@ -178,7 +188,8 @@ void TIMER2_IRQHandler(void) {
 
 			// 设置CAN发送标志
 			CAN0_transmiter.can0_send_flag = SET;
-
+			mode = FOC.control_mode;
+			// 过流检测
 			if (sampler.get_current('U') > 2000 ||
 				sampler.get_current('V') > 2000 ||
 				sampler.get_current('W') > 2000) {
@@ -202,7 +213,7 @@ void TIMER2_IRQHandler(void) {
 			if (ms10_counter >= 10) {
 				ms10_counter = 0;
 				// 可以添加每10ms执行的代码
-
+				FOC.update_power();
 				static u8 ms100_counter = 0;
 				ms100_counter++;
 				if (ms100_counter >= 10) {