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

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

Robot_V2/device/Can.cpp

@@ -1,5 +1,6 @@
 #include "Can.h"
 #include "DengFOC.h"
+#include "Filter.h"
 #include "adc.h"
 #include "ang_sensor.h"
 #include "gd32c10x.h"
@@ -23,6 +24,7 @@ extern CAN_transmiter CAN0_transmiter;
 extern FOC_Controlroller FOC;
 extern sampler sampler;
 extern angle_sensor KTH7823;
+extern EncoderVelocityKalman Vel_kalman;
 
 // 添加DMA相关变量
 dma_parameter_struct dma_rx_parameter;
@@ -431,7 +433,8 @@ void CAN_transmiter::update(void) {
 	motor_temp = sampler.get_drv_temp() * 100;
 	Power_Voltage = sampler.get_vbus_voltage() * 1000;
 	rt_angle_union.f = KTH7823.getAngle();
-	rt_velocity_union.f = KTH7823.getVelocity();
+	// rt_velocity_union.f = KTH7823.getVelocity();
+	rt_velocity_union.f = FOC.rt_velocity;
 	Status_code = (gpio_input_bit_get(GPIOB, GPIO_PIN_2) == SET) ? 0 : 1;
 
 	Target_Control_Mode = FOC.control_mode;
@@ -440,6 +443,7 @@ void CAN_transmiter::update(void) {
 	u_current_union.f = sampler.get_current('U') * 1000;
 	w_current_union.f = sampler.get_current('W') * 1000;
 	v_current_union.f = sampler.get_current('V') * 1000;
+	// testfloat_union.f = KTH7823.getVelocity();
 }
 
 /*!
@@ -621,7 +625,8 @@ u8 CAN_transmiter::CANFD_all_Send() {
 	pack_u16_to_bytes(transmit_data, 11, Power_Voltage);
 	pack_float_to_bytes(transmit_data, 13, rt_angle_union);
 	pack_float_to_bytes(transmit_data, 17, rt_velocity_union);
- 	success_flag += CANFD_Send(STATUS_ID, 24, transmit_data);
+	pack_float_to_bytes(transmit_data, 21, testfloat_union);
+	success_flag += CANFD_Send(STATUS_ID, 32, transmit_data);
 
 	// 打包config数据
 	transmit_data[0] = controller_Mode;
@@ -677,7 +682,7 @@ void CAN_transmiter::communication_check() {
 
 			target_velocity_union.f =
 				unpack_bytes_to_float(receive_message.rx_data, 11).f;
-			FOC.target_velocity = target_velocity_union.f * ANGLE_FACTOR;
+			FOC.target_velocity = target_velocity_union.f;
 
 			can0_receive_flag = RESET;
 		}