diff --git a/Robot_V2/Algorithm/FOC.cpp b/Robot_V2/Algorithm/FOC.cpp
index 82184f1..a093f79 100644
--- a/Robot_V2/Algorithm/FOC.cpp
+++ b/Robot_V2/Algorithm/FOC.cpp
@@ -6,11 +6,11 @@
 #include "config.h"
 #include "motor.h"
 #include "pid.h"
+#include <arm_math.h>
 #include <cmath>
 #include <gd32c10x.h>
 #include <stdint.h>
 
-
 FOC_Controlroller FOC; // FOC控制初始化
 extern sampler sampler;
 extern angle_sensor KTH7823;
@@ -22,16 +22,16 @@ extern CAN_transmiter CAN0_transmiter;
 #define constrain(value, min, max, target_min, target_max)                     \
 	((value - min) / (max - min) * (target_max - target_min))
 
-/**
+/**@
  * @brief 归一化角度
  *
  * @param angle
  * @return float
  */
 inline float _normalizeAngle(float angle) {
-	float a = fmod(angle, 2 * PI);
+	float a = fmod(angle, 360.0f);
 	// 取余运算可以用于归一化，列出特殊值例子算便知
-	return a >= 0 ? a : (a + 2 * PI);
+	return a >= 0 ? a : (a + 360.0f);
 }
 
 // 通过Ia,Ib,Ic计算Iq,Id(目前仅输出Iq)
@@ -40,16 +40,19 @@ inline float FOC_Controlroller::cal_Iq_Id(float current_a, float current_b,
 	// 计算电气角度
 	float angle_el = _normalizeAngle(shaft_angle * PP);
 
+	float I_alpha, I_beta;
 	// Clarke变换 (使用三相电流)
-	float I_alpha = (2.0f / 3.0f) * current_a - (1.0f / 3.0f) * current_b -
-					(1.0f / 3.0f) * current_c;
-	float I_beta = _1_SQRT3 * current_b - _1_SQRT3 * current_c;
+	// arm_clarke_f32(current_a, current_b, &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;
 
 	// Park变换
 	float sin_el, cos_el;
-	cos_el = cos_lut[(u16)(angle_el * LUT_SCALE) & LUT_MASK];
-	sin_el = sin_lut[(u16)(angle_el * LUT_SCALE) & LUT_MASK];
-	float I_q = I_beta * cos_el - I_alpha * sin_el;
+	arm_sin_cos_f32(angle_el, &sin_el, &cos_el);
+	float I_q, I_d;
+	I_q = I_beta * cos_el - I_alpha * sin_el;
+	// arm_park_f32(I_alpha, I_beta, &I_d, &I_q, sin_el, cos_el);
 
 	return I_q * 1000.0f; // 返回Iq值，乘以1000以转换为mA
 }
@@ -73,8 +76,7 @@ inline void FOC_Controlroller::setPhaseVoltage(float Uq, float Ud) {
 
 	// 帕克逆变换 (直接计算，避免函数调用开销)
 	float sin_el, cos_el;
-	sin_el = sin_lut[(u16)(angle_el * LUT_SCALE) & LUT_MASK];
-	cos_el = cos_lut[(u16)(angle_el * LUT_SCALE) & LUT_MASK];
+	arm_sin_cos_f32(angle_el, &sin_el, &cos_el);
 	float Ualpha = Ud * cos_el - Uq * sin_el;
 	float Ubeta = Ud * sin_el + Uq * cos_el;
 
@@ -93,7 +95,6 @@ inline void FOC_Controlroller::setPhaseVoltage(float Uq, float Ud) {
 	duty_b = duty_b * voltage_exchange;
 	duty_c = duty_c * voltage_exchange;
 	Set_3Pwm(duty_a, duty_b, duty_c);
-	// std::printf("PWM:%f,%f,%f\n", dc_a, dc_b, dc_c);
 }
 
 /**
@@ -156,8 +157,8 @@ void FOC_Controlroller::update_power(void) {
 float FOC_Controlroller::velocity_open_loop() {
 	// 通过时间间隔和目标速度计算需要转动的机械角度，存储在shaft_angle中，在此之前，还需要对轴角度归一化
 	// 预计算常量 1e-4 = 100微秒控制周期
-	shaft_angle =
-		_normalizeAngle(shaft_angle + target_velocity * ANGLE_FACTOR * 1e-4f);
+	shaft_angle = _normalizeAngle(shaft_angle + target_velocity * ANGLE_FACTOR *
+													1.0 / TIMER2_FREQUENCY);
 
 	// 因为在开环控制中，Ud始终为0，只控制Uq
 	const float Uq = voltage_power_supply / 2.0f;
@@ -195,7 +196,7 @@ float FOC_Controlroller::angle_control() {
 	float Uq = angle_loop(target_angle - rt_angle);
 
 	// 获取电机轴角度
-	shaft_angle = KTH7823.getcurrent_sensorangle() * DEG2RAD;
+	shaft_angle = KTH7823.getcurrent_sensorangle();
 	// 设置相电压
 	setPhaseVoltage(Uq, 0.0f);
 	return shaft_angle;
@@ -224,7 +225,7 @@ float FOC_Controlroller::angle_control_with_velocity_limit() {
 	// CAN0_transmiter.testfloat_union.f = target_velocity - rt_velocity;
 
 	// 获取电机轴角度
-	shaft_angle = KTH7823.getcurrent_sensorangle() * DEG2RAD;
+	shaft_angle = KTH7823.getcurrent_sensorangle();
 
 	// 设置相电压
 	setPhaseVoltage(Uq, 0.0f);
@@ -243,12 +244,13 @@ float FOC_Controlroller::velocity_control() {
 	float Uq = vel_loop((target_velocity - rt_velocity) * 25);
 
 	// 获取电机轴角度
-	shaft_angle = KTH7823.getcurrent_sensorangle() * DEG2RAD;
+	shaft_angle = KTH7823.getcurrent_sensorangle();
 	// 设置相电压
 	setPhaseVoltage(Uq, 0.0f);
-	// CAN0_transmiter.testfloat_union.f =
-	cal_Iq_Id(sampler.get_current('U'), sampler.get_current('V'),
-			  sampler.get_current('W'));
+	CAN0_transmiter.testfloat_union.f =
+		cal_Iq_Id(sampler.get_current('U'), sampler.get_current('V'),
+				  sampler.get_current('W'));
+
 	return rt_velocity;
 }
 
@@ -259,7 +261,7 @@ float FOC_Controlroller::current_control() {
 	float Uq = current_loop(target_current - Iq);
 	CAN0_transmiter.testfloat_union.f = Iq;
 	// 获取电机轴角度
-	shaft_angle = KTH7823.getcurrent_sensorangle() * DEG2RAD;
+	shaft_angle = KTH7823.getcurrent_sensorangle();
 	// 设置相电压
 	setPhaseVoltage(Uq, 0.0f);
 	return 1;
diff --git a/Robot_V2/Algorithm/FOC.h b/Robot_V2/Algorithm/FOC.h
index a4a8d38..e882a55 100644
--- a/Robot_V2/Algorithm/FOC.h
+++ b/Robot_V2/Algorithm/FOC.h
@@ -45,15 +45,15 @@ class FOC_Controlroller {
 	float v_half = voltage_power_supply * 0.5f; // 电源电压的一半
 
 	PIDController vel_loop =
-		PIDController{0.028f, 0.2f, 1e-4, 1000000, voltage_power_supply}; // 12V
+		PIDController{0.01f, 0.20f, 5e-5, 1000000, voltage_power_supply}; // 12V
 	PIDController angle_loop = PIDController{20.0f, 100.0f, 0.2f, 10000000,
 											 voltage_power_supply}; // 12V
 	PIDController current_loop =
 		PIDController{0.006f, 0.02f, 0, 100000, voltage_power_supply}; // 12V
 	PIDController vel_inVelAngloop =
-		PIDController{0.35f, 5.0f, 1e-3, 1000000, voltage_power_supply};
+		PIDController{0.12f, 3.0f, 5e-4, 1000000, voltage_power_supply};
 	PIDController ang_inVelAngloop =
-		PIDController{30.0f, 0.0f, 1e-3, 1000000, 999};
+		PIDController{100.0f, 0.0f, 5e-4, 1000000, 999};
 
 	void setPhaseVoltage(float Uq, float Ud);
 	float cal_Iq_Id(float current_a, float current_b, float current_c);
@@ -62,351 +62,8 @@ class FOC_Controlroller {
 // 定义函数指针类型
 typedef float (FOC_Controlroller::*FOCFunctionPtr)(void);
 
-float _normalizeAngle(float angle);
+inline float _normalizeAngle(float angle);
 inline float _electricalAngle();
 
-__attribute__((aligned(4))) const float sin_lut[LUT_SIZE] = {
-	0.000000f,	0.006136f,	0.012272f,	0.018407f,	0.024541f,	0.030675f,
-	0.036807f,	0.042938f,	0.049068f,	0.055195f,	0.061321f,	0.067444f,
-	0.073565f,	0.079682f,	0.085797f,	0.091909f,	0.098017f,	0.104122f,
-	0.110222f,	0.116319f,	0.122411f,	0.128498f,	0.134581f,	0.140658f,
-	0.146730f,	0.152797f,	0.158858f,	0.164913f,	0.170962f,	0.177004f,
-	0.183040f,	0.189069f,	0.195090f,	0.201105f,	0.207111f,	0.213110f,
-	0.219101f,	0.225084f,	0.231058f,	0.237024f,	0.242980f,	0.248928f,
-	0.254866f,	0.260794f,	0.266713f,	0.272621f,	0.278520f,	0.284408f,
-	0.290285f,	0.296151f,	0.302006f,	0.307850f,	0.313682f,	0.319502f,
-	0.325310f,	0.331106f,	0.336890f,	0.342661f,	0.348419f,	0.354164f,
-	0.359895f,	0.365613f,	0.371317f,	0.377007f,	0.382683f,	0.388345f,
-	0.393992f,	0.399624f,	0.405241f,	0.410843f,	0.416430f,	0.422000f,
-	0.427555f,	0.433094f,	0.438616f,	0.444122f,	0.449611f,	0.455084f,
-	0.460539f,	0.465976f,	0.471397f,	0.476799f,	0.482184f,	0.487550f,
-	0.492898f,	0.498228f,	0.503538f,	0.508830f,	0.514103f,	0.519356f,
-	0.524590f,	0.529804f,	0.534998f,	0.540171f,	0.545325f,	0.550458f,
-	0.555570f,	0.560662f,	0.565732f,	0.570781f,	0.575808f,	0.580814f,
-	0.585798f,	0.590760f,	0.595699f,	0.600616f,	0.605511f,	0.610383f,
-	0.615232f,	0.620057f,	0.624859f,	0.629638f,	0.634393f,	0.639124f,
-	0.643832f,	0.648514f,	0.653173f,	0.657807f,	0.662416f,	0.667000f,
-	0.671559f,	0.676093f,	0.680601f,	0.685084f,	0.689541f,	0.693971f,
-	0.698376f,	0.702755f,	0.707107f,	0.711432f,	0.715731f,	0.720003f,
-	0.724247f,	0.728464f,	0.732654f,	0.736817f,	0.740951f,	0.745058f,
-	0.749136f,	0.753187f,	0.757209f,	0.761202f,	0.765167f,	0.769103f,
-	0.773010f,	0.776888f,	0.780737f,	0.784557f,	0.788346f,	0.792107f,
-	0.795837f,	0.799537f,	0.803208f,	0.806848f,	0.810457f,	0.814036f,
-	0.817585f,	0.821103f,	0.824589f,	0.828045f,	0.831470f,	0.834863f,
-	0.838225f,	0.841555f,	0.844854f,	0.848120f,	0.851355f,	0.854558f,
-	0.857729f,	0.860867f,	0.863973f,	0.867046f,	0.870087f,	0.873095f,
-	0.876070f,	0.879012f,	0.881921f,	0.884797f,	0.887640f,	0.890449f,
-	0.893224f,	0.895966f,	0.898674f,	0.901349f,	0.903989f,	0.906596f,
-	0.909168f,	0.911706f,	0.914210f,	0.916679f,	0.919114f,	0.921514f,
-	0.923880f,	0.926210f,	0.928506f,	0.930767f,	0.932993f,	0.935184f,
-	0.937339f,	0.939459f,	0.941544f,	0.943593f,	0.945607f,	0.947586f,
-	0.949528f,	0.951435f,	0.953306f,	0.955141f,	0.956940f,	0.958703f,
-	0.960431f,	0.962121f,	0.963776f,	0.965394f,	0.966976f,	0.968522f,
-	0.970031f,	0.971504f,	0.972940f,	0.974339f,	0.975702f,	0.977028f,
-	0.978317f,	0.979570f,	0.980785f,	0.981964f,	0.983105f,	0.984210f,
-	0.985278f,	0.986308f,	0.987301f,	0.988258f,	0.989177f,	0.990058f,
-	0.990903f,	0.991710f,	0.992480f,	0.993212f,	0.993907f,	0.994565f,
-	0.995185f,	0.995767f,	0.996313f,	0.996820f,	0.997290f,	0.997723f,
-	0.998118f,	0.998476f,	0.998795f,	0.999078f,	0.999322f,	0.999529f,
-	0.999699f,	0.999831f,	0.999925f,	0.999981f,	1.000000f,	0.999981f,
-	0.999925f,	0.999831f,	0.999699f,	0.999529f,	0.999322f,	0.999078f,
-	0.998795f,	0.998476f,	0.998118f,	0.997723f,	0.997290f,	0.996820f,
-	0.996313f,	0.995767f,	0.995185f,	0.994565f,	0.993907f,	0.993212f,
-	0.992480f,	0.991710f,	0.990903f,	0.990058f,	0.989177f,	0.988258f,
-	0.987301f,	0.986308f,	0.985278f,	0.984210f,	0.983105f,	0.981964f,
-	0.980785f,	0.979570f,	0.978317f,	0.977028f,	0.975702f,	0.974339f,
-	0.972940f,	0.971504f,	0.970031f,	0.968522f,	0.966976f,	0.965394f,
-	0.963776f,	0.962121f,	0.960431f,	0.958703f,	0.956940f,	0.955141f,
-	0.953306f,	0.951435f,	0.949528f,	0.947586f,	0.945607f,	0.943593f,
-	0.941544f,	0.939459f,	0.937339f,	0.935184f,	0.932993f,	0.930767f,
-	0.928506f,	0.926210f,	0.923880f,	0.921514f,	0.919114f,	0.916679f,
-	0.914210f,	0.911706f,	0.909168f,	0.906596f,	0.903989f,	0.901349f,
-	0.898674f,	0.895966f,	0.893224f,	0.890449f,	0.887640f,	0.884797f,
-	0.881921f,	0.879012f,	0.876070f,	0.873095f,	0.870087f,	0.867046f,
-	0.863973f,	0.860867f,	0.857729f,	0.854558f,	0.851355f,	0.848120f,
-	0.844854f,	0.841555f,	0.838225f,	0.834863f,	0.831470f,	0.828045f,
-	0.824589f,	0.821103f,	0.817585f,	0.814036f,	0.810457f,	0.806848f,
-	0.803208f,	0.799537f,	0.795837f,	0.792107f,	0.788346f,	0.784557f,
-	0.780737f,	0.776888f,	0.773010f,	0.769103f,	0.765167f,	0.761202f,
-	0.757209f,	0.753187f,	0.749136f,	0.745058f,	0.740951f,	0.736817f,
-	0.732654f,	0.728464f,	0.724247f,	0.720003f,	0.715731f,	0.711432f,
-	0.707107f,	0.702755f,	0.698376f,	0.693971f,	0.689541f,	0.685084f,
-	0.680601f,	0.676093f,	0.671559f,	0.667000f,	0.662416f,	0.657807f,
-	0.653173f,	0.648514f,	0.643832f,	0.639124f,	0.634393f,	0.629638f,
-	0.624859f,	0.620057f,	0.615232f,	0.610383f,	0.605511f,	0.600616f,
-	0.595699f,	0.590760f,	0.585798f,	0.580814f,	0.575808f,	0.570781f,
-	0.565732f,	0.560662f,	0.555570f,	0.550458f,	0.545325f,	0.540171f,
-	0.534998f,	0.529804f,	0.524590f,	0.519356f,	0.514103f,	0.508830f,
-	0.503538f,	0.498228f,	0.492898f,	0.487550f,	0.482184f,	0.476799f,
-	0.471397f,	0.465976f,	0.460539f,	0.455084f,	0.449611f,	0.444122f,
-	0.438616f,	0.433094f,	0.427555f,	0.422000f,	0.416430f,	0.410843f,
-	0.405241f,	0.399624f,	0.393992f,	0.388345f,	0.382683f,	0.377007f,
-	0.371317f,	0.365613f,	0.359895f,	0.354164f,	0.348419f,	0.342661f,
-	0.336890f,	0.331106f,	0.325310f,	0.319502f,	0.313682f,	0.307850f,
-	0.302006f,	0.296151f,	0.290285f,	0.284408f,	0.278520f,	0.272621f,
-	0.266713f,	0.260794f,	0.254866f,	0.248928f,	0.242980f,	0.237024f,
-	0.231058f,	0.225084f,	0.219101f,	0.213110f,	0.207111f,	0.201105f,
-	0.195090f,	0.189069f,	0.183040f,	0.177004f,	0.170962f,	0.164913f,
-	0.158858f,	0.152797f,	0.146730f,	0.140658f,	0.134581f,	0.128498f,
-	0.122411f,	0.116319f,	0.110222f,	0.104122f,	0.098017f,	0.091909f,
-	0.085797f,	0.079682f,	0.073565f,	0.067444f,	0.061321f,	0.055195f,
-	0.049068f,	0.042938f,	0.036807f,	0.030675f,	0.024541f,	0.018407f,
-	0.012272f,	0.006136f,	0.000000f,	-0.006136f, -0.012272f, -0.018407f,
-	-0.024541f, -0.030675f, -0.036807f, -0.042938f, -0.049068f, -0.055195f,
-	-0.061321f, -0.067444f, -0.073565f, -0.079682f, -0.085797f, -0.091909f,
-	-0.098017f, -0.104122f, -0.110222f, -0.116319f, -0.122411f, -0.128498f,
-	-0.134581f, -0.140658f, -0.146730f, -0.152797f, -0.158858f, -0.164913f,
-	-0.170962f, -0.177004f, -0.183040f, -0.189069f, -0.195090f, -0.201105f,
-	-0.207111f, -0.213110f, -0.219101f, -0.225084f, -0.231058f, -0.237024f,
-	-0.242980f, -0.248928f, -0.254866f, -0.260794f, -0.266713f, -0.272621f,
-	-0.278520f, -0.284408f, -0.290285f, -0.296151f, -0.302006f, -0.307850f,
-	-0.313682f, -0.319502f, -0.325310f, -0.331106f, -0.336890f, -0.342661f,
-	-0.348419f, -0.354164f, -0.359895f, -0.365613f, -0.371317f, -0.377007f,
-	-0.382683f, -0.388345f, -0.393992f, -0.399624f, -0.405241f, -0.410843f,
-	-0.416430f, -0.422000f, -0.427555f, -0.433094f, -0.438616f, -0.444122f,
-	-0.449611f, -0.455084f, -0.460539f, -0.465976f, -0.471397f, -0.476799f,
-	-0.482184f, -0.487550f, -0.492898f, -0.498228f, -0.503538f, -0.508830f,
-	-0.514103f, -0.519356f, -0.524590f, -0.529804f, -0.534998f, -0.540171f,
-	-0.545325f, -0.550458f, -0.555570f, -0.560662f, -0.565732f, -0.570781f,
-	-0.575808f, -0.580814f, -0.585798f, -0.590760f, -0.595699f, -0.600616f,
-	-0.605511f, -0.610383f, -0.615232f, -0.620057f, -0.624859f, -0.629638f,
-	-0.634393f, -0.639124f, -0.643832f, -0.648514f, -0.653173f, -0.657807f,
-	-0.662416f, -0.667000f, -0.671559f, -0.676093f, -0.680601f, -0.685084f,
-	-0.689541f, -0.693971f, -0.698376f, -0.702755f, -0.707107f, -0.711432f,
-	-0.715731f, -0.720003f, -0.724247f, -0.728464f, -0.732654f, -0.736817f,
-	-0.740951f, -0.745058f, -0.749136f, -0.753187f, -0.757209f, -0.761202f,
-	-0.765167f, -0.769103f, -0.773010f, -0.776888f, -0.780737f, -0.784557f,
-	-0.788346f, -0.792107f, -0.795837f, -0.799537f, -0.803208f, -0.806848f,
-	-0.810457f, -0.814036f, -0.817585f, -0.821103f, -0.824589f, -0.828045f,
-	-0.831470f, -0.834863f, -0.838225f, -0.841555f, -0.844854f, -0.848120f,
-	-0.851355f, -0.854558f, -0.857729f, -0.860867f, -0.863973f, -0.867046f,
-	-0.870087f, -0.873095f, -0.876070f, -0.879012f, -0.881921f, -0.884797f,
-	-0.887640f, -0.890449f, -0.893224f, -0.895966f, -0.898674f, -0.901349f,
-	-0.903989f, -0.906596f, -0.909168f, -0.911706f, -0.914210f, -0.916679f,
-	-0.919114f, -0.921514f, -0.923880f, -0.926210f, -0.928506f, -0.930767f,
-	-0.932993f, -0.935184f, -0.937339f, -0.939459f, -0.941544f, -0.943593f,
-	-0.945607f, -0.947586f, -0.949528f, -0.951435f, -0.953306f, -0.955141f,
-	-0.956940f, -0.958703f, -0.960431f, -0.962121f, -0.963776f, -0.965394f,
-	-0.966976f, -0.968522f, -0.970031f, -0.971504f, -0.972940f, -0.974339f,
-	-0.975702f, -0.977028f, -0.978317f, -0.979570f, -0.980785f, -0.981964f,
-	-0.983105f, -0.984210f, -0.985278f, -0.986308f, -0.987301f, -0.988258f,
-	-0.989177f, -0.990058f, -0.990903f, -0.991710f, -0.992480f, -0.993212f,
-	-0.993907f, -0.994565f, -0.995185f, -0.995767f, -0.996313f, -0.996820f,
-	-0.997290f, -0.997723f, -0.998118f, -0.998476f, -0.998795f, -0.999078f,
-	-0.999322f, -0.999529f, -0.999699f, -0.999831f, -0.999925f, -0.999981f,
-	-1.000000f, -0.999981f, -0.999925f, -0.999831f, -0.999699f, -0.999529f,
-	-0.999322f, -0.999078f, -0.998795f, -0.998476f, -0.998118f, -0.997723f,
-	-0.997290f, -0.996820f, -0.996313f, -0.995767f, -0.995185f, -0.994565f,
-	-0.993907f, -0.993212f, -0.992480f, -0.991710f, -0.990903f, -0.990058f,
-	-0.989177f, -0.988258f, -0.987301f, -0.986308f, -0.985278f, -0.984210f,
-	-0.983105f, -0.981964f, -0.980785f, -0.979570f, -0.978317f, -0.977028f,
-	-0.975702f, -0.974339f, -0.972940f, -0.971504f, -0.970031f, -0.968522f,
-	-0.966976f, -0.965394f, -0.963776f, -0.962121f, -0.960431f, -0.958703f,
-	-0.956940f, -0.955141f, -0.953306f, -0.951435f, -0.949528f, -0.947586f,
-	-0.945607f, -0.943593f, -0.941544f, -0.939459f, -0.937339f, -0.935184f,
-	-0.932993f, -0.930767f, -0.928506f, -0.926210f, -0.923880f, -0.921514f,
-	-0.919114f, -0.916679f, -0.914210f, -0.911706f, -0.909168f, -0.906596f,
-	-0.903989f, -0.901349f, -0.898674f, -0.895966f, -0.893224f, -0.890449f,
-	-0.887640f, -0.884797f, -0.881921f, -0.879012f, -0.876070f, -0.873095f,
-	-0.870087f, -0.867046f, -0.863973f, -0.860867f, -0.857729f, -0.854558f,
-	-0.851355f, -0.848120f, -0.844854f, -0.841555f, -0.838225f, -0.834863f,
-	-0.831470f, -0.828045f, -0.824589f, -0.821103f, -0.817585f, -0.814036f,
-	-0.810457f, -0.806848f, -0.803208f, -0.799537f, -0.795837f, -0.792107f,
-	-0.788346f, -0.784557f, -0.780737f, -0.776888f, -0.773010f, -0.769103f,
-	-0.765167f, -0.761202f, -0.757209f, -0.753187f, -0.749136f, -0.745058f,
-	-0.740951f, -0.736817f, -0.732654f, -0.728464f, -0.724247f, -0.720003f,
-	-0.715731f, -0.711432f, -0.707107f, -0.702755f, -0.698376f, -0.693971f,
-	-0.689541f, -0.685084f, -0.680601f, -0.676093f, -0.671559f, -0.667000f,
-	-0.662416f, -0.657807f, -0.653173f, -0.648514f, -0.643832f, -0.639124f,
-	-0.634393f, -0.629638f, -0.624859f, -0.620057f, -0.615232f, -0.610383f,
-	-0.605511f, -0.600616f, -0.595699f, -0.590760f, -0.585798f, -0.580814f,
-	-0.575808f, -0.570781f, -0.565732f, -0.560662f, -0.555570f, -0.550458f,
-	-0.545325f, -0.540171f, -0.534998f, -0.529804f, -0.524590f, -0.519356f,
-	-0.514103f, -0.508830f, -0.503538f, -0.498228f, -0.492898f, -0.487550f,
-	-0.482184f, -0.476799f, -0.471397f, -0.465976f, -0.460539f, -0.455084f,
-	-0.449611f, -0.444122f, -0.438616f, -0.433094f, -0.427555f, -0.422000f,
-	-0.416430f, -0.410843f, -0.405241f, -0.399624f, -0.393992f, -0.388345f,
-	-0.382683f, -0.377007f, -0.371317f, -0.365613f, -0.359895f, -0.354164f,
-	-0.348419f, -0.342661f, -0.336890f, -0.331106f, -0.325310f, -0.319502f,
-	-0.313682f, -0.307850f, -0.302006f, -0.296151f, -0.290285f, -0.284408f,
-	-0.278520f, -0.272621f, -0.266713f, -0.260794f, -0.254866f, -0.248928f,
-	-0.242980f, -0.237024f, -0.231058f, -0.225084f, -0.219101f, -0.213110f,
-	-0.207111f, -0.201105f, -0.195090f, -0.189069f, -0.183040f, -0.177004f,
-	-0.170962f, -0.164913f, -0.158858f, -0.152797f, -0.146730f, -0.140658f,
-	-0.134581f, -0.128498f, -0.122411f, -0.116319f, -0.110222f, -0.104122f,
-	-0.098017f, -0.091909f, -0.085797f, -0.079682f, -0.073565f, -0.067444f,
-	-0.061321f, -0.055195f, -0.049068f, -0.042938f, -0.036807f, -0.030675f,
-	-0.024541f, -0.018407f, -0.012272f, -0.006136f};
-__attribute__((aligned(4))) const float cos_lut[LUT_SIZE] = {
-	1.000000f,	0.999981f,	0.999925f,	0.999831f,	0.999699f,	0.999529f,
-	0.999322f,	0.999078f,	0.998795f,	0.998476f,	0.998118f,	0.997723f,
-	0.997290f,	0.996820f,	0.996313f,	0.995767f,	0.995185f,	0.994565f,
-	0.993907f,	0.993212f,	0.992480f,	0.991710f,	0.990903f,	0.990058f,
-	0.989177f,	0.988258f,	0.987301f,	0.986308f,	0.985278f,	0.984210f,
-	0.983105f,	0.981964f,	0.980785f,	0.979570f,	0.978317f,	0.977028f,
-	0.975702f,	0.974339f,	0.972940f,	0.971504f,	0.970031f,	0.968522f,
-	0.966976f,	0.965394f,	0.963776f,	0.962121f,	0.960431f,	0.958703f,
-	0.956940f,	0.955141f,	0.953306f,	0.951435f,	0.949528f,	0.947586f,
-	0.945607f,	0.943593f,	0.941544f,	0.939459f,	0.937339f,	0.935184f,
-	0.932993f,	0.930767f,	0.928506f,	0.926210f,	0.923880f,	0.921514f,
-	0.919114f,	0.916679f,	0.914210f,	0.911706f,	0.909168f,	0.906596f,
-	0.903989f,	0.901349f,	0.898674f,	0.895966f,	0.893224f,	0.890449f,
-	0.887640f,	0.884797f,	0.881921f,	0.879012f,	0.876070f,	0.873095f,
-	0.870087f,	0.867046f,	0.863973f,	0.860867f,	0.857729f,	0.854558f,
-	0.851355f,	0.848120f,	0.844854f,	0.841555f,	0.838225f,	0.834863f,
-	0.831470f,	0.828045f,	0.824589f,	0.821103f,	0.817585f,	0.814036f,
-	0.810457f,	0.806848f,	0.803208f,	0.799537f,	0.795837f,	0.792107f,
-	0.788346f,	0.784557f,	0.780737f,	0.776888f,	0.773010f,	0.769103f,
-	0.765167f,	0.761202f,	0.757209f,	0.753187f,	0.749136f,	0.745058f,
-	0.740951f,	0.736817f,	0.732654f,	0.728464f,	0.724247f,	0.720003f,
-	0.715731f,	0.711432f,	0.707107f,	0.702755f,	0.698376f,	0.693971f,
-	0.689541f,	0.685084f,	0.680601f,	0.676093f,	0.671559f,	0.667000f,
-	0.662416f,	0.657807f,	0.653173f,	0.648514f,	0.643832f,	0.639124f,
-	0.634393f,	0.629638f,	0.624859f,	0.620057f,	0.615232f,	0.610383f,
-	0.605511f,	0.600616f,	0.595699f,	0.590760f,	0.585798f,	0.580814f,
-	0.575808f,	0.570781f,	0.565732f,	0.560662f,	0.555570f,	0.550458f,
-	0.545325f,	0.540171f,	0.534998f,	0.529804f,	0.524590f,	0.519356f,
-	0.514103f,	0.508830f,	0.503538f,	0.498228f,	0.492898f,	0.487550f,
-	0.482184f,	0.476799f,	0.471397f,	0.465976f,	0.460539f,	0.455084f,
-	0.449611f,	0.444122f,	0.438616f,	0.433094f,	0.427555f,	0.422000f,
-	0.416430f,	0.410843f,	0.405241f,	0.399624f,	0.393992f,	0.388345f,
-	0.382683f,	0.377007f,	0.371317f,	0.365613f,	0.359895f,	0.354164f,
-	0.348419f,	0.342661f,	0.336890f,	0.331106f,	0.325310f,	0.319502f,
-	0.313682f,	0.307850f,	0.302006f,	0.296151f,	0.290285f,	0.284408f,
-	0.278520f,	0.272621f,	0.266713f,	0.260794f,	0.254866f,	0.248928f,
-	0.242980f,	0.237024f,	0.231058f,	0.225084f,	0.219101f,	0.213110f,
-	0.207111f,	0.201105f,	0.195090f,	0.189069f,	0.183040f,	0.177004f,
-	0.170962f,	0.164913f,	0.158858f,	0.152797f,	0.146730f,	0.140658f,
-	0.134581f,	0.128498f,	0.122411f,	0.116319f,	0.110222f,	0.104122f,
-	0.098017f,	0.091909f,	0.085797f,	0.079682f,	0.073565f,	0.067444f,
-	0.061321f,	0.055195f,	0.049068f,	0.042938f,	0.036807f,	0.030675f,
-	0.024541f,	0.018407f,	0.012272f,	0.006136f,	0.000000f,	-0.006136f,
-	-0.012272f, -0.018407f, -0.024541f, -0.030675f, -0.036807f, -0.042938f,
-	-0.049068f, -0.055195f, -0.061321f, -0.067444f, -0.073565f, -0.079682f,
-	-0.085797f, -0.091909f, -0.098017f, -0.104122f, -0.110222f, -0.116319f,
-	-0.122411f, -0.128498f, -0.134581f, -0.140658f, -0.146730f, -0.152797f,
-	-0.158858f, -0.164913f, -0.170962f, -0.177004f, -0.183040f, -0.189069f,
-	-0.195090f, -0.201105f, -0.207111f, -0.213110f, -0.219101f, -0.225084f,
-	-0.231058f, -0.237024f, -0.242980f, -0.248928f, -0.254866f, -0.260794f,
-	-0.266713f, -0.272621f, -0.278520f, -0.284408f, -0.290285f, -0.296151f,
-	-0.302006f, -0.307850f, -0.313682f, -0.319502f, -0.325310f, -0.331106f,
-	-0.336890f, -0.342661f, -0.348419f, -0.354164f, -0.359895f, -0.365613f,
-	-0.371317f, -0.377007f, -0.382683f, -0.388345f, -0.393992f, -0.399624f,
-	-0.405241f, -0.410843f, -0.416430f, -0.422000f, -0.427555f, -0.433094f,
-	-0.438616f, -0.444122f, -0.449611f, -0.455084f, -0.460539f, -0.465976f,
-	-0.471397f, -0.476799f, -0.482184f, -0.487550f, -0.492898f, -0.498228f,
-	-0.503538f, -0.508830f, -0.514103f, -0.519356f, -0.524590f, -0.529804f,
-	-0.534998f, -0.540171f, -0.545325f, -0.550458f, -0.555570f, -0.560662f,
-	-0.565732f, -0.570781f, -0.575808f, -0.580814f, -0.585798f, -0.590760f,
-	-0.595699f, -0.600616f, -0.605511f, -0.610383f, -0.615232f, -0.620057f,
-	-0.624859f, -0.629638f, -0.634393f, -0.639124f, -0.643832f, -0.648514f,
-	-0.653173f, -0.657807f, -0.662416f, -0.667000f, -0.671559f, -0.676093f,
-	-0.680601f, -0.685084f, -0.689541f, -0.693971f, -0.698376f, -0.702755f,
-	-0.707107f, -0.711432f, -0.715731f, -0.720003f, -0.724247f, -0.728464f,
-	-0.732654f, -0.736817f, -0.740951f, -0.745058f, -0.749136f, -0.753187f,
-	-0.757209f, -0.761202f, -0.765167f, -0.769103f, -0.773010f, -0.776888f,
-	-0.780737f, -0.784557f, -0.788346f, -0.792107f, -0.795837f, -0.799537f,
-	-0.803208f, -0.806848f, -0.810457f, -0.814036f, -0.817585f, -0.821103f,
-	-0.824589f, -0.828045f, -0.831470f, -0.834863f, -0.838225f, -0.841555f,
-	-0.844854f, -0.848120f, -0.851355f, -0.854558f, -0.857729f, -0.860867f,
-	-0.863973f, -0.867046f, -0.870087f, -0.873095f, -0.876070f, -0.879012f,
-	-0.881921f, -0.884797f, -0.887640f, -0.890449f, -0.893224f, -0.895966f,
-	-0.898674f, -0.901349f, -0.903989f, -0.906596f, -0.909168f, -0.911706f,
-	-0.914210f, -0.916679f, -0.919114f, -0.921514f, -0.923880f, -0.926210f,
-	-0.928506f, -0.930767f, -0.932993f, -0.935184f, -0.937339f, -0.939459f,
-	-0.941544f, -0.943593f, -0.945607f, -0.947586f, -0.949528f, -0.951435f,
-	-0.953306f, -0.955141f, -0.956940f, -0.958703f, -0.960431f, -0.962121f,
-	-0.963776f, -0.965394f, -0.966976f, -0.968522f, -0.970031f, -0.971504f,
-	-0.972940f, -0.974339f, -0.975702f, -0.977028f, -0.978317f, -0.979570f,
-	-0.980785f, -0.981964f, -0.983105f, -0.984210f, -0.985278f, -0.986308f,
-	-0.987301f, -0.988258f, -0.989177f, -0.990058f, -0.990903f, -0.991710f,
-	-0.992480f, -0.993212f, -0.993907f, -0.994565f, -0.995185f, -0.995767f,
-	-0.996313f, -0.996820f, -0.997290f, -0.997723f, -0.998118f, -0.998476f,
-	-0.998795f, -0.999078f, -0.999322f, -0.999529f, -0.999699f, -0.999831f,
-	-0.999925f, -0.999981f, -1.000000f, -0.999981f, -0.999925f, -0.999831f,
-	-0.999699f, -0.999529f, -0.999322f, -0.999078f, -0.998795f, -0.998476f,
-	-0.998118f, -0.997723f, -0.997290f, -0.996820f, -0.996313f, -0.995767f,
-	-0.995185f, -0.994565f, -0.993907f, -0.993212f, -0.992480f, -0.991710f,
-	-0.990903f, -0.990058f, -0.989177f, -0.988258f, -0.987301f, -0.986308f,
-	-0.985278f, -0.984210f, -0.983105f, -0.981964f, -0.980785f, -0.979570f,
-	-0.978317f, -0.977028f, -0.975702f, -0.974339f, -0.972940f, -0.971504f,
-	-0.970031f, -0.968522f, -0.966976f, -0.965394f, -0.963776f, -0.962121f,
-	-0.960431f, -0.958703f, -0.956940f, -0.955141f, -0.953306f, -0.951435f,
-	-0.949528f, -0.947586f, -0.945607f, -0.943593f, -0.941544f, -0.939459f,
-	-0.937339f, -0.935184f, -0.932993f, -0.930767f, -0.928506f, -0.926210f,
-	-0.923880f, -0.921514f, -0.919114f, -0.916679f, -0.914210f, -0.911706f,
-	-0.909168f, -0.906596f, -0.903989f, -0.901349f, -0.898674f, -0.895966f,
-	-0.893224f, -0.890449f, -0.887640f, -0.884797f, -0.881921f, -0.879012f,
-	-0.876070f, -0.873095f, -0.870087f, -0.867046f, -0.863973f, -0.860867f,
-	-0.857729f, -0.854558f, -0.851355f, -0.848120f, -0.844854f, -0.841555f,
-	-0.838225f, -0.834863f, -0.831470f, -0.828045f, -0.824589f, -0.821103f,
-	-0.817585f, -0.814036f, -0.810457f, -0.806848f, -0.803208f, -0.799537f,
-	-0.795837f, -0.792107f, -0.788346f, -0.784557f, -0.780737f, -0.776888f,
-	-0.773010f, -0.769103f, -0.765167f, -0.761202f, -0.757209f, -0.753187f,
-	-0.749136f, -0.745058f, -0.740951f, -0.736817f, -0.732654f, -0.728464f,
-	-0.724247f, -0.720003f, -0.715731f, -0.711432f, -0.707107f, -0.702755f,
-	-0.698376f, -0.693971f, -0.689541f, -0.685084f, -0.680601f, -0.676093f,
-	-0.671559f, -0.667000f, -0.662416f, -0.657807f, -0.653173f, -0.648514f,
-	-0.643832f, -0.639124f, -0.634393f, -0.629638f, -0.624859f, -0.620057f,
-	-0.615232f, -0.610383f, -0.605511f, -0.600616f, -0.595699f, -0.590760f,
-	-0.585798f, -0.580814f, -0.575808f, -0.570781f, -0.565732f, -0.560662f,
-	-0.555570f, -0.550458f, -0.545325f, -0.540171f, -0.534998f, -0.529804f,
-	-0.524590f, -0.519356f, -0.514103f, -0.508830f, -0.503538f, -0.498228f,
-	-0.492898f, -0.487550f, -0.482184f, -0.476799f, -0.471397f, -0.465976f,
-	-0.460539f, -0.455084f, -0.449611f, -0.444122f, -0.438616f, -0.433094f,
-	-0.427555f, -0.422000f, -0.416430f, -0.410843f, -0.405241f, -0.399624f,
-	-0.393992f, -0.388345f, -0.382683f, -0.377007f, -0.371317f, -0.365613f,
-	-0.359895f, -0.354164f, -0.348419f, -0.342661f, -0.336890f, -0.331106f,
-	-0.325310f, -0.319502f, -0.313682f, -0.307850f, -0.302006f, -0.296151f,
-	-0.290285f, -0.284408f, -0.278520f, -0.272621f, -0.266713f, -0.260794f,
-	-0.254866f, -0.248928f, -0.242980f, -0.237024f, -0.231058f, -0.225084f,
-	-0.219101f, -0.213110f, -0.207111f, -0.201105f, -0.195090f, -0.189069f,
-	-0.183040f, -0.177004f, -0.170962f, -0.164913f, -0.158858f, -0.152797f,
-	-0.146730f, -0.140658f, -0.134581f, -0.128498f, -0.122411f, -0.116319f,
-	-0.110222f, -0.104122f, -0.098017f, -0.091909f, -0.085797f, -0.079682f,
-	-0.073565f, -0.067444f, -0.061321f, -0.055195f, -0.049068f, -0.042938f,
-	-0.036807f, -0.030675f, -0.024541f, -0.018407f, -0.012272f, -0.006136f,
-	-0.000000f, 0.006136f,	0.012272f,	0.018407f,	0.024541f,	0.030675f,
-	0.036807f,	0.042938f,	0.049068f,	0.055195f,	0.061321f,	0.067444f,
-	0.073565f,	0.079682f,	0.085797f,	0.091909f,	0.098017f,	0.104122f,
-	0.110222f,	0.116319f,	0.122411f,	0.128498f,	0.134581f,	0.140658f,
-	0.146730f,	0.152797f,	0.158858f,	0.164913f,	0.170962f,	0.177004f,
-	0.183040f,	0.189069f,	0.195090f,	0.201105f,	0.207111f,	0.213110f,
-	0.219101f,	0.225084f,	0.231058f,	0.237024f,	0.242980f,	0.248928f,
-	0.254866f,	0.260794f,	0.266713f,	0.272621f,	0.278520f,	0.284408f,
-	0.290285f,	0.296151f,	0.302006f,	0.307850f,	0.313682f,	0.319502f,
-	0.325310f,	0.331106f,	0.336890f,	0.342661f,	0.348419f,	0.354164f,
-	0.359895f,	0.365613f,	0.371317f,	0.377007f,	0.382683f,	0.388345f,
-	0.393992f,	0.399624f,	0.405241f,	0.410843f,	0.416430f,	0.422000f,
-	0.427555f,	0.433094f,	0.438616f,	0.444122f,	0.449611f,	0.455084f,
-	0.460539f,	0.465976f,	0.471397f,	0.476799f,	0.482184f,	0.487550f,
-	0.492898f,	0.498228f,	0.503538f,	0.508830f,	0.514103f,	0.519356f,
-	0.524590f,	0.529804f,	0.534998f,	0.540171f,	0.545325f,	0.550458f,
-	0.555570f,	0.560662f,	0.565732f,	0.570781f,	0.575808f,	0.580814f,
-	0.585798f,	0.590760f,	0.595699f,	0.600616f,	0.605511f,	0.610383f,
-	0.615232f,	0.620057f,	0.624859f,	0.629638f,	0.634393f,	0.639124f,
-	0.643832f,	0.648514f,	0.653173f,	0.657807f,	0.662416f,	0.667000f,
-	0.671559f,	0.676093f,	0.680601f,	0.685084f,	0.689541f,	0.693971f,
-	0.698376f,	0.702755f,	0.707107f,	0.711432f,	0.715731f,	0.720003f,
-	0.724247f,	0.728464f,	0.732654f,	0.736817f,	0.740951f,	0.745058f,
-	0.749136f,	0.753187f,	0.757209f,	0.761202f,	0.765167f,	0.769103f,
-	0.773010f,	0.776888f,	0.780737f,	0.784557f,	0.788346f,	0.792107f,
-	0.795837f,	0.799537f,	0.803208f,	0.806848f,	0.810457f,	0.814036f,
-	0.817585f,	0.821103f,	0.824589f,	0.828045f,	0.831470f,	0.834863f,
-	0.838225f,	0.841555f,	0.844854f,	0.848120f,	0.851355f,	0.854558f,
-	0.857729f,	0.860867f,	0.863973f,	0.867046f,	0.870087f,	0.873095f,
-	0.876070f,	0.879012f,	0.881921f,	0.884797f,	0.887640f,	0.890449f,
-	0.893224f,	0.895966f,	0.898674f,	0.901349f,	0.903989f,	0.906596f,
-	0.909168f,	0.911706f,	0.914210f,	0.916679f,	0.919114f,	0.921514f,
-	0.923880f,	0.926210f,	0.928506f,	0.930767f,	0.932993f,	0.935184f,
-	0.937339f,	0.939459f,	0.941544f,	0.943593f,	0.945607f,	0.947586f,
-	0.949528f,	0.951435f,	0.953306f,	0.955141f,	0.956940f,	0.958703f,
-	0.960431f,	0.962121f,	0.963776f,	0.965394f,	0.966976f,	0.968522f,
-	0.970031f,	0.971504f,	0.972940f,	0.974339f,	0.975702f,	0.977028f,
-	0.978317f,	0.979570f,	0.980785f,	0.981964f,	0.983105f,	0.984210f,
-	0.985278f,	0.986308f,	0.987301f,	0.988258f,	0.989177f,	0.990058f,
-	0.990903f,	0.991710f,	0.992480f,	0.993212f,	0.993907f,	0.994565f,
-	0.995185f,	0.995767f,	0.996313f,	0.996820f,	0.997290f,	0.997723f,
-	0.998118f,	0.998476f,	0.998795f,	0.999078f,	0.999322f,	0.999529f,
-	0.999699f,	0.999831f,	0.999925f,	0.999981f};
+
 #endif
\ No newline at end of file
diff --git a/Robot_V2/Algorithm/Filter.cpp b/Robot_V2/Algorithm/Filter.cpp
index 7a7821e..449dc00 100644
--- a/Robot_V2/Algorithm/Filter.cpp
+++ b/Robot_V2/Algorithm/Filter.cpp
@@ -1,17 +1,17 @@
 #include "Filter.h"
 #include "Can.h"
 #include "ang_sensor.h"
+#include "config.h"
 #include <algorithm>
 #include <cstring>
 #include <stdint.h>
-#include "config.h"
+
 extern angle_sensor KTH7823;
 extern CAN_transmiter CAN0_transmiter;
-extern u32 system_runtime_us;
+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);
 
-
 /*!
  \brief 	中值滤波器函数
  \param[in] values 待滤波的数据数组
@@ -126,10 +126,10 @@ float EncoderAngleKalman::getAngle() const { return kalman.getState(); }
 	\retval     当前角度估计值 (degree)
 */
 float EncoderAngleKalman::getVelocity() const {
-	static u32 last_us100_counter = system_runtime_us;
-	float vel = (kalman.getState() - last_angle) * 1e4 *
-				(system_runtime_us - last_us100_counter);
-	last_us100_counter = system_runtime_us;
+	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;
 	return vel;
 }
 
diff --git a/Robot_V2/Robot_V2.cproject.yml b/Robot_V2/Robot_V2.cproject.yml
index f042e28..ab6bd24 100644
--- a/Robot_V2/Robot_V2.cproject.yml
+++ b/Robot_V2/Robot_V2.cproject.yml
@@ -59,6 +59,7 @@ project:
     - component: Device:GD32C10x_StdPeripherals:BKP
     - component: Device:GD32C10x_StdPeripherals:CRC
     - component: Device:GD32C10x_StdPeripherals:RTC
+    - component: CMSIS:DSP
   output:
     type:
       - elf
@@ -76,6 +77,7 @@ project:
         - -D__MICROLIB
         - -Wno-register
         - -O3 # 添加优化级别
+        - -DARM_MATH_CM4 # Cortex-M4核心定义
       C:
         - -D__packed=__attribute__((packed))
       ASM:
@@ -86,3 +88,4 @@ project:
         - --entry=Reset_Handler
         - --remove # 移除未使用的代码段
         - --diag_suppress=L6207,L6210,L6218,L6220,L6222,L6224,L6226,L6228,L6230,L6232,L6234,L6236,L6238,L6240,L6242,L6244,L6246,L6248,L6308,L6329,L6310,L6312,L6314,L6316,L6318,L6320,L6322,L6324,L6326,L6328,L6330,L6332,L6334,L6336,L6338,L6340,L6342,L6344,L6346,L6348,L6350,L6352,L6354,L6311,L6313,L6315,L6317,L6319,L6321,L6323,L6325,L6327,L6329,L6331,L6333,L6335,L6337,L6339,L6341,L6343,L6345,L6347,L6349,L6351,L6353
+        # 移除了 __TARGET_FPU_VFP，因为它不是链接器参数
diff --git a/Robot_V2/device/ADC.cpp b/Robot_V2/device/ADC.cpp
index fe19cad..8d6ebf8 100644
--- a/Robot_V2/device/ADC.cpp
+++ b/Robot_V2/device/ADC.cpp
@@ -2,6 +2,7 @@
 #include "Can.h"
 #include "FOC.h"
 #include "Filter.h"
+#include "arm_math.h"
 #include "config.h"
 #include "math.h"
 #include "systick.h"
diff --git a/Robot_V2/device/Can.cpp b/Robot_V2/device/Can.cpp
index 7b350f2..eb765a6 100644
--- a/Robot_V2/device/Can.cpp
+++ b/Robot_V2/device/Can.cpp
@@ -444,6 +444,7 @@ u8 CAN_transmiter::init() {
 void CAN_transmiter::update(void) {
 	target_angle_union.f = FOC.target_angle;
 	target_velocity_union.f = FOC.target_velocity;
+	target_current_union.f = FOC.target_current;
 	limit_velocity_union.f = FOC.limit_velocity;
 	limit_voltage = FOC.limit_voltage * 1000;
 	mcu_temp = sampler.get_mcu_temp() * 100;
@@ -591,6 +592,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;
+			FOC.target_current = FOC.target_velocity * 10.0f;
 		} else if (receive_message.rx_efid == CANFD_RESET_ID) {
 			u8 first_byte = receive_message.rx_data[0];
 			u8 eigth_byte = receive_message.rx_data[7];
diff --git a/Robot_V2/device/Can.h b/Robot_V2/device/Can.h
index 6d5282c..eeaf828 100644
--- a/Robot_V2/device/Can.h
+++ b/Robot_V2/device/Can.h
@@ -87,9 +87,10 @@ class CAN_transmiter {
 	u8 Target_Control_Mode = 0;				   // 目标控制模式
 	u8 EEPROMStatus = 0;					   // EEPROM状态码
 	float_union_t target_velocity_union = {0}; // 目标速度
-	float_union_t u_current_union = {0};	   // U相电流
-	float_union_t w_current_union = {0};	   // W相电流
-	float_union_t v_current_union = {0};	   // V相电流
+	float_union_t target_current_union = {0};
+	float_union_t u_current_union = {0}; // U相电流
+	float_union_t w_current_union = {0}; // W相电流
+	float_union_t v_current_union = {0}; // V相电流
 
 	can_trasnmit_message_struct transmit_message; // 发送消息结构体
 	u8 transmit_mailbox = CAN_NOMAILBOX;		  // 初始化为无mailbox状态
diff --git a/Robot_V2/device/Timer.cpp b/Robot_V2/device/Timer.cpp
index 1b41c42..18ca60e 100644
--- a/Robot_V2/device/Timer.cpp
+++ b/Robot_V2/device/Timer.cpp
@@ -12,7 +12,6 @@
 #include <stdint.h>
 #include <stdio.h>
 
-
 volatile u32 total_us100_counter = 0; // 总计数器，每100微秒增加1
 extern u16 adc_value[7];
 extern FOC_Controlroller FOC;
@@ -96,66 +95,75 @@ void TIMER2_IRQHandler(void) {
 	if (timer_interrupt_flag_get(TIMER2, TIMER_INT_UP) != RESET) {
 		// 清除更新中断标志
 		timer_interrupt_flag_clear(TIMER2, TIMER_INT_UP);
-		// 每100us执行一次的操作
-		scheduler.taskFlags |= (1 << TASK_ANGLE_UPDATE) |
-							   (1 << TASK_VELOCITY_UPDATE) |
-							   (1 << TASK_MOTOR_CONTROL);
 
-		switch (total_us100_counter) { // 每1ms执行的代码，负载均衡
-		case 0: {
-			scheduler.taskFlags |= (1 << TASK_MCU_TEMP_UPDATE);
-			break;
-		}
-		case 1: {
-			scheduler.taskFlags |= (1 << TASK_DRV_TEMP_UPDATE);
-			break;
-		}
-		case 2: {
-			scheduler.taskFlags |= (1 << TASK_VBUS_UPDATE);
-			break;
-		}
-		case 3: {
-			break;
-		}
-		case 9: {
-			scheduler.taskFlags |= (1 << TASK_CAN_TRANSMIT);
-			break;
-		}
-		}
+		// 每50us执行一次的操作 - 电机控制
+		scheduler.taskFlags |= (1 << TASK_MOTOR_CONTROL);
+		scheduler.taskFlags |=
+			(1 << TASK_ANGLE_UPDATE) | (1 << TASK_VELOCITY_UPDATE);
 
-		total_us100_counter++;
-		if (total_us100_counter > 9) {
-			total_us100_counter = 0;
-			// 可以添加每1ms执行的代码，无负载均衡
-			static u8 ms1_counter = 0;
+		// 使用标志位控制，每100us执行一次其他任务
+		static bool isEvenCall = false;
+		isEvenCall = !isEvenCall;
 
-			switch (ms1_counter) { // 每10ms执行的代码，负载均衡
+		if (isEvenCall) {
+			// 每100us执行一次的操作 - 保持原有频率
+
+			switch (total_us100_counter) { // 每1ms执行的代码，负载均衡
 			case 0: {
-				scheduler.taskFlags |= (1 << TASK_THERMAL_CONTROL);
+				scheduler.taskFlags |= (1 << TASK_MCU_TEMP_UPDATE);
 				break;
 			}
 			case 1: {
-				scheduler.taskFlags |= (1 << TASK_MAGNETIC_CHECK);
+				scheduler.taskFlags |= (1 << TASK_DRV_TEMP_UPDATE);
 				break;
 			}
 			case 2: {
+				scheduler.taskFlags |= (1 << TASK_VBUS_UPDATE);
+				break;
+			}
+			case 3: {
+				break;
+			}
+			case 9: {
+				scheduler.taskFlags |= (1 << TASK_CAN_TRANSMIT);
 				break;
 			}
 			}
-			ms1_counter++;
-			if (ms1_counter > 9) {
-				ms1_counter = 0;
-				// 可以添加每10ms执行的代码，无负载均衡
-				static u8 ms10_counter = 0;
-				ms10_counter++;
-				if (ms10_counter > 9) {
-					ms10_counter = 0;
-					// 可以添加每100ms执行的代码
-					static u8 sec_counter = 0;
-					sec_counter++;
-					if (sec_counter > 9) {
-						sec_counter = 0;
-						// 可以添加每1s执行的代码
+
+			total_us100_counter++;
+			if (total_us100_counter > 9) {
+				total_us100_counter = 0;
+				// 可以添加每1ms执行的代码，无负载均衡
+				static u8 ms1_counter = 0;
+
+				switch (ms1_counter) { // 每10ms执行的代码，负载均衡
+				case 0: {
+					scheduler.taskFlags |= (1 << TASK_THERMAL_CONTROL);
+					break;
+				}
+				case 1: {
+					scheduler.taskFlags |= (1 << TASK_MAGNETIC_CHECK);
+					break;
+				}
+				case 2: {
+					break;
+				}
+				}
+				ms1_counter++;
+				if (ms1_counter > 9) {
+					ms1_counter = 0;
+					// 可以添加每10ms执行的代码，无负载均衡
+					static u8 ms10_counter = 0;
+					ms10_counter++;
+					if (ms10_counter > 9) {
+						ms10_counter = 0;
+						// 可以添加每100ms执行的代码
+						static u8 sec_counter = 0;
+						sec_counter++;
+						if (sec_counter > 9) {
+							sec_counter = 0;
+							// 可以添加每1s执行的代码
+						}
 					}
 				}
 			}
diff --git a/Robot_V2/device/ang_sensor.cpp b/Robot_V2/device/ang_sensor.cpp
index 520db62..c1d9d6c 100644
--- a/Robot_V2/device/ang_sensor.cpp
+++ b/Robot_V2/device/ang_sensor.cpp
@@ -253,7 +253,7 @@ float angle_sensor::update(void) {
 	float delta = current_angle_deg - m_lastAngle;
 
 	// 处理角度绕回情况（0度和360度之间的跳变）
-	// 如果变化超过180度，说明发生了绕回
+	// 如果变化超过300度，说明发生了绕回
 	if (delta > ANGLE_WRAP_THRESHOLD) {
 		// 从接近360度绕回到接近0度
 		delta -= ANGLE_WRAP_VALUE;
diff --git a/Robot_V2/main.cpp b/Robot_V2/main.cpp
index d844ece..12356ac 100644
--- a/Robot_V2/main.cpp
+++ b/Robot_V2/main.cpp
@@ -9,10 +9,11 @@
 #include "health.h"
 #include "initial.h"
 #include "motor.h"
-#include "sys/config.h"
 #include "sys/flash.h"
 #include "sys/initial.h"
 #include "systick.h"
+#include <arm_math.h>
+#include <cmath>
 #include <cstdio>
 #include <stdint.h>
 extern CAN_transmiter CAN0_transmiter;		// CAN初始化
@@ -25,8 +26,8 @@ extern FOC_Controlroller FOC;				// FOC控制
 extern FlashEEPROM flashEEP;				// EEPROM类
 
 TaskScheduler scheduler = {0};
-volatile u32 system_runtime_us = 0; // 电机驱动计算轮次
-volatile u32 cnt_test = 0;			// 负载检测
+volatile u32 motor_runtime_50us = 0; // 电机驱动计算轮次
+volatile u32 cnt_test = 0;			 // 负载检测
 int main(void) {
 	u8 init_status = hardware_init();
 
@@ -35,9 +36,8 @@ int main(void) {
 	}
 	OTA_initial(); //  OTA初始化,A/B分区处理
 
-	watchdog_init(50);	// 看门狗初始化
 	angsens_MG_check(); // 角度传感器磁感应强度测试
-
+	watchdog_init(50);	// 看门狗初始化
 	while (1) {
 		__disable_irq();
 		u32 currentTasks =
@@ -60,7 +60,6 @@ u8 hardware_init(void) {
 		RCU_FLAG_FWDGTRST) { // 如果是看门狗复位，本次看门狗时间延长并休眠
 		printf("continue sleep\r\n");
 		watchdog_init(5000); // 看门狗初始化
-		delay_1ms(3000);
 		prepare_deep_sleep(false);
 	}
 	User_timer_init(); // 任务Timer初始化
@@ -76,6 +75,7 @@ u8 hardware_init(void) {
 }
 
 void do_task(u32 currentTasks) {
+	cnt_test++;
 	// 按照优先级和重要性处理任务
 	if (currentTasks & (1 << TASK_CURRENT_UPDATE)) { // 更新电流采样值
 		sampler.update_current();
@@ -88,7 +88,7 @@ void do_task(u32 currentTasks) {
 		FOC.rt_velocity = Vel_kalman.update(Angle_kalman.getVelocity());
 	}
 	if (currentTasks & (1 << TASK_MOTOR_CONTROL)) { // 电机控制任务(最高优先级)
-		system_runtime_us++;
+		motor_runtime_50us++;
 
 		// 函数指针数组，包含各种控制模式对应的函数
 		static FOCFunctionPtr control_functions[] = {
@@ -122,8 +122,8 @@ void do_task(u32 currentTasks) {
 	if (currentTasks & (1 << TASK_VBUS_UPDATE)) { // 更新vbus电压值
 		power_manage();
 	}
-	if (currentTasks & (1 << TASK_CAN_TRANSMIT)) { // CAN通信
-		// CAN0_transmiter.testfloat_union.f = cnt_test; // 负载检测
+	if (currentTasks & (1 << TASK_CAN_TRANSMIT)) {	  // CAN通信
+		CAN0_transmiter.testfloat_union.f = cnt_test; // 负载检测
 		cnt_test = 0;
 		CAN0_transmiter.can0_send_flag = SET;
 		CAN0_transmiter.communication_check();
diff --git a/Robot_V2/sys/config.h b/Robot_V2/sys/config.h
index cc4d311..c0c66b4 100644
--- a/Robot_V2/sys/config.h
+++ b/Robot_V2/sys/config.h
@@ -3,7 +3,7 @@
 #include <stdint.h>
 
 // 如果使用CMSIS DSP库，需要定义ARM_MATH_CM4
-// #define ARM_MATH_CM4
+#define ARM_MATH_CM4
 
 #define EEP_DEBUG 1
 #define OTA_DEBUG 1
@@ -39,9 +39,9 @@ constexpr u8 SIZE_FLOAT = sizeof(float);
 constexpr u8 SIZE_U32 = sizeof(uint32_t);
 constexpr float ADC_MAX = 65536.0f;
 constexpr float PI = 3.14159265358979323846f;
-constexpr float ANGLE_FACTOR = 25.0f / 360.0f * 2.0f * PI;
-constexpr float DEG2RAD = 1.0f / 360.0f * 2 * PI;
-constexpr float RAD2DEG = 360.0f / 2 * PI;
+constexpr float ANGLE_FACTOR = 25.0f / 180.0f * PI;
+constexpr float DEG2RAD = 1.0f / 180.0f * PI;
+constexpr float RAD2DEG = 180.0f / PI;	   // 修正RAD2DEG定义，应该是180/π
 constexpr float _1_SQRT3 = 0.57735026919f; // 1/sqrt(3)
 constexpr float sqrt3dv2 = 0.86602540378f; // sqrt(3)/2的预计算值
 constexpr u8 DEFAULT_VALUE = 0;
@@ -89,7 +89,8 @@ constexpr float ANGLESENS_FACTOR = 1.0f / ADC_MAX * 360.0f;
 
 // TIMER2-任务调度
 constexpr int TIMER2_PRESCALER = 119; // 120MHz / 120 = 1MHz
-constexpr int TIMER2_PERIOD = 99;	  // 1MHz / 100 = 100us
+constexpr int TIMER2_PERIOD = 49;	  // 1MHz / 50 = 50us
+constexpr float TIMER2_FREQUENCY = 1 / 5e-5;
 
 // TIMER1-PWM
 constexpr int TIMER1_PRESCALER = 0; // 不分频，计数器时钟为120MHz
diff --git a/Robot_V2/sys/flash.h b/Robot_V2/sys/flash.h
index 0c67474..77a02dc 100644
--- a/Robot_V2/sys/flash.h
+++ b/Robot_V2/sys/flash.h
@@ -1,8 +1,8 @@
 #ifndef __FLASH_H
 #define __FLASH_H
 
+#include "gd32c10x.h" // 添加GD32主头文件以确保外设函数声明可用
 #include <stdint.h>
-#include "gd32c10x.h"  // 添加GD32主头文件以确保外设函数声明可用
 
 // 修改后 - 强制4字节对齐
 #define ALIGN_4BYTE(addr) ((addr) & 0xFFFFFFFC) // 4字节对齐
@@ -71,7 +71,7 @@ class FlashEEPROM {
 };
 
 void flash_eeprom_example_main(void);
-u8 OTA_SET(u32 addr, u32 data) ;
+u8 OTA_SET(u32 addr, u32 data);
 fmc_state_enum move_app_data(void);
 
 #endif /* __FLASH_H */
\ No newline at end of file
diff --git a/Robot_V2/sys/initial.cpp b/Robot_V2/sys/initial.cpp
index f8d7d86..500fca6 100644
--- a/Robot_V2/sys/initial.cpp
+++ b/Robot_V2/sys/initial.cpp
@@ -1,4 +1,5 @@
 #include "initial.h"
+#include "Can.h"
 #include "config.h"
 #include "flash.h"
 #include "gd32c10x.h"
@@ -7,7 +8,6 @@
 #include "gd32c10x_rcu.h"
 #include <cstdio>
 #include <stdint.h>
-#include "can.h"
 #include <stdio.h>
 
 extern CAN_transmiter CAN0_transmiter; // CAN初始化
@@ -97,9 +97,13 @@ u16 get_reset_cause(void) {
 }
 
 void OTA_initial(void) {
+
 	// 确认当前APP可用
 	OTA_SET(n_APP_LATEST_CHECK_ADDR, OTA_AVAL);
 	OTA_SET(n_APP_LATEST_EN_ADDR, OTA_AVAL);
+	u8 transmit_data[2] = {0x0A, 0xAC};
+	// 确认APP可用
+	CAN0_transmiter.CANFD_AsyncSend(CANFD_OTA_ACK_ID, 2, transmit_data);
 
 	volatile u32 *flash_ptr = (volatile u32 *)n_APP_BACKUP_AVAL_ADDR;
 	u32 back_aval = *flash_ptr;
@@ -108,9 +112,14 @@ void OTA_initial(void) {
 
 	// 如果BACK不可用
 	if (back_aval == OTA_NOT_AVAL) {
+		std::printf("notaval\r\n");
 		move_app_data();
 	} else if (back_aval == OTA_AVAL &&
 			   back_check == OTA_NOT_AVAL) { // BACK已刷写，但未检查
+		std::printf("aval\r\n");
 		move_app_data();
 	}
+	else {
+		printf("check\r\n");
+	}
 }
diff --git a/Robot_V2/sys/systick.cpp b/Robot_V2/sys/systick.cpp
index b7f6c4a..82ff49b 100644
--- a/Robot_V2/sys/systick.cpp
+++ b/Robot_V2/sys/systick.cpp
@@ -34,9 +34,11 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include "systick.h"
 #include "Can.h"
+#include "core_cmFunc.h"
 #include "flash.h"
 #include "gd32c10x.h"
 #include "gd32c10x_exti.h"
+#include "initial.h"
 #include "motor.h"
 #include <cstdio>
 
@@ -157,6 +159,7 @@ void prepare_deep_sleep(bool save_data) {
 	// 确保中断标志已被清除
 	exti_interrupt_flag_clear(EXTI_8);
 
+	watchdog_init(5000); // 看门狗初始化
 	// 进入深度睡眠
 	pmu_to_deepsleepmode(PMU_LDO_LOWPOWER, WFI_CMD);
 }
@@ -177,9 +180,3 @@ void EXTI5_9_IRQHandler(void) {
 		system_soft_reset();
 	}
 }
-
-
-void OTA_initial(){
-	
-
-}
\ No newline at end of file