feat(FOC): 添加带速度限制的角度闭环控制功能

新增angle_control_with_velocity_limit函数，实现带速度限制的角度控制，
包含角度PID和速度PID双环控制，防止电机转速过快导致的不稳定。

- 新增带速度限制的角度控制函数
- 配置新的PID控制器用于速度内环和角度外环

Robot_V2/Algorithm/DengFOC.h

@@ -25,6 +25,7 @@ class FOC_Controlroller {
 	float Ua = 0, Ub = 0, Uc = 0;
 	float velocity_open_loop();
 	float angle_control(float target_angle);
+	float angle_control_with_velocity_limit(float target_angle);
 	float velocity_control(float target_velocity);
 	float current_control(float target_current);
 	float angle_control_NPID(float target_angle);
@@ -42,13 +43,16 @@ class FOC_Controlroller {
 	float zero_electric_angle = 0;
 	float v_half = voltage_power_supply * 0.5f; // 电源电压的一半
 
-
-	PIDController vel_loop = PIDController{0.028f, 0.2f, 1e-4, 1000000,
-										   voltage_power_supply}; // 12V
+	PIDController vel_loop =
+		PIDController{0.028f, 0.2f, 1e-4, 1000000, voltage_power_supply}; // 12V
 	PIDController angle_loop = PIDController{20.0f, 100.0f, 0.2f, 10000000,
 											 voltage_power_supply}; // 12V
 	PIDController current_loop =
 		PIDController{0.006f, 0.02f, 0, 100000, voltage_power_supply}; // 12V
+	PIDController vel_inVelAngloop =
+		PIDController{0.35f, 5.0f,1e-3, 1000000, voltage_power_supply};
+	PIDController ang_inVelAngloop =
+		PIDController{30.0f, 0.0f, 1e-3, 1000000, 999};
 
 	void setPhaseVoltage(float Uq, float Ud);
 	float cal_Iq_Id(float current_a, float current_b, float current_c);