fix(FOC): 修复角度控制中的冗余CAN传输与传感器角度获取错误

- 修正速度控制中角度传感器数据读取错误，使用current_sensorangle替代getRawAngle
- 在Timer中断中统一使用angle_control进行角度闭环控制
- 完善角度传感器类的角度读取逻辑，增加current_angle_deg和current_angle_rad成员变量
- 添加getcurrent_sensorangle接口用于获取当前原始角度值

Robot_V2/Algorithm/DengFOC.cpp

@@ -167,8 +167,6 @@ float FOC_Controlroller::angle_control(float target_angle) {
 	shaft_angle = KTH7823.getRawAngle() * DEG2RAD;
 	// PID 控制计算
 	float Uq = angle_loop(shaft_angle - target_angle * ANGLE_FACTOR);
-	CAN0_transmiter.testfloat_union.f =
-		angle_loop(target_angle * ANGLE_FACTOR - shaft_angle);
 	// 设置相电压
 	setPhaseVoltage(Uq, 0.0f);
 	return shaft_angle;
@@ -200,11 +198,9 @@ float FOC_Controlroller::angle_control_NPID(float target_angle) {
  * @return float: 当前速度
  */
 float FOC_Controlroller::velocity_control(float target_velocity) {
-	shaft_angle = KTH7823.getRawAngle() * DEG2RAD;
+	shaft_angle = KTH7823.getcurrent_sensorangle() * DEG2RAD;
 	// PID 控制计算
 	float Uq = vel_loop(rt_velocity - target_velocity);
-
-	CAN0_transmiter.testfloat_union.f = Uq;
 	// 设置相电压
 	setPhaseVoltage(Uq, 0.0f);
 	return rt_velocity;

Robot_V2/Algorithm/pid.cpp

@@ -12,22 +12,26 @@ 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) {}
+	  error_prev(0.0f), output_prev(0.0f), integral_prev(0.0f), timestamp_prev(0) {}
 
 // PID 控制器函数
 float PIDController::operator()(float error) {
 	// P环
 	float proportional = P * error;
+	
 	// Tustin 散点积分（I环）
-	float integral =
-		integral_prev + I * TIME_SLICE * 0.5f * (error + error_prev);
+	// 添加积分限幅，防止积分饱和
+	float integral = integral_prev + I * TIME_SLICE * 0.5f * (error + error_prev);
 	integral = _constrain(integral, -limit, limit);
+	
 	// D环（微分环节）
 	float derivative = D * (error - error_prev) / TIME_SLICE;
+	
 	// 将P,I,D三环的计算值加起来
 	float output = proportional + integral + derivative;
 	output = _constrain(output, -limit, limit);
 
+	// 输出斜坡限制
 	if (output_ramp > 0) {
 		// 对PID的变化速率进行限制
 		float output_rate = (output - output_prev) / TIME_SLICE;
@@ -36,6 +40,7 @@ float PIDController::operator()(float error) {
 		else if (output_rate < -output_ramp)
 			output = output_prev - output_ramp * TIME_SLICE;
 	}
+	
 	// 保存值（为了下一次循环）
 	integral_prev = integral;
 	output_prev = output;
@@ -53,7 +58,7 @@ float PIDController::operator()(float error) {
  * @return void
  */
 void PIDController::set_PID(float P, float I, float D) {
-	P = P;
-	I = I;
-	D = D;
+	this->P = P;
+	this->I = I;
+	this->D = D;
 }

Robot_V2/device/Timer.cpp

@@ -175,7 +175,7 @@ void TIMER2_IRQHandler(void) {
 		FOC.rt_velocity = Vel_kalman.update(KTH7823.getVelocity());
 
 		if (FOC.control_mode == 1) {
-			FOC.angle_control_NPID(FOC.target_angle); // 角度闭环控制
+			FOC.angle_control(FOC.target_angle); // 角度闭环控制
 		} else if (FOC.control_mode == 2) {
 			FOC.velocity_control(FOC.target_velocity);// 速度闭环控制
 		} else if (FOC.control_mode == 4) {

Robot_V2/device/ang_sensor.cpp

@@ -210,6 +210,8 @@ float angle_sensor::read_sensorAngle(void) {
 	// 使用DMA发送命令并接收数据
 	if (!spi_rw_half_word_dma(SPI0, DEFAULT_COMMAND, &u16data)) {
 		SPI_CS_DISABLE();
+		current_angle_deg = u16data * ANGLE_FACTOR;
+		current_angle_rad = u16data * RAD_FACTOR;
 		return u16data * ANGLE_FACTOR;
 	} else {
 		SPI_CS_DISABLE();
@@ -223,17 +225,16 @@ float angle_sensor::read_sensorAngle(void) {
  * @return 累积角度值
  */
 float angle_sensor::update(void) {
-
-	float currentAngle = read_sensorAngle();
+	read_sensorAngle();
 
 	if (m_firstUpdate) {
-		m_lastAngle = currentAngle;
+		m_lastAngle = current_angle_deg;
 		m_firstUpdate = false;
 		return m_cumulativeAngle;
 	}
 
 	// 计算角度变化
-	float delta = currentAngle - m_lastAngle;
+	float delta = current_angle_deg - m_lastAngle;
 
 	// 处理角度绕回情况（0度和360度之间的跳变）
 	// 如果变化超过180度，说明发生了绕回
@@ -249,7 +250,7 @@ float angle_sensor::update(void) {
 	velocity[VEL_FILTER_SIZE - 1] = delta * VELOCITY_SCALE;
 
 	m_cumulativeAngle += delta;
-	m_lastAngle = currentAngle;
+	m_lastAngle = current_angle_deg;
 
 	return m_cumulativeAngle;
 }
@@ -288,6 +289,14 @@ float angle_sensor::getRawVelocity(void) const {
 	return velocity_filted / (VEL_FILTER_SIZE - 1);
 	// return velocity[VEL_FILTER_SIZE - 1];
 }
+/**
+ * @brief 获取角度值(原始级)
+ * @return 角度值(原始级)
+ */
+float angle_sensor::getcurrent_sensorangle(void) const { 
+	return current_angle_deg;
+}
+
 
 /**
  * @brief 重置累积角度值

Robot_V2/device/ang_sensor.h

@@ -23,12 +23,15 @@ class angle_sensor {
 	float getRawAngle(void) const;
 	float getVelocity(void) const;
 	float getRawVelocity(void) const;
+	float getcurrent_sensorangle(void) const;
 
   protected:
 	void KTH7823_init(void);
+	float m_cumulativeAngle;
+	float current_angle_deg;
+	float current_angle_rad;
 	bool m_firstUpdate;
 	float m_lastAngle;
-	float m_cumulativeAngle;
 	float m_lastcumulativeAngle;
 	float velocity[VEL_FILTER_SIZE] = {0.0f};
 };