fix(FOC): 修正大角度下会导致角度控制失控的问题

移除冗余的 `_electricalAngle` 函数，修正 `setPhaseVoltage` 中电气角度的计算方式
在定时器中断中增加实时角度更新，支持新增的无 PID 角度控制模式。

Robot_V2/Algorithm/DengFOC.cpp

@@ -52,10 +52,6 @@ inline float _normalizeAngle(float angle) {
 	// 取余运算可以用于归一化，列出特殊值例子算便知
 	return a >= 0 ? a : (a + 2 * PI);
 }
-inline float _electricalAngle() {
-	return _normalizeAngle((float)(DIR * PP) * KTH7823.read_sensorAngle() *
-						   -zero_electric_angle);
-}
 /**
  * @brief 电机控制类构造函数
  *
@@ -71,7 +67,8 @@ FOC_Controlroller::FOC_Controlroller() { ; }
  */
 inline void FOC_Controlroller::setPhaseVoltage(float Uq, float Ud) {
 	// 计算电气角度
-	float angle_el = _normalizeAngle(shaft_angle * PP + zero_electric_angle);
+	float angle_el = _normalizeAngle(shaft_angle * PP);
+
 	// 帕克逆变换 (直接计算，避免函数调用开销)
 	const float sin_el = sin(angle_el);
 	const float cos_el = cos(angle_el);
@@ -164,9 +161,11 @@ float FOC_Controlroller::velocity_open_loop() {
  * @return float: 当前角度
  */
 float FOC_Controlroller::angle_control(float target_angle) {
-	shaft_angle = KTH7823.getRawAngle() * DEG2RAD;
 	// PID 控制计算
-	float Uq = angle_loop(shaft_angle - target_angle * ANGLE_FACTOR);
+	float Uq = angle_loop(target_angle - rt_angle);
+	
+	// 获取电机轴角度
+	shaft_angle = KTH7823.getcurrent_sensorangle() * DEG2RAD;
 	// 设置相电压
 	setPhaseVoltage(Uq, 0.0f);
 	return shaft_angle;
@@ -179,13 +178,13 @@ float FOC_Controlroller::angle_control(float target_angle) {
  * @return float: 当前角度
  */
 float FOC_Controlroller::angle_control_NPID(float target_angle) {
-	shaft_angle = KTH7823.getRawAngle() * DEG2RAD;
-	float Kp = 20.0f;
+	static float Kp = 20.0f;
 	// 控制力矩计算
-	float Uq =
-		1.0f -
-		_constrain(Kp * (target_angle * ANGLE_FACTOR - DIR * shaft_angle),
-				   -voltage_power_supply, voltage_power_supply);
+	float Uq = _constrain(Kp * (target_angle - rt_angle), -voltage_power_supply,
+						  voltage_power_supply);
+
+	// 获取电机轴角度
+	shaft_angle = KTH7823.getcurrent_sensorangle() * DEG2RAD;
 	// 设置相电压
 	setPhaseVoltage(Uq, 0.0f);
 	return shaft_angle;
@@ -198,9 +197,11 @@ float FOC_Controlroller::angle_control_NPID(float target_angle) {
  * @return float: 当前速度
  */
 float FOC_Controlroller::velocity_control(float target_velocity) {
-	shaft_angle = KTH7823.getcurrent_sensorangle() * DEG2RAD;
 	// PID 控制计算
-	float Uq = vel_loop(rt_velocity - target_velocity);
+	float Uq = vel_loop(target_velocity - rt_velocity);
+
+	// 获取电机轴角度
+	shaft_angle = KTH7823.getcurrent_sensorangle() * DEG2RAD;
 	// 设置相电压
 	setPhaseVoltage(Uq, 0.0f);
 	return rt_velocity;

Robot_V2/Algorithm/DengFOC.h

@@ -35,12 +35,12 @@ class FOC_Controlroller {
 	float shaft_angle = 0;
 	float Ualpha, Ubeta = 0, Ua = 0, Ub = 0, Uc = 0;
 	float zero_electric_angle = 0;
-	float voltage_power_supply = 24.0f; // 电源电压
+	float voltage_power_supply = 24.0f;			// 电源电压
 	float v_half = voltage_power_supply * 0.5f; // 电源电压的一半
 
-	PIDController vel_loop =
-		PIDController{1.5f, 1.0f, 0.0005, 100000, voltage_power_supply};
-	PIDController angle_loop = PIDController{140.0f, 80.0f, 0.005f, 100000, voltage_power_supply};
+	PIDController vel_loop =PIDController{1.75f, 1.0f, 0.0002f, 1000000, voltage_power_supply};
+	PIDController angle_loop =
+		PIDController{140.0f, 80.0f, 0.005f, 10000000, voltage_power_supply};
 
 	void setPhaseVoltage(float Uq, float Ud);
 };

Robot_V2/Algorithm/pid.cpp

@@ -12,7 +12,7 @@ PIDController::PIDController(float P, float I, float D, float ramp, float limit)
 	  ,
 	  limit(limit) // PID控制器输出限幅
 	  ,
-	  error_prev(0.0f), output_prev(0.0f), integral_prev(0.0f), timestamp_prev(0) {}
+	  error_prev(0.0f), output_prev(0.0f), integral_prev(0.0f) {}
 
 // PID 控制器函数
 float PIDController::operator()(float error) {
@@ -29,6 +29,7 @@ float PIDController::operator()(float error) {
 	
 	// 将P,I,D三环的计算值加起来
 	float output = proportional + integral + derivative;
+	CAN0_transmiter.testfloat_union.f = D;
 	output = _constrain(output, -limit, limit);
 
 	// 输出斜坡限制
@@ -40,7 +41,7 @@ float PIDController::operator()(float error) {
 		else if (output_rate < -output_ramp)
 			output = output_prev - output_ramp * TIME_SLICE;
 	}
-	
+	CAN0_transmiter.testfloat_union.f = output;
 	// 保存值（为了下一次循环）
 	integral_prev = integral;
 	output_prev = output;