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

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

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;
 }