From 871e33e10fb4a1006ef92c753084e0ed8af21b45 Mon Sep 17 00:00:00 2001
From: cbb <2625274797@qq.com>
Date: Sun, 23 Nov 2025 18:31:47 +0800
Subject: [PATCH] =?UTF-8?q?fix:=E4=BF=AE=E5=A4=8D=E4=BB=BB=E5=8A=A1?=
 =?UTF-8?q?=E8=B0=83=E5=BA=A6=E4=B8=8D=E5=87=86=E7=A1=AE=E7=9A=84=E9=97=AE?=
 =?UTF-8?q?=E9=A2=98=EF=BC=8C=20feat:=E4=BC=98=E5=8C=96=E7=94=B5=E6=B5=81?=
 =?UTF-8?q?=E9=87=87=E6=A0=B7=E9=80=9F=E7=8E=87=20feat:=E4=BC=98=E5=8C=96P?=
 =?UTF-8?q?IDramp=E6=95=88=E7=8E=87=20feat:=E4=BC=98=E5=8C=96=E8=A7=92?=
 =?UTF-8?q?=E5=BA=A6=E9=87=87=E6=A0=B7=E6=95=88=E7=8E=87=20fix:=E5=9B=9E?=
 =?UTF-8?q?=E6=BB=9ACAN=E6=89=93=E5=8C=85=E9=80=BB=E8=BE=91=20feat:?=
 =?UTF-8?q?=E5=8E=BB=E9=99=A4timer3=20feat(FOC):=20=E4=BC=98=E5=8C=96?=
 =?UTF-8?q?=E5=B8=95=E5=85=8B=E9=80=86=E5=8F=98=E6=8D=A2=E4=B8=8E=E5=85=8B?=
 =?UTF-8?q?=E6=8B=89=E5=85=8B=E9=80=86=E5=8F=98=E6=8D=A2=E8=AE=A1=E7=AE=97?=
 =?UTF-8?q?=E6=96=B9=E5=BC=8F=20=E5=B0=86=E5=8E=9F=E6=9C=89=E7=9A=84?=
 =?UTF-8?q?=E4=B8=89=E8=A7=92=E5=87=BD=E6=95=B0=E8=AE=A1=E7=AE=97=E6=9B=BF?=
 =?UTF-8?q?=E6=8D=A2=E4=B8=BA=E6=9F=A5=E8=A1=A8=E6=B3=95=EF=BC=8C=E6=8F=90?=
 =?UTF-8?q?=E9=AB=98=E8=BF=90=E7=AE=97=E6=95=88=E7=8E=87=E3=80=82=E5=90=8C?=
 =?UTF-8?q?=E6=97=B6=E6=B7=BB=E5=8A=A0=E4=BA=86=20sqrt(3)/2=20=E7=9A=84?=
 =?UTF-8?q?=E9=A2=84=E8=AE=A1=E7=AE=97=E5=B8=B8=E9=87=8F=EF=BC=8C=E5=B9=B6?=
 =?UTF-8?q?=E4=BC=98=E5=8C=96=E4=BA=86=E7=94=B5=E5=8E=8B=E9=99=90=E5=88=B6?=
 =?UTF-8?q?=E9=83=A8=E5=88=86=E7=9A=84=E8=AE=A1=E7=AE=97=E9=80=BB=E8=BE=91?=
 =?UTF-8?q?=EF=BC=8C=E5=87=8F=E5=B0=91=E9=87=8D=E5=A4=8D=E8=BF=90=E7=AE=97?=
 =?UTF-8?q?=E3=80=82?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 .continue/mcpServers/new-mcp-server.yaml |  10 +
 Robot_V2.csolution.yml                   |   2 +-
 Robot_V2/Algorithm/DengFOC.cpp           |  28 +-
 Robot_V2/Algorithm/DengFOC.h             | 350 +++++++++++++++++++++++
 Robot_V2/Algorithm/pid.cpp               |  14 +-
 Robot_V2/device/ADC.cpp                  |  32 ++-
 Robot_V2/device/Can.cpp                  |   7 +-
 Robot_V2/device/Can.h                    |  33 ++-
 Robot_V2/device/Timer.cpp                | 144 +++-------
 Robot_V2/device/Timer.h                  |   2 -
 Robot_V2/device/adc.h                    |   2 +
 Robot_V2/device/ang_sensor.cpp           |  42 +--
 Robot_V2/main.cpp                        |  25 +-
 Robot_V2/sys/initial.cpp                 |   1 -
 14 files changed, 503 insertions(+), 189 deletions(-)
 create mode 100644 .continue/mcpServers/new-mcp-server.yaml

diff --git a/.continue/mcpServers/new-mcp-server.yaml b/.continue/mcpServers/new-mcp-server.yaml
new file mode 100644
index 0000000..0e32aa6
--- /dev/null
+++ b/.continue/mcpServers/new-mcp-server.yaml
@@ -0,0 +1,10 @@
+name: New MCP server
+version: 0.0.1
+schema: v1
+mcpServers:
+  - name: New MCP server
+    command: npx
+    args:
+      - -y
+      - <your-mcp-server>
+    env: {}
diff --git a/Robot_V2.csolution.yml b/Robot_V2.csolution.yml
index 955d4c3..b255718 100644
--- a/Robot_V2.csolution.yml
+++ b/Robot_V2.csolution.yml
@@ -1,6 +1,6 @@
 # A solution is a collection of related projects that share same base configuration.
 solution:
-  created-for: CMSIS-Toolbox@2.6.0
+  created-for: CMSIS-Toolbox@2.12.0
   cdefault: # use CMSIS-Toolbox default Compiler options
 
   # Compilers that can be selected
diff --git a/Robot_V2/Algorithm/DengFOC.cpp b/Robot_V2/Algorithm/DengFOC.cpp
index 0e7000d..3580c04 100644
--- a/Robot_V2/Algorithm/DengFOC.cpp
+++ b/Robot_V2/Algorithm/DengFOC.cpp
@@ -17,8 +17,10 @@ constexpr float _2_SQRT3 = 1.15470053838f;
 constexpr float _3PI_2 = 4.71238898038f;
 constexpr u8 PP = 11;
 constexpr float DIR = 1.0;
-constexpr float sqrt3 = 1.73205080757f; // sqrt(3)的预计算值
+constexpr float sqrt3 = 1.73205080757f;	   // sqrt(3)的预计算值
+constexpr float sqrt3dv2 = 0.86602540378f; // sqrt(3)/2的预计算值
 constexpr float zero_electric_angle = 0.0f;
+constexpr u16 LUT_SCALE = (LUT_SIZE / (2.0f * PI));
 
 FOC_Controlroller FOC; // FOC控制初始化
 extern sampler sampler;
@@ -63,24 +65,26 @@ inline void FOC_Controlroller::setPhaseVoltage(float Uq, float Ud) {
 	float angle_el = _normalizeAngle(shaft_angle * PP);
 
 	// 帕克逆变换 (直接计算，避免函数调用开销)
-	const float sin_el = sin(angle_el);
-	const float cos_el = cos(angle_el);
-	Ualpha = -Uq * sin_el;
-	Ubeta = Uq * cos_el;
+	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];
+	float Ualpha = Ud * cos_el - Uq * sin_el;
+	float Ubeta = Ud * sin_el + Uq * cos_el;
 
-	// 克拉克逆变换 (预计算常量值)
+	// 克拉克逆变换 (使用预计算常量优化)
 	Ua = Ualpha + v_half;
-	Ub = (sqrt3 * Ubeta - Ualpha) * 0.5f + v_half;
-	Uc = (-Ualpha - sqrt3 * Ubeta) * 0.5f + v_half;
+	Ub = sqrt3dv2 * Ubeta - 0.5f * Ualpha + v_half;
+	Uc = -0.5f * Ualpha - sqrt3dv2 * Ubeta + v_half;
 
-	// 计算占空比
+	// 计算占空比并限制范围
 	float dc_a = _constrain(Ua / v_safe, 0.0f, 1.0f);
 	float dc_b = _constrain(Ub / v_safe, 0.0f, 1.0f);
 	float dc_c = _constrain(Uc / v_safe, 0.0f, 1.0f);
 	// 设置PWM输出
-	dc_a = dc_a * limit_voltage / v_safe;
-	dc_b = dc_b * limit_voltage / v_safe;
-	dc_c = dc_c * limit_voltage / v_safe;
+	float voltage_exchange = limit_voltage / v_safe;
+	dc_a = dc_a * voltage_exchange;
+	dc_b = dc_b * voltage_exchange;
+	dc_c = dc_c * voltage_exchange;
 	Set_3Pwm(dc_a, dc_b, dc_c);
 	// std::printf("PWM:%f,%f,%f\n", dc_a, dc_b, dc_c);
 }
diff --git a/Robot_V2/Algorithm/DengFOC.h b/Robot_V2/Algorithm/DengFOC.h
index 89e9471..446c891 100644
--- a/Robot_V2/Algorithm/DengFOC.h
+++ b/Robot_V2/Algorithm/DengFOC.h
@@ -3,7 +3,9 @@
 #define _DENGFOC_H
 #include "pid.h"
 #include <stdint.h>
+
 class FOC_Controlroller {
+	
   public:
 	FOC_Controlroller();
 
@@ -57,4 +59,352 @@ void DFOC_Vbus(float power_supply);
 inline float _electricalAngle();
 float DFOC_M0_Angle(void);
 
+
+constexpr u16 LUT_SIZE = 1024;
+constexpr u16 LUT_MASK = (LUT_SIZE - 1);
+
+__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/pid.cpp b/Robot_V2/Algorithm/pid.cpp
index 7b9d5f8..61595c1 100644
--- a/Robot_V2/Algorithm/pid.cpp
+++ b/Robot_V2/Algorithm/pid.cpp
@@ -2,6 +2,7 @@
 #define _constrain(amt, low, high)                                             \
 	((amt) < (low) ? (low) : ((amt) > (high) ? (high) : (amt)))
 #include "Can.h"
+#include "math.h"
 
 extern CAN_transmiter CAN0_transmiter;
 constexpr float TIME_SLICE = 1e-4;
@@ -31,14 +32,11 @@ float PIDController::operator()(float error) {
 	output = _constrain(output, -limit, limit);
 
 	// 输出斜坡限制
-	if (output_ramp > 0) {
-		// 对PID的变化速率进行限制
-		float output_rate = (output - output_prev) / TIME_SLICE;
-		if (output_rate > output_ramp)
-			output = output_prev + output_ramp * TIME_SLICE;
-		else if (output_rate < -output_ramp)
-			output = output_prev - output_ramp * TIME_SLICE;
-	}
+    // 简化的斜坡限制（单行逻辑）
+    if (output_ramp > 0) {
+        float max_delta = output_ramp * TIME_SLICE;
+        output = output_prev + fmaxf(-max_delta, fminf(max_delta, output - output_prev));
+    }
 	// 保存值（为了下一次循环）
 	integral_prev = integral;
 	output_prev = output;
diff --git a/Robot_V2/device/ADC.cpp b/Robot_V2/device/ADC.cpp
index 8c797fc..32017f1 100644
--- a/Robot_V2/device/ADC.cpp
+++ b/Robot_V2/device/ADC.cpp
@@ -1,6 +1,6 @@
 #include "adc.h"
-#include "Filter.h"
 #include "Can.h"
+#include "Filter.h"
 #include "math.h"
 #include <cstring>
 #include <gd32c10x_dma.h>
@@ -20,7 +20,7 @@ constexpr float MIN_CURRENT = -40.0f;
 constexpr float MAX_VCC = 65.0f;
 constexpr float MIN_VCC = -0.1f;
 extern CAN_transmiter CAN0_transmiter;
-sampler sampler;					 // 采样器初始化
+sampler sampler; // 采样器初始化
 
 u16 adc_value[6] = {0}; // 显式初始化数组元素为0
 /*!
@@ -139,7 +139,7 @@ sampler::sampler() {
 float sampler::get_mcu_temp() {
 	tempora_sample = medium_filter(mcu_temp_samples, FILTER_SIZE + 1);
 	// 检查温度读数是否在有效范围内
-	if (tempora_sample < MIN_TEMP || tempora_sample > MAX_TEMP) {
+	if (tempora_sample < MIN_TEMP or tempora_sample > MAX_TEMP) {
 		return ERROR_ADC_VAL;
 	}
 	return tempora_sample;
@@ -186,7 +186,7 @@ float sampler::get_current(char type) {
 		return ERROR_ADC_VAL;
 	}
 	}
-	if (tempora_sample < MIN_CURRENT || tempora_sample > MAX_CURRENT) {
+	if (tempora_sample < MIN_CURRENT or tempora_sample > MAX_CURRENT) {
 		return ERROR_ADC_VAL;
 	}
 	return tempora_sample;
@@ -202,7 +202,7 @@ float sampler::get_current(char type) {
 float sampler::get_vbus_voltage() {
 	tempora_sample = medium_filter(vbus_samples, FILTER_SIZE + 1);
 	// 检查电压读数是否在有效范围内
-	if (vbus_samples[FILTER_SIZE / 2] > MAX_VCC ||
+	if (vbus_samples[FILTER_SIZE / 2] > MAX_VCC or
 		vbus_samples[FILTER_SIZE / 2] < MIN_VCC) {
 		return ERROR_ADC_VAL;
 	}
@@ -237,16 +237,20 @@ void sampler::update_drv_temp() {
  * @brief 更新相电流
  */
 void sampler::update_current() {
-	memmove(currU_samples, currU_samples + 1, FILTER_SIZE * sizeof(float));
-	memmove(currV_samples, currV_samples + 1, FILTER_SIZE * sizeof(float));
-	memmove(currW_samples, currW_samples + 1, FILTER_SIZE * sizeof(float));
+	u8 idx = current_sample_index;
+	// 优化计算
+	const float scale = ADC_SCALE * CURRENT_FACTOR;
+	const float offset = CURRENT_OFFSET * CURRENT_FACTOR;
 
-	currU_samples[FILTER_SIZE] =
-		(adc_value[2] * ADC_SCALE + CURRENT_OFFSET) * CURRENT_FACTOR;
-	currV_samples[FILTER_SIZE] =
-		(adc_value[3] * ADC_SCALE + CURRENT_OFFSET) * CURRENT_FACTOR;
-	currW_samples[FILTER_SIZE] =
-		(adc_value[4] * ADC_SCALE + CURRENT_OFFSET) * CURRENT_FACTOR;
+	currU_samples[idx] = adc_value[2] * scale + offset;
+	currV_samples[idx] = adc_value[3] * scale + offset;
+	currW_samples[idx] = adc_value[4] * scale + offset;
+
+	// 更新索引
+	current_sample_index++;
+	if (current_sample_index >= FILTER_SIZE - 1) {
+		current_sample_index = 0;
+	}
 }
 
 /**
diff --git a/Robot_V2/device/Can.cpp b/Robot_V2/device/Can.cpp
index dc362ac..28647eb 100644
--- a/Robot_V2/device/Can.cpp
+++ b/Robot_V2/device/Can.cpp
@@ -6,6 +6,7 @@
 #include "gd32c10x.h"
 #include "gd32c10x_can.h"
 #include "gd32c10x_dma.h"
+#include "systick.h"
 #include <cstdio>
 #include <cstdlib>
 #include <cstring>
@@ -258,8 +259,12 @@ void can_loopback_init(void) {
 	can_parameter.time_segment_2 = CAN_BT_BS2_2TQ;
 	can_parameter.prescaler = 6;
 	if (can_init(CANX, &can_parameter) != SUCCESS) {
-		while (1)
+		u16 i = 200;
+		while (--i>0){
+			delay_1ms(10);
 			printf("can init error \r\n");
+		}
+		system_soft_reset();
 	}
 
 	/* initialize filter */
diff --git a/Robot_V2/device/Can.h b/Robot_V2/device/Can.h
index bba05cf..56dc00a 100644
--- a/Robot_V2/device/Can.h
+++ b/Robot_V2/device/Can.h
@@ -17,30 +17,39 @@ typedef union {
 } float_union_t;
 
 // 添加辅助函数用于将u16类型数据打包到u8数组中
-static inline void pack_u16_to_bytes(u8 * const buffer, const u16 offset, const u16 value) {
-	*(reinterpret_cast<u16*>(buffer + offset)) = value;
+static inline void pack_u16_to_bytes(u8 *buffer, u16 offset, u16 value) {
+	buffer[offset] = (u8)(value & 0xFF);
+	buffer[offset + 1] = (u8)((value >> 8) & 0xFF);
 }
 
 // 添加辅助函数用于将float类型数据打包到u8数组中
-static inline void pack_float_to_bytes(u8 * const buffer, const u16 offset,
-									   const float_union_t &float_union) {
-	*(reinterpret_cast<u32*>(buffer + offset)) = *(reinterpret_cast<const u32*>(float_union.b));
+static inline void pack_float_to_bytes(u8 *buffer, u16 offset,
+									   float_union_t &float_union) {
+	buffer[offset] = float_union.b[0];
+	buffer[offset + 1] = float_union.b[1];
+	buffer[offset + 2] = float_union.b[2];
+	buffer[offset + 3] = float_union.b[3];
 }
 
 // 添加辅助函数用于从u8数组中解包short类型数据
-static inline short unpack_bytes_to_short(const u8 * const buffer, const u16 offset) {
-	return *(reinterpret_cast<const short*>(buffer + offset));
+static inline short unpack_bytes_to_short(u8 *buffer, u16 offset) {
+	short value = (short)(buffer[offset] | (buffer[offset + 1] << 8));
+	return value;
 }
 
 // 添加辅助函数用于从u8数组中解包u16类型数据
-static inline u16 unpack_bytes_to_u16(const u8 * const buffer, const u16 offset) {
-	return *(reinterpret_cast<const u16*>(buffer + offset));
+static inline u16 unpack_bytes_to_u16(u8 *buffer, u16 offset) {
+	u16 value = (u16)(buffer[offset] | (buffer[offset + 1] << 8));
+	return value;
 }
 
 // 添加辅助函数用于从u8数组中解包float类型数据
-static inline float_union_t unpack_bytes_to_float(const u8 * const buffer, const u16 offset) {
-	static float_union_t float_union;
-	*(reinterpret_cast<u32*>(float_union.b)) = *(reinterpret_cast<const u32*>(buffer + offset));
+static inline float_union_t unpack_bytes_to_float(u8 *buffer, u16 offset) {
+	float_union_t float_union;
+	float_union.b[0] = buffer[offset];
+	float_union.b[1] = buffer[offset + 1];
+	float_union.b[2] = buffer[offset + 2];
+	float_union.b[3] = buffer[offset + 3];
 	return float_union;
 }
 
diff --git a/Robot_V2/device/Timer.cpp b/Robot_V2/device/Timer.cpp
index ec34b61..431df26 100644
--- a/Robot_V2/device/Timer.cpp
+++ b/Robot_V2/device/Timer.cpp
@@ -8,6 +8,7 @@
 #include "gd32c10x_rcu.h"
 #include "gd32c10x_timer.h"
 #include "initial.h"
+#include <cstdio>
 #include <stdbool.h>
 #include <stdint.h>
 #include <stdio.h>
@@ -43,10 +44,8 @@ void nvic_config(void) {
 	/* 设置NVIC优先级分组，1位抢占优先级，3位响应优先级 */
 	nvic_priority_group_set(NVIC_PRIGROUP_PRE2_SUB2);
 	/* 使能TIMER2中断，设置抢占优先级为1，子优先级为1 */
-	nvic_irq_enable(TIMER2_IRQn, 1, 1);
-	nvic_irq_enable(TIMER3_IRQn, 1, 2);
-	nvic_irq_enable(TIMER1_IRQn, 2,
-					1); // 使能TIMER1中断，抢占优先级2，子优先级1
+	nvic_irq_enable(TIMER2_IRQn, 0, 1);
+	nvic_irq_enable(TIMER1_IRQn, 0, 1);
 }
 
 /**
@@ -90,47 +89,6 @@ void TIMER2_config(void) {
 	timer_enable(TIMER2);
 }
 
-/**
- * @brief 配置定时器3
- * @return void
- *
- *配置定时器3，用于诊断恢复
- */
-void TIMER3_config(void) {
-	/* ----------------------------------------------------------------------------
-	TIMER3 Configuration:
-	TIMER3CLK = SystemCoreClock/120 = 1MHz, the period is 50us(50/1000000 =
-	50us).
-	----------------------------------------------------------------------------
-  */
-	timer_parameter_struct timer_struct;
-
-	/* 使能TIMER3时钟 */
-	rcu_periph_clock_enable(RCU_TIMER3);
-
-	/* 复位TIMER3外设到默认状态 */
-	timer_deinit(TIMER3);
-	/* 初始化TIMER初始化参数结构体 */
-	timer_struct_para_init(&timer_struct);
-	/* TIMER3配置参数设置 */
-	timer_struct.prescaler =
-		TIMER3_PRESCALER; /* 预分频值：120分频(从0开始计数) */
-	timer_struct.alignedmode = TIMER_COUNTER_EDGE;	  /* 边沿对齐模式 */
-	timer_struct.counterdirection = TIMER_COUNTER_UP; /* 向上计数模式 */
-	timer_struct.period =
-		TIMER3_PERIOD; /* 自动重装载值：50次计数(从0开始计数) */
-	timer_struct.clockdivision = TIMER_CKDIV_DIV1; /* 时钟分频值：1分频 */
-	timer_init(TIMER3, &timer_struct);
-
-	/* 清除更新中断标志位 */
-	timer_interrupt_flag_clear(TIMER3, TIMER_INT_FLAG_UP);
-	/* 使能更新中断 */
-	timer_interrupt_enable(TIMER3, TIMER_INT_UP);
-
-	/* 使能TIMER3 */
-	timer_enable(TIMER3);
-}
-
 /**
  * @brief   配置TIMER2中断
  * @return void
@@ -155,76 +113,71 @@ 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) {
 		// 清除更新中断标志
 		timer_interrupt_flag_clear(TIMER2, TIMER_INT_UP);
-
 		// 每100us执行一次的操作
-		total_us100_counter++;
 		scheduler.taskFlags |= (1 << TASK_ANGLE_UPDATE) |
 							   (1 << TASK_VELOCITY_UPDATE) |
 							   (1 << TASK_MOTOR_CONTROL);
 
 		switch (total_us100_counter) { // 每1ms执行的代码，负载均衡
-		case 1: {
+		case 0: {
 			scheduler.taskFlags |= (1 << TASK_MCU_TEMP_UPDATE);
 			break;
 		}
-		case 2: {
+		case 1: {
 			scheduler.taskFlags |= (1 << TASK_DRV_TEMP_UPDATE);
 			break;
 		}
-		case 3: {
+		case 2: {
 			scheduler.taskFlags |= (1 << TASK_VBUS_UPDATE);
 			break;
 		}
-		case 4: {
+		case 3: {
+			break;
+		}
+		case 9: {
 			CAN0_transmiter.can0_send_flag = SET;
 			scheduler.taskFlags |= (1 << TASK_CAN_TRANSMIT);
 			break;
 		}
-		case 9: {
-			mode = FOC.control_mode;
-			break;
 		}
-		}
-		if (total_us100_counter >= 10) {
+
+		total_us100_counter++;
+		if (total_us100_counter > 9) {
 			total_us100_counter = 0;
 			// 可以添加每1ms执行的代码，无负载均衡
-			static u8 ms10_counter = 0;
-			ms10_counter++;
-			switch (ms10_counter) { // 每10ms执行的代码，负载均衡
-			case 1: {
+			static u8 ms1_counter = 0;
+
+			switch (ms1_counter) { // 每10ms执行的代码，负载均衡
+			case 0: {
 				scheduler.taskFlags |= (1 << TASK_THERMAL_CONTROL);
 				break;
 			}
-			case 2: {
+			case 1: {
 				scheduler.taskFlags |= (1 << TASK_MAGNETIC_CHECK);
 				break;
 			}
+			case 2: {
+				break;
 			}
-			if (ms10_counter >= 10) {
-				ms10_counter = 0;
+			}
+			ms1_counter++;
+			if (ms1_counter > 9) {
+				ms1_counter = 0;
 				// 可以添加每10ms执行的代码，无负载均衡
-				static u8 ms100_counter = 0;
-				ms100_counter++;
-				switch (ms100_counter) { // 每100ms执行的代码，负载均衡
-				case 1: {
-
-					break;
-				}
-					if (ms100_counter >= 10) {
-						ms100_counter = 0;
-						// 可以添加每100ms执行的代码
-
-						static u8 sec_counter = 0;
-						sec_counter++;
-						if (sec_counter >= 10) {
-							sec_counter = 0;
-							// 可以添加每1s执行的代码
-						}
+				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执行的代码
 					}
 				}
 			}
@@ -232,33 +185,6 @@ void TIMER2_IRQHandler(void) {
 	}
 }
 
-/**
- * @brief   TIMER3中断处理函数
- * @return void
- */
-void TIMER3_IRQHandler(void) {
-	// 检查更新中断标志并处理
-	if (timer_interrupt_flag_get(TIMER3, TIMER_INT_UP) != RESET) {
-		// 清除更新中断标志
-		timer_interrupt_flag_clear(TIMER3, TIMER_INT_UP);
-		static u8 count = 0;
-		count++;
-
-		// 10ms测试代码
-		if (count >= 10) {
-			count = 0;
-			// 100ms测试代码
-
-			static u8 count_10 = 0;
-			count_10++;
-			if (count_10 >= 10) {
-				count_10 = 0;
-				// 1s测试代码
-			}
-		}
-	}
-}
-
 /**
  * @brief  TIMER1 GPIO配置函数
  */
@@ -343,8 +269,6 @@ void timer1_pwm_config(void) {
 
 	// 使能CH3比较中断
 	timer_interrupt_enable(TIMER1, TIMER_INT_CH3);
-	nvic_irq_enable(TIMER1_IRQn, 0,
-					2); // 使能TIMER1中断，抢占优先级2，子优先级1
 
 	// 启动TIMER1
 	timer_enable(TIMER1);
diff --git a/Robot_V2/device/Timer.h b/Robot_V2/device/Timer.h
index 908dd70..ed220de 100644
--- a/Robot_V2/device/Timer.h
+++ b/Robot_V2/device/Timer.h
@@ -11,8 +11,6 @@ void User_timer_init(void);
 void Set_Pwm(uint16_t channel, float duty_cycle);
 void Set_3Pwm(float duty_cycle0, float duty_cycle1,float duty_cycle2);
 void TIMER2_IRQHandler(void);
-void TIMER3_config(void);
-void TIMER3_IRQHandler(void);
 
 void timer1_pwm_config(void);
 void timer1_gpio_config(void);
diff --git a/Robot_V2/device/adc.h b/Robot_V2/device/adc.h
index 244464a..9c126e2 100644
--- a/Robot_V2/device/adc.h
+++ b/Robot_V2/device/adc.h
@@ -7,6 +7,7 @@ extern "C" {
 
 #include "stdint.h"
 constexpr u8 FILTER_SIZE = 6;
+constexpr u8 BLOCK_SIZE = FILTER_SIZE * sizeof(float);
 class sampler {
   private:
 	float vbus_samples[FILTER_SIZE + 1];
@@ -15,6 +16,7 @@ class sampler {
 	float currW_samples[FILTER_SIZE + 1];
 	float mcu_temp_samples[FILTER_SIZE + 1];
 	float drv_temp_samples[FILTER_SIZE + 1];
+	u8 current_sample_index;
 
 	float tempora_sample;
 
diff --git a/Robot_V2/device/ang_sensor.cpp b/Robot_V2/device/ang_sensor.cpp
index 5206dc5..a82d181 100644
--- a/Robot_V2/device/ang_sensor.cpp
+++ b/Robot_V2/device/ang_sensor.cpp
@@ -1,5 +1,6 @@
 #include "ang_sensor.h"
 #include "Filter.h"
+#include "core_cmFunc.h"
 #include "gd32c10x_dma.h"
 #include "gd32c10x_gpio.h"
 #include "gd32c10x_spi.h"
@@ -105,8 +106,8 @@ int spi_rw_half_word_dma(uint32_t spi_periph, uint16_t tx_data,
 	dma_transfer_number_config(DMA0, DMA_CH1, 1);
 	dma_channel_enable(DMA0, DMA_CH1);
 
-	// 等待传输完成，超时时间为1000个循环
-	uint32_t timeout = 1000;
+	// 等待传输完成，超时时间为100个循环
+	uint32_t timeout = 100;
 	while ((!spi_dma_tx_complete || !spi_dma_rx_complete) && --timeout) {
 		// 空循环等待DMA传输完成
 	}
@@ -209,22 +210,31 @@ angle_sensor::angle_sensor(void) { KTH7823_init(); }
  * @brief 读取传感器数据
  * @return 传感器数据
  */
-float angle_sensor::read_sensorAngle(void) {
-	u16 u16data = FAULT_RECEIVE;
+inline float angle_sensor::read_sensorAngle(void) {
+    uint16_t raw_data;
+    
 
-	SPI_CS_ENABLE();
-	// 使用DMA发送命令并接收数据
-	if (!spi_rw_half_word_dma(SPI0, DEFAULT_COMMAND, &u16data)) {
-		SPI_CS_DISABLE();
-		current_angle_deg = u16data * ANGLE_FACTOR;
-		current_angle_rad = u16data * RAD_FACTOR;
-		return current_angle_deg;
-	} else {
-		SPI_CS_DISABLE();
-		// 处理读取错误
+    SPI_CS_ENABLE();	
+    u8 result = spi_rw_half_word_dma(SPI0, DEFAULT_COMMAND, &raw_data);
+    SPI_CS_DISABLE();
+    
+    if (result == 0) {
+        // 单次浮点乘法优化：只计算一次度数，然后转换为弧度
+        float angle_deg = (float)raw_data * ANGLE_FACTOR;
+        current_angle_deg = angle_deg;
+        current_angle_rad = angle_deg * DEG2RAD;  // M_PI/180.0f
+        return angle_deg;
+    }
+    
+    #ifdef DEBUG
+    // 仅在调试模式下输出错误，避免发布版本中的I/O开销
+    if (result != 0) {
+        // 使用更高效的串口输出替代printf（如果可用）
 		printf("Error reading angle from sensor.\n");
-		return FAULT_RECEIVE; // 错误返回
-	}
+    }
+    #endif
+    
+    return (float)FAULT_RECEIVE;
 }
 /**
  * @brief 更新传感器角度（输出级）
diff --git a/Robot_V2/main.cpp b/Robot_V2/main.cpp
index 4b01bf2..0468896 100644
--- a/Robot_V2/main.cpp
+++ b/Robot_V2/main.cpp
@@ -3,41 +3,41 @@
 #include "DengFOC.h"
 #include "Filter.h"
 #include "Timer.h"
+#include "adc.h"
+#include "ang_sensor.h"
 #include "core_cm4.h"
+#include "core_cmFunc.h"
 #include "health.h"
 #include "initial.h"
 #include "sys/health.h"
 #include "sys/systick.h"
 #include "systick.h"
-#include "ang_sensor.h"
-#include "adc.h"
 
 TaskScheduler scheduler = {0};
 
 int main(void) {
-	systick_config();  // 系统时钟初始化
-	USART0_init();	   // 调试串口初始化
-	User_timer_init(); // Timer初始化
-	driver_init();	   // 电机驱动初始化
+	systick_config();							// 系统时钟初始化
+	USART0_init();								// 调试串口初始化
+	User_timer_init();							// Timer初始化
+	driver_init();								// 电机驱动初始化
 	extern FOC_Controlroller FOC;				// FOC控制
 	extern sampler sampler;						// 采样器
 	extern EncoderVelocityKalman Vel_kalman;	// 速度滤波
-	extern EncoderAngleKalman Angle_kalman;	// 角度滤波
+	extern EncoderAngleKalman Angle_kalman;		// 角度滤波
 	extern angle_sensor KTH7823;				// 角度传感器
 	extern CAN_transmiter CAN0_transmiter;		// CAN
 	extern ThermalController thermalController; // 热量控制器
 
 	delay_1ms(5);
-	angsens_MG_check(); // 角度传感器磁感应强度测试
-	watchdog_init(5000);					// 看门狗初始化
+	angsens_MG_check();	 // 角度传感器磁感应强度测试
+	watchdog_init(5000); // 看门狗初始化
 	while (1) {
-		fwdgt_counter_reload(); // 看门狗复位
-
+		__disable_irq();
 		// 保存当前任务标志快照以提高效率并确保一致性
 		u32 currentTasks = scheduler.taskFlags;
-
 		// 一次性清除所有已处理的任务标志
 		scheduler.taskFlags &= ~currentTasks;
+		__enable_irq();
 
 		// 按照优先级和重要性处理任务
 		if (currentTasks & (1 << TASK_CURRENT_UPDATE)) { // 更新电流采样值
@@ -94,6 +94,7 @@ int main(void) {
 		}
 		if (currentTasks & (1 << TASK_CAN_TRANSMIT)) { // CAN通信
 			CAN0_transmiter.communication_check();
+			fwdgt_counter_reload(); // 看门狗复位
 		}
 	}
 }
\ No newline at end of file
diff --git a/Robot_V2/sys/initial.cpp b/Robot_V2/sys/initial.cpp
index ef129e5..0b47d45 100644
--- a/Robot_V2/sys/initial.cpp
+++ b/Robot_V2/sys/initial.cpp
@@ -39,7 +39,6 @@ u8 watchdog_init(u32 time) {
 void driver_init(void) {
 	timer1_gpio_config();
 	timer1_pwm_config();
-	TIMER3_config();
 }
 
 int fputc(int ch, FILE *f) throw() {