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

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

Robot_V2/Algorithm/DengFOC.h

@@ -21,8 +21,7 @@ class FOC_Controlroller {
 
 	float velocity_open_loop();
 	float angle_control(float target_angle);
-	// void SET_VEL_PID(float P, float I, float D, float ramp);   // 速度环PID设置
-	// void SET_ANGLE_PID(float P, float I, float D, float ramp); // 角度环PID设置
+	float velocity_control(float target_velocity);
 
 	void update_power(void);
 	void DFOC_alignSensor(int _PP);
@@ -36,9 +35,10 @@ class FOC_Controlroller {
 	float Ualpha, Ubeta = 0, Ua = 0, Ub = 0, Uc = 0;
 	float zero_electric_angle = 0;
 	float voltage_power_supply = 24.0f; // 电源电压
+	float v_half = voltage_power_supply * 0.5f; // 电源电压的一半
 
 	PIDController vel_loop =
-		PIDController{2, 0, 0, 100000, voltage_power_supply / 2};
+		PIDController{0.8f, 3.0f, 0, 100000, voltage_power_supply};
 	PIDController angle_loop = PIDController{2, 0, 0, 100000, 100};
 
 	void setPhaseVoltage(float Uq, float Ud, float angle_el);