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;
+	static float Kp = 20.0f;
-	float Kp = 20.0f;
 	// 控制力矩计算
-	float Uq =
+	float Uq = _constrain(Kp * (target_angle - rt_angle), -voltage_power_supply,
-		1.0f -
+						  voltage_power_supply);
-		_constrain(Kp * (target_angle * ANGLE_FACTOR - DIR * shaft_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 vel_loop =PIDController{1.75f, 1.0f, 0.0002f, 1000000, voltage_power_supply};
-		PIDController{1.5f, 1.0f, 0.0005, 100000, voltage_power_supply};
+	PIDController angle_loop =
-	PIDController angle_loop = PIDController{140.0f, 80.0f, 0.005f, 100000, voltage_power_supply};
+		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;

Robot_V2/device/ADC.cpp

@@ -1,12 +1,13 @@
 #include "adc.h"
 #include "Filter.h"
+#include "Can.h"
+#include "gd32c10x_can.h"
+#include "gd32c10x_eval.h"
 #include "math.h"
 #include <cstring>
-#include "gd32c10x_eval.h"
 #include <gd32c10x_dma.h>
 #include <stdint.h>
 
-
 constexpr float ERROR_ADC_VAL = 255.0f;
 constexpr float ADC_SCALE = 3.3f / 4096.0f;
 constexpr float TEMP_FACTOR = 1000.0f / 4.3f;
@@ -17,10 +18,11 @@ constexpr float CURRENT_FACTOR =
 	1.0f / 50.0f * 100.0f; // 50倍放大，100倍电流采样电阻, 单位mA
 constexpr float MAX_TEMP = 150.0f;
 constexpr float MIN_TEMP = -40.0f;
-constexpr float MAX_CURRENT = 65.0f;
+constexpr float MAX_CURRENT = 40.0f;
-constexpr float MIN_CURRENT = -0.1f;
+constexpr float MIN_CURRENT = -40.0f;
 constexpr float MAX_VCC = 65.0f;
 constexpr float MIN_VCC = -0.1f;
+extern CAN_transmiter CAN0_transmiter;
 
 u16 adc_value[6] = {0}; // 显式初始化数组元素为0
 /*!

Robot_V2/device/Timer.cpp

@@ -173,11 +173,14 @@ void TIMER2_IRQHandler(void) {
 		sampler.update_current(); // 更新电流采样值
 		KTH7823.update();		  // 更新角度传感器值
 		FOC.rt_velocity = Vel_kalman.update(KTH7823.getVelocity());
+		FOC.rt_angle = KTH7823.getAngle();
 
 		if (FOC.control_mode == 1) {
 			FOC.angle_control(FOC.target_angle); // 角度闭环控制
 		} else if (FOC.control_mode == 2) {
-			FOC.velocity_control(FOC.target_velocity);// 速度闭环控制
+			FOC.velocity_control(FOC.target_velocity); // 速度闭环控制
+		} else if (FOC.control_mode == 3) {
+			FOC.angle_control_NPID(FOC.target_angle); // 角度无PID控制
 		} else if (FOC.control_mode == 4) {
 			FOC.velocity_open_loop(); // 速度开环控制
 		}

Robot_V2/device/ang_sensor.cpp

@@ -15,7 +15,7 @@ constexpr float RAD_FACTOR = 1.0f / 65536.0f * 2.0f * 3.14159265358979323846f;
 constexpr u16 DEFAULT_COMMAND = 0x0000;
 constexpr u16 FAULT_RECEIVE = 0xFFFF;
 constexpr float VELOCITY_SCALE = 1e4f;
-constexpr float ANGLE_WRAP_THRESHOLD = 270.0f;
+constexpr float ANGLE_WRAP_THRESHOLD = 300.0f;
 constexpr float ANGLE_WRAP_VALUE = 360.0f;
 constexpr int SIZE_FLOAT = sizeof(float);
 
@@ -212,7 +212,7 @@ float angle_sensor::read_sensorAngle(void) {
 		SPI_CS_DISABLE();
 		current_angle_deg = u16data * ANGLE_FACTOR;
 		current_angle_rad = u16data * RAD_FACTOR;
-		return u16data * ANGLE_FACTOR;
+		return current_angle_deg;
 	} else {
 		SPI_CS_DISABLE();
 		// 处理读取错误