From 971374d8ec77401edf7f3882f8b8b5fe4f65f6e6 Mon Sep 17 00:00:00 2001
From: cbb <2625274797@qq.com>
Date: Thu, 6 Nov 2025 02:07:15 +0800
Subject: [PATCH] =?UTF-8?q?feat(FOC):=20=E5=A2=9E=E5=8A=A0=E9=97=AD?=
 =?UTF-8?q?=E7=8E=AF=E9=80=9F=E5=BA=A6=E6=8E=A7=E5=88=B6=E7=AE=97=E6=B3=95?=
 =?UTF-8?q?=20-=20=E9=87=8D=E6=9E=84setPhaseVoltage=E5=87=BD=E6=95=B0?=
 =?UTF-8?q?=E5=AE=9E=E7=8E=B0=EF=BC=8C=E6=8F=90=E9=AB=98=E6=89=A7=E8=A1=8C?=
 =?UTF-8?q?=E6=95=88=E7=8E=87=20-=20=E5=A2=9E=E5=8A=A0update=5Fpower?=
 =?UTF-8?q?=E5=87=BD=E6=95=B0=EF=BC=8C=E5=8A=A8=E6=80=81=E6=9B=B4=E6=96=B0?=
 =?UTF-8?q?=E7=94=B5=E6=BA=90=E7=94=B5=E5=8E=8B=20-=20=E4=BC=98=E5=8C=96ve?=
 =?UTF-8?q?locity=5Fopen=5Floop=E5=87=BD=E6=95=B0=EF=BC=8C=E8=B0=83?=
 =?UTF-8?q?=E7=94=A8=E3=80=81setPhaseVoltage=E9=81=BF=E5=85=8D=E9=87=8D?=
 =?UTF-8?q?=E5=A4=8D=E4=BB=A3=E7=A0=81=20-=20=E5=AE=9E=E7=8E=B0angle=5Fcon?=
 =?UTF-8?q?trol=E5=92=8Cvelocity=5Fcontrol=E9=97=AD=E7=8E=AF=E6=8E=A7?=
 =?UTF-8?q?=E5=88=B6=E5=87=BD=E6=95=B0=20-=20=E8=B0=83=E6=95=B4PID?=
 =?UTF-8?q?=E6=8E=A7=E5=88=B6=E5=99=A8=E5=8F=82=E6=95=B0=E5=88=9D=E5=A7=8B?=
 =?UTF-8?q?=E5=8C=96=E5=8F=8A=E5=A2=9E=E5=8A=A0=E5=8F=82=E6=95=B0=E8=AE=BE?=
 =?UTF-8?q?=E7=BD=AE=E6=8E=A5=E5=8F=A3=20-=20=E6=94=B9=E8=BF=9B=E7=94=B5?=
 =?UTF-8?q?=E6=B5=81=E5=92=8C=E7=BC=96=E7=A0=81=E5=99=A8=E9=80=9F=E5=BA=A6?=
 =?UTF-8?q?=E5=8D=A1=E5=B0=94=E6=9B=BC=E6=BB=A4=E6=B3=A2=E5=99=A8=E5=AE=9E?=
 =?UTF-8?q?=E7=8E=B0=20-=20=E6=9B=B4=E6=96=B0CAN=E9=80=9A=E4=BF=A1?=
 =?UTF-8?q?=E6=95=B0=E6=8D=AE=E6=89=93=E5=8C=85=E9=80=BB=E8=BE=91=EF=BC=8C?=
 =?UTF-8?q?=E5=A2=9E=E5=8A=A0=E6=B5=8B=E8=AF=95=E5=AD=97=E6=AE=B5=20-=20?=
 =?UTF-8?q?=E5=9C=A8=E5=AE=9A=E6=97=B6=E5=99=A8=E4=B8=AD=E6=96=AD=E4=B8=AD?=
 =?UTF-8?q?=E9=9B=86=E6=88=90=E5=A4=9A=E7=A7=8D=E6=8E=A7=E5=88=B6=E6=A8=A1?=
 =?UTF-8?q?=E5=BC=8F=E5=88=87=E6=8D=A2=E9=80=BB=E8=BE=91=20-=20=E5=AE=8C?=
 =?UTF-8?q?=E5=96=84=E8=A7=92=E5=BA=A6=E4=BC=A0=E6=84=9F=E5=99=A8=E6=95=B0?=
 =?UTF-8?q?=E6=8D=AE=E5=A4=84=E7=90=86=EF=BC=8C=E6=94=B9=E8=BF=9B=E9=80=9F?=
 =?UTF-8?q?=E5=BA=A6=E8=AE=A1=E7=AE=97=E6=96=B9=E6=B3=95=20-=20=E5=87=8F?=
 =?UTF-8?q?=E5=B0=8F=E9=80=9F=E5=BA=A6=E6=BB=A4=E6=B3=A2=E6=95=B0=E7=BB=84?=
 =?UTF-8?q?=E5=A4=A7=E5=B0=8F=EF=BC=8C=E6=8F=90=E5=8D=87=E5=93=8D=E5=BA=94?=
 =?UTF-8?q?=E9=80=9F=E5=BA=A6?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 Robot_V2/Algorithm/DengFOC.cpp | 146 ++++++++++++++++-----------------
 Robot_V2/Algorithm/DengFOC.h   |   6 +-
 Robot_V2/Algorithm/Filter.cpp  | 100 +++++++++++++++-------
 Robot_V2/Algorithm/Filter.h    | 115 ++++++++++++++++++++++++--
 Robot_V2/Algorithm/pid.cpp     |  96 +++++++++++++---------
 Robot_V2/device/Can.cpp        |  11 ++-
 Robot_V2/device/Can.h          |   1 +
 Robot_V2/device/Timer.cpp      |  21 +++--
 Robot_V2/device/ang_sensor.cpp |  27 +++---
 Robot_V2/device/ang_sensor.h   |   2 +-
 Robot_V2/main.cpp              |   1 -
 Robot_V2/pid.h                 |   1 +
 12 files changed, 355 insertions(+), 172 deletions(-)

diff --git a/Robot_V2/Algorithm/DengFOC.cpp b/Robot_V2/Algorithm/DengFOC.cpp
index 7cd50ca..3d60a01 100644
--- a/Robot_V2/Algorithm/DengFOC.cpp
+++ b/Robot_V2/Algorithm/DengFOC.cpp
@@ -1,4 +1,5 @@
 #include "DengFOC.h"
+#include "Can.h"
 #include "Filter.h"
 #include "Timer.h"
 #include "adc.h"
@@ -14,21 +15,22 @@
 constexpr double PI = 3.1415926535897932384626433832795f;
 constexpr float DEG2RAD = 1.0f / 360.0f * 6.28318530718f;
 constexpr float RAD2DEG = 360.0f / 6.28318530718f;
+constexpr float ANGLE_FACTOR = 25.0f / 360.0f * 2.0f * 3.14159265358979323846f;
 constexpr float _1_SQRT3 = 0.57735026919f;
 constexpr float _2_SQRT3 = 1.15470053838f;
 constexpr float _3PI_2 = 4.71238898038f;
 constexpr u8 PP = 11;
-constexpr float DIR = -1.0;
+constexpr float DIR = 1.0;
 constexpr float sqrt3 = 1.73205080757f; // sqrt(3)的预计算值
 constexpr float zero_electric_angle = 0.0f;
 
 extern sampler sampler;
 extern angle_sensor KTH7823;
+extern CAN_transmiter CAN0_transmiter;
 
 CurrentKalman curU_kalman(0.0f, 0.0026f, 4.8f, 0.0001f, 1e-6f, 0.02f);
 CurrentKalman curV_kalman(0.0f, 0.0026f, 4.8f, 0.0001f, 1e-6f, 0.02f);
 CurrentKalman curW_kalman(0.0f, 0.0026f, 4.8f, 0.0001f, 1e-6f, 0.02f);
-
 // 初始变量及函数定义
 #define _constrain(amt, low, high)                                             \
 	((amt) < (low) ? (low) : ((amt) > (high) ? (high) : (amt)))
@@ -65,15 +67,17 @@ FOC_Controlroller::FOC_Controlroller() { ; }
  */
 inline void FOC_Controlroller::setPhaseVoltage(float Uq, float Ud,
 											   float angle_el) {
-	angle_el = _normalizeAngle(angle_el);
-	// 帕克逆变换
-	Ualpha = -Uq * sin(angle_el);
-	Ubeta = Uq * cos(angle_el);
+	// 帕克逆变换 (直接计算，避免函数调用开销)
+	const float sin_el = sin(angle_el);
+	const float cos_el = cos(angle_el);
+	Ualpha = -Uq * sin_el;
+	Ubeta = Uq * cos_el;
+
+	// 克拉克逆变换 (预计算常量值)
+	Ua = Ualpha + v_half;
+	Ub = (sqrt3 * Ubeta - Ualpha) * 0.5f + v_half;
+	Uc = (-Ualpha - sqrt3 * Ubeta) * 0.5f + v_half;
 
-	// 克拉克逆变换
-	Ua = Ualpha + voltage_power_supply / 2;
-	Ub = (sqrt3 * Ubeta - Ualpha) / 2 + voltage_power_supply / 2;
-	Uc = (-Ualpha - sqrt3 * Ubeta) / 2 + voltage_power_supply / 2;
 	// 计算占空比
 	Ua = constrain(Ua, 0.0f, voltage_power_supply, 0.0f, limit_voltage);
 	Ub = constrain(Ub, 0.0f, voltage_power_supply, 0.0f, limit_voltage);
@@ -84,6 +88,33 @@ inline void FOC_Controlroller::setPhaseVoltage(float Uq, float Ud,
 	Set_3Pwm(dc_a, dc_b, dc_c);
 	// std::printf("PWM:%f,%f,%f\n", dc_a, dc_b, dc_c);
 }
+/**
+ * @brief 电机关机
+ *
+ */
+void FOC_Controlroller::shut_down() {
+	is_shut_down = 100;
+	gpio_bit_reset(GPIOB, GPIO_PIN_12);
+}
+/**
+ * @brief 电机恢复
+ *
+ */
+void FOC_Controlroller::resume() {
+	over_current = 0;
+	temp_error = 0;
+	voltage_error = 0;
+	is_shut_down = 0;
+	gpio_bit_set(GPIOB, GPIO_PIN_12);
+}
+/**
+ * @brief 更新电源电压
+ *
+ */
+void FOC_Controlroller::update_power(void) {
+	voltage_power_supply = sampler.get_vbus_voltage();
+	v_half = voltage_power_supply * 0.5f;
+}
 
 /**
  * @brief 速度开环控制
@@ -94,36 +125,16 @@ inline void FOC_Controlroller::setPhaseVoltage(float Uq, float Ud,
 float FOC_Controlroller::velocity_open_loop() {
 	// 通过时间间隔和目标速度计算需要转动的机械角度，存储在shaft_angle中，在此之前，还需要对轴角度归一化
 	// 预计算常量 1e-4 = 100微秒控制周期
-	shaft_angle = _normalizeAngle(shaft_angle + target_velocity * 1e-4f);
+	shaft_angle =
+		_normalizeAngle(shaft_angle + target_velocity * ANGLE_FACTOR * 1e-4f);
 
 	// 直接计算电气角度，避免函数调用开销
-	float angle_el = shaft_angle * PP + zero_electric_angle;
-	angle_el = _normalizeAngle(angle_el);
+	float angle_el = _normalizeAngle(shaft_angle * PP + zero_electric_angle);
 
 	// 因为在开环控制中，Ud始终为0，只控制Uq
 	const float Uq = voltage_power_supply / 2.0f;
-	const float Ud = 0.0f;
 
-	// 帕克逆变换 (直接计算，避免函数调用开销)
-	const float sin_el = sin(angle_el);
-	const float cos_el = cos(angle_el);
-	Ualpha = -Uq * sin_el;
-	Ubeta = Uq * cos_el;
-
-	// 克拉克逆变换 (预计算常量值)
-	const float v_half = voltage_power_supply * 0.5f; // 电源电压的一半
-	Ua = Ualpha + v_half;
-	Ub = (sqrt3 * Ubeta - Ualpha) * 0.5f + v_half;
-	Uc = (-Ualpha - sqrt3 * Ubeta) * 0.5f + v_half;
-
-	// 计算占空比
-	Ua = constrain(Ua, 0.0f, voltage_power_supply, 0.0f, limit_voltage * 2);
-	Ub = constrain(Ub, 0.0f, voltage_power_supply, 0.0f, limit_voltage * 2);
-	Uc = constrain(Uc, 0.0f, voltage_power_supply, 0.0f, limit_voltage * 2);
-	const float dc_a = _constrain(Ua / voltage_power_supply, 0.0f, 1.0f);
-	const float dc_b = _constrain(Ub / voltage_power_supply, 0.0f, 1.0f);
-	const float dc_c = _constrain(Uc / voltage_power_supply, 0.0f, 1.0f);
-	Set_3Pwm(dc_a, dc_b, dc_c);
+	setPhaseVoltage(Uq, 0.0f, angle_el);
 
 	return limit_voltage;
 }
@@ -142,55 +153,40 @@ float FOC_Controlroller::velocity_open_loop() {
 // 	std::printf("0电角度:%f", zero_electric_angle);
 // }
 
-void FOC_Controlroller::shut_down() {
-	is_shut_down = 100;
-	gpio_bit_reset(GPIOB, GPIO_PIN_12);
-}
-
-void FOC_Controlroller::resume() {
-	over_current = 0;
-	temp_error = 0;
-	voltage_error = 0;
-	is_shut_down = 0;
-	gpio_bit_set(GPIOB, GPIO_PIN_12);
-}
-
-void FOC_Controlroller::update_power(void) {
-	voltage_power_supply = sampler.get_vbus_voltage();
+/**
+ * @brief 角度闭环控制
+ *
+ * @param target_angle: 目标角度
+ * @return float: q轴电压
+ */
+float FOC_Controlroller::angle_control(float target_angle) {
+	shaft_angle = KTH7823.getRawAngle() * DEG2RAD;
+	static float Kp = 2.0f;
+	float Uq =
+		1.0f - _constrain(Kp * (target_angle - DIR * shaft_angle) * RAD2DEG,
+						  -voltage_power_supply, voltage_power_supply);
+	static float Ud = 0.0f;
+	// 直接计算电气角度，避免函数调用开销
+	float angle_el = _normalizeAngle(shaft_angle * PP + zero_electric_angle);
+	// 设置相电压
+	setPhaseVoltage(Uq, 0.0f, angle_el);
+	return shaft_angle;
 }
 
 /**
- * @brief 角度闭环控制
+ * @brief 速度闭环控制
  *
  * @param target_velocity: 目标速度
  * @return float: q轴电压
  */
-float FOC_Controlroller::angle_control(float target_angle) {
-	float Sensor_angel = KTH7823.getRawAngle() * DEG2RAD;
-	float Kp = 2.0f;
+float FOC_Controlroller::velocity_control(float target_velocity) {
+	// PID 控制计算
+	float Uq = v_half - vel_loop(target_velocity - rt_velocity);
 
-	float Uq = _constrain(Kp * (target_angle - DIR * Sensor_angel) * RAD2DEG,
-						  -voltage_power_supply, voltage_power_supply);
-	shaft_angle = _normalizeAngle(KTH7823.read_sensorAngle() * DEG2RAD);
-	float Ud = 0.0f;
+	shaft_angle = KTH7823.getRawAngle() * DEG2RAD;
 	// 直接计算电气角度，避免函数调用开销
 	float angle_el = _normalizeAngle(shaft_angle * PP + zero_electric_angle);
-
-	// 帕克逆变换
-	Ualpha = -Uq * sin(angle_el);
-	Ubeta = Uq * cos(angle_el);
-
-	// 克拉克逆变换
-	Ua = Ualpha + voltage_power_supply / 2;
-	Ub = (sqrt3 * Ubeta - Ualpha) / 2 + voltage_power_supply / 2;
-	Uc = (-Ualpha - sqrt3 * Ubeta) / 2 + voltage_power_supply / 2;
-	// 计算占空比
-	Ua = constrain(Ua, 0.0f, voltage_power_supply, 0.0f, limit_voltage);
-	Ub = constrain(Ub, 0.0f, voltage_power_supply, 0.0f, limit_voltage);
-	Uc = constrain(Uc, 0.0f, voltage_power_supply, 0.0f, limit_voltage);
-	float dc_a = _constrain(Ua / voltage_power_supply, 0.0f, 1.0f);
-	float dc_b = _constrain(Ub / voltage_power_supply, 0.0f, 1.0f);
-	float dc_c = _constrain(Uc / voltage_power_supply, 0.0f, 1.0f);
-	Set_3Pwm(dc_a, dc_b, dc_c);
-	return Sensor_angel;
+	// 设置相电压
+	setPhaseVoltage(Uq, 0.0f, angle_el);
+	return rt_velocity;
 }
\ No newline at end of file
diff --git a/Robot_V2/Algorithm/DengFOC.h b/Robot_V2/Algorithm/DengFOC.h
index dca0a28..dcb059e 100644
--- a/Robot_V2/Algorithm/DengFOC.h
+++ b/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);
diff --git a/Robot_V2/Algorithm/Filter.cpp b/Robot_V2/Algorithm/Filter.cpp
index 8db95a2..d585394 100644
--- a/Robot_V2/Algorithm/Filter.cpp
+++ b/Robot_V2/Algorithm/Filter.cpp
@@ -1,10 +1,16 @@
 #include "Filter.h"
+#include "ang_sensor.h"
 #include <algorithm>
 #include <array>
 #include <cstring>
 #include <stdint.h>
 
-//队列实现
+
+extern angle_sensor KTH7823;
+
+EncoderVelocityKalman Vel_kalman(KTH7823.getVelocity(), 1e-5f, 1e-6f, 0.05f);
+
+// 队列实现
 void CircularBuffer::enqueue(const float value) {
 	buffer[head] = value;
 	head = (head + 1) % HISTORY_SIZE;
@@ -73,54 +79,92 @@ float medium_filter(float values[], u8 size) {
 */
 CurrentKalman::CurrentKalman(float initial_current, float L, float R, float T,
 							 float Q, float R_noise)
-	: x(initial_current), P(1.0f), // 初始误差协方差
-	  Q(Q),						   // 过程噪声
-	  R(R_noise),				   // 测量噪声
-	  L(L), R_m(R), T(T) {
-		
-		CircularBuffer buffer;
-	  }
+	: kalman(initial_current, 1.0f, Q, R_noise), L(L), R_m(R), T(T) {}
 /*!
 	\brief      更新卡尔曼滤波器状态
-	\param[in]  measured_current 测量电流值
+	\param[in]  measured_value 测量值
 	\param[in]  applied_voltage 应用电压值
 	\retval     估计的电流值
 	\note       实现卡尔曼滤波的预测和更新步骤
 */
-float CurrentKalman::update(float measured_current, float applied_voltage) {
+float CurrentKalman::update(float measured_value, float applied_voltage) {
 	// 1. 预测步骤 - 基于电机模型
-	float predicted_current = x + T * (applied_voltage - R_m * x) / L;
-
-	// 2. 计算预测误差协方差
-	float P_pred = P + Q;
-
-	// 3. 计算卡尔曼增益
-	float K = P_pred / (P_pred + R);
-
-	// 4. 更新状态估计
-	x = predicted_current + K * (measured_current - predicted_current);
-
-	// 5. 更新估计误差协方差
-	P = (1 - K) * P_pred;
-
-	return x;
+	float predicted_current = kalman.getState() + T * (applied_voltage - R_m * kalman.getState()) / L;
+	
+	// 2. 预测误差协方差已经在kalman.predict中处理
+	
+	// 3. 执行预测
+	kalman.predict(predicted_current);
+	
+	// 4. 更新步骤 - 直接使用测量值更新
+	return kalman.update(measured_value);
 }
 /*!
 	\brief      获取当前电流估计值
 	\retval     当前电流估计值
 */
-float CurrentKalman::getCurrent() const { return x; }
+float CurrentKalman::getCurrent() const { return kalman.getState(); }
 /*!
 	\brief      设置过程噪声协方差
 	\param[in]  q 过程噪声协方差值
 	\retval     none
 */
-void CurrentKalman::setProcessNoise(float q) { Q = q; }
+void CurrentKalman::setProcessNoise(float q) { kalman.setProcessNoise(q); }
 /*!
 	\brief      设置测量噪声协方差
 	\param[in]  r 测量噪声协方差值
 	\retval     none
 */
-void CurrentKalman::setMeasurementNoise(float r) { R = r; }
+void CurrentKalman::setMeasurementNoise(float r) { kalman.setMeasurementNoise(r); }
 
+// EncoderVelocityKalman类的实现
+/*!
+	\brief      构造函数，初始化编码器速度卡尔曼滤波器参数
+	\param[in]  initial_velocity 初始速度值 (rad/s)
+	\param[in]  T 控制周期(s)
+	\param[in]  Q 过程噪声协方差(典型值: 1e-4 ~ 1e-2)
+	\param[in]  R 测量噪声协方差(典型值: 速度范围的1%~5%)
+	\retval     none
+*/
+EncoderVelocityKalman::EncoderVelocityKalman(float initial_velocity, float T,
+											 float Q, float R)
+	: kalman(initial_velocity, T, Q, R) {}
 
+/*!
+	\brief      更新卡尔曼滤波器状态
+	\param[in]  measured_velocity 测量的速度值 (rad/s)
+	\retval     估计的速度值 (rad/s)
+	\note       实现卡尔曼滤波的预测和更新步骤，假设速度变化符合随机游走模型
+*/
+float EncoderVelocityKalman::update(float measured_velocity) {
+	// 1. 预测步骤 - 假设速度符合随机游走模型（加速度为0）
+	//    由于速度变化缓慢，我们可以简单预测速度保持不变
+	kalman.predict(kalman.getState());
+
+	// 2. 更新步骤 - 直接使用测量值更新
+	return kalman.update(measured_velocity);
+}
+
+/*!
+	\brief      获取当前速度估计值
+	\retval     当前速度估计值 (rad/s)
+*/
+float EncoderVelocityKalman::getVelocity() const { return kalman.getState(); }
+
+/*!
+	\brief      设置过程噪声协方差
+	\param[in]  q 过程噪声协方差值
+	\retval     none
+*/
+void EncoderVelocityKalman::setProcessNoise(float q) {
+	kalman.setProcessNoise(q);
+}
+
+/*!
+	\brief      设置测量噪声协方差
+	\param[in]  r 测量噪声协方差值
+	\retval     none
+*/
+void EncoderVelocityKalman::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 2d23541..0fc085a 100644
--- a/Robot_V2/Algorithm/Filter.h
+++ b/Robot_V2/Algorithm/Filter.h
@@ -34,6 +34,92 @@ class CircularBuffer {
 
 float medium_filter(float values[], u8 size);
 
+/*!
+	\brief 通用卡尔曼滤波器模板类
+	\note  可用于各种基于线性系统的状态估计
+*/
+template <typename T> class KalmanFilter {
+  private:
+	T x; // 状态估计
+	T P; // 估计误差协方差
+	T Q; // 过程噪声协方差
+	T R; // 测量噪声协方差
+
+  public:
+	/*!
+		\brief 构造函数
+		\param[in] initial_state 初始状态值
+		\param[in] initial_error_covariance 初始误差协方差
+		\param[in] process_noise 过程噪声协方差
+		\param[in] measurement_noise 测量噪声协方差
+	*/
+	KalmanFilter(T initial_state, T initial_error_covariance, T process_noise,
+				 T measurement_noise)
+		: x(initial_state), P(initial_error_covariance), Q(process_noise),
+		  R(measurement_noise) {}
+
+	/*!
+		\brief 预测步骤
+		\param[in] state_prediction 状态预测值
+		\retval none
+	*/
+	void predict(T state_prediction) {
+		x = state_prediction;
+		P += Q;
+	}
+
+	/*!
+		\brief 更新步骤
+		\param[in] measurement 测量值
+		\param[in] measurement_prediction 测量预测值
+		\retval 估计的状态值
+	*/
+	T update(T measurement, T measurement_prediction) {
+		T y = measurement - measurement_prediction; // 测量残差
+		T S = P + R;								// 残差协方差
+		T K = P / S;								// 卡尔曼增益
+
+		x += K * y;		 // 更新状态估计
+		P = (1 - K) * P; // 更新误差协方差
+
+		return x;
+	}
+
+	/*!
+		\brief 直接更新步骤（适用于状态直接可测的情况）
+		\param[in] measurement 测量值
+		\retval 估计的状态值
+	*/
+	T update(T measurement) {
+		T K = P / (P + R); // 卡尔曼增益
+
+		x += K * (measurement - x); // 更新状态估计
+		P = (1 - K) * P;			// 更新误差协方差
+
+		return x;
+	}
+
+	/*!
+		\brief 获取当前状态估计值
+		\retval 当前状态估计值
+	*/
+	T getState() const { return x; }
+
+	/*!
+		\brief 设置过程噪声协方差
+		\param[in] process_noise 过程噪声协方差值
+		\retval none
+	*/
+	void setProcessNoise(T process_noise) { Q = process_noise; }
+
+	/*!
+		\brief 设置测量噪声协方差
+		\param[in] measurement_noise 测量噪声协方差值
+		\retval none
+	*/
+	void setMeasurementNoise(T measurement_noise) { R = measurement_noise; }
+};
+
 /*!
 	\brief 电流卡尔曼滤波器类
 	\note  基于电机模型的卡尔曼滤波器实现，用于电流状态估计
@@ -45,10 +131,7 @@ float medium_filter(float values[], u8 size);
 */
 class CurrentKalman {
   private:
-	float x;   // 电流状态估计
-	float P;   // 估计误差协方差
-	float Q;   // 过程噪声协方差
-	float R;   // 测量噪声协方差
+	KalmanFilter<float> kalman;
 	float L;   // 电机电感(H)
 	float R_m; // 电机电阻(Ω)
 	float T;   // 控制周期(s)
@@ -66,6 +149,28 @@ class CurrentKalman {
 	void setMeasurementNoise(float r);
 };
 
+/*!
+	\brief 编码器速度卡尔曼滤波器类
+	\note  用于滤除编码器速度测量噪声的卡尔曼滤波器实现
+*/
+class EncoderVelocityKalman {
+  private:
+	KalmanFilter<float> kalman;
+
+  public:
+	EncoderVelocityKalman(float initial_velocity,
+						  float initial_error_covariance, float process_noise,
+						  float measurement_noise);
+
+	float update(float measured_velocity);
+
+	float getVelocity() const;
+
+	void setProcessNoise(float q);
+
+	void setMeasurementNoise(float r);
+};
+
 #endif // __cplusplus
 
-#endif // _FILTER_H
+#endif // _FILTER_H
\ No newline at end of file
diff --git a/Robot_V2/Algorithm/pid.cpp b/Robot_V2/Algorithm/pid.cpp
index 3b836e3..02e4ca5 100644
--- a/Robot_V2/Algorithm/pid.cpp
+++ b/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;
 }
\ No newline at end of file
diff --git a/Robot_V2/device/Can.cpp b/Robot_V2/device/Can.cpp
index 61b1e8f..8a51e0d 100644
--- a/Robot_V2/device/Can.cpp
+++ b/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;
 		}
diff --git a/Robot_V2/device/Can.h b/Robot_V2/device/Can.h
index 1a12631..727193d 100644
--- a/Robot_V2/device/Can.h
+++ b/Robot_V2/device/Can.h
@@ -106,6 +106,7 @@ class CAN_transmiter {
 	can_receive_message_struct receive_message; // 接收消息结构体
 	FlagStatus can0_receive_flag;				// CAN0接收标志
 	FlagStatus can0_send_flag;					// CAN0发送标志
+	float_union_t testfloat_union = {0};			// 测试用
 
 	void communication_check();					// 通信检查
 };
diff --git a/Robot_V2/device/Timer.cpp b/Robot_V2/device/Timer.cpp
index e18b371..a98e4a9 100644
--- a/Robot_V2/device/Timer.cpp
+++ b/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) {
diff --git a/Robot_V2/device/ang_sensor.cpp b/Robot_V2/device/ang_sensor.cpp
index 2ed33d9..df63e62 100644
--- a/Robot_V2/device/ang_sensor.cpp
+++ b/Robot_V2/device/ang_sensor.cpp
@@ -10,9 +10,14 @@
 #define SPI_CS_DISABLE() gpio_bit_set(GPIOA, GPIO_PIN_15)
 
 constexpr float ANGLE_FACTOR = 1.0f / 65536.0f * 360.0f;
+constexpr float RAD_FACTOR = 1.0f / 65536.0f * 2.0f * 3.14159265358979323846f;
 constexpr u16 DEFAULT_COMMAND = 0x0000;
 constexpr u16 FAULT_RECEIVE = 0xFFFF;
 constexpr u16 REDDUCTION_RATIO = 25;
+constexpr float VELOCITY_SCALE = 1e4f;
+constexpr float ANGLE_WRAP_THRESHOLD = 270.0f;
+constexpr float ANGLE_WRAP_VALUE = 360.0f;
+constexpr int SIZE_FLOAT = sizeof(float);
 
 // DMA传输完成标志
 volatile bool spi_dma_tx_complete = false;
@@ -203,7 +208,7 @@ float angle_sensor::read_sensorAngle(void) {
 
 	SPI_CS_ENABLE();
 	// 使用DMA发送命令并接收数据
-	if (spi_rw_half_word_dma(SPI0, DEFAULT_COMMAND, &u16data) == 0) {
+	if (!spi_rw_half_word_dma(SPI0, DEFAULT_COMMAND, &u16data)) {
 		SPI_CS_DISABLE();
 		return u16data * ANGLE_FACTOR;
 	} else {
@@ -218,6 +223,7 @@ float angle_sensor::read_sensorAngle(void) {
  * @return 累积角度值
  */
 float angle_sensor::update(void) {
+
 	float currentAngle = read_sensorAngle();
 
 	if (m_firstUpdate) {
@@ -231,17 +237,17 @@ float angle_sensor::update(void) {
 
 	// 处理角度绕回情况（0度和360度之间的跳变）
 	// 如果变化超过180度，说明发生了绕回
-	if (delta > 270.0f) {
+	if (delta > ANGLE_WRAP_THRESHOLD) {
 		// 从接近360度绕回到接近0度
-		delta -= 360.0f;
-	} else if (delta < -270.0f) {
+		delta -= ANGLE_WRAP_VALUE;
+	} else if (delta < -ANGLE_WRAP_THRESHOLD) {
 		// 从接近0度绕到接近360度
-		delta += 360.0f;
+		delta += ANGLE_WRAP_VALUE;
 	}
-	
-	std::memmove(velocity, velocity + 1, VEL_FILTER_SIZE * sizeof(float));
-	velocity[VEL_FILTER_SIZE - 1] = delta * 1e4;
-	
+
+	std::memmove(velocity, velocity + 1, SIZE_FLOAT * (VEL_FILTER_SIZE - 1));
+	velocity[VEL_FILTER_SIZE - 1] = delta * VELOCITY_SCALE;
+
 	m_cumulativeAngle += delta;
 	m_lastAngle = currentAngle;
 
@@ -279,7 +285,8 @@ float angle_sensor::getRawVelocity(void) const {
 	for (int i = 0; i < VEL_FILTER_SIZE - 1; i++) {
 		velocity_filted += velocity[i];
 	}
-	return velocity_filted / VEL_FILTER_SIZE;
+	return velocity_filted / (VEL_FILTER_SIZE - 1);
+	// return velocity[VEL_FILTER_SIZE - 1];
 }
 
 /**
diff --git a/Robot_V2/device/ang_sensor.h b/Robot_V2/device/ang_sensor.h
index 23cc3a0..f255ce3 100644
--- a/Robot_V2/device/ang_sensor.h
+++ b/Robot_V2/device/ang_sensor.h
@@ -11,7 +11,7 @@ u16 spi_rw_half_word(u32 spi_periph, const u16 i_HalfWord);
 int spi_rw_half_word_dma(uint32_t spi_periph, uint16_t tx_data,
 						 uint16_t *rx_data);
 
-constexpr u16 VEL_FILTER_SIZE = 24;
+constexpr u16 VEL_FILTER_SIZE = 3;
 
 class angle_sensor {
   public:
diff --git a/Robot_V2/main.cpp b/Robot_V2/main.cpp
index af788eb..83a9d28 100644
--- a/Robot_V2/main.cpp
+++ b/Robot_V2/main.cpp
@@ -7,7 +7,6 @@
 #include "core_cm4.h"
 #include "initial.h"
 #include "systick.h"
-#include <cstdio>
 
 // 全局变量定义
 CAN_transmiter CAN0_transmiter; // CAN初始化
diff --git a/Robot_V2/pid.h b/Robot_V2/pid.h
index f0335cb..695b51d 100644
--- a/Robot_V2/pid.h
+++ b/Robot_V2/pid.h
@@ -8,6 +8,7 @@ public:
     ~PIDController() = default;
 
     float operator() (float error);
+	void set_PID(float P, float I, float D);
 
     float P; //!< 比例增益(P环增益)
     float I; //!< 积分增益（I环增益）