From a26dd1cc8eed7757a92940cf7c4d523c93f4f3d8 Mon Sep 17 00:00:00 2001
From: cbb <2625274797@qq.com>
Date: Mon, 9 Mar 2026 01:35:26 +0800
Subject: [PATCH] =?UTF-8?q?feat(can):=20=E5=A2=9E=E5=BC=BACAN=E9=80=9A?=
 =?UTF-8?q?=E4=BF=A1=E7=BC=93=E5=86=B2=E5=8C=BA=E7=AE=A1=E7=90=86=E6=9C=BA?=
 =?UTF-8?q?=E5=88=B6=20-=20=E5=AE=9E=E7=8E=B0=E5=BE=AA=E7=8E=AF=E7=BC=93?=
 =?UTF-8?q?=E5=86=B2=E5=8C=BA=E9=98=9F=E5=88=97=E7=AE=A1=E7=90=86CAN?=
 =?UTF-8?q?=E6=B6=88=E6=81=AF=E6=8E=A5=E6=94=B6=20-=20=E6=B7=BB=E5=8A=A0?=
 =?UTF-8?q?=E7=BC=93=E5=86=B2=E5=8C=BA=E6=BB=A1/=E7=A9=BA=E7=8A=B6?=
 =?UTF-8?q?=E6=80=81=E6=A3=80=E6=9F=A5=E5=87=BD=E6=95=B0=20-=20=E5=AE=9E?=
 =?UTF-8?q?=E7=8E=B0=E6=B6=88=E6=81=AF=E5=87=BA=E9=98=9F=E5=88=97dequeue?=
 =?UTF-8?q?=5Frx=5Fmessage=E5=8A=9F=E8=83=BD=20-=20=E5=9C=A8=E4=B8=AD?=
 =?UTF-8?q?=E6=96=AD=E5=A4=84=E7=90=86=E7=A8=8B=E5=BA=8F=E4=B8=AD=E6=B7=BB?=
 =?UTF-8?q?=E5=8A=A0=E6=B6=88=E6=81=AF=E5=85=A5=E9=98=9F=E5=88=97=E6=9C=BA?=
 =?UTF-8?q?=E5=88=B6=20-=20=E4=BC=98=E5=8C=96CAN=E6=B6=88=E6=81=AF?=
 =?UTF-8?q?=E5=A4=84=E7=90=86=E6=B5=81=E7=A8=8B=EF=BC=8C=E6=94=AF=E6=8C=81?=
 =?UTF-8?q?=E6=89=B9=E9=87=8F=E5=A4=84=E7=90=86?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

refactor(motor): 优化电机控制类结构和安全机制
- 重命名PID控制器参数变量名以提高可读性
- 添加过热关机标志位thermal_shutdown和相关处理函数
- 实现过热关机shut_down_thermal和过热恢复resume_from_thermal功能
- 调整关机逻辑，添加drv_stop调用确保电机完全停止
- 修正电流环控制中的占空比设置，改为单相保持

feat(motor): 实现FOC控制算法改进和零点校准功能
- 实现电机零点校准函数calibrate_zero_position用于位置补偿
- 修正FOC控制器中的电气角度计算，集成KTH7823校准角度补偿
- 新增d轴电流计算功能
- 重构PID参数初始化方式，使用统一的电压限制配置
- 更新SVPWM相电压设置算法，改进角度归一化处理
- 修改缺省扇区输出占空比从0.0f改为0.5f以提高稳定性

fix(control): 修复控制回路方向和频率计算问题
- 修正角度控制回路中的误差计算方向
- 修正开环速度控制中的频率参数使用
- 修正速度环控制中的误差计算方向
- 修正MIT模式下多级PID控制器的参数设置和误差计算

refactor(filter): 修正编码器速度计算算法
- 移除速度计算中的冗余乘法因子2

feat(pid): 扩展PID控制器功能
- 添加reset方法用于重置PID内部状态

chore(build): 更新开发工具版本和ADC配置
- 更新csolution版本至2.13.0
- 调整ADC采样时间配置以优化性能
---
 Robot_V2/Algorithm/FOC.cpp                    | 218 ++++++++++------
 Robot_V2/Algorithm/FOC.h                      |  60 +++--
 Robot_V2/Algorithm/Filter.cpp                 |   2 +-
 Robot_V2/Algorithm/pid.cpp                    |  13 +-
 .../RTE/_Debug_GD32C103CB/RTE_Components.h    |   2 +-
 Robot_V2/device/ADC.cpp                       |   2 +-
 Robot_V2/device/Can.cpp                       | 112 ++++++---
 Robot_V2/device/Can.h                         |  20 +-
 Robot_V2/device/Timer.cpp                     | 236 +++++++++---------
 Robot_V2/device/ang_sensor.cpp                |   8 +-
 Robot_V2/device/ang_sensor.h                  |   2 +
 Robot_V2/main.cpp                             |  56 +++--
 Robot_V2/sys/config.h                         |  85 ++++---
 Robot_V2/sys/health.cpp                       |  11 +-
 14 files changed, 514 insertions(+), 313 deletions(-)

diff --git a/Robot_V2/Algorithm/FOC.cpp b/Robot_V2/Algorithm/FOC.cpp
index dbd74ea..b6e06ae 100644
--- a/Robot_V2/Algorithm/FOC.cpp
+++ b/Robot_V2/Algorithm/FOC.cpp
@@ -7,6 +7,7 @@
 #include "config.h"
 #include "motor.h"
 #include "pid.h"
+#include "systick.h"
 #include <arm_math.h>
 #include <cmath>
 #include <gd32c10x.h>
@@ -24,11 +25,35 @@ extern CurrentKalman Current_kalman;
 #define constrain(value, min, max, target_min, target_max)                     \
 	((value - min) / (max - min) * (target_max - target_min))
 
+// 校准步骤（代码模板）
+void calibrate_zero_position(void) {
+	// Set_3Pwm(.25, 0, 0); // 起劲
+	// delay_1ms(1000);
+	// KTH7823.update();
+
+	// // 改进校准算法：获取当前传感器角度并计算补偿值
+	// float current_sensor_angle = KTH7823.getcurrent_sensorangle();
+	// float electrical_angle = fmodf(current_sensor_angle * PP, 360.0f); //
+	// 计算当前电角度
+
+	// // 计算需要的补偿值，使得当前位置对应0°电角度
+	// KTH7823.calibration_angle = -electrical_angle;
+
+	// // 对补偿角进行归一化处理，确保在[-180, 180]范围内
+	// KTH7823.calibration_angle = fmodf(KTH7823.calibration_angle + 180.0f,
+	// 360.0f); if (KTH7823.calibration_angle < 0) { KTH7823.calibration_angle
+	// += 360.0f;
+	// }
+	// KTH7823.calibration_angle -= 180.0f;
+
+	// Set_3Pwm(0, 0, 0); // 停止
+}
+
 // 通过Ia,Ib,Ic计算Iq,Id(目前仅输出Iq)
 inline float FOC_Controller::cal_Iq_Id(float current_a, float current_b,
 									   float current_c) {
 	// 计算电气角度
-	float angle_el = fmod(shaft_angle * PP, 360.0f);
+	float angle_el = fmod(shaft_angle * PP - KTH7823.calibration_angle, 360.0f);
 
 	// Clarke变换 (使用三相电流)
 	float I_alpha, I_beta;
@@ -41,9 +66,11 @@ inline float FOC_Controller::cal_Iq_Id(float current_a, float current_b,
 	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;
+	I_d = I_beta * sin_el + I_alpha * cos_el;
 
 	// 返回Iq值，乘以1000以转换为mA
 	return Current_kalman.update(I_q * 1000 - KE_FACTOR * rt_velocity);
+	// return I_q * 1000 - KE_FACTOR * rt_velocity;
 }
 
 /**
@@ -53,25 +80,16 @@ inline float FOC_Controller::cal_Iq_Id(float current_a, float current_b,
  */
 u8 FOC_Controller::init() {
 	motor_init(); // 电机驱动初始化
-	vel_loop.set_PID(DEFAULT_vel_loop_p * 100 / voltage_power_supply,
-					 vel_loop_Ki,
-					 vel_loop_Kd); // 设置电机速度控制参数
-	angle_loop.set_PID(DEFAULT_angle_loop_p * 100 / voltage_power_supply,
-					   100.0f,
-					   0.2f); // 设置电机角度控制参数
-	current_loop.set_PID(DEFAULT_current_loop_p * 100 / voltage_power_supply,
-						 current_loop_Ki,
-						 current_loop_Kd); // 设置电机电流控制参数
 
-	vel_inVelAngloop.set_PID(DEFAULT_vel_angloop_p * 100, vel_angloop_Ki,
-							 vel_angloop_Kd); // 设置电机速度控制参数
-	ang_inVelAngloop.set_PID(DEFAULT_ang_angloop_p * 100 / voltage_power_supply,
-							 ang_angloop_Ki,
-							 ang_angloop_Kd); // 设置电机角度控制参数
+	FOC.cur_loop_p = DEFAULT_current_loop_p * 100 / limit_voltage;
+	FOC.cur_MIT_p = DEFAULT_current_MIT_p * 100 / limit_voltage;
+	FOC.vel_loop_p = DEFAULT_vel_loop_p * 100 / limit_voltage;
+	FOC.vel_angloop_p = DEFAULT_vel_angloop_p * 100;
+	FOC.vel_MIT_p = DEFAULT_vel_MIT_p * 100;
+	FOC.ang_loop_p = DEFAULT_angle_loop_p * 100 / limit_voltage;
+	FOC.ang_angloop_p = DEFAULT_angle_angloop_p * 100 / limit_voltage;
+	FOC.ang_MIT_p = DEFAULT_angle_MIT_p * 100 / limit_voltage;
 
-	vel_inMITloop.set_PID(DEFAULT_ang_angloop_p * 100 / voltage_power_supply,
-						  vel_MIT_Ki,
-						  vel_MIT_Kd); // 设置电机角度控制参数
 	return 0;
 }
 
@@ -126,14 +144,17 @@ inline void FOC_Controller::setPhaseVoltage(float Uq, float Ud) {
 }
 
 /**
- * @brief 使用SVPWM算法设置电机输出（优化版）
+ * @brief 使用SVPWM算法设置电机输出
  *
  * @param Uq: q轴电压
  * @param Ud: d轴电压
  */
 inline void FOC_Controller::setSVPWM_PhaseVoltage(float Uq, float Ud) {
-	// 计算电气角度（保持原有角度单位和范围）
-	float angle_el = fmod(shaft_angle * PP, 360.0f);
+	// 计算电气角度
+	float angle_el = shaft_angle * PP + KTH7823.calibration_angle;
+
+	// 对电角度进行归一化，确保在[0, 360)范围内
+	angle_el = fmodf(angle_el, 360.0f);
 
 	// 帕克逆变换
 	float sin_el, cos_el;
@@ -143,7 +164,7 @@ inline void FOC_Controller::setSVPWM_PhaseVoltage(float Uq, float Ud) {
 
 	// 计算角度并归一化到 [0, 2π)
 	float angle = atan2f(Ubeta, Ualpha);
-	if (angle < 0)
+	if (angle < 0.0f)
 		angle += 2.0f * PI;
 
 	// 扇区与局部角度
@@ -213,9 +234,9 @@ inline void FOC_Controller::setSVPWM_PhaseVoltage(float Uq, float Ud) {
 		duty_c = 0.5f * T0 + T1;
 		break;
 	default:
-		duty_a = 0.0f;
-		duty_b = 0.0f;
-		duty_c = 0.0f;
+		duty_a = 0.5f;
+		duty_b = 0.5f;
+		duty_c = 0.5f;
 		break;
 	}
 
@@ -224,12 +245,36 @@ inline void FOC_Controller::setSVPWM_PhaseVoltage(float Uq, float Ud) {
 }
 
 /**
- * @brief 电机关机
+ * @brief 电机关机（通用关机，不带原因标记）
  *
  */
 void FOC_Controller::shut_down() {
-	is_shut_down = 1;
+	drv_stop();
 	motor_sleep();
+	is_shut_down = true;
+}
+
+/**
+ * @brief 过热关机
+ *
+ */
+void FOC_Controller::shut_down_thermal() {
+	drv_stop();
+	motor_sleep();
+	is_shut_down = true;
+	thermal_shutdown = true;
+}
+
+/**
+ * @brief 过热恢复
+ *
+ */
+void FOC_Controller::resume_from_thermal() {
+	thermal_shutdown = false;
+	if (!voltage_error && !over_current) {
+		is_shut_down = false;
+		motor_resume();
+	}
 }
 /**
  * @brief 占空比置0
@@ -246,7 +291,7 @@ float FOC_Controller::drv_hold() {
 	} else if (HOLD_PWM_DUTY < 0.0f) {
 		hold_pwm_duty = 0.0f;
 	}
-	Set_3Pwm(hold_pwm_duty, hold_pwm_duty, hold_pwm_duty);
+	Set_3Pwm(hold_pwm_duty, 0, 0);
 	return hold_pwm_duty;
 }
 /**
@@ -283,17 +328,17 @@ void FOC_Controller::update_power(float voltage) {
 float FOC_Controller::velocity_open_loop() {
 	// 通过时间间隔和目标速度计算需要转动的机械角度，存储在shaft_angle中，在此之前，还需要对轴角度归一化
 	shaft_angle = fmod(shaft_angle + target_velocity * REDUCTION_RATIO * 1.0 /
-										 TIMER2_FREQUENCY,
+										 MOTOR_FREQUENCY / 2,
 					   360.0f);
+	// 获取电机轴角度
+	float shaft_angle = KTH7823.getcurrent_sensorangle();
+	CAN0_transmiter.testfloat_union.f = shaft_angle;
+	rt_current = cal_Iq_Id(sampler.get_current('U'), sampler.get_current('V'),
+						   sampler.get_current('W'));
 
 	// 因为在开环控制中，Ud始终为0，只控制Uq
 	const float Uq = voltage_power_supply / 2.0f;
-
 	setSVPWM_PhaseVoltage(Uq, 0.0f);
-	// CAN0_transmiter.testfloat_union.f = Uq;
-	// cal_Iq_Id(sampler.get_current('U'), sampler.get_current('V'),
-	// 		  sampler.get_current('W'));
-
 	return limit_voltage;
 }
 
@@ -305,7 +350,7 @@ float FOC_Controller::velocity_open_loop() {
  */
 float FOC_Controller::angle_control() {
 	// PID 控制计算
-	float Uq = angle_loop(target_angle - rt_angle);
+	float Uq = angle_loop(rt_angle - target_angle);
 
 	// 获取电机轴角度
 	shaft_angle = KTH7823.getcurrent_sensorangle();
@@ -316,6 +361,46 @@ float FOC_Controller::angle_control() {
 	return shaft_angle;
 }
 
+/**
+ * @brief 速度闭环控制
+ *
+ * @param target_velocity: 目标速度
+ * @return float: 当前速度
+ */
+float FOC_Controller::velocity_control() {
+	// PID 控制计算
+	float Uq = vel_loop((target_velocity - rt_velocity) * REDUCTION_RATIO);
+	CAN0_transmiter.testfloat_union.f = Uq;
+
+	// 获取电机轴角度
+	shaft_angle = KTH7823.getcurrent_sensorangle();
+	// 设置相电压
+	setSVPWM_PhaseVoltage(Uq, 0.0f);
+	rt_current = cal_Iq_Id(sampler.get_current('U'), sampler.get_current('V'),
+						   sampler.get_current('W'));
+
+	return rt_velocity;
+}
+
+float FOC_Controller::current_control() {
+	// PID 控制计算
+	float Iq = rt_current =
+		cal_Iq_Id(sampler.get_current('U'), sampler.get_current('V'),
+				  sampler.get_current('W'));
+	float Uq = current_loop(target_current - Iq);
+	if (Uq > limit_voltage) {
+		Uq = limit_voltage;
+	} else if (Uq < -limit_voltage) {
+		Uq = -limit_voltage;
+	}
+	// 获取电机轴角度
+	shaft_angle = KTH7823.getcurrent_sensorangle();
+	CAN0_transmiter.testfloat_union.f = shaft_angle;
+	// 设置相电压
+	setSVPWM_PhaseVoltage(Uq, 0.0f);
+	return 1;
+}
+
 /**
  * @brief 带速度限制的角度闭环控制
  *
@@ -327,6 +412,7 @@ float FOC_Controller::angle_control_with_velocity_limit() {
 	// 计算角度误差
 	float angle_error = target_angle - rt_angle;
 
+	ang_inVelAngloop.set_P(ang_angloop_p);
 	// 使用角度PID控制器计算目标速度
 	float target_velocity = ang_inVelAngloop(angle_error);
 
@@ -339,13 +425,24 @@ float FOC_Controller::angle_control_with_velocity_limit() {
 	} else {
 		vel_inVelAngloop.set_P(vel_angloop_p);
 	}
-	float Uq = vel_inVelAngloop(rt_velocity - target_velocity);
+	float Uq = vel_inVelAngloop(target_velocity - rt_velocity);
+	CAN0_transmiter.testfloat_union.f = Uq;
+
+	// // 实现力度控制：如果电流超过目标电流，则限制输出
+	// if (abs(rt_current) > abs(target_current)) {
+	// 	// 当实际电流超过目标电流时，减小输出以限制电流
+	// 	// 可以采用比例限制或直接限制输出电压的方式
+	// 	float current_ratio = abs(target_current) / abs(rt_current);
+	// 	if (current_ratio < 1.0f) {
+	// 		Uq *= current_ratio;  // 按比例缩小输出电压
+	// 	}
+	// }
 
-	rt_current = cal_Iq_Id(sampler.get_current('U'), sampler.get_current('V'),
-						   sampler.get_current('W'));
 	// 获取电机轴角度
 	shaft_angle = KTH7823.getcurrent_sensorangle();
-
+	// 获取当前实际电流值
+	rt_current = cal_Iq_Id(sampler.get_current('U'), sampler.get_current('V'),
+						   sampler.get_current('W'));
 	// 设置相电压
 	setSVPWM_PhaseVoltage(Uq, 0.0f);
 
@@ -363,13 +460,13 @@ float FOC_Controller::MIT_Mode() {
 	// 第一层：角度环（外环）
 	// 根据目标角度和实际角度的差值，通过角度环PID控制器计算目标速度
 	float angle_error = target_angle - rt_angle;
+	ang_inMITloop.set_P(ang_MIT_p);
 	float target_velocity =
-		ang_inVelAngloop(angle_error); // 使用angle_loop作为角度到速度的映射
-	CAN0_transmiter.testfloat_union.f = target_velocity;
+		ang_inMITloop(angle_error); // 使用angle_loop作为角度到速度的映射
 
 	// 第二层：速度环（中环）
 	// 根据目标速度和实际速度的差值，通过速度环PID控制器计算目标电流
-	float velocity_error = rt_velocity - target_velocity;
+	float velocity_error = (rt_velocity - target_velocity);
 	vel_inMITloop.set_P(vel_MIT_p);
 	float target_current = vel_inMITloop(velocity_error);
 
@@ -380,7 +477,8 @@ float FOC_Controller::MIT_Mode() {
 
 	// 通过电流环PID控制器计算输出电压
 	float current_error = target_current - rt_current;
-	float Uq = current_loop(current_error);
+	current_inMITloop.set_P(cur_MIT_p);
+	float Uq = current_inMITloop(current_error);
 
 	// 获取电机轴角度
 	shaft_angle = KTH7823.getcurrent_sensorangle();
@@ -390,37 +488,3 @@ float FOC_Controller::MIT_Mode() {
 
 	return shaft_angle;
 }
-
-/**
- * @brief 速度闭环控制
- *
- * @param target_velocity: 目标速度
- * @return float: 当前速度
- */
-float FOC_Controller::velocity_control() {
-	// PID 控制计算
-	float Uq = vel_loop((rt_velocity - target_velocity) * 25);
-
-	// 获取电机轴角度
-	shaft_angle = KTH7823.getcurrent_sensorangle();
-	// 设置相电压
-	setSVPWM_PhaseVoltage(Uq, 0.0f);
-	rt_current = cal_Iq_Id(sampler.get_current('U'), sampler.get_current('V'),
-						   sampler.get_current('W'));
-
-	return rt_velocity;
-}
-
-float FOC_Controller::current_control() {
-	// PID 控制计算
-	float Iq = rt_current =
-		cal_Iq_Id(sampler.get_current('U'), sampler.get_current('V'),
-				  sampler.get_current('W'));
-	current_loop.set_P(current_loop_p);
-	float Uq = current_loop(target_current - Iq);
-	// 获取电机轴角度
-	shaft_angle = KTH7823.getcurrent_sensorangle();
-	// 设置相电压
-	setSVPWM_PhaseVoltage(Uq, 0.0f);
-	return 1;
-}
\ No newline at end of file
diff --git a/Robot_V2/Algorithm/FOC.h b/Robot_V2/Algorithm/FOC.h
index 266c532..e2f525e 100644
--- a/Robot_V2/Algorithm/FOC.h
+++ b/Robot_V2/Algorithm/FOC.h
@@ -27,20 +27,23 @@ class FOC_Controller {
 	bool over_current = false;
 	bool voltage_error = false;
 	bool is_shut_down = false;
+	bool thermal_shutdown = false; // 过热关机标志
+
 	float voltage_power_supply = 24.0f; // 电源电压
-	float v_safe = voltage_power_supply * 1.05f;
+	float v_safe = voltage_power_supply / 1.05f;
 	float duty_a = 0, duty_b = 0, duty_c = 0;
 	float Ua = 0, Ub = 0, Uc = 0;
 
-	float current_loop_p=DEFAULT_current_loop_p;
+	float cur_loop_p = DEFAULT_current_loop_p;
+	float vel_loop_p = DEFAULT_vel_loop_p;
+	float ang_loop_p = DEFAULT_angle_loop_p;
 
-	float vel_angloop_p=DEFAULT_vel_angloop_p;
-	float vel_loop_p=DEFAULT_vel_loop_p;
+	float vel_angloop_p = DEFAULT_vel_angloop_p;
+	float ang_angloop_p = DEFAULT_angle_angloop_p;
 
-	float ang_angloop_p=DEFAULT_ang_angloop_p;	
-	float angle_loop_p=DEFAULT_angle_loop_p;
-	
-	float vel_MIT_p=DEFAULT_vel_MIT_p;
+	float ang_MIT_p = DEFAULT_angle_MIT_p;
+	float vel_MIT_p = DEFAULT_vel_MIT_p;
+	float cur_MIT_p = DEFAULT_current_MIT_p;
 
 	float velocity_open_loop(void);
 	float angle_control(void);
@@ -55,24 +58,35 @@ class FOC_Controller {
 	void resume(void);
 	void drv_stop(void);
 	float drv_hold(void);
+	void shut_down_thermal(void);	// 过热关机
+	void resume_from_thermal(void); // 过热恢复
 
+	void setSVPWM_PhaseVoltage(float Uq, float Ud);
 	// 此处默认值不作数
-	PIDController vel_loop = PIDController{vel_loop_p * 100, 0.0f, 0, 10000,
-										   voltage_power_supply}; // 12V
-	PIDController angle_loop = PIDController{angle_loop_p * 100, 0, 0, 10000000,
-											 voltage_power_supply}; // 12V
-	PIDController current_loop = PIDController{current_loop_p * 100, 0, 0, 1500,
-											   voltage_power_supply}; // 12V
-	PIDController vel_inVelAngloop =	
-		PIDController{vel_angloop_p * 100, 0, 0, 1e6, v_safe};
+	PIDController vel_loop =
+		PIDController{DEFAULT_vel_loop_p * 100 / limit_voltage, vel_loop_Ki,
+					  vel_loop_Kd, 10000, voltage_power_supply}; // 12V
+	PIDController angle_loop =
+		PIDController{DEFAULT_angle_loop_p * 100 / limit_voltage, 100.0f, 0.2f,
+					  10000000, voltage_power_supply}; // 12V
+	PIDController current_loop = PIDController{
+		DEFAULT_current_loop_p * 100 / limit_voltage, current_loop_Ki,
+		current_loop_Kd, 1500, voltage_power_supply}; // 12V
+	PIDController vel_inVelAngloop =
+		PIDController{DEFAULT_vel_angloop_p * 100, vel_angloop_Ki,
+					  vel_angloop_Kd, 1e6, v_safe};
 	PIDController ang_inVelAngloop =
-		PIDController{ang_angloop_p * 100, 0, 0, 1e4, 100};
+		PIDController{DEFAULT_angle_angloop_p * 100 / voltage_power_supply,
+					  ang_angloop_Ki, ang_angloop_Kd, 1e4, 100};
 
-		
 	PIDController ang_inMITloop =
-		PIDController{ang_angloop_p * 100, 0, 0, 1e4, 100};
-	PIDController vel_inMITloop =
-		PIDController{vel_MIT_p * 100, 0, 0, 1e9, 6000};
+		PIDController{DEFAULT_angle_MIT_p * 100 / limit_voltage, angle_MIT_Ki,
+					  angle_MIT_Kd, 1e5, 100};
+	PIDController vel_inMITloop = PIDController{
+		DEFAULT_vel_MIT_p * 100, vel_MIT_Ki, vel_MIT_Kd, 1e9, 1000};
+	PIDController current_inMITloop = PIDController{
+		DEFAULT_current_loop_p * 100 / limit_voltage, current_loop_Ki,
+		current_loop_Kd, 1500, voltage_power_supply}; // 12V
 
 	void update_pid_limits() {
 		vel_loop.limit = voltage_power_supply;
@@ -86,16 +100,14 @@ class FOC_Controller {
 	float Ualpha, Ubeta = 0;
 	float zero_electric_angle = 0;
 	float v_half = voltage_power_supply * 0.5f; // 电源电压的一半
-	float startup_ramp_val = 0.0f;      // 软启动系数
-	float startup_ramp_step = 0.01f;    // 启动斜坡步长，可调节以控制启动速度
 	void setPhaseVoltage(float Uq, float Ud);
-	void setSVPWM_PhaseVoltage(float Uq, float Ud);
 	float cal_Iq_Id(float current_a, float current_b, float current_c);
 };
 
 // 定义函数指针类型
 typedef float (FOC_Controller::*FOCFunctionPtr)(void);
 
+void calibrate_zero_position(void);
 inline float _normalizeAngle(float angle);
 inline float _electricalAngle();
 
diff --git a/Robot_V2/Algorithm/Filter.cpp b/Robot_V2/Algorithm/Filter.cpp
index a6d3a2c..9ca37ee 100644
--- a/Robot_V2/Algorithm/Filter.cpp
+++ b/Robot_V2/Algorithm/Filter.cpp
@@ -145,7 +145,7 @@ float EncoderAngleKalman::getVelocity() const {
 	float vel = 0.0f;
 	if (time_counts > 0) {
 		vel = (angle_now - velocity_last_angle) * TIMER2_FREQUENCY /
-			  time_counts * 2;
+			  time_counts ;
 	}
 
 	velocity_last_angle = angle_now;
diff --git a/Robot_V2/Algorithm/pid.cpp b/Robot_V2/Algorithm/pid.cpp
index 213a1db..1bedb2e 100644
--- a/Robot_V2/Algorithm/pid.cpp
+++ b/Robot_V2/Algorithm/pid.cpp
@@ -76,4 +76,15 @@ void PIDController::set_P(float pp) { this->P = pp; }
  * @param limit: 输出限幅
  * @return void
  */
-void PIDController::set_limit(float limit) { this->limit = limit; }
\ No newline at end of file
+void PIDController::set_limit(float limit) { this->limit = limit; }
+
+/**
+ * @brief 重置PID控制器状态
+ *
+ * @return void
+ */
+void PIDController::reset() {
+	error_prev = 0.0f;
+	output_prev = 0.0f;
+	integral_prev = 0.0f;
+}
diff --git a/Robot_V2/RTE/_Debug_GD32C103CB/RTE_Components.h b/Robot_V2/RTE/_Debug_GD32C103CB/RTE_Components.h
index 5b9eec1..4d6d49e 100644
--- a/Robot_V2/RTE/_Debug_GD32C103CB/RTE_Components.h
+++ b/Robot_V2/RTE/_Debug_GD32C103CB/RTE_Components.h
@@ -1,6 +1,6 @@
 /*
  * CSOLUTION generated file: DO NOT EDIT!
- * Generated by: csolution version 2.12.0+p41-gfeadfc5e
+ * Generated by: csolution version 2.13.0+p3-g40b6564e
  *
  * Project: 'Robot_V2.Debug+GD32C103CB' 
  * Target:  'Debug+GD32C103CB' 
diff --git a/Robot_V2/device/ADC.cpp b/Robot_V2/device/ADC.cpp
index 7a0ea2d..f980a31 100644
--- a/Robot_V2/device/ADC.cpp
+++ b/Robot_V2/device/ADC.cpp
@@ -81,7 +81,7 @@ void adc_config(void) {
 	adc_channel_length_config(ADC0, ADC_REGULAR_CHANNEL, 6);
 	/* 配置ADC规则通道 */
 	adc_regular_channel_config(ADC0, 0, ADC_CHANNEL_16,
-							   ADC_SAMPLETIME_239POINT5);
+							   ADC_SAMPLETIME_55POINT5);
 	adc_regular_channel_config(ADC0, 1, ADC_CHANNEL_9, ADC_SAMPLETIME_28POINT5);
 	adc_regular_channel_config(ADC0, 2, ADC_CHANNEL_3, ADC_SAMPLETIME_28POINT5);
 	adc_regular_channel_config(ADC0, 3, ADC_CHANNEL_4, ADC_SAMPLETIME_28POINT5);
diff --git a/Robot_V2/device/Can.cpp b/Robot_V2/device/Can.cpp
index c548084..9f6a848 100644
--- a/Robot_V2/device/Can.cpp
+++ b/Robot_V2/device/Can.cpp
@@ -19,8 +19,6 @@
 #include <random>
 
 constexpr u16 CANX = CAN0;
-
-CAN_transmiter CAN0_transmiter; // CAN初始化
 extern FOC_Controller FOC;
 extern sampler sampler;
 extern angle_sensor KTH7823;
@@ -35,6 +33,51 @@ dma_parameter_struct dma_tx_parameter;
 uint8_t can_rx_buffer[8]; // CAN接收缓冲区
 uint8_t can_tx_buffer[8]; // CAN发送缓冲区
 
+CAN_transmiter CAN0_transmiter; // CAN初始化
+
+// CAN接收缓冲区相关函数实现
+/*!
+	\brief      检查CAN接收缓冲区是否已满
+	\param[in]  none
+	\param[out] none
+	\retval     bool 返回true表示缓冲区已满，false表示未满
+	\note       通过比较头指针的下一个位置和尾指针来判断缓冲区是否已满
+				使用循环缓冲区策略，当(rx_buffer_head + 1) %
+   CAN_RX_BUFFER_SIZE等于rx_buffer_tail时， 表示缓冲区已达到最大容量
+*/
+bool CAN_transmiter::is_rx_buffer_full() {
+	return ((rx_buffer_head + 1) % CAN_RX_BUFFER_SIZE) == rx_buffer_tail;
+}
+
+/*!
+	\brief      检查CAN接收缓冲区是否为空
+	\param[in]  none
+	\param[out] none
+	\retval     bool 返回true表示缓冲区为空，false表示缓冲区不为空
+	\note       通过比较头指针和尾指针是否相等来判断缓冲区是否为空
+*/
+bool CAN_transmiter::is_rx_buffer_empty() {
+	return rx_buffer_head == rx_buffer_tail;
+}
+
+/**
+ * @brief 从接收缓冲区中取出一条CAN消息
+ *
+ * 该函数检查接收缓冲区是否为空，如果不为空则取出缓冲区队列头部的消息，
+ * 并更新队列尾部索引。如果缓冲区为空，则返回false。
+ *
+ * @param message 引用类型的输出参数，用于存储取出的CAN接收消息结构体
+ * @return bool 返回操作是否成功，true表示成功取出消息，false表示缓冲区为空
+ */
+bool CAN_transmiter::dequeue_rx_message(can_receive_message_struct &message) {
+	if (!is_rx_buffer_empty()) {
+		message = rx_message_buffer[rx_buffer_tail];
+		rx_buffer_tail = (rx_buffer_tail + 1) % CAN_RX_BUFFER_SIZE;
+		return true;
+	}
+	return false;
+}
+
 /*!
 	\brief      initialize CAN GPIO
 	\param[in]  none
@@ -521,30 +564,32 @@ void CAN_transmiter::communication_check() {
 		can0_send_flag = RESET;
 	}
 
-	// CAN0 接收数据标志
-	if (SET == can0_receive_flag) {
-		// printf("\r\n can0 %X receive data:", receive_message.rx_efid);
-		// for (i = 0; i < receive_message.rx_dlen; i++) {
-		// 	printf(" %02x", receive_message.rx_data[i]);
+	// 处理所有接收到的CAN消息（从环形缓冲区中）
+	can_receive_message_struct current_message;
+	while (dequeue_rx_message(current_message)) {
+		// printf("\r\n can0 %X receive data:", current_message.rx_efid);
+		// for (i = 0; i < current_message.rx_dlen; i++) {
+		// 	printf(" %02x", current_message.rx_data[i]);
 		// }
-		if (receive_message.rx_efid == CANFD_CMD_ID) {
+		if (current_message.rx_efid == CANFD_CMD_ID) {
 			// 控制模式
-			FOC.control_mode = receive_message.rx_data[0];
+			FOC.control_mode = current_message.rx_data[0];
 			// 目标电压限制
 			FOC.limit_voltage =
-				float(unpack_bytes_to_u16(receive_message.rx_data, 1)) / 1000;
+				float(unpack_bytes_to_u16(current_message.rx_data, 1)) / 1000;
 			// 目标电流
 			target_current_union.f =
-				unpack_bytes_to_float(receive_message.rx_data, 3).f;
+				unpack_bytes_to_float(current_message.rx_data, 3).f;
 			FOC.current_loop.set_limit(target_current_union.f);
 			FOC.vel_inMITloop.set_limit(target_current_union.f);
 			// 目标速度
 			target_velocity_union.f =
-				unpack_bytes_to_float(receive_message.rx_data, 7).f;
+				unpack_bytes_to_float(current_message.rx_data, 7).f;
 			FOC.ang_inVelAngloop.set_limit(target_velocity_union.f);
+			FOC.ang_inMITloop.set_limit(target_velocity_union.f);
 			// 目标角度
 			target_angle_union.f =
-				unpack_bytes_to_float(receive_message.rx_data, 11).f;
+				unpack_bytes_to_float(current_message.rx_data, 11).f;
 			// 赋值
 			FOC.target_angle = target_angle_union.f;
 			FOC.target_velocity = target_velocity_union.f;
@@ -553,26 +598,26 @@ void CAN_transmiter::communication_check() {
 			// 解析PID参数
 			float voltage = FOC.limit_voltage;
 			// PID参数
-			u16 P_config = unpack_bytes_to_u16(receive_message.rx_data, 15);
-			FOC.current_loop_p = DEFAULT_current_loop_p * P_config / voltage;
+			u16 P_config = unpack_bytes_to_u16(current_message.rx_data, 15);
+			FOC.cur_loop_p = DEFAULT_current_loop_p * P_config / voltage;
+			FOC.cur_MIT_p = DEFAULT_current_MIT_p * P_config / voltage;
 
-			P_config = unpack_bytes_to_u16(receive_message.rx_data, 17);
+			P_config = unpack_bytes_to_u16(current_message.rx_data, 17);
 			FOC.vel_loop_p = DEFAULT_vel_loop_p * P_config / voltage;
-
-			P_config = unpack_bytes_to_u16(receive_message.rx_data, 19);
-			FOC.angle_loop_p = DEFAULT_angle_loop_p * P_config / voltage;
-			FOC.ang_angloop_p = DEFAULT_ang_angloop_p * P_config / voltage;
-
-			P_config = unpack_bytes_to_u16(receive_message.rx_data, 21);
 			FOC.vel_angloop_p = DEFAULT_vel_angloop_p * P_config;
 			FOC.vel_MIT_p = DEFAULT_vel_MIT_p * P_config;
 
-		} else if (receive_message.rx_efid == CANFD_RESET_ID) {
-			u8 first_byte = receive_message.rx_data[0];
-			u8 eigth_byte = receive_message.rx_data[7];
+			P_config = unpack_bytes_to_u16(current_message.rx_data, 19);
+			FOC.ang_loop_p = DEFAULT_angle_loop_p * P_config / voltage;
+			FOC.ang_angloop_p = DEFAULT_angle_angloop_p * P_config / voltage;
+			FOC.ang_MIT_p = DEFAULT_angle_MIT_p * P_config / voltage;
+		} else if (current_message.rx_efid == CANFD_RESET_ID) {
+			u8 first_byte = current_message.rx_data[0];
+			u8 eigth_byte = current_message.rx_data[7];
 			// 电机重置
 			if (first_byte & 0x01) {
 				motor_reset();
+				testfloat_union.f = KTH7823.getcurrent_sensorangle();
 			}
 			// EEPROM重置
 			if (first_byte >> 1 & 0x01) {
@@ -593,16 +638,17 @@ void CAN_transmiter::communication_check() {
 				app_prepare_deep_sleep(true);
 				// CAN_Sleep();
 			}
-		} else if (receive_message.rx_efid == CANFD_VERSION_ID) {
+		} else if (current_message.rx_efid == CANFD_VERSION_ID) {
 			// 处理版本号请求，发送当前系统版本信息
 			Version.sendVersionInfoViaCan();
-		} else if (receive_message.rx_efid == CANFD_BOOT_ID) {
+		} else if (current_message.rx_efid == CANFD_BOOT_ID) {
+			motor_sleep();
 			flashEEP.save_system_config(false);
 			fmc_unlock();
 			OTA_SET(n_APP_LATEST_EN_ADDR, OTA_NOT_AVAL); // 设置APP为不激活
 			fmc_lock();
 			jumpToBOOT();
-		} else if (receive_message.rx_efid == 0x00000102) {
+		} else if (current_message.rx_efid == 0x00000102) {
 			volatile u32 *flash_ptr = (volatile u32 *)n_APP_LATEST_EN_ADDR;
 			u32 app_enable = *flash_ptr;
 			flash_ptr = (volatile u32 *)n_APP_LATEST_CHECK_ADDR;
@@ -614,9 +660,8 @@ void CAN_transmiter::communication_check() {
 				CANFD_AsyncSend(CANFD_OTA_ACK_ID, 2, transmit_data);
 			}
 		} else {
-			printf("Unknown CAN ID: %X\n", receive_message.rx_efid);
+			printf("Unknown CAN ID: %X\n", current_message.rx_efid);
 		}
-		can0_receive_flag = RESET;
 	}
 }
 
@@ -662,10 +707,11 @@ void CAN0_RX0_IRQHandler(void) {
 		dma_channel_enable(DMA0, DMA_CH4);
 
 		// 读取接收到的数据
-		can_message_receive(CAN0, CAN_FIFO0, &CAN0_transmiter.receive_message);
+		can_receive_message_struct temp_message;
+		can_message_receive(CAN0, CAN_FIFO0, &temp_message);
 
-		// 接收标志位置1
-		CAN0_transmiter.can0_receive_flag = SET;
+		// 将接收到的消息加入环形缓冲区
+		CAN0_transmiter.enqueue_rx_message_from_isr(temp_message);
 	}
 
 	// 检查其他可能的中断源
diff --git a/Robot_V2/device/Can.h b/Robot_V2/device/Can.h
index 0b4c2f9..09beca5 100644
--- a/Robot_V2/device/Can.h
+++ b/Robot_V2/device/Can.h
@@ -71,6 +71,8 @@ void can_loopback_test(void);
 class CAN_transmiter {
 
   private:
+	static const uint8_t CAN_RX_BUFFER_SIZE = 8; // CAN接收缓冲区大小
+	
 	bool is_sleep = false; // 是否休眠
 
 	u8 Target_Control_Mode = 0; // 目标控制模式
@@ -100,16 +102,23 @@ class CAN_transmiter {
 	u8 transmit_mailbox = CAN_NOMAILBOX;		  // 初始化为无mailbox状态
 	u8 transmit_data[64];						  // CANFD最大64字节
 
+	// CAN接收消息队列
+	can_receive_message_struct rx_message_buffer[CAN_RX_BUFFER_SIZE];
+	volatile uint8_t rx_buffer_head = 0;
+	volatile uint8_t rx_buffer_tail = 0;
+	
 	void update();		// 更新数据
 	u8 CAN_Receive();	// CAN接收
 	u8 CANFD_Receive(); // CANFD接收
+	bool is_rx_buffer_full();
+	bool is_rx_buffer_empty();
+	bool dequeue_rx_message(can_receive_message_struct& message);
 
   public:
 	// 添加构造函数声明
 	u8 init(void);
 
 	can_receive_message_struct receive_message; // 接收消息结构体
-	FlagStatus can0_receive_flag;				// CAN0接收标志
 	FlagStatus can0_send_flag;					// CAN0发送标志
 	float_union_t testfloat_union = {0};		// 测试用
 	u16 test_u16 = 0;
@@ -123,6 +132,15 @@ class CAN_transmiter {
 	u8 CANFD_AsyncSend(u32 ID, u8 length, u8 *data); // CANFD异步发送(使用DMA)
 	u8 CAN_all_Send();								 // CAN发送
 	u8 CANFD_all_Send();							 // CANFD发送
+	
+	// 中断处理函数需要访问的公共接口
+	void enqueue_rx_message_from_isr(const can_receive_message_struct& message) {
+		if (!is_rx_buffer_full()) {
+			rx_message_buffer[rx_buffer_head] = message;
+			rx_buffer_head = (rx_buffer_head + 1) % CAN_RX_BUFFER_SIZE;
+		}
+		// 如果缓冲区满，丢弃新消息
+	}
 };
 
 #ifdef __cplusplus
diff --git a/Robot_V2/device/Timer.cpp b/Robot_V2/device/Timer.cpp
index 828f338..c5377d5 100644
--- a/Robot_V2/device/Timer.cpp
+++ b/Robot_V2/device/Timer.cpp
@@ -9,7 +9,7 @@
 #include "gd32c10x_timer.h"
 #include "initial.h"
 
-volatile u32 total_us100_counter = 0; // 总计数器，每100微秒增加1
+volatile u32 total_us100_counter = 0; // 总计数器，每 100 微秒增加 1
 extern u16 adc_value[7];
 extern FOC_Controller FOC;
 extern TaskScheduler scheduler;
@@ -17,25 +17,56 @@ extern CAN_transmiter CAN0_transmiter;
 // 添加系统运行时间计数器
 extern u32 cnt_test;
 
+// 任务调度查找表 - 每 100us 执行的任务 (1ms 周期内)
+static const u32 task_schedule_100us[10] = {
+    (1u << TASK_MCU_TEMP_UPDATE),      // 0: MCU 温度更新
+    (1u << TASK_DRV_TEMP_UPDATE),      // 1: 驱动温度更新
+    (1u << TASK_VBUS_UPDATE),          // 2: 电压更新
+    0,                                  // 3: 空闲
+    0,                                  // 4: 空闲
+    0,                                  // 5: 空闲
+    0,                                  // 6: 空闲
+    0,                                  // 7: 空闲
+    0,                                  // 8: 空闲
+    (1u << TASK_CAN_TRANSMIT)          // 9: CAN 发送
+};
+
+// 任务调度查找表 - 每 1ms 执行的任务 (10ms 周期内)
+static const u32 task_schedule_1ms[10] = {
+    (1u << TASK_THERMAL_CONTROL),      // 0: 热量控制
+    (1u << TASK_MAGNETIC_CHECK),       // 1: 磁感应强度检测
+    0, 0, 0, 0, 0, 0, 0, 0             // 空闲
+};
+
+// 任务调度查找表 - 每 10ms 执行的任务 (100ms 周期内)
+static const u32 task_schedule_10ms[10] = {
+    0, 0, 0, 0, 0, 0, 0, 0, 0, 0       // 当前无任务，预留扩展
+};
+
+// 任务调度查找表 - 每 100ms 执行的任务 (1s 周期内)
+static const u32 task_schedule_100ms[10] = {
+    0, 0, 0, 0, 0, 0, 0, 0, 0, 0       // 当前无任务，预留扩展
+};
+
 /**
  * @brief 设置引脚中断优先级
  * @return void
  *
- *设置引脚中断优先级
+ * 设置引脚中断优先级
  */
 void nvic_config(void) {
-	/* 设置NVIC优先级分组，1位抢占优先级，3位响应优先级 */
+	/* 设置 NVIC 优先级分组，1 位抢占优先级，3 位响应优先级 */
 	nvic_priority_group_set(NVIC_PRIGROUP_PRE2_SUB2);
-	/* 使能TIMER2中断，设置抢占优先级为1，子优先级为1 */
+	/* 使能 TIMER2 中断，设置抢占优先级为 1，子优先级为 1 */
 	nvic_irq_enable(TIMER2_IRQn, 0, 1);
 	nvic_irq_enable(TIMER1_IRQn, 0, 2);
 }
 
 /**
- * @brief 配置定时器2-任务管理
+ * @brief 配置定时器 2-任务管理
  * @return void
  *
- *配置定时器2，用于定时执行FOC控制、采样、CAN发送等任务
+ * 配置定时器 2，用于定时执行 FOC 控制、采样、CAN 发送等任务
  */
 void TIMER2_config(void) {
 	/* ----------------------------------------------------------------------------
@@ -46,21 +77,21 @@ void TIMER2_config(void) {
   */
 	timer_parameter_struct timer_struct;
 
-	/* 使能TIMER2时钟 */
+	/* 使能 TIMER2 时钟 */
 	rcu_periph_clock_enable(RCU_TIMER2);
 
-	/* 复位TIMER2外设到默认状态 */
+	/* 复位 TIMER2 外设到默认状态 */
 	timer_deinit(TIMER2);
-	/* 初始化TIMER初始化参数结构体 */
+	/* 初始化 TIMER 初始化参数结构体 */
 	timer_struct_para_init(&timer_struct);
-	/* TIMER2配置参数设置 */
+	/* TIMER2 配置参数设置 */
 	timer_struct.prescaler =
-		TIMER2_PRESCALER; /* 预分频值：120分频(从0开始计数) */
+		TIMER2_PRESCALER; /* 预分频值：120 分频 (从 0 开始计数) */
 	timer_struct.alignedmode = TIMER_COUNTER_EDGE;	  /* 边沿对齐模式 */
 	timer_struct.counterdirection = TIMER_COUNTER_UP; /* 向上计数模式 */
 	timer_struct.period =
-		TIMER2_PERIOD; /* 自动重装载值：100次计数(从0开始计数) */
-	timer_struct.clockdivision = TIMER_CKDIV_DIV1; /* 时钟分频值：1分频 */
+		TIMER2_PERIOD; /* 自动重装载值：100 次计数 (从 0 开始计数) */
+	timer_struct.clockdivision = TIMER_CKDIV_DIV1; /* 时钟分频值：1 分频 */
 	timer_init(TIMER2, &timer_struct);
 
 	/* 清除更新中断标志位 */
@@ -68,108 +99,85 @@ void TIMER2_config(void) {
 	/* 使能更新中断 */
 	timer_interrupt_enable(TIMER2, TIMER_INT_UP);
 
-	/* 使能TIMER2 */
+	/* 使能 TIMER2 */
 	timer_enable(TIMER2);
 }
 
 /**
- * @brief   配置TIMER2-任务管理中断
+ * @brief   配置 TIMER2-任务管理中断
  * @return void
  */
 void User_timer_init(void) {
-	/* 配置TIMER2 */
+	/* 配置 TIMER2 */
 	TIMER2_config();
-	/* 配置TIMER2中断 */
+	/* 配置 TIMER2 中断 */
 	nvic_config();
 }
 
 /**
- * @brief   TIMER2-任务管理中断处理函数
+ * @brief   TIMER2-任务管理中断处理函数 (优化版)
  * @return void
+ * @note    使用查找表替代多层 switch-case，减少代码复杂度和执行时间
+ *          中断周期：50us
+ *          - 每 50us: 电机控制任务 (角度、速度更新)
+ *          - 每 100us: 传感器采样、CAN 通信
+ *          - 每 1ms: 热量控制、磁检
+ *          - 每 10ms/100ms/1s: 预留扩展任务
  */
 void TIMER2_IRQHandler(void) {
-	// 检查更新中断标志并处理
-	if (timer_interrupt_flag_get(TIMER2, TIMER_INT_UP) != RESET) {
-		// 清除更新中断标志
-		timer_interrupt_flag_clear(TIMER2, TIMER_INT_UP);
+    // 快速路径：检查并清除中断标志
+    if (timer_interrupt_flag_get(TIMER2, TIMER_INT_UP) == RESET) {
+        return;
+    }
+    timer_interrupt_flag_clear(TIMER2, TIMER_INT_UP);
 
-		// 每50us执行一次的操作 - 电机控制
-		scheduler.taskFlags |=
-			(1 << TASK_ANGLE_UPDATE) | (1 << TASK_VELOCITY_UPDATE);
+    // 固定任务：每 50us 执行电机控制
+    scheduler.taskFlags |= (1u << TASK_ANGLE_UPDATE) | 
+                           (1u << TASK_VELOCITY_UPDATE) | 
+                           (1u << TASK_MOTOR_CONTROL);
 
-		// 使用标志位控制，每100us执行一次其他任务
-		static bool isEvenCall = false;
-		isEvenCall = !isEvenCall;
-
-		if (isEvenCall) {
-			// 每100us执行一次的操作 - 保持原有频率
-
-			scheduler.taskFlags |= (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;
-			}
-			}
-
-			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执行的代码
-						}
-					}
-				}
-			}
-		}
-	}
+    // 每 100us 执行一次的任务（使用位运算替代布尔变量）
+    static u32 call_counter = 0;
+    call_counter++;
+    
+    if ((call_counter & 1u) == 0u) {  // 偶数次调用（每 100us）
+        // 使用查找表设置 1ms 周期内的任务
+        scheduler.taskFlags |= task_schedule_100us[total_us100_counter];
+        
+        total_us100_counter++;
+        if (total_us100_counter >= 10u) {
+            total_us100_counter = 0;
+            
+            // 每 1ms 执行的任务
+            static u32 ms1_counter = 0;
+            ms1_counter++;
+            if (ms1_counter >= 10u) {
+                ms1_counter = 0;
+                
+                // 每 10ms 执行的任务
+                static u32 ms10_counter = 0;
+                ms10_counter++;
+                if (ms10_counter >= 10u) {
+                    ms10_counter = 0;
+                    
+                    // 每 100ms 执行的任务
+                    static u32 ms100_counter = 0;
+                    ms100_counter++;
+                    if (ms100_counter >= 10u) {
+                        ms100_counter = 0;
+                        // 每 1s 任务预留位置
+                    }
+                    scheduler.taskFlags |= task_schedule_100ms[ms100_counter];
+                }
+                scheduler.taskFlags |= task_schedule_10ms[ms10_counter];
+            }
+            scheduler.taskFlags |= task_schedule_1ms[ms1_counter];
+        }
+    }
 }
 
 /**
- * @brief  TIMER1-PWM GPIO配置函数
+ * @brief  TIMER1-PWM GPIO 配置函数
  */
 void timer1_gpio_config(void) {
 	// 时钟使能
@@ -178,7 +186,7 @@ void timer1_gpio_config(void) {
 	rcu_periph_clock_enable(RCU_AF);
 	rcu_periph_clock_enable(RCU_TIMER1);
 
-	// TIMER1 GPIO初始化
+	// TIMER1 GPIO 初始化
 	gpio_init(GPIOA, GPIO_MODE_AF_PP, GPIO_OSPEED_50MHZ, GPIO_PIN_0);
 	gpio_init(GPIOA, GPIO_MODE_AF_PP, GPIO_OSPEED_50MHZ, GPIO_PIN_1);
 	gpio_init(GPIOA, GPIO_MODE_AF_PP, GPIO_OSPEED_50MHZ, GPIO_PIN_2);
@@ -188,7 +196,7 @@ void timer1_gpio_config(void) {
 	gpio_bit_set(GPIOB, GPIO_PIN_0);
 	gpio_bit_set(GPIOB, GPIO_PIN_12);
 
-	/* 配置TIMER1完全重映射*/
+	/* 配置 TIMER1 完全重映射*/
 	gpio_pin_remap_config(GPIO_TIMER1_FULL_REMAP, DISABLE);
 }
 
@@ -199,13 +207,13 @@ void timer1_pwm_config(void) {
 	timer_parameter_struct timer_initpara;
 	timer_oc_parameter_struct timer_ocinitpara;
 
-	// 使能TIMER1时钟
+	// 使能 TIMER1 时钟
 	rcu_periph_clock_enable(RCU_TIMER1);
 
-	// 复位TIMER1
+	// 复位 TIMER1
 	timer_deinit(TIMER1);
 
-	// 配置TIMER1基本参数
+	// 配置 TIMER1 基本参数
 	timer_struct_para_init(&timer_initpara);
 	timer_initpara.prescaler = TIMER1_PRESCALER;		// 预分频器
 	timer_initpara.counterdirection = TIMER_COUNTER_UP; // 向上计数
@@ -218,23 +226,23 @@ void timer1_pwm_config(void) {
 	timer_ocinitpara.outputstate = TIMER_CCX_ENABLE;
 	timer_ocinitpara.ocpolarity = TIMER_OC_POLARITY_HIGH;
 
-	// 配置三个通道为PWM模式
+	// 配置三个通道为 PWM 模式
 	timer_channel_output_config(TIMER1, TIMER_CH_0, &timer_ocinitpara);
 	timer_channel_output_config(TIMER1, TIMER_CH_1, &timer_ocinitpara);
 	timer_channel_output_config(TIMER1, TIMER_CH_2, &timer_ocinitpara);
 
-	// 配置第四个通道(CH3)用于在PWM周期x%位置触发中断
+	// 配置第四个通道 (CH3) 用于在 PWM 周期 x% 位置触发中断
 	timer_ocinitpara.outputstate =
-		TIMER_CCX_DISABLE; // 不输出PWM波形，仅用于触发中断
+		TIMER_CCX_DISABLE; // 不输出 PWM 波形，仅用于触发中断
 	timer_ocinitpara.ocpolarity = TIMER_OC_POLARITY_HIGH;
 	timer_channel_output_config(TIMER1, TIMER_CH_3, &timer_ocinitpara);
 
-	// 设置PWM模式和占空比
+	// 设置 PWM 模式和占空比
 	timer_channel_output_mode_config(TIMER1, TIMER_CH_0, TIMER_OC_MODE_PWM0);
 	timer_channel_output_mode_config(TIMER1, TIMER_CH_1, TIMER_OC_MODE_PWM0);
 	timer_channel_output_mode_config(TIMER1, TIMER_CH_2, TIMER_OC_MODE_PWM0);
 	timer_channel_output_mode_config(TIMER1, TIMER_CH_3,
-									 TIMER_OC_MODE_PWM0); // CH3也设为PWM模式
+									 TIMER_OC_MODE_PWM0); // CH3 也设为 PWM 模式
 
 	timer_channel_output_pulse_value_config(TIMER1, TIMER_CH_0,
 											TIMER1_DEFAULT_PULSE);
@@ -242,7 +250,7 @@ void timer1_pwm_config(void) {
 											TIMER1_DEFAULT_PULSE);
 	timer_channel_output_pulse_value_config(TIMER1, TIMER_CH_2,
 											TIMER1_DEFAULT_PULSE);
-	// 设置CH3比较值为PWM周期x%位置
+	// 设置 CH3 比较值为 PWM 周期 x% 位置
 	timer_channel_output_pulse_value_config(TIMER1, TIMER_CH_3,
 											TIMER1_CH3_COMPARE_VALUE);
 
@@ -250,26 +258,26 @@ void timer1_pwm_config(void) {
 	timer_primary_output_config(TIMER1, ENABLE);
 	timer_auto_reload_shadow_enable(TIMER1);
 
-	// 使能CH3比较中断
+	// 使能 CH3 比较中断
 	timer_interrupt_enable(TIMER1, TIMER_INT_CH3);
 
-	// 启动TIMER1
+	// 启动 TIMER1
 	timer_enable(TIMER1);
 }
 
-// 添加TIMER1-PWM中断处理函数
+// 添加 TIMER1-PWM 中断处理函数
 /**
- * @brief   TIMER1中断处理函数
+ * @brief   TIMER1 中断处理函数
  * @return void
  */
 void TIMER1_IRQHandler(void) {
-	// 检查CH3比较中断标志并处理
+	// 检查 CH3 比较中断标志并处理
 	if (timer_interrupt_flag_get(TIMER1, TIMER_INT_FLAG_CH3) != RESET) {
-		// 清除CH3比较中断标志
+		// 清除 CH3 比较中断标志
 		timer_interrupt_flag_clear(TIMER1, TIMER_INT_FLAG_CH3);
 
 		scheduler.taskFlags |= (1 << TASK_CURRENT_UPDATE);
-		// 例如触发ADC采样或其他操作
+		// 例如触发 ADC 采样或其他操作
 	}
 }
 
@@ -305,4 +313,4 @@ void Set_3Pwm(float duty_cycle0, float duty_cycle1, float duty_cycle2) {
 	TIMER_CH0CV(TIMER1) = (uint32_t)(duty_cycle0 * TIMER1_PERIOD);
 	TIMER_CH1CV(TIMER1) = (uint32_t)(duty_cycle1 * TIMER1_PERIOD);
 	TIMER_CH2CV(TIMER1) = (uint32_t)(duty_cycle2 * TIMER1_PERIOD);
-}
+}
\ No newline at end of file
diff --git a/Robot_V2/device/ang_sensor.cpp b/Robot_V2/device/ang_sensor.cpp
index 8f64084..ca2f435 100644
--- a/Robot_V2/device/ang_sensor.cpp
+++ b/Robot_V2/device/ang_sensor.cpp
@@ -6,10 +6,10 @@
 #include "gd32c10x_dma.h"
 #include "gd32c10x_gpio.h"
 #include "gd32c10x_spi.h"
+#include <cmath>
 #include <cstdio>
 #include <cstring>
 #include <stdint.h>
-#define ANG_SENS_DEBUG 1
 
 #define ANG_SPI_CS_ENABLE() gpio_bit_reset(GPIOA, GPIO_PIN_15)
 #define ANG_SPI_CS_DISABLE() gpio_bit_set(GPIOA, GPIO_PIN_15)
@@ -199,13 +199,14 @@ u8 angle_sensor::init(void) {
 	spi_dma_init();		 // DMA初始化
 	return 0;
 }
+
 /**
  * @ 角度传感器类 设置当前角度
  * @param angle 输出级角度值
  * @return 0成功/1失败
  */
 u8 angle_sensor::angle_set(float angle) {
-	m_cumulativeAngle = angle * REDUCTION_RATIO; // 从Flash读取上次保存的角度
+	m_cumulativeAngle = angle * REDUCTION_RATIO;
 	m_firstUpdate = true;
 	return 0;
 }
@@ -237,6 +238,7 @@ inline float angle_sensor::read_sensorAngle(void) {
 
 	return (float)FAULT_RECEIVE;
 }
+
 /**
  * @brief 更新传感器角度（输出级）
  * @return 累积角度值
@@ -286,6 +288,6 @@ float angle_sensor::getRawAngle(void) const { return m_cumulativeAngle; }
  * @brief 获取角度值(原始级)
  * @return 角度值(原始级)
  */
-float angle_sensor::getcurrent_sensorangle(void) const {
+float angle_sensor::getcurrent_sensorangle(void) const{
 	return current_angle_deg;
 }
diff --git a/Robot_V2/device/ang_sensor.h b/Robot_V2/device/ang_sensor.h
index 2503b12..7d2c33b 100644
--- a/Robot_V2/device/ang_sensor.h
+++ b/Robot_V2/device/ang_sensor.h
@@ -14,6 +14,7 @@ int spi_rw_half_word_dma(uint32_t spi_periph, uint16_t tx_data,
 
 class angle_sensor {
   public:
+	float calibration_angle = 195.0f;
 	u8 init(void);
 	float read_sensorAngle(void);
 	float update(void);
@@ -22,6 +23,7 @@ class angle_sensor {
 	float getRawAngle(void) const;
 	float getcurrent_sensorangle(void) const;
 	u8 angle_set(float angle);
+	u8 set_calibration(float angle);
 
   protected:
 	float m_cumulativeAngle;
diff --git a/Robot_V2/main.cpp b/Robot_V2/main.cpp
index 7fcf125..c78d2d6 100644
--- a/Robot_V2/main.cpp
+++ b/Robot_V2/main.cpp
@@ -16,14 +16,27 @@ extern angle_sensor KTH7823;				// 角度传感器
 extern EncoderVelocityKalman Vel_kalman;	// 速度滤波
 extern EncoderAngleKalman Angle_kalman;		// 角度滤波
 extern ThermalController thermalController; // 热量控制器
-extern FOC_Controller FOC;				// FOC控制
+extern FOC_Controller FOC;					// FOC控制
 extern FlashEEPROM flashEEP;				// EEPROM类
 extern VersionManager Version;
 
 // 全局版本管理器实例
 TaskScheduler scheduler = {0};
 u32 motor_runtime_50us = 0; // 电机驱动计算轮次
-u32 cnt_test = 0;			 // 负载检测
+u32 cnt_test = 0;			// 负载检测
+
+// 函数指针数组，包含各种控制模式对应的函数
+static FOCFunctionPtr control_functions[] = {
+	&FOC_Controller::drv_hold,		   // 0: 占空比置
+	&FOC_Controller::angle_control,	   // 1: 位置闭环控制
+	&FOC_Controller::velocity_control, // 2: 速度闭环控制
+	&FOC_Controller::
+		angle_control_with_velocity_limit, // 3:
+										   // 角度闭环控制，带速度限制
+	&FOC_Controller::MIT_Mode,			   // 4: MIT 模式
+	&FOC_Controller::current_control,	   // 5: 电流闭环控制
+	&FOC_Controller::velocity_open_loop,   // 6: 速度开环控制
+};
 int main(void) {
 	u8 init_status = hardware_init();
 
@@ -82,24 +95,35 @@ void do_task(u32 currentTasks) {
 	if (currentTasks & (1 << TASK_MOTOR_CONTROL)) { // 电机控制任务(最高优先级)
 		motor_runtime_50us++;
 
-		// 函数指针数组，包含各种控制模式对应的函数
-		static FOCFunctionPtr control_functions[] = {
-			&FOC_Controller::drv_hold,		  // 0: 占空比置
-			&FOC_Controller::angle_control,	  // 1: 位置闭环控制
-			&FOC_Controller::velocity_control, // 2: 速度闭环控制
-			&FOC_Controller::
-				angle_control_with_velocity_limit, // 3:
-												   // 角度闭环控制，带速度限制
-			&FOC_Controller::MIT_Mode,			// 4: MIT 模式
-			&FOC_Controller::current_control,	// 5: 电流闭环控制
-			&FOC_Controller::velocity_open_loop, // 6: 速度开环控制
-		};
+		// 静态变量用于跟踪上一次的控制模式
+		static int last_control_mode = -1;
 
 		// 边界检查，防止数组越界
 		if (FOC.control_mode >= 0 &&
 			FOC.control_mode < 7) { // 更新为7个控制模式
-			// 调用对应的控制函数
-			(FOC.*control_functions[FOC.control_mode])();
+
+			// 检查控制模式是否发生变化
+			if (FOC.control_mode != last_control_mode) {
+				// 重置所有PID控制器的状态，避免积分项累积
+				FOC.vel_loop.reset();
+				FOC.angle_loop.reset();
+				FOC.current_loop.reset();
+				FOC.vel_inVelAngloop.reset();
+				FOC.ang_inVelAngloop.reset();
+				FOC.ang_inMITloop.reset();
+				FOC.vel_inMITloop.reset();
+				FOC.current_inMITloop.reset();
+
+				// 更新上次控制模式
+				last_control_mode = FOC.control_mode;
+			}
+			if (!FOC.is_shut_down) {
+				// 调用对应的控制函数
+				(FOC.*control_functions[FOC.control_mode])();
+			} else {
+				// 调用对应的控制函数
+				FOC.drv_stop();
+			}
 		}
 	}
 	if (currentTasks & (1 << TASK_MCU_TEMP_UPDATE)) { // 更新mcu温度传感器值
diff --git a/Robot_V2/sys/config.h b/Robot_V2/sys/config.h
index 43a0e00..f8d0db3 100644
--- a/Robot_V2/sys/config.h
+++ b/Robot_V2/sys/config.h
@@ -9,12 +9,13 @@
 #define EEP_DEBUG 1
 #define OTA_DEBUG 1
 #define SLEEP_DEBUG 1
+#define ANG_SENS_DEBUG 1
 
 // 版本号定义 - 可根据实际项目进行调整
 constexpr u16 SOFTWARE_VERSION_MAJOR = 26;
-constexpr u16 SOFTWARE_VERSION_MINOR = 2;
-constexpr u16 SOFTWARE_VERSION_PATCH = 8;
-constexpr u16 SOFTWARE_VERSION_BUILD = 5;
+constexpr u16 SOFTWARE_VERSION_MINOR = 3;
+constexpr u16 SOFTWARE_VERSION_PATCH = 9;
+constexpr u16 SOFTWARE_VERSION_BUILD = 1;
 
 constexpr u16 HARDWARE_VERSION_MAJOR = 25;
 constexpr u16 HARDWARE_VERSION_MINOR = 12;
@@ -54,36 +55,10 @@ constexpr u32 OTA_AVAL = 0x00000000; // 全零表示可用
 constexpr u8 PP = 11;
 constexpr float DIR = -1.0f;
 constexpr u16 REDUCTION_RATIO = 25;
-constexpr float CURRENT_OFFSET = -1.62f; // 当前电流传感器的零点偏移
-constexpr float HOLD_PWM_DUTY = 0.95f;
-constexpr u16 ANGLESENS_OFFSET = +0;
+constexpr float CURRENT_OFFSET = -3.31f/2.0f; // 当前电流传感器的零点偏移
+constexpr float HOLD_PWM_DUTY = 0.1f;
+constexpr u16 ANGLESENS_OFFSET = +15;
 constexpr float KE_FACTOR = 0.0f;
-// PID配置
-constexpr float DEFAULT_current_loop_p = 0.1f / 100 * 12; // 电流P
-constexpr float current_loop_Ki = 0.55f;				  // 电流P
-constexpr float current_loop_Kd = 3e-4;					  // 电流P
-
-constexpr float DEFAULT_vel_loop_p = 0.018f / 100 * 12; // 速度P
-constexpr float vel_loop_Ku = 3.600000e-03 * 0.7 * 100 / 12;
-constexpr float vel_loop_Tu = 17.0f * 1e-3f;
-constexpr float vel_loop_Ki = 1.2 * vel_loop_Ku / vel_loop_Tu;
-constexpr float vel_loop_Kd = 0.075 * vel_loop_Ku * vel_loop_Tu * 2;
-
-constexpr float DEFAULT_vel_angloop_p = 0.4f / 100; // 速度 速度-角度P
-constexpr float vel_angloop_Ki = 8.0f;				// 速度 速度-角度I
-constexpr float vel_angloop_Kd = 5e-05f;			// 速度 速度-角度D
-
-constexpr float DEFAULT_ang_angloop_p = 150.0f / 100 * 12; // 角度 速度-角度P
-constexpr float ang_angloop_Ki = 0.00f;					   // 角度 速度-角度I
-constexpr float ang_angloop_Kd = 8e-04;					   // 角度 速度-角度D
-
-constexpr float DEFAULT_angle_loop_p = 60.0f / 100 * 12; // 角度P
-
-constexpr float DEFAULT_vel_MIT_p = 9.0f / 100; // 速度 MIT P
-constexpr float vel_MIT_Ku = 6.72f * 100 / 12;
-constexpr float vel_MIT_Tu = 62.0f * 1e-3f;
-constexpr float vel_MIT_Ki = 0.1f;
-constexpr float vel_MIT_Kd = 0.01f;
 
 // 角度卡尔曼滤波器参数
 // 参数调节建议：
@@ -104,13 +79,46 @@ constexpr float VELOCITY_KALMAN_INITIAL_COVARIANCE = 1e-5f;
 constexpr float VELOCITY_KALMAN_PROCESS_NOISE_Q =
 	1e-6f; // 过程噪声协方差 - 通常保持较小
 constexpr float VELOCITY_KALMAN_MEASUREMENT_NOISE_R =
-	1.0f; // 测量噪声协方差 - 主要调节参数
+	1.0f; // 测量噪声协方差 - 主要调节参数1.0
 
 // 电流卡尔曼滤波器参数
 constexpr float CURRENT_KALMAN_INITIAL_STATE = 0.0f;
-constexpr float CURRENT_KALMAN_INITIAL_COVARIANCE = 100.0f;		// 增大初始协方差
-constexpr float CURRENT_KALMAN_PROCESS_NOISE_Q = 10.0f;			// 降低过程噪声
-constexpr float CURRENT_KALMAN_MEASUREMENT_NOISE_R = 1e-3f;      // 增大测量噪声
+constexpr float CURRENT_KALMAN_INITIAL_COVARIANCE = 100.0f; // 增大初始协方差
+constexpr float CURRENT_KALMAN_PROCESS_NOISE_Q = 10.0f;		// 降低过程噪声
+constexpr float CURRENT_KALMAN_MEASUREMENT_NOISE_R = 5e-5f; // 增大测量噪声
+
+// PID配置
+constexpr float DEFAULT_current_loop_p = 0.1f / 100 * 12; // 电流P
+constexpr float current_loop_Ki = 0.55f;				  // 电流P
+constexpr float current_loop_Kd = 3e-4f;				  // 电流P
+
+constexpr float DEFAULT_vel_loop_p = 0.018f / 100 * 12; // 速度P
+constexpr float vel_loop_Ku = 3.600000e-03 * 0.7 * 100 / 12;
+constexpr float vel_loop_Tu = 17.0f * 1e-3f;
+constexpr float vel_loop_Ki = 1.2 * vel_loop_Ku / vel_loop_Tu;
+constexpr float vel_loop_Kd = 0.075 * vel_loop_Ku * vel_loop_Tu * 2;
+
+constexpr float DEFAULT_vel_angloop_p = 0.4f / 100; // 速度 速度-角度P
+constexpr float vel_angloop_Ki = 5.0f;				// 速度 速度-角度I
+constexpr float vel_angloop_Kd = 5e-05f;			// 速度 速度-角度D
+
+constexpr float DEFAULT_angle_angloop_p = 150.0f / 100 * 12; // 角度 速度-角度P
+constexpr float ang_angloop_Ki = 0.00f;						 // 角度 速度-角度I
+constexpr float ang_angloop_Kd = 8e-04f;					 // 角度 速度-角度D
+
+constexpr float DEFAULT_angle_loop_p = 60.0f / 100 * 12; // 角度P
+
+constexpr float DEFAULT_angle_MIT_p = 100.0f / 100 * 12; // 角度 MIT P
+constexpr float angle_MIT_Ki = 0.0f;
+constexpr float angle_MIT_Kd = 8e-04f;
+
+constexpr float DEFAULT_vel_MIT_p = 5.0f / 100; // 速度 MIT P
+constexpr float vel_MIT_Ki = 15.0f;
+constexpr float vel_MIT_Kd = 1e-04f;
+
+constexpr float DEFAULT_current_MIT_p = 0.2f / 100 * 12; // 电流 MIT P
+constexpr float current_MIT_Ki = 0.1f;
+constexpr float current_MIT_Kd = 3e-4f;
 
 // 常量定义
 constexpr u8 SIZE_FLOAT = sizeof(float);
@@ -120,6 +128,7 @@ constexpr float PI = 3.14159265358979323846f;
 constexpr float INV_PI_3 = 3.0f / PI;
 constexpr float PI_3 = PI / 3.0f;
 constexpr float _1_SQRT3 = 0.57735026919f; // 1/sqrt(3)
+constexpr float _3PI_2 = 4.71238898038f;
 constexpr float SQRT3 = 1.7320508075688772f;
 constexpr float SQRT3DV2 = 0.8660254037844386f; // sqrt(3)/2
 constexpr float COS_PI_3 = 0.5f;
@@ -175,15 +184,17 @@ constexpr float ANGLESENS_FACTOR = 1.0f / ADC_MAX * 360.0f;
 // TIMER2-任务调度
 constexpr int TIMER2_PRESCALER = 119; // 120MHz / 120 = 1MHz
 constexpr int TIMER2_PERIOD = 49;	  // 1 / 1MHz * 50 = 50us
+constexpr int MOTOR_PERIOD = 2;		  // 100us/50us = 2
 constexpr float TIME_SLICE =
 	(TIMER2_PERIOD + 1) * (TIMER2_PRESCALER + 1) / 120000000.0f; // 周期
 constexpr float TIMER2_FREQUENCY = 1 / TIME_SLICE;				 // 频率
+constexpr float MOTOR_FREQUENCY = 1 / TIME_SLICE / MOTOR_PERIOD; // 频率
 
 // TIMER1-PWM
 constexpr int TIMER1_PRESCALER = 0; // 不分频，计数器时钟为120MHz
 constexpr int TIMER1_PERIOD = 5999; // 120MHz / (0+1) / (5999+1) = 20kHz
 constexpr int TIMER1_DEFAULT_PULSE = 0;
 constexpr int TIMER1_CH3_COMPARE_VALUE =
-	(TIMER1_PERIOD + 1) * 0.5 - 1; // PWM周期50%位置触发中断
+	(TIMER1_PERIOD + 1) * 0.41 - 1; // PWM周期50%位置触发中断
 
 #endif
\ No newline at end of file
diff --git a/Robot_V2/sys/health.cpp b/Robot_V2/sys/health.cpp
index c8625bb..64ca22f 100644
--- a/Robot_V2/sys/health.cpp
+++ b/Robot_V2/sys/health.cpp
@@ -40,7 +40,7 @@ u32 ThermalController::update() {
 			remainingTime10Ms = 1; // 可以恢复工作
 		}
 		if (remainingTime10Ms == 1) { // 过热恢复
-			FOC.resume();
+			FOC.resume_from_thermal();
 			CAN0_transmiter.ThermalStatus = 0;
 			return --remainingTime10Ms;
 		}
@@ -73,7 +73,7 @@ u32 ThermalController::update() {
 	}
 
 	if (thermal_value > thermal_capacity) {
-		FOC.shut_down();
+		FOC.shut_down_thermal();
 		remainingTime10Ms = PROTECT_TIME; // 过热停机
 		CAN0_transmiter.ThermalStatus = 1;
 		return remainingTime10Ms;
@@ -99,6 +99,9 @@ void power_manage(void) {
 		FOC.shut_down();
 		flashEEP.save_system_config(true);
 	} else {
-		FOC.resume();
+		// 只有在没有过热关机时才恢复
+		if (!FOC.thermal_shutdown) {
+			FOC.resume();
+		}
 	}
-}
\ No newline at end of file
+}