diff --git a/Robot_V2/Algorithm/FOC.cpp b/Robot_V2/Algorithm/FOC.cpp
index e2e875e..032b167 100644
--- a/Robot_V2/Algorithm/FOC.cpp
+++ b/Robot_V2/Algorithm/FOC.cpp
@@ -11,8 +11,7 @@
 #include <cmath>
 #include <gd32c10x.h>
 
-
-FOC_Controlroller FOC; // FOC控制初始化
+FOC_Controller FOC; // FOC控制初始化
 extern sampler sampler;
 extern angle_sensor KTH7823;
 extern CAN_transmiter CAN0_transmiter;
@@ -24,27 +23,14 @@ extern EncoderVelocityKalman Vel_kalman; // 速度滤波
 #define constrain(value, min, max, target_min, target_max)                     \
 	((value - min) / (max - min) * (target_max - target_min))
 
-/**@
- * @brief 归一化角度
- *
- * @param angle
- * @return float
- */
-inline float _normalizeAngle(float angle) {
-	float a = fmod(angle, 360.0f);
-	// 取余运算可以用于归一化，列出特殊值例子算便知
-	return a >= 0 ? a : (a + 360.0f);
-}
-
 // 通过Ia,Ib,Ic计算Iq,Id(目前仅输出Iq)
-inline float FOC_Controlroller::cal_Iq_Id(float current_a, float current_b,
-										  float current_c) {
+inline float FOC_Controller::cal_Iq_Id(float current_a, float current_b,
+									   float current_c) {
 	// 计算电气角度
-	float angle_el = _normalizeAngle(shaft_angle * PP);
+	float angle_el = fmod(shaft_angle * PP, 360.0f);
 
 	float I_alpha, I_beta;
 	// Clarke变换 (使用三相电流)
-	// arm_clarke_f32(current_a, current_b, &I_alpha, &I_beta);
 	I_alpha = (2.0f / 3.0f) * current_a - (1.0f / 3.0f) * current_b -
 			  (1.0f / 3.0f) * current_c;
 	I_beta = _1_SQRT3 * current_b - _1_SQRT3 * current_c;
@@ -54,10 +40,9 @@ inline float FOC_Controlroller::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;
-	// arm_park_f32(I_alpha, I_beta, &I_d, &I_q, sin_el, cos_el);
 
 	return I_q * 1000.0f -
-		   KE_FACTOR * rt_velocity; // 返回Iq值，乘以100以转换为mA
+		   KE_FACTOR * rt_velocity; // 返回Iq值，乘以1000以转换为mA
 }
 
 /**
@@ -65,20 +50,22 @@ inline float FOC_Controlroller::cal_Iq_Id(float current_a, float current_b,
  *
  * @return u8 0表示初始化成功，1表示初始化失败
  */
-u8 FOC_Controlroller::init() {
-	motor_init();									 // 电机驱动初始化
-	vel_loop.set_PID(vel_loop_p * 100 / limit_voltage, 0.20f, 5e-5); // 设置电机速度控制参数
-	angle_loop.set_PID(angle_loop_p * 100 / limit_voltage, 100.0f,
+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(current_loop_p * 100 / limit_voltage, 5e-7f,
+	current_loop.set_PID(DEFAULT_current_loop_p * 100 / voltage_power_supply, 5e-7f,
 						 7e-8); // 设置电机电流控制参数
 
-	vel_inVelAngloop.set_PID(vel_angloop_p * 100 / limit_voltage, 3.0f,
-							 5e-4); // 设置电机速度控制参数
-	ang_inVelAngloop.set_PID(ang_angloop_p * 100 / limit_voltage, 0.0f,
-							 5e-4); // 设置电机角度控制参数
-
+	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); // 设置电机角度控制参数
 
+	CAN0_transmiter.testfloat_union.f = DEFAULT_ang_angloop_p * 100 / voltage_power_supply;
 	return 0;
 }
 
@@ -86,18 +73,18 @@ u8 FOC_Controlroller::init() {
  * @brief 电机控制类构造函数
  *
  */
-FOC_Controlroller::FOC_Controlroller() { ; }
+FOC_Controller::FOC_Controller() { ; }
 
 /**
- * @brief 设置电机输出，使用优化的SVPWM算法
+ * @brief 设置电机输出
  *
  * @param Uq: q轴电压
  * @param Ud: d轴电压
  * @param angle_el: 电机轴角度
  */
-inline void FOC_Controlroller::setPhaseVoltage(float Uq, float Ud) {
+inline void FOC_Controller::setPhaseVoltage(float Uq, float Ud) {
 	// 计算电气角度
-	float angle_el = _normalizeAngle(shaft_angle * PP);
+	float angle_el = fmod(shaft_angle * PP, 360.0f);
 
 	// 帕克逆变换 (直接计算，避免函数调用开销)
 	float sin_el, cos_el;
@@ -107,18 +94,126 @@ inline void FOC_Controlroller::setPhaseVoltage(float Uq, float Ud) {
 
 	// 克拉克逆变换 (使用预计算常量优化)
 	Ua = Ualpha + v_half;
-	Ub = sqrt3dv2 * Ubeta - 0.5f * Ualpha + v_half;
-	Uc = -0.5f * Ualpha - sqrt3dv2 * Ubeta + v_half;
+	Ub = SQRT3DV2 * Ubeta - 0.5f * Ualpha + v_half;
+	Uc = -0.5f * Ualpha - SQRT3DV2 * Ubeta + v_half;
+
+	if (Ua > v_safe)
+		Ua = v_safe;
+	else if (Ua < -v_safe)
+		Ua = -v_safe;
+	if (Ub > v_safe)
+		Ub = v_safe;
+	else if (Ub < -v_safe)
+		Ub = -v_safe;
+	if (Uc > v_safe)
+		Uc = v_safe;
+	else if (Uc < -v_safe)
+		Uc = -v_safe;
 
-	// 计算占空比并限制范围
-	duty_a = _constrain(Ua / v_safe, 0.0f, 1.0f);
-	duty_b = _constrain(Ub / v_safe, 0.0f, 1.0f);
-	duty_c = _constrain(Uc / v_safe, 0.0f, 1.0f);
 	// 设置PWM输出
 	float voltage_exchange = limit_voltage / v_safe;
-	duty_a = duty_a * voltage_exchange;
-	duty_b = duty_b * voltage_exchange;
-	duty_c = duty_c * voltage_exchange;
+	duty_a = Ua / voltage_power_supply * voltage_exchange;
+	duty_b = Ub / voltage_power_supply * voltage_exchange;
+	duty_c = Uc / voltage_power_supply * voltage_exchange;
+
+	Set_3Pwm(duty_a, duty_b, duty_c);
+}
+
+/**
+ * @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 sin_el, cos_el;
+	arm_sin_cos_f32(angle_el, &sin_el, &cos_el);
+	float Ualpha = Ud * cos_el - Uq * sin_el;
+	float Ubeta = Ud * sin_el + Uq * cos_el;
+
+	// 计算角度并归一化到 [0, 2π)
+	float angle = atan2f(Ubeta, Ualpha);
+	if (angle < 0)
+		angle += 2.0f * PI;
+
+	// 扇区与局部角度
+	int sector = (int)(angle * INV_PI_3) + 1;
+	if (sector < 1)
+		sector = 1;
+	else if (sector > 6)
+		sector = 6;
+	float local_angle = angle - (sector - 1) * PI_3;
+
+	// 模长计算
+	float magnitude = sqrtf(Ualpha * Ualpha + Ubeta * Ubeta);
+
+	// 计算电压调制比，同时限制最大输出电压
+	float modulation_index =
+		(magnitude / voltage_power_supply); // 90%作为安全裕量
+
+	// 电压限制
+	if (limit_voltage > voltage_power_supply) {
+		limit_voltage = voltage_power_supply;
+	}
+	float limit_percent = limit_voltage / v_safe;
+	if (modulation_index > limit_percent) {
+		// 限制电压
+		modulation_index = limit_percent;
+		// 同时相应地调整magnitude，保持电压矢量方向不变，根据调整后的调制比反推magnitude
+		magnitude = voltage_power_supply * limit_percent;
+	}
+
+	// 使用arm_sin_cos_f32计算local_angle的sin和cos
+	float sin_l, cos_l;
+	arm_sin_cos_f32(local_angle, &sin_l, &cos_l);
+	float T1 = modulation_index * (SQRT3DV2 * cos_l - COS_PI_3 * sin_l);
+	float T2 = modulation_index * sin_l;
+	float T0 = 1.0f - T1 - T2;
+
+	// 计算占空比（按规范化时间映射）
+	switch (sector) {
+	case 1:
+		duty_a = 0.5f * T0 + T1 + T2;
+		duty_b = 0.5f * T0 + T2;
+		duty_c = 0.5f * T0;
+		break;
+	case 2:
+		duty_a = 0.5f * T0 + T1;
+		duty_b = 0.5f * T0 + T1 + T2;
+		duty_c = 0.5f * T0;
+		break;
+	case 3:
+		duty_a = 0.5f * T0;
+		duty_b = 0.5f * T0 + T1 + T2;
+		duty_c = 0.5f * T0 + T2;
+		break;
+	case 4:
+		duty_a = 0.5f * T0;
+		duty_b = 0.5f * T0 + T1;
+		duty_c = 0.5f * T0 + T1 + T2;
+		break;
+	case 5:
+		duty_a = 0.5f * T0 + T2;
+		duty_b = 0.5f * T0;
+		duty_c = 0.5f * T0 + T1 + T2;
+		break;
+	case 6:
+		duty_a = 0.5f * T0 + T1 + T2;
+		duty_b = 0.5f * T0;
+		duty_c = 0.5f * T0 + T1;
+		break;
+	default:
+		duty_a = 0.0f;
+		duty_b = 0.0f;
+		duty_c = 0.0f;
+		break;
+	}
+
+	// 设置PWM输出
 	Set_3Pwm(duty_a, duty_b, duty_c);
 }
 
@@ -126,7 +221,7 @@ inline void FOC_Controlroller::setPhaseVoltage(float Uq, float Ud) {
  * @brief 电机关机
  *
  */
-void FOC_Controlroller::shut_down() {
+void FOC_Controller::shut_down() {
 	is_shut_down = 1;
 	motor_sleep();
 }
@@ -134,11 +229,11 @@ void FOC_Controlroller::shut_down() {
  * @brief 占空比置0
  *
  */
-void FOC_Controlroller::drv_stop() { Set_3Pwm(0.0f, 0.0f, 0.0f); }
+void FOC_Controller::drv_stop() { Set_3Pwm(0.0f, 0.0f, 0.0f); }
 /**
  * @brief 电机锁止
  */
-float FOC_Controlroller::drv_hold() {
+float FOC_Controller::drv_hold() {
 	float hold_pwm_duty = HOLD_PWM_DUTY;
 	if (HOLD_PWM_DUTY > 0.95f) {
 		hold_pwm_duty = 0.95f;
@@ -152,7 +247,7 @@ float FOC_Controlroller::drv_hold() {
  * @brief 电机恢复
  *
  */
-void FOC_Controlroller::resume() {
+void FOC_Controller::resume() {
 	over_current = false;
 	voltage_error = false;
 	is_shut_down = false;
@@ -162,12 +257,12 @@ void FOC_Controlroller::resume() {
  * @brief 更新电源电压
  *
  */
-void FOC_Controlroller::update_power(void) {
-	voltage_power_supply = sampler.get_vbus_voltage();
-	if (voltage_power_supply < 36.0f and voltage_power_supply > 8.0f) {
+void FOC_Controller::update_power(float voltage) {
+	if (voltage < 36.0f and voltage > 8.0f) {
 		voltage_error = false;
-		v_half = voltage_power_supply * 0.5f;
-		v_safe = voltage_power_supply * 0.95f;
+		voltage_power_supply = voltage;
+		v_half = voltage * 0.5f;
+		v_safe = voltage * 1.05f;
 	} else {
 		voltage_error = true;
 	}
@@ -179,51 +274,39 @@ void FOC_Controlroller::update_power(void) {
  * @param target_velocity: 目标速度
  * @return float: q轴电压
  */
-float FOC_Controlroller::velocity_open_loop() {
+float FOC_Controller::velocity_open_loop() {
 	// 通过时间间隔和目标速度计算需要转动的机械角度，存储在shaft_angle中，在此之前，还需要对轴角度归一化
-	shaft_angle = _normalizeAngle(shaft_angle + target_velocity * ANGLE_FACTOR *
-													REDUCTION_RATIO * 1.0 /
-													TIMER2_FREQUENCY);
+	shaft_angle = fmod(shaft_angle + target_velocity * REDUCTION_RATIO * 1.0 /
+										 TIMER2_FREQUENCY,
+					   360.0f);
 
 	// 因为在开环控制中，Ud始终为0，只控制Uq
 	const float Uq = voltage_power_supply / 2.0f;
 
-	setPhaseVoltage(Uq, 0.0f);
-	CAN0_transmiter.testfloat_union.f = shaft_angle;
+	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;
 }
 
-/**
- * @brief 电机初始化
- *
- * @param _PP: 电机极队数
- */
-// void FOC_Controlroller::DFOC_alignSensor(int _PP) {
-// 	u8 PP = _PP;
-// 	setPhaseVoltage(3, 0, _3PI_2);
-// 	delay_1ms(3000);
-// 	zero_electric_angle = _electricalAngle();
-// 	setPhaseVoltage(0, 0, _3PI_2);
-// 	std::printf("0电角度:%f", zero_electric_angle);
-// }
-
 /**
  * @brief 角度闭环控制
  *
  * @param target_angle: 目标角度
  * @return float: 当前角度
  */
-float FOC_Controlroller::angle_control() {
+float FOC_Controller::angle_control() {
 	// PID 控制计算
 	float Uq = angle_loop(target_angle - rt_angle);
 
 	// 获取电机轴角度
 	shaft_angle = KTH7823.getcurrent_sensorangle();
+	rt_current = cal_Iq_Id(sampler.get_current('U'), sampler.get_current('V'),
+						   sampler.get_current('W'));
 	// 设置相电压
-	setPhaseVoltage(Uq, 0.0f);
+	setSVPWM_PhaseVoltage(Uq, 0.0f);
 	return shaft_angle;
 }
 
@@ -234,26 +317,70 @@ float FOC_Controlroller::angle_control() {
  * @param max_velocity: 最大速度限制 (单位: rad/s)
  * @return float: 当前角度
  */
-float FOC_Controlroller::angle_control_with_velocity_limit() {
+float FOC_Controller::angle_control_with_velocity_limit() {
 	// 计算角度误差
 	float angle_error = target_angle - rt_angle;
 
 	// 使用角度PID控制器计算目标速度
 	float target_velocity = ang_inVelAngloop(angle_error);
 
-	// 对目标速度进行限幅
-	target_velocity =
-		_constrain(target_velocity, -limit_velocity, limit_velocity);
-
 	// 使用速度PID控制器计算最终输出电压
+	// 根据目标速度大小，选择不同的PID参数
+	if (target_velocity < -120.0f) {
+		vel_inVelAngloop.set_P(vel_angloop_p * 2.0f);
+	} else if (target_velocity < -30.0f) {
+		vel_inVelAngloop.set_P(vel_angloop_p * 1.5f);
+	} else {
+		vel_inVelAngloop.set_P(vel_angloop_p);
+	}
 	float Uq = vel_inVelAngloop(rt_velocity - target_velocity);
-	CAN0_transmiter.testfloat_union.f = shaft_angle;
+
+	rt_current = cal_Iq_Id(sampler.get_current('U'), sampler.get_current('V'),
+						   sampler.get_current('W'));
+	// 获取电机轴角度
+	shaft_angle = KTH7823.getcurrent_sensorangle();
+
+	// 设置相电压
+	setSVPWM_PhaseVoltage(Uq, 0.0f);
+
+	return shaft_angle;
+}
+
+/**
+ * @brief 带速度限制的角度闭环控制
+ *
+ * @param target_angle: 目标角度
+ * @param max_velocity: 最大速度限制 (单位: rad/s)
+ * @return float: 当前角度
+ */
+float FOC_Controller::MIT_Mode() {
+	// 第一层：角度环（外环）
+	// 根据目标角度和实际角度的差值，通过角度环PID控制器计算目标速度
+	float angle_error = rt_angle - target_angle;
+	float target_velocity =
+		ang_inVelAngloop(angle_error); // 使用angle_loop作为角度到速度的映射
+
+	// 第二层：速度环（中环）
+	// 根据目标速度和实际速度的差值，通过速度环PID控制器计算目标电流
+	float velocity_error = target_velocity - rt_velocity;
+	float target_current =
+		vel_inVelAngloop(velocity_error); // 使用vel_loop作为速度到电流的映射
+
+	CAN0_transmiter.testfloat_union.f = target_current;
+	// 第三层：电流环（内环）
+	// 计算当前实际电流
+	rt_current = cal_Iq_Id(sampler.get_current('U'), sampler.get_current('V'),
+						   sampler.get_current('W'));
+
+	// 通过电流环PID控制器计算输出电压
+	float current_error = target_current - rt_current;
+	float Uq = current_loop(current_error);
 
 	// 获取电机轴角度
 	shaft_angle = KTH7823.getcurrent_sensorangle();
 
 	// 设置相电压
-	setPhaseVoltage(Uq, 0.0f);
+	setSVPWM_PhaseVoltage(Uq, 0.0f);
 
 	return shaft_angle;
 }
@@ -264,30 +391,30 @@ float FOC_Controlroller::angle_control_with_velocity_limit() {
  * @param target_velocity: 目标速度
  * @return float: 当前速度
  */
-float FOC_Controlroller::velocity_control() {
+float FOC_Controller::velocity_control() {
 	// PID 控制计算
 	float Uq = vel_loop((rt_velocity - target_velocity) * 25);
 
 	// 获取电机轴角度
 	shaft_angle = KTH7823.getcurrent_sensorangle();
 	// 设置相电压
-	setPhaseVoltage(Uq, 0.0f);
-	CAN0_transmiter.testfloat_union.f =
-		cal_Iq_Id(sampler.get_current('U'), sampler.get_current('V'),
-				  sampler.get_current('W'));
+	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_Controlroller::current_control() {
+float FOC_Controller::current_control() {
 	// PID 控制计算
-	float Iq = cal_Iq_Id(sampler.get_current('U'), sampler.get_current('V'),
-						 sampler.get_current('W'));
+	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);
 	CAN0_transmiter.testfloat_union.f = Iq;
 	// 获取电机轴角度
 	shaft_angle = KTH7823.getcurrent_sensorangle();
 	// 设置相电压
-	setPhaseVoltage(Uq, 0.0f);
+	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 9c40f1c..0b9302e 100644
--- a/Robot_V2/Algorithm/FOC.h
+++ b/Robot_V2/Algorithm/FOC.h
@@ -5,52 +5,68 @@
 #include "pid.h"
 #include <cstdint>
 
-class FOC_Controlroller {
+class FOC_Controller {
 
   public:
-	FOC_Controlroller();
-	
+	FOC_Controller();
+
 	// 初始化函数
 	u8 init();
 
 	u8 control_mode = 0;
 	float limit_voltage = 0.0f;
-	float limit_velocity = 0.0f;
+
 	float target_angle = 0.0f;
 	float target_velocity = 0.0f;
 	float target_current = 0.0f;
+
 	float rt_angle = 0.0f;
 	float rt_velocity = 0.0f;
+	float rt_current = 0.0f;
+
 	bool over_current = false;
 	bool voltage_error = false;
 	bool is_shut_down = false;
 	float voltage_power_supply = 24.0f; // 电源电压
-	float v_safe = voltage_power_supply * 0.95f;
+	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 vel_angloop_p=DEFAULT_vel_angloop_p;
+	float vel_loop_p=DEFAULT_vel_loop_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 velocity_open_loop(void);
 	float angle_control(void);
 	float angle_control_with_velocity_limit(void);
+	float MIT_Mode(void);
 	float velocity_control(void);
 	float current_control(void);
 
-	void update_power(void);
+	void update_power(float voltage);
 	void DFOC_alignSensor(int _PP);
 	void shut_down(void);
 	void resume(void);
 	void drv_stop(void);
 	float drv_hold(void);
-	PIDController vel_loop = PIDController{
-		vel_loop_p * 100, 0.20f, 5e-5, 1000000, voltage_power_supply}; // 12V
-	PIDController angle_loop =
-		PIDController{angle_loop_p * 100, 100.0f, 0.2f, 10000000,
-					  voltage_power_supply}; // 12V
-	PIDController current_loop = PIDController{
-		current_loop_p * 100, 5e-7f, 7e-8, 500, 24}; // 12V
-	PIDController vel_inVelAngloop = PIDController{
-		vel_angloop_p * 100, 3.0f, 5e-4, 1000000, voltage_power_supply};
+
+	// 此处默认值不作数
+	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, 1000000, v_safe};
 	PIDController ang_inVelAngloop =
-		PIDController{ang_angloop_p * 100, 0.0f, 5e-4, 1000000, 999};
+		PIDController{ang_angloop_p * 100, 0, 0, 20000, 100};
 
 	void update_pid_limits() {
 		vel_loop.limit = voltage_power_supply;
@@ -64,12 +80,15 @@ class FOC_Controlroller {
 	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_Controlroller::*FOCFunctionPtr)(void);
+typedef float (FOC_Controller::*FOCFunctionPtr)(void);
 
 inline float _normalizeAngle(float angle);
 inline float _electricalAngle();
diff --git a/Robot_V2/Algorithm/pid.cpp b/Robot_V2/Algorithm/pid.cpp
index 82406bd..213a1db 100644
--- a/Robot_V2/Algorithm/pid.cpp
+++ b/Robot_V2/Algorithm/pid.cpp
@@ -2,12 +2,12 @@
 #define _constrain(amt, low, high)                                             \
 	((amt) < (low) ? (low) : ((amt) > (high) ? (high) : (amt)))
 #include "Can.h"
+#include "config.h"
 #include <algorithm>
 #include <cmath>
 
-extern CAN_transmiter CAN0_transmiter;
-constexpr float TIME_SLICE = 1e-4;
 
+extern CAN_transmiter CAN0_transmiter;
 PIDController::PIDController(float P, float I, float D, float ramp, float limit)
 	: P(P), I(I), D(D), output_ramp(ramp) // PID控制器加速度限幅
 	  ,
@@ -68,4 +68,12 @@ void PIDController::set_PID(float P, float I, float D) {
  * @param P: P参数
  * @return void
  */
-void PIDController::set_P_percent(float pp) { this->P = pp; }
\ No newline at end of file
+void PIDController::set_P(float pp) { this->P = pp; }
+
+/**
+ * @brief 设置输出限幅
+ *
+ * @param limit: 输出限幅
+ * @return void
+ */
+void PIDController::set_limit(float limit) { this->limit = limit; }
\ No newline at end of file
diff --git a/Robot_V2/Algorithm/pid.h b/Robot_V2/Algorithm/pid.h
index 59dfecf..42ef31e 100644
--- a/Robot_V2/Algorithm/pid.h
+++ b/Robot_V2/Algorithm/pid.h
@@ -8,7 +8,9 @@ class PIDController {
 
 	float operator()(float error);
 	void set_PID(float P, float I, float D);
-	void set_P_percent(float pp);
+	void set_P(float pp);
+	void set_limit(float limit);
+	void reset();  // 重置PID控制器状态
 
 	float P; //!< 比例增益(P环增益)
 	float I; //!< 积分增益（I环增益）
@@ -21,5 +23,7 @@ class PIDController {
 	float output_prev;			  //!< 最后一个 pid 输出值
 	float integral_prev;		  //!< 最后一个积分分量值
 	unsigned long timestamp_prev; //!< 上次执行时间戳
+	float integral_separation_threshold;  //!< 积分分离阈值，用于抗积分饱和
 };
+
 #endif
\ No newline at end of file
diff --git a/Robot_V2/device/ADC.cpp b/Robot_V2/device/ADC.cpp
index 2ea86bf..ce44c17 100644
--- a/Robot_V2/device/ADC.cpp
+++ b/Robot_V2/device/ADC.cpp
@@ -15,7 +15,7 @@ constexpr float VCC_FACTOR = 11.0f;
 constexpr float CURRENT_FACTOR =
 	1.0f / 50.0f * 100.0f; // 50倍放大，100倍电流采样电阻, 单位mA
 extern CAN_transmiter CAN0_transmiter;
-extern FOC_Controlroller FOC;
+extern FOC_Controller FOC;
 sampler sampler;		// 采样器初始化
 u16 adc_value[6] = {0}; // 显式初始化数组元素为0
 /*!
@@ -240,7 +240,7 @@ void sampler::update_drv_temp() {
  */
 void sampler::update_current() {
 	u8 idx = current_sample_index;
-	extern FOC_Controlroller FOC;
+	extern FOC_Controller FOC;
 	// 优化计算
 	const float scale = ADC_SCALE * CURRENT_FACTOR;
 	const float offset = CURRENT_OFFSET * CURRENT_FACTOR;
diff --git a/Robot_V2/device/Can.cpp b/Robot_V2/device/Can.cpp
index 648afc3..41b0cdf 100644
--- a/Robot_V2/device/Can.cpp
+++ b/Robot_V2/device/Can.cpp
@@ -21,7 +21,7 @@
 constexpr u16 CANX = CAN0;
 
 CAN_transmiter CAN0_transmiter; // CAN初始化
-extern FOC_Controlroller FOC;
+extern FOC_Controller FOC;
 extern sampler sampler;
 extern angle_sensor KTH7823;
 extern EncoderAngleKalman Angle_kalman;	 // 角度滤波
@@ -447,13 +447,15 @@ void CAN_transmiter::update(void) {
 	target_angle_union.f = FOC.target_angle;
 	target_velocity_union.f = FOC.target_velocity;
 	target_current_union.f = FOC.target_current;
-	limit_velocity_union.f = FOC.limit_velocity;
+
+	rt_angle_union.f = FOC.rt_angle;
+	rt_velocity_union.f = FOC.rt_velocity;
+	rt_current_union.f = FOC.rt_current;
+
 	limit_voltage = FOC.limit_voltage * 1000;
 	mcu_temp = sampler.get_mcu_temp() * 100;
 	motor_temp = sampler.get_drv_temp() * 100;
 	Power_Voltage = sampler.get_vbus_voltage() * 1000;
-	rt_angle_union.f = FOC.rt_angle;
-	rt_velocity_union.f = FOC.rt_velocity;
 	MotorStatus = gpio_input_bit_get(GPIOB, GPIO_PIN_2) == RESET;
 	EEPROMStatus = flashEEP.EEPROMStatus;
 	Target_Control_Mode = FOC.control_mode;
@@ -465,57 +467,6 @@ void CAN_transmiter::update(void) {
 	v_current_union.f = sampler.get_current('V') * 1000;
 }
 
-/**
- * @brief  CAN发送函数
- * @retval 0: 发送失败
- *         1: 发送成功
- */
-u8 CAN_transmiter::CAN_all_Send() {
-	u32 success_flag = 0;
-
-	constexpr u32 RT_Vel_Ang_FEED = 0x101;
-	constexpr u32 SET_Vel_Ang_FEED = 0x110;
-	constexpr u32 Current_FEED = 0x117;
-	constexpr u32 Status_FEED = 0x118;
-	constexpr u32 Limit_Vel_Vol_FEED = 0x119;
-
-	// 打包RT_Vel_Ang_FEED - 只清零实际使用的8字节
-	memset(transmit_data, 0, 8);
-	pack_float_to_bytes(transmit_data, 0, rt_angle_union);
-	pack_float_to_bytes(transmit_data, 4, rt_velocity_union);
-	success_flag += CAN_AsyncSend(RT_Vel_Ang_FEED, 8, transmit_data);
-
-	// 打包SET_Vel_Ang_FEED - 只清零实际使用的8字节
-	memset(transmit_data, 0, 8);
-	pack_float_to_bytes(transmit_data, 0, target_angle_union);
-	pack_float_to_bytes(transmit_data, 4, target_velocity_union);
-	success_flag += CAN_AsyncSend(SET_Vel_Ang_FEED, 8, transmit_data);
-
-	// 打包Current_FEED - 只清零实际使用的6字节
-	memset(transmit_data, 0, 6);
-	pack_u16_to_bytes(transmit_data, 0, u16(u_current_union.f));
-	pack_u16_to_bytes(transmit_data, 2, u16(w_current_union.f));
-	pack_u16_to_bytes(transmit_data, 4, u16(v_current_union.f));
-	success_flag += CAN_AsyncSend(Current_FEED, 6, transmit_data);
-
-	// 打包Status_FEED - 只清零实际使用的8字节
-	memset(transmit_data, 0, 8);
-	transmit_data[0] = MotorStatus;
-	transmit_data[1] = controller_Mode;
-	pack_u16_to_bytes(transmit_data, 2, motor_temp);
-	pack_u16_to_bytes(transmit_data, 4, mcu_temp);
-	pack_u16_to_bytes(transmit_data, 6, Power_Voltage);
-	success_flag += CAN_AsyncSend(Status_FEED, 8, transmit_data);
-
-	// 打包Limit_Vel_Vol_FEED - 只清零实际使用的6字节
-	memset(transmit_data, 0, 6);
-	pack_float_to_bytes(transmit_data, 0, limit_velocity_union);
-	pack_u16_to_bytes(transmit_data, 4, limit_voltage);
-	success_flag += CAN_AsyncSend(Limit_Vel_Vol_FEED, 6, transmit_data);
-
-	return success_flag;
-}
-
 /**
  * @brief  CANFD发送函数
  * @retval 0: 发送失败
@@ -538,17 +489,19 @@ u8 CAN_transmiter::CANFD_all_Send() {
 	pack_u16_to_bytes(transmit_data, 11, Power_Voltage);
 	pack_float_to_bytes(transmit_data, 13, rt_angle_union);
 	pack_float_to_bytes(transmit_data, 17, rt_velocity_union);
-	pack_float_to_bytes(transmit_data, 21, testfloat_union);
+	pack_float_to_bytes(transmit_data, 21, rt_current_union);
+	pack_float_to_bytes(transmit_data, 25, testfloat_union);
+	pack_u16_to_bytes(transmit_data, 29, test_u16);
 	success_flag += CANFD_AsyncSend(CANFD_STATUS_ID, 32, transmit_data);
 
-	// 打包config数据 - 只清零实际使用的16字节
-	memset(transmit_data, 0, 16);
+	// 打包config数据 - 只清零实际使用的24字节
+	memset(transmit_data, 0, 24);
 	transmit_data[0] = controller_Mode;
 	pack_u16_to_bytes(transmit_data, 1, limit_voltage);
-	pack_float_to_bytes(transmit_data, 3, limit_velocity_union);
-	pack_float_to_bytes(transmit_data, 7, target_angle_union);
-	pack_float_to_bytes(transmit_data, 11, target_velocity_union);
-	success_flag += CANFD_AsyncSend(CANFD_CONFIG_ID, 16, transmit_data);
+	pack_float_to_bytes(transmit_data, 3, target_current_union);
+	pack_float_to_bytes(transmit_data, 7, target_velocity_union);
+	pack_float_to_bytes(transmit_data, 11, target_angle_union);
+	success_flag += CANFD_AsyncSend(CANFD_CONFIG_ID, 24, transmit_data);
 
 	return success_flag;
 }
@@ -577,34 +530,42 @@ void CAN_transmiter::communication_check() {
 		if (receive_message.rx_efid == CANFD_CMD_ID) {
 			// 控制模式
 			FOC.control_mode = receive_message.rx_data[0];
-			// 目标角度
+			// 目标电压限制
 			FOC.limit_voltage =
 				float(unpack_bytes_to_u16(receive_message.rx_data, 1)) / 1000;
-			// 目标电流&目标速度
-			FOC.limit_velocity =
+			// 目标电流
+			target_current_union.f =
 				unpack_bytes_to_float(receive_message.rx_data, 3).f;
-			FOC.target_current = FOC.limit_velocity;
-			// 目标角度
-			target_angle_union.f =
-				unpack_bytes_to_float(receive_message.rx_data, 7).f;
-			FOC.target_angle = target_angle_union.f;
+			FOC.current_loop.set_limit(target_current_union.f);
 			// 目标速度
 			target_velocity_union.f =
+				unpack_bytes_to_float(receive_message.rx_data, 7).f;
+			FOC.ang_inVelAngloop.set_limit(target_velocity_union.f);
+			// 目标角度
+			target_angle_union.f =
 				unpack_bytes_to_float(receive_message.rx_data, 11).f;
+			// 赋值
+			FOC.target_angle = target_angle_union.f;
 			FOC.target_velocity = target_velocity_union.f;
-			FOC.target_current = FOC.target_velocity * 10.0f;
+			FOC.target_current = target_current_union.f;
 
 			// 解析PID参数
-			u16 P_config = unpack_bytes_to_u16(receive_message.rx_data, 15);
 			float voltage = FOC.limit_voltage;
-			FOC.current_loop.set_P_percent(current_loop_p * P_config / 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;
+
 			P_config = unpack_bytes_to_u16(receive_message.rx_data, 17);
-			FOC.vel_loop.set_P_percent(vel_loop_p * P_config / voltage);
+			FOC.vel_loop_p = DEFAULT_vel_loop_p * P_config / voltage;
+			FOC.vel_MIT_p = DEFAULT_vel_MIT_p * P_config / voltage;
+
 			P_config = unpack_bytes_to_u16(receive_message.rx_data, 19);
-			FOC.angle_loop.set_P_percent(angle_loop_p * P_config / voltage);
+			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_inVelAngloop.set_P_percent(vel_angloop_p * P_config /
-											   voltage);
+			FOC.vel_angloop_p = DEFAULT_vel_angloop_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];
diff --git a/Robot_V2/device/Can.h b/Robot_V2/device/Can.h
index d897add..0b4c2f9 100644
--- a/Robot_V2/device/Can.h
+++ b/Robot_V2/device/Can.h
@@ -6,6 +6,7 @@
 extern "C" {
 #endif
 
+#include <cstdint>
 #include "gd32c10x_can.h"
 
 // 定义一个联合体类型用于float和byte数组之间的转换
@@ -72,24 +73,29 @@ class CAN_transmiter {
   private:
 	bool is_sleep = false; // 是否休眠
 
-	u8 controller_Mode = 0;					   // 电机模式
-	float_union_t limit_velocity_union = {0};  // 限制速度
-	u16 limit_voltage = 0;					   // 限制电压
-	short mcu_temp = 0;						   // MCU温度
-	short motor_temp = 0;					   // 电机温度
-	u16 Power_Voltage = 0;					   // 电源电压
-	float_union_t rt_angle_union = {0};		   // 实时角度
-	float_union_t rt_velocity_union = {0};	   // 实时速度
-	bool MotorStatus = 0;					   // 电机状态码
+	u8 Target_Control_Mode = 0; // 目标控制模式
+	u8 controller_Mode = 0;		// 当前模式
+	short mcu_temp = 0;			// MCU温度
+	short motor_temp = 0;		// 电机温度
+	u16 Power_Voltage = 0;		// 电源电压
+
+	u16 limit_voltage = 0;					  // 限制电压
+
+	float_union_t rt_angle_union = {0};	   // 实时角度
+	float_union_t rt_velocity_union = {0}; // 实时速度
+	float_union_t rt_current_union = {0};  // 实时电流
+
 	float_union_t target_angle_union = {0};	   // 目标角度
-	u8 Target_Control_Mode = 0;				   // 目标控制模式
-	u8 EEPROMStatus = 0;					   // EEPROM状态码
 	float_union_t target_velocity_union = {0}; // 目标速度
-	float_union_t target_current_union = {0};
+	float_union_t target_current_union = {0};  // 目标电流
+
 	float_union_t u_current_union = {0}; // U相电流
 	float_union_t w_current_union = {0}; // W相电流
 	float_union_t v_current_union = {0}; // V相电流
 
+	bool MotorStatus = 0; // 电机状态码
+	u8 EEPROMStatus = 0;  // EEPROM状态码
+
 	can_trasnmit_message_struct transmit_message; // 发送消息结构体
 	u8 transmit_mailbox = CAN_NOMAILBOX;		  // 初始化为无mailbox状态
 	u8 transmit_data[64];						  // CANFD最大64字节
@@ -106,6 +112,7 @@ class CAN_transmiter {
 	FlagStatus can0_receive_flag;				// CAN0接收标志
 	FlagStatus can0_send_flag;					// CAN0发送标志
 	float_union_t testfloat_union = {0};		// 测试用
+	u16 test_u16 = 0;
 
 	bool MagStatus = 0;								 // 磁状态码
 	bool ThermalStatus = 0;							 // 热能状态码
diff --git a/Robot_V2/device/Timer.cpp b/Robot_V2/device/Timer.cpp
index 5838bdd..04a9d53 100644
--- a/Robot_V2/device/Timer.cpp
+++ b/Robot_V2/device/Timer.cpp
@@ -11,7 +11,7 @@
 
 volatile u32 total_us100_counter = 0; // 总计数器，每100微秒增加1
 extern u16 adc_value[7];
-extern FOC_Controlroller FOC;
+extern FOC_Controller FOC;
 extern TaskScheduler scheduler;
 extern CAN_transmiter CAN0_transmiter;
 // 添加系统运行时间计数器
@@ -94,9 +94,9 @@ void TIMER2_IRQHandler(void) {
 		timer_interrupt_flag_clear(TIMER2, TIMER_INT_UP);
 
 		// 每50us执行一次的操作 - 电机控制
-		scheduler.taskFlags |= (1 << TASK_MOTOR_CONTROL);
 		scheduler.taskFlags |=
 			(1 << TASK_ANGLE_UPDATE) | (1 << TASK_VELOCITY_UPDATE);
+		scheduler.taskFlags |= (1 << TASK_MOTOR_CONTROL);
 
 		// 使用标志位控制，每100us执行一次其他任务
 		static bool isEvenCall = false;
diff --git a/Robot_V2/main.cpp b/Robot_V2/main.cpp
index ba0b5af..be7b189 100644
--- a/Robot_V2/main.cpp
+++ b/Robot_V2/main.cpp
@@ -8,7 +8,6 @@
 #include "flash.h"
 #include "health.h"
 #include "initial.h"
-#include "motor.h"
 #include "systick.h"
 
 extern CAN_transmiter CAN0_transmiter;		// CAN初始化
@@ -17,7 +16,7 @@ extern angle_sensor KTH7823;				// 角度传感器
 extern EncoderVelocityKalman Vel_kalman;	// 速度滤波
 extern EncoderAngleKalman Angle_kalman;		// 角度滤波
 extern ThermalController thermalController; // 热量控制器
-extern FOC_Controlroller FOC;				// FOC控制
+extern FOC_Controller FOC;				// FOC控制
 extern FlashEEPROM flashEEP;				// EEPROM类
 extern VersionManager Version;
 
@@ -63,7 +62,7 @@ u8 hardware_init(void) {
 	init_status += Angle_kalman.init(KTH7823.getAngle()); // 角度滤波初始化
 	init_status += Vel_kalman.init(0.0f);				  // 速度滤波初始化
 	init_status += CAN0_transmiter.init();				  // CAN初始化
-	init_status += FOC.init(); // FOC初始化
+	init_status += FOC.init();							  // FOC初始化
 	return init_status;
 }
 
@@ -85,17 +84,20 @@ void do_task(u32 currentTasks) {
 
 		// 函数指针数组，包含各种控制模式对应的函数
 		static FOCFunctionPtr control_functions[] = {
-			&FOC_Controlroller::drv_hold, // 0: 占空比置
-			&FOC_Controlroller::
-				angle_control_with_velocity_limit, // 1:
+			&FOC_Controller::drv_hold,		  // 0: 占空比置
+			&FOC_Controller::angle_control,	  // 1: 位置闭环控制
+			&FOC_Controller::velocity_control, // 2: 速度闭环控制
+			&FOC_Controller::
+				angle_control_with_velocity_limit, // 3:
 												   // 角度闭环控制，带速度限制
-			&FOC_Controlroller::velocity_control,  // 2: 速度闭环控制
-			&FOC_Controlroller::current_control,   // 3: 电流闭环控制
-			&FOC_Controlroller::velocity_open_loop, // 4: 速度开环控制
+			&FOC_Controller::MIT_Mode,			// 4: MIT 模式
+			&FOC_Controller::current_control,	// 5: 电流闭环控制
+			&FOC_Controller::velocity_open_loop, // 6: 速度开环控制
 		};
 
 		// 边界检查，防止数组越界
-		if (FOC.control_mode >= 0 && FOC.control_mode < 5) {  // 更新为7个控制模式
+		if (FOC.control_mode >= 0 &&
+			FOC.control_mode < 7) { // 更新为7个控制模式
 			// 调用对应的控制函数
 			(FOC.*control_functions[FOC.control_mode])();
 		}
@@ -115,8 +117,8 @@ void do_task(u32 currentTasks) {
 	if (currentTasks & (1 << TASK_VBUS_UPDATE)) { // 更新vbus电压值
 		power_manage();
 	}
-	if (currentTasks & (1 << TASK_CAN_TRANSMIT)) {	  // CAN通信
-		// CAN0_transmiter.testfloat_union.f = cnt_test; // 负载检测
+	if (currentTasks & (1 << TASK_CAN_TRANSMIT)) { // CAN通信
+		CAN0_transmiter.test_u16 = cnt_test;	   // 负载检测
 		cnt_test = 0;
 		CAN0_transmiter.can0_send_flag = SET;
 		CAN0_transmiter.communication_check();
diff --git a/Robot_V2/main.h b/Robot_V2/main.h
index ab1c246..da98c6e 100644
--- a/Robot_V2/main.h
+++ b/Robot_V2/main.h
@@ -36,6 +36,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #define MAIN_H
 #include <cstdint>
 
+
 void do_task(u32 currentTasks);
 u8 hardware_init(void);
 
diff --git a/Robot_V2/sys/config.h b/Robot_V2/sys/config.h
index 9a4e872..dbe2496 100644
--- a/Robot_V2/sys/config.h
+++ b/Robot_V2/sys/config.h
@@ -1,5 +1,6 @@
 #ifndef __CONFIG_H
 #define __CONFIG_H
+#include "gd32c10x_eval.h"
 #include <cstdint>
 
 // 如果使用CMSIS DSP库，需要定义ARM_MATH_CM4
@@ -58,22 +59,43 @@ constexpr float HOLD_PWM_DUTY = 0.95f;
 constexpr u16 ANGLESENS_OFFSET = +0;
 constexpr float KE_FACTOR = 14.0f;
 // PID配置
-constexpr float current_loop_p = 6e-2f / 100 * 12; // 电流P
-constexpr float vel_loop_p = 0.01f / 100 * 12;	   // 速度P
-constexpr float angle_loop_p = 20.0f / 100 * 12;	   // 角度P
-constexpr float vel_angloop_p = 0.12f / 100 * 12;   // 速度 速度-角度P
-constexpr float ang_angloop_p = 100.0f / 100 * 12;  // 角度 速度-角度P
+constexpr float DEFAULT_current_loop_p = 6e-2f / 100 * 12; // 电流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.22f / 100; // 速度 速度-角度P
+constexpr float vel_angloop_Ki = 12.0f;					  // 角度 速度-角度I
+constexpr float vel_angloop_Kd = 6e-04f;				  // 角度 速度-角度D
+
+constexpr float DEFAULT_ang_angloop_p = 120.0f / 100 * 12; // 角度 速度-角度P
+constexpr float ang_angloop_Ki = 0.00f;					  // 角度 速度-角度I
+constexpr float ang_angloop_Kd = 8e-04;					  // 角度 速度-角度D
+
+constexpr float DEFAULT_angle_loop_p = 60.0f / 100 * 12; // 角度P
+
+constexpr float DEFAULT_vel_MIT_p = 25.0f / 100 * 12; // 速度 速度-角度P
+constexpr float vel_MIT_Ku = 6.72f * 100 / 12;
+constexpr float vel_MIT_Tu = 62.0f * 1e-3f;
+constexpr float vel_MIT_Ki = 3e-2f;
+constexpr float vel_MIT_Kd = 5.355000e-05f;
 // 常量定义
 constexpr u8 SIZE_FLOAT = sizeof(float);
 constexpr u8 SIZE_U32 = sizeof(uint32_t);
 constexpr float ADC_MAX = 65536.0f;
 constexpr float PI = 3.14159265358979323846f;
+constexpr float INV_PI_3 = 3.0f / PI;
+constexpr float PI_3 = PI / 3.0f;
+constexpr float _1_SQRT3 = 0.57735026919f; // 1/sqrt(3)
+constexpr float SQRT3 = 1.7320508075688772f;
+constexpr float SQRT3DV2 = 0.8660254037844386f; // sqrt(3)/2
+constexpr float COS_PI_3 = 0.5f;
 constexpr float ANGLE_FACTOR = 25.0f / 180.0f * PI;
 constexpr float DEG2RAD = 1.0f / 180.0f * PI;
-constexpr float RAD2DEG = 180.0f / PI;	   // 修正RAD2DEG定义，应该是180/π
-constexpr float _1_SQRT3 = 0.57735026919f; // 1/sqrt(3)
-constexpr float sqrt3dv2 = 0.86602540378f; // sqrt(3)/2的预计算值
+constexpr float RAD2DEG = 180.0f / PI; // 修正RAD2DEG定义，应该是180/π
 constexpr u8 DEFAULT_VALUE = 0;
 constexpr u8 CONFIG_SAVED_NUM = 6;
 
@@ -122,8 +144,10 @@ constexpr float ANGLESENS_FACTOR = 1.0f / ADC_MAX * 360.0f;
 
 // TIMER2-任务调度
 constexpr int TIMER2_PRESCALER = 119; // 120MHz / 120 = 1MHz
-constexpr int TIMER2_PERIOD = 49;	  // 1MHz / 50 = 50us
-constexpr float TIMER2_FREQUENCY = 1 / 5e-5;
+constexpr int TIMER2_PERIOD = 49;	  // 1 / 1MHz * 50 = 50us
+constexpr float TIME_SLICE =
+	(TIMER2_PERIOD + 1) * (TIMER2_PRESCALER + 1) / 120000000.0f; // 周期
+constexpr float TIMER2_FREQUENCY = 1 / TIME_SLICE;				 // 频率
 
 // TIMER1-PWM
 constexpr int TIMER1_PRESCALER = 0; // 不分频，计数器时钟为120MHz
diff --git a/Robot_V2/sys/flash.cpp b/Robot_V2/sys/flash.cpp
index a03d875..a1106b7 100644
--- a/Robot_V2/sys/flash.cpp
+++ b/Robot_V2/sys/flash.cpp
@@ -13,7 +13,7 @@
 FlashEEPROM flashEEP = FlashEEPROM();
 extern angle_sensor KTH7823;
 extern CAN_transmiter CAN0_transmiter;
-extern FOC_Controlroller FOC; // FOC控制
+extern FOC_Controller FOC; // FOC控制
 
 fmc_state_enum write_ADDRESS(u32 address, u32 data) {
 	fmc_unlock();
@@ -125,7 +125,7 @@ u8 FlashEEPROM::read_saved_system_config(void) {
 
 	FOC.control_mode = (u8)data[0];
 	FOC.limit_voltage = data[1];
-	FOC.limit_velocity = data[2];
+	FOC.target_current = data[2];
 	KTH7823.angle_set(data[3]);
 	FOC.target_angle = data[4];
 	FOC.target_velocity = data[5];
@@ -389,7 +389,7 @@ FlashEepromStatus FlashEEPROM::save_system_config(bool saved) {
 	u32 CRC_NUM = 0x0000;
 	float data[CONFIG_SAVED_NUM] = {
 		(float)FOC.control_mode, FOC.limit_voltage,
-		FOC.limit_velocity,		 FOC.rt_angle,
+		FOC.target_current,		 FOC.rt_angle,
 		FOC.target_angle,		 FOC.target_velocity};
 
 	// 如果系统配置标记已保存，则无需再次保存
diff --git a/Robot_V2/sys/health.cpp b/Robot_V2/sys/health.cpp
index cd0a76b..c8625bb 100644
--- a/Robot_V2/sys/health.cpp
+++ b/Robot_V2/sys/health.cpp
@@ -8,7 +8,7 @@
 
 extern sampler sampler;
 extern CAN_transmiter CAN0_transmiter;
-extern FOC_Controlroller FOC;
+extern FOC_Controller FOC;
 extern FlashEEPROM flashEEP;
 ThermalController thermalController; // 热量控制器初始化
 
@@ -93,7 +93,7 @@ void angsens_MG_check(void) {
 void power_manage(void) {
 	float last_vbus = sampler.get_vbus_voltage();
 	sampler.update_vbus_voltage();
-	FOC.update_power();
+	FOC.update_power(last_vbus);
 	float vbus = sampler.get_vbus_voltage();
 	if ((vbus < 22.0f) and (last_vbus > vbus) and vbus > 8.0f) {
 		FOC.shut_down();
diff --git a/vcpkg-configuration.json b/vcpkg-configuration.json
index 4024b20..d6a865b 100644
--- a/vcpkg-configuration.json
+++ b/vcpkg-configuration.json
@@ -9,8 +9,8 @@
  "requires": {
   "arm:tools/open-cmsis-pack/cmsis-toolbox": "2.12.0",
   "arm:tools/kitware/cmake": "3.31.5",
-  "arm:tools/ninja-build/ninja": "1.12.1",
   "arm:tools/arm/mdk-toolbox": "1.1.0",
-  "arm:compilers/arm/armclang": "6.24.0"
+  "arm:compilers/arm/armclang": "6.24.0",
+  "arm:tools/ninja-build/ninja": "1.12.1"
  }
 }
\ No newline at end of file