From 96898411488c940174576793ff3a64d27406f970 Mon Sep 17 00:00:00 2001
From: cbb <2625274797@qq.com>
Date: Sun, 8 Feb 2026 18:53:55 +0800
Subject: [PATCH] =?UTF-8?q?feat(FOC):=20=E6=B7=BB=E5=8A=A0=E7=94=B5?=
 =?UTF-8?q?=E6=B5=81=E5=8D=A1=E5=B0=94=E6=9B=BC=E6=BB=A4=E6=B3=A2=E5=99=A8?=
 =?UTF-8?q?=E5=B9=B6=E4=BC=98=E5=8C=96MIT=E6=8E=A7=E5=88=B6=E6=A8=A1?=
 =?UTF-8?q?=E5=BC=8F=20-=20=E4=BC=98=E5=8C=96MIT=E6=8E=A7=E5=88=B6?=
 =?UTF-8?q?=E6=A8=A1=E5=BC=8F=E7=9A=84=E8=A7=92=E5=BA=A6=E5=92=8C=E9=80=9F?=
 =?UTF-8?q?=E5=BA=A6=E8=AE=A1=E7=AE=97=20-=20=E8=B0=83=E6=95=B4=E5=A4=9A?=
 =?UTF-8?q?=E4=B8=AAPID=E6=8E=A7=E5=88=B6=E5=99=A8=E7=9A=84=E5=8F=82?=
 =?UTF-8?q?=E6=95=B0=E9=85=8D=E7=BD=AE=20-=20=E5=A2=9E=E5=BC=BA=E5=8D=A1?=
 =?UTF-8?q?=E5=B0=94=E6=9B=BC=E6=BB=A4=E6=B3=A2=E5=99=A8=E7=9A=84=E6=95=B0?=
 =?UTF-8?q?=E5=80=BC=E7=A8=B3=E5=AE=9A=E6=80=A7?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 Robot_V2/Algorithm/FOC.cpp     | 38 +++++++++-------
 Robot_V2/Algorithm/FOC.h       | 10 ++++-
 Robot_V2/Algorithm/Filter.cpp  | 82 +++++++++++++++++++++++++++++++---
 Robot_V2/Algorithm/Filter.h    | 81 +++++++++++++++++++++++++++++----
 Robot_V2/device/ADC.cpp        | 16 ++++---
 Robot_V2/device/Can.cpp        |  3 +-
 Robot_V2/device/Timer.cpp      |  2 +-
 Robot_V2/device/ang_sensor.cpp |  1 -
 Robot_V2/main.cpp              |  4 +-
 Robot_V2/sys/config.h          | 58 ++++++++++++++++++------
 10 files changed, 237 insertions(+), 58 deletions(-)

diff --git a/Robot_V2/Algorithm/FOC.cpp b/Robot_V2/Algorithm/FOC.cpp
index 032b167..dbd74ea 100644
--- a/Robot_V2/Algorithm/FOC.cpp
+++ b/Robot_V2/Algorithm/FOC.cpp
@@ -16,6 +16,7 @@ extern sampler sampler;
 extern angle_sensor KTH7823;
 extern CAN_transmiter CAN0_transmiter;
 extern EncoderVelocityKalman Vel_kalman; // 速度滤波
+extern CurrentKalman Current_kalman;
 
 // 初始变量及函数定义
 #define _constrain(amt, low, high)                                             \
@@ -29,8 +30,8 @@ inline float FOC_Controller::cal_Iq_Id(float current_a, float current_b,
 	// 计算电气角度
 	float angle_el = fmod(shaft_angle * PP, 360.0f);
 
-	float I_alpha, I_beta;
 	// Clarke变换 (使用三相电流)
+	float I_alpha, I_beta;
 	I_alpha = (2.0f / 3.0f) * current_a - (1.0f / 3.0f) * current_b -
 			  (1.0f / 3.0f) * current_c;
 	I_beta = _1_SQRT3 * current_b - _1_SQRT3 * current_c;
@@ -41,8 +42,8 @@ inline float FOC_Controller::cal_Iq_Id(float current_a, float current_b,
 	float I_q, I_d;
 	I_q = I_beta * cos_el - I_alpha * sin_el;
 
-	return I_q * 1000.0f -
-		   KE_FACTOR * rt_velocity; // 返回Iq值，乘以1000以转换为mA
+	// 返回Iq值，乘以1000以转换为mA
+	return Current_kalman.update(I_q * 1000 - KE_FACTOR * rt_velocity);
 }
 
 /**
@@ -52,12 +53,15 @@ inline float FOC_Controller::cal_Iq_Id(float current_a, float current_b,
  */
 u8 FOC_Controller::init() {
 	motor_init(); // 电机驱动初始化
-	vel_loop.set_PID(DEFAULT_vel_loop_p * 100 / voltage_power_supply, vel_loop_Ki,
+	vel_loop.set_PID(DEFAULT_vel_loop_p * 100 / voltage_power_supply,
+					 vel_loop_Ki,
 					 vel_loop_Kd); // 设置电机速度控制参数
-	angle_loop.set_PID(DEFAULT_angle_loop_p * 100 / voltage_power_supply, 100.0f,
+	angle_loop.set_PID(DEFAULT_angle_loop_p * 100 / voltage_power_supply,
+					   100.0f,
 					   0.2f); // 设置电机角度控制参数
-	current_loop.set_PID(DEFAULT_current_loop_p * 100 / voltage_power_supply, 5e-7f,
-						 7e-8); // 设置电机电流控制参数
+	current_loop.set_PID(DEFAULT_current_loop_p * 100 / voltage_power_supply,
+						 current_loop_Ki,
+						 current_loop_Kd); // 设置电机电流控制参数
 
 	vel_inVelAngloop.set_PID(DEFAULT_vel_angloop_p * 100, vel_angloop_Ki,
 							 vel_angloop_Kd); // 设置电机速度控制参数
@@ -65,7 +69,9 @@ u8 FOC_Controller::init() {
 							 ang_angloop_Ki,
 							 ang_angloop_Kd); // 设置电机角度控制参数
 
-	CAN0_transmiter.testfloat_union.f = DEFAULT_ang_angloop_p * 100 / voltage_power_supply;
+	vel_inMITloop.set_PID(DEFAULT_ang_angloop_p * 100 / voltage_power_supply,
+						  vel_MIT_Ki,
+						  vel_MIT_Kd); // 设置电机角度控制参数
 	return 0;
 }
 
@@ -262,7 +268,7 @@ void FOC_Controller::update_power(float voltage) {
 		voltage_error = false;
 		voltage_power_supply = voltage;
 		v_half = voltage * 0.5f;
-		v_safe = voltage * 1.05f;
+		v_safe = voltage * 1.1f;
 	} else {
 		voltage_error = true;
 	}
@@ -347,7 +353,7 @@ float FOC_Controller::angle_control_with_velocity_limit() {
 }
 
 /**
- * @brief 带速度限制的角度闭环控制
+ * @brief MIT控制
  *
  * @param target_angle: 目标角度
  * @param max_velocity: 最大速度限制 (单位: rad/s)
@@ -356,17 +362,17 @@ float FOC_Controller::angle_control_with_velocity_limit() {
 float FOC_Controller::MIT_Mode() {
 	// 第一层：角度环（外环）
 	// 根据目标角度和实际角度的差值，通过角度环PID控制器计算目标速度
-	float angle_error = rt_angle - target_angle;
+	float angle_error = target_angle - rt_angle;
 	float target_velocity =
 		ang_inVelAngloop(angle_error); // 使用angle_loop作为角度到速度的映射
+	CAN0_transmiter.testfloat_union.f = target_velocity;
 
 	// 第二层：速度环（中环）
 	// 根据目标速度和实际速度的差值，通过速度环PID控制器计算目标电流
-	float velocity_error = target_velocity - rt_velocity;
-	float target_current =
-		vel_inVelAngloop(velocity_error); // 使用vel_loop作为速度到电流的映射
+	float velocity_error = rt_velocity - target_velocity;
+	vel_inMITloop.set_P(vel_MIT_p);
+	float target_current = vel_inMITloop(velocity_error);
 
-	CAN0_transmiter.testfloat_union.f = target_current;
 	// 第三层：电流环（内环）
 	// 计算当前实际电流
 	rt_current = cal_Iq_Id(sampler.get_current('U'), sampler.get_current('V'),
@@ -410,8 +416,8 @@ float FOC_Controller::current_control() {
 	float Iq = rt_current =
 		cal_Iq_Id(sampler.get_current('U'), sampler.get_current('V'),
 				  sampler.get_current('W'));
+	current_loop.set_P(current_loop_p);
 	float Uq = current_loop(target_current - Iq);
-	CAN0_transmiter.testfloat_union.f = Iq;
 	// 获取电机轴角度
 	shaft_angle = KTH7823.getcurrent_sensorangle();
 	// 设置相电压
diff --git a/Robot_V2/Algorithm/FOC.h b/Robot_V2/Algorithm/FOC.h
index 0b9302e..266c532 100644
--- a/Robot_V2/Algorithm/FOC.h
+++ b/Robot_V2/Algorithm/FOC.h
@@ -64,9 +64,15 @@ class FOC_Controller {
 	PIDController current_loop = PIDController{current_loop_p * 100, 0, 0, 1500,
 											   voltage_power_supply}; // 12V
 	PIDController vel_inVelAngloop =	
-		PIDController{vel_angloop_p * 100, 0, 0, 1000000, v_safe};
+		PIDController{vel_angloop_p * 100, 0, 0, 1e6, v_safe};
 	PIDController ang_inVelAngloop =
-		PIDController{ang_angloop_p * 100, 0, 0, 20000, 100};
+		PIDController{ang_angloop_p * 100, 0, 0, 1e4, 100};
+
+		
+	PIDController ang_inMITloop =
+		PIDController{ang_angloop_p * 100, 0, 0, 1e4, 100};
+	PIDController vel_inMITloop =
+		PIDController{vel_MIT_p * 100, 0, 0, 1e9, 6000};
 
 	void update_pid_limits() {
 		vel_loop.limit = voltage_power_supply;
diff --git a/Robot_V2/Algorithm/Filter.cpp b/Robot_V2/Algorithm/Filter.cpp
index 4a5f776..a6d3a2c 100644
--- a/Robot_V2/Algorithm/Filter.cpp
+++ b/Robot_V2/Algorithm/Filter.cpp
@@ -8,8 +8,20 @@
 extern angle_sensor KTH7823;
 extern CAN_transmiter CAN0_transmiter;
 extern u32 motor_runtime_50us;
-EncoderAngleKalman Angle_kalman(0, 1.0f, 1e-4f, 0.25f);
-EncoderVelocityKalman Vel_kalman(0, 1e-5f, 1e-6f, 0.02f);
+
+// 使用配置文件中的参数
+EncoderAngleKalman Angle_kalman(ANGLE_KALMAN_INITIAL_STATE,
+								ANGLE_KALMAN_INITIAL_COVARIANCE,
+								ANGLE_KALMAN_PROCESS_NOISE_Q,
+								ANGLE_KALMAN_MEASUREMENT_NOISE_R);
+
+EncoderVelocityKalman Vel_kalman(VELOCITY_KALMAN_INITIAL_STATE,
+								 VELOCITY_KALMAN_INITIAL_COVARIANCE,
+								 VELOCITY_KALMAN_PROCESS_NOISE_Q,
+								 VELOCITY_KALMAN_MEASUREMENT_NOISE_R);
+CurrentKalman Current_kalman(	CURRENT_KALMAN_INITIAL_STATE, CURRENT_KALMAN_INITIAL_COVARIANCE,
+							CURRENT_KALMAN_PROCESS_NOISE_Q,
+							CURRENT_KALMAN_MEASUREMENT_NOISE_R);
 
 /*!
  \brief 	中值滤波器函数
@@ -96,7 +108,9 @@ EncoderAngleKalman::EncoderAngleKalman(float initial_angle,
 									   float measurement_noise)
 	: kalman(initial_angle, initial_error_covariance, process_noise,
 			 measurement_noise),
-	  last_angle(initial_angle), initialized(true) {}
+	  last_angle(initial_angle), initialized(true),
+	  velocity_last_angle(initial_angle),
+	  velocity_last_us50_counter(motor_runtime_50us) {}
 
 /*!
 	\brief      更新卡尔曼滤波器状态
@@ -125,10 +139,18 @@ float EncoderAngleKalman::getAngle() const { return kalman.getState(); }
 	\retval     当前角度估计值 (degree)
 */
 float EncoderAngleKalman::getVelocity() const {
-	static u32 last_us50_counter = motor_runtime_50us;
-	float vel = (kalman.getState() - last_angle) * TIMER2_FREQUENCY *
-				(motor_runtime_50us - last_us50_counter);
-	last_us50_counter = motor_runtime_50us;
+	float angle_now = kalman.getState();
+	u32 time_counts = motor_runtime_50us - velocity_last_us50_counter;
+
+	float vel = 0.0f;
+	if (time_counts > 0) {
+		vel = (angle_now - velocity_last_angle) * TIMER2_FREQUENCY /
+			  time_counts * 2;
+	}
+
+	velocity_last_angle = angle_now;
+	velocity_last_us50_counter = motor_runtime_50us;
+
 	return vel;
 }
 
@@ -146,4 +168,50 @@ void EncoderAngleKalman::setProcessNoise(float q) { kalman.setProcessNoise(q); }
 */
 void EncoderAngleKalman::setMeasurementNoise(float r) {
 	kalman.setMeasurementNoise(r);
+}
+
+// CurrentKalman类的实现
+/*!
+	\brief      构造函数，初始化电流卡尔曼滤波器参数
+	\param[in]  initial_current 初始电流值 (A)
+	\param[in]  initial_error_covariance 初始误差协方差
+	\param[in]  process_noise 过程噪声协方差
+	\param[in]  measurement_noise 测量噪声协方差
+	\retval     none
+*/
+CurrentKalman::CurrentKalman(float initial_current,
+							 float initial_error_covariance,
+							 float process_noise, float measurement_noise)
+	: kalman(initial_current, initial_error_covariance, process_noise,
+			 measurement_noise) {}
+
+/*!
+	\brief      更新卡尔曼滤波器状态
+	\param[in]  measured_current 测量的电流值 (A)
+	\retval     估计的电流值 (A)
+*/
+float CurrentKalman::update(float measured_current) {
+	return kalman.update(measured_current);
+}
+
+/*!
+	\brief      获取当前电流估计值
+	\retval     当前电流估计值 (A)
+*/
+float CurrentKalman::getCurrent() const { return kalman.getState(); }
+
+/*!
+	\brief      设置过程噪声协方差
+	\param[in]  q 过程噪声协方差值
+	\retval     none
+*/
+void CurrentKalman::setProcessNoise(float q) { kalman.setProcessNoise(q); }
+
+/*!
+	\brief      设置测量噪声协方差
+	\param[in]  r 测量噪声协方差值
+	\retval     none
+*/
+void CurrentKalman::setMeasurementNoise(float r) {
+	kalman.setMeasurementNoise(r);
 }
\ No newline at end of file
diff --git a/Robot_V2/Algorithm/Filter.h b/Robot_V2/Algorithm/Filter.h
index a0b3fb8..5b631fd 100644
--- a/Robot_V2/Algorithm/Filter.h
+++ b/Robot_V2/Algorithm/Filter.h
@@ -5,16 +5,17 @@
 extern "C" {
 #endif
 
-// C风格函数声明可以放在这里
-// 例如: void some_c_function(float* data, int size);
-
 #ifdef __cplusplus
 } // extern "C"
 #endif
 
 #ifdef __cplusplus
-#include <cstring>
+#include "Can.h"
 #include <cstdint>
+#include <cstring>
+
+
+extern CAN_transmiter CAN0_transmiter;
 
 float medium_filter(float values[], u8 size);
 
@@ -75,10 +76,19 @@ template <typename T> class KalmanFilter {
 		\retval 估计的状态值
 	*/
 	T update(T measurement) {
-		T K = P / (P + R); // 卡尔曼增益
 
+		T K = P / (P + R); // 卡尔曼增益
+		
 		x += K * (measurement - x); // 更新状态估计
-		P = (1 - K) * P;			// 更新误差协方差
+
+		P = (1 - K) * P; // 更新误差协方差
+		
+		// 添加协方差下限保护，防止数值不稳定
+		const T MIN_COVARIANCE = static_cast<T>(1e-6);
+		if (P < MIN_COVARIANCE) {
+			P = MIN_COVARIANCE;
+		}
+		
 		return x;
 	}
 
@@ -101,6 +111,25 @@ template <typename T> class KalmanFilter {
 		\retval none
 	*/
 	void setMeasurementNoise(T measurement_noise) { R = measurement_noise; }
+
+	/*!
+		\brief 重置估计误差协方差
+		\param[in] error_covariance 误差协方差值
+		\retval none
+	*/
+	void resetCovariance(T error_covariance) { P = error_covariance; }
+
+	/*!
+		\brief 获取当前误差协方差
+		\retval 当前误差协方差
+	*/
+	T getCovariance() const { return P; }
+
+	/*!
+		\brief 获取当前过程噪声协方差
+		\retval 当前过程噪声协方差
+	*/
+	T getProcessNoise() const { return Q; }
 };
 
 /*!
@@ -124,8 +153,9 @@ class EncoderVelocityKalman {
 
 	void setMeasurementNoise(float r);
 
-	u8 init(float init_velocity) {
+	u8 init(float init_velocity, float initial_error_covariance = 1.0f) {
 		kalman.x = init_velocity;
+		kalman.resetCovariance(initial_error_covariance); // 重置估计误差协方差
 		return 0;
 	}
 };
@@ -139,6 +169,9 @@ class EncoderAngleKalman {
 	KalmanFilter<float> kalman;
 	bool initialized;
 	float last_angle;
+	// 添加成员变量来替代静态变量
+	mutable u32 velocity_last_us50_counter;
+	mutable float velocity_last_angle;
 
   public:
 	EncoderAngleKalman(float initial_angle, float initial_error_covariance,
@@ -153,12 +186,44 @@ class EncoderAngleKalman {
 	float getstate() const { return kalman.getState(); };
 
 	void setMeasurementNoise(float r);
-	u8 init(float init_angle) {
+	u8 init(float init_angle, float initial_error_covariance = 1.0f) {
 		kalman.x = init_angle;
+		kalman.resetCovariance(initial_error_covariance); // 重置估计误差协方差
+		last_angle = init_angle;						  // 同时重置last_angle
+		// 重置速度计算相关的变量
+		velocity_last_angle = init_angle;
+		extern u32 motor_runtime_50us;
+		velocity_last_us50_counter = motor_runtime_50us;
 		return 0;
 	}
 };
 
+/*!
+	\brief 电流卡尔曼滤波器类
+	\note  用于滤除电流测量噪声的卡尔曼滤波器实现
+*/
+class CurrentKalman {
+  private:
+	KalmanFilter<float> kalman;
+
+  public:
+	CurrentKalman(float initial_current, float initial_error_covariance,
+				  float process_noise, float measurement_noise);
+
+	float update(float measured_current);
+
+	float getCurrent() const;
+
+	void setProcessNoise(float q);
+
+	void setMeasurementNoise(float r);
+
+	u8 init(float init_current, float initial_error_covariance = 1.0f) {
+		kalman.x = init_current;
+		kalman.resetCovariance(initial_error_covariance); // 重置估计误差协方差
+		return 0;
+	}
+};
 #endif // __cplusplus
 
 #endif // _FILTER_H
\ No newline at end of file
diff --git a/Robot_V2/device/ADC.cpp b/Robot_V2/device/ADC.cpp
index ce44c17..7a0ea2d 100644
--- a/Robot_V2/device/ADC.cpp
+++ b/Robot_V2/device/ADC.cpp
@@ -5,9 +5,9 @@
 #include "config.h"
 #include "math.h"
 #include "systick.h"
+#include <cstdint>
 #include <cstring>
 #include <gd32c10x_dma.h>
-#include <cstdint>
 
 constexpr float ADC_SCALE = 3.3f / 4096.0f;
 constexpr float TEMP_FACTOR = 1000.0f / 4.3f;
@@ -16,6 +16,7 @@ constexpr float CURRENT_FACTOR =
 	1.0f / 50.0f * 100.0f; // 50倍放大，100倍电流采样电阻, 单位mA
 extern CAN_transmiter CAN0_transmiter;
 extern FOC_Controller FOC;
+
 sampler sampler;		// 采样器初始化
 u16 adc_value[6] = {0}; // 显式初始化数组元素为0
 /*!
@@ -167,20 +168,25 @@ float sampler::get_drv_temp() {
  * 通过ADC采样计算各相电流值
  */
 float sampler::get_current(char type) {
+
+	u8 idx = current_sample_index;
 	switch (type) {
 	case 'U': {
 		// return currU_samples[current_sample_index];
-		tempora_sample = medium_filter(currU_samples, ADC_FILTER_SIZE + 1);
+		// tempora_sample = medium_filter(currU_samples, ADC_FILTER_SIZE + 1);
+		tempora_sample = currU_samples[idx];
 		break;
 	}
 	case 'V': {
 		// return currV_samples[current_sample_index];
-		tempora_sample = medium_filter(currV_samples, ADC_FILTER_SIZE + 1);
+		// tempora_sample = medium_filter(currV_samples, ADC_FILTER_SIZE + 1);
+		tempora_sample = currV_samples[idx];
 		break;
 	}
 	case 'W': {
 		// return currW_samples[current_sample_index];
-		tempora_sample = medium_filter(currW_samples, ADC_FILTER_SIZE + 1);
+		// tempora_sample = medium_filter(currW_samples, ADC_FILTER_SIZE + 1);
+		tempora_sample = currW_samples[idx];
 		break;
 	}
 	default: {
@@ -254,8 +260,6 @@ void sampler::update_current() {
 	if (current_sample_index >= ADC_FILTER_SIZE - 1) {
 		current_sample_index = 0;
 	}
-	// 卡尔曼滤波更新
-	float voltage = FOC.voltage_power_supply;
 }
 
 /**
diff --git a/Robot_V2/device/Can.cpp b/Robot_V2/device/Can.cpp
index 41b0cdf..c548084 100644
--- a/Robot_V2/device/Can.cpp
+++ b/Robot_V2/device/Can.cpp
@@ -537,6 +537,7 @@ void CAN_transmiter::communication_check() {
 			target_current_union.f =
 				unpack_bytes_to_float(receive_message.rx_data, 3).f;
 			FOC.current_loop.set_limit(target_current_union.f);
+			FOC.vel_inMITloop.set_limit(target_current_union.f);
 			// 目标速度
 			target_velocity_union.f =
 				unpack_bytes_to_float(receive_message.rx_data, 7).f;
@@ -557,7 +558,6 @@ void CAN_transmiter::communication_check() {
 
 			P_config = unpack_bytes_to_u16(receive_message.rx_data, 17);
 			FOC.vel_loop_p = DEFAULT_vel_loop_p * P_config / voltage;
-			FOC.vel_MIT_p = DEFAULT_vel_MIT_p * P_config / voltage;
 
 			P_config = unpack_bytes_to_u16(receive_message.rx_data, 19);
 			FOC.angle_loop_p = DEFAULT_angle_loop_p * P_config / voltage;
@@ -565,6 +565,7 @@ void CAN_transmiter::communication_check() {
 
 			P_config = unpack_bytes_to_u16(receive_message.rx_data, 21);
 			FOC.vel_angloop_p = DEFAULT_vel_angloop_p * P_config;
+			FOC.vel_MIT_p = DEFAULT_vel_MIT_p * P_config;
 
 		} else if (receive_message.rx_efid == CANFD_RESET_ID) {
 			u8 first_byte = receive_message.rx_data[0];
diff --git a/Robot_V2/device/Timer.cpp b/Robot_V2/device/Timer.cpp
index 04a9d53..828f338 100644
--- a/Robot_V2/device/Timer.cpp
+++ b/Robot_V2/device/Timer.cpp
@@ -96,7 +96,6 @@ void TIMER2_IRQHandler(void) {
 		// 每50us执行一次的操作 - 电机控制
 		scheduler.taskFlags |=
 			(1 << TASK_ANGLE_UPDATE) | (1 << TASK_VELOCITY_UPDATE);
-		scheduler.taskFlags |= (1 << TASK_MOTOR_CONTROL);
 
 		// 使用标志位控制，每100us执行一次其他任务
 		static bool isEvenCall = false;
@@ -105,6 +104,7 @@ void TIMER2_IRQHandler(void) {
 		if (isEvenCall) {
 			// 每100us执行一次的操作 - 保持原有频率
 
+			scheduler.taskFlags |= (1 << TASK_MOTOR_CONTROL);
 			switch (total_us100_counter) { // 每1ms执行的代码，负载均衡
 			case 0: {
 				scheduler.taskFlags |= (1 << TASK_MCU_TEMP_UPDATE);
diff --git a/Robot_V2/device/ang_sensor.cpp b/Robot_V2/device/ang_sensor.cpp
index a8f0056..8f64084 100644
--- a/Robot_V2/device/ang_sensor.cpp
+++ b/Robot_V2/device/ang_sensor.cpp
@@ -265,7 +265,6 @@ float angle_sensor::update(void) {
 
 	m_cumulativeAngle += delta;
 	m_lastAngle = current_angle_deg;
-
 	return m_cumulativeAngle;
 }
 
diff --git a/Robot_V2/main.cpp b/Robot_V2/main.cpp
index be7b189..7fcf125 100644
--- a/Robot_V2/main.cpp
+++ b/Robot_V2/main.cpp
@@ -22,8 +22,8 @@ extern VersionManager Version;
 
 // 全局版本管理器实例
 TaskScheduler scheduler = {0};
-volatile u32 motor_runtime_50us = 0; // 电机驱动计算轮次
-volatile u32 cnt_test = 0;			 // 负载检测
+u32 motor_runtime_50us = 0; // 电机驱动计算轮次
+u32 cnt_test = 0;			 // 负载检测
 int main(void) {
 	u8 init_status = hardware_init();
 
diff --git a/Robot_V2/sys/config.h b/Robot_V2/sys/config.h
index dbe2496..43a0e00 100644
--- a/Robot_V2/sys/config.h
+++ b/Robot_V2/sys/config.h
@@ -12,9 +12,9 @@
 
 // 版本号定义 - 可根据实际项目进行调整
 constexpr u16 SOFTWARE_VERSION_MAJOR = 26;
-constexpr u16 SOFTWARE_VERSION_MINOR = 1;
-constexpr u16 SOFTWARE_VERSION_PATCH = 24;
-constexpr u16 SOFTWARE_VERSION_BUILD = 1;
+constexpr u16 SOFTWARE_VERSION_MINOR = 2;
+constexpr u16 SOFTWARE_VERSION_PATCH = 8;
+constexpr u16 SOFTWARE_VERSION_BUILD = 5;
 
 constexpr u16 HARDWARE_VERSION_MAJOR = 25;
 constexpr u16 HARDWARE_VERSION_MINOR = 12;
@@ -57,9 +57,11 @@ constexpr u16 REDUCTION_RATIO = 25;
 constexpr float CURRENT_OFFSET = -1.62f; // 当前电流传感器的零点偏移
 constexpr float HOLD_PWM_DUTY = 0.95f;
 constexpr u16 ANGLESENS_OFFSET = +0;
-constexpr float KE_FACTOR = 14.0f;
+constexpr float KE_FACTOR = 0.0f;
 // PID配置
-constexpr float DEFAULT_current_loop_p = 6e-2f / 100 * 12; // 电流P
+constexpr float DEFAULT_current_loop_p = 0.1f / 100 * 12; // 电流P
+constexpr float current_loop_Ki = 0.55f;				  // 电流P
+constexpr float current_loop_Kd = 3e-4;					  // 电流P
 
 constexpr float DEFAULT_vel_loop_p = 0.018f / 100 * 12; // 速度P
 constexpr float vel_loop_Ku = 3.600000e-03 * 0.7 * 100 / 12;
@@ -67,21 +69,49 @@ constexpr float vel_loop_Tu = 17.0f * 1e-3f;
 constexpr float vel_loop_Ki = 1.2 * vel_loop_Ku / vel_loop_Tu;
 constexpr float vel_loop_Kd = 0.075 * vel_loop_Ku * vel_loop_Tu * 2;
 
-constexpr float DEFAULT_vel_angloop_p = 0.22f / 100; // 速度 速度-角度P
-constexpr float vel_angloop_Ki = 12.0f;					  // 角度 速度-角度I
-constexpr float vel_angloop_Kd = 6e-04f;				  // 角度 速度-角度D
+constexpr float DEFAULT_vel_angloop_p = 0.4f / 100; // 速度 速度-角度P
+constexpr float vel_angloop_Ki = 8.0f;				// 速度 速度-角度I
+constexpr float vel_angloop_Kd = 5e-05f;			// 速度 速度-角度D
 
-constexpr float DEFAULT_ang_angloop_p = 120.0f / 100 * 12; // 角度 速度-角度P
-constexpr float ang_angloop_Ki = 0.00f;					  // 角度 速度-角度I
-constexpr float ang_angloop_Kd = 8e-04;					  // 角度 速度-角度D
+constexpr float DEFAULT_ang_angloop_p = 150.0f / 100 * 12; // 角度 速度-角度P
+constexpr float ang_angloop_Ki = 0.00f;					   // 角度 速度-角度I
+constexpr float ang_angloop_Kd = 8e-04;					   // 角度 速度-角度D
 
 constexpr float DEFAULT_angle_loop_p = 60.0f / 100 * 12; // 角度P
 
-constexpr float DEFAULT_vel_MIT_p = 25.0f / 100 * 12; // 速度 速度-角度P
+constexpr float DEFAULT_vel_MIT_p = 9.0f / 100; // 速度 MIT P
 constexpr float vel_MIT_Ku = 6.72f * 100 / 12;
 constexpr float vel_MIT_Tu = 62.0f * 1e-3f;
-constexpr float vel_MIT_Ki = 3e-2f;
-constexpr float vel_MIT_Kd = 5.355000e-05f;
+constexpr float vel_MIT_Ki = 0.1f;
+constexpr float vel_MIT_Kd = 0.01f;
+
+// 角度卡尔曼滤波器参数
+// 参数调节建议：
+// - 增大Q值：增加对测量的信任，提高响应速度
+// - 减小Q值：增加对预测的信任，提高平滑度
+// - 增大R值：增加平滑度，减少噪声，但会降低响应速度
+// - 减小R值：提高响应速度，但会增加噪声
+constexpr float ANGLE_KALMAN_INITIAL_STATE = 0.0f;
+constexpr float ANGLE_KALMAN_INITIAL_COVARIANCE = 0.0f;
+constexpr float ANGLE_KALMAN_PROCESS_NOISE_Q =
+	1e-3f; // 过程噪声协方差 - 调节此参数控制平滑度
+constexpr float ANGLE_KALMAN_MEASUREMENT_NOISE_R =
+	0.0062f; // 测量噪声协方差 - 调节此参数控制响应速度
+
+// 速度卡尔曼滤波器参数
+constexpr float VELOCITY_KALMAN_INITIAL_STATE = 0.0f;
+constexpr float VELOCITY_KALMAN_INITIAL_COVARIANCE = 1e-5f;
+constexpr float VELOCITY_KALMAN_PROCESS_NOISE_Q =
+	1e-6f; // 过程噪声协方差 - 通常保持较小
+constexpr float VELOCITY_KALMAN_MEASUREMENT_NOISE_R =
+	1.0f; // 测量噪声协方差 - 主要调节参数
+
+// 电流卡尔曼滤波器参数
+constexpr float CURRENT_KALMAN_INITIAL_STATE = 0.0f;
+constexpr float CURRENT_KALMAN_INITIAL_COVARIANCE = 100.0f;		// 增大初始协方差
+constexpr float CURRENT_KALMAN_PROCESS_NOISE_Q = 10.0f;			// 降低过程噪声
+constexpr float CURRENT_KALMAN_MEASUREMENT_NOISE_R = 1e-3f;      // 增大测量噪声
+
 // 常量定义
 constexpr u8 SIZE_FLOAT = sizeof(float);
 constexpr u8 SIZE_U32 = sizeof(uint32_t);