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

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

Robot_V2/Algorithm/pid.cpp

@@ -1,52 +1,66 @@
 #include "pid.h"
 #include "Timer.h"
-#define _constrain(amt,low,high) ((amt)<(low)?(low):((amt)>(high)?(high):(amt)))
+#define _constrain(amt, low, high)                                             \
+	((amt) < (low) ? (low) : ((amt) > (high) ? (high) : (amt)))
+#include "Can.h"
 
+extern CAN_transmiter CAN0_transmiter;
 
 PIDController::PIDController(float P, float I, float D, float ramp, float limit)
-    : P(P)
-    , I(I)
-    , D(D)
-    , output_ramp(ramp)    // PID控制器加速度限幅
-    , limit(limit)         // PID控制器输出限幅
-    , error_prev(0.0f)
-    , output_prev(0.0f)
-    , integral_prev(0.0f)
-{
-    timestamp_prev = get_system_runtime_us() ;
+	: P(P), I(I), D(D), output_ramp(ramp) // PID控制器加速度限幅
+	  ,
+	  limit(limit) // PID控制器输出限幅
+	  ,
+	  error_prev(0.0f), output_prev(0.0f), integral_prev(0.0f) {
+	timestamp_prev = get_system_runtime_us();
 }
 
 // PID 控制器函数
-float PIDController::operator() (float error){
-    // 计算两次循环中间的间隔时间
-    unsigned long timestamp_now = get_system_runtime_us();
-    float Ts = (timestamp_now - timestamp_prev) * 1e-6f;
-    if(Ts <= 0 || Ts > 0.5f) Ts = 1e-3f;
-    
-    // P环
-    float proportional = P * error;
-    // Tustin 散点积分（I环）
-    float integral = integral_prev + I*Ts*0.5f*(error + error_prev);
-    integral = _constrain(integral, -limit, limit);
-    // D环（微分环节）
-    float derivative = D*(error - error_prev)/Ts;
+float PIDController::operator()(float error) {
+	// 计算两次循环中间的间隔时间
+	unsigned long timestamp_now = get_system_runtime_us();
+	float Ts = (timestamp_now - timestamp_prev) * 1e-6f;
+	if (Ts <= 0 || Ts > 0.5f)
+		Ts = 1e-3f;
 
-    // 将P,I,D三环的计算值加起来
-    float output = proportional + integral + derivative;
-    output = _constrain(output, -limit, limit);
+	// P环
+	float proportional = P * error;
+	// Tustin 散点积分（I环）
+	float integral = integral_prev + I * Ts * 0.5f * (error + error_prev);
+	integral = _constrain(integral, -limit, limit);
+	// D环（微分环节）
+	float derivative = D * (error - error_prev) / Ts;
+	// 将P,I,D三环的计算值加起来
+	float output = proportional + integral + derivative;
+	output = _constrain(output, -limit, limit);
 
-    if(output_ramp > 0){
-        // 对PID的变化速率进行限制
-        float output_rate = (output - output_prev)/Ts;
-        if (output_rate > output_ramp)
-            output = output_prev + output_ramp*Ts;
-        else if (output_rate < -output_ramp)
-            output = output_prev - output_ramp*Ts;
-    }
-    // 保存值（为了下一次循环）
-    integral_prev = integral;
-    output_prev = output;
-    error_prev = error;
-    timestamp_prev = timestamp_now;
-    return output;
+	if (output_ramp > 0) {
+		// 对PID的变化速率进行限制
+		float output_rate = (output - output_prev) / Ts;
+		if (output_rate > output_ramp)
+			output = output_prev + output_ramp * Ts;
+		else if (output_rate < -output_ramp)
+			output = output_prev - output_ramp * Ts;
+	}
+	// 保存值（为了下一次循环）
+	integral_prev = integral;
+	output_prev = output;
+	error_prev = error;
+	timestamp_prev = timestamp_now;
+
+	return output;
+}
+
+/**
+ * @brief 设置PID参数
+ *
+ * @param P: P参数
+ * @param I: I参数
+ * @param D: D参数
+ * @return void
+ */
+void PIDController::set_PID(float P, float I, float D) {
+	P = P;
+	I = I;
+	D = D;
 }