feat(FOC): 重构FOC控制器实现并修复SVPWM算法支持

- 修复类名拼写错误：FOC_Controlroller -> FOC_Controller
- 移除不必要的角度归一化函数，直接使用fmod优化性能
- 添加完整的SVPWM算法实现，提高电机控制效率
- 重构PID参数配置，使用动态参数调整机制
- 增加电流反馈功能，实时监测电机运行状态
- 优化电压安全限制，提高系统稳定性
- 更新控制模式映射，支持更多控制策略
- 改进CAN通信协议，增加电流反馈传输

Robot_V2/Algorithm/FOC.h

@@ -5,52 +5,68 @@
 #include "pid.h"
 #include <cstdint>
 
-class FOC_Controlroller {
+class FOC_Controller {
 
   public:
-	FOC_Controlroller();
-	
+	FOC_Controller();
+
 	// 初始化函数
 	u8 init();
 
 	u8 control_mode = 0;
 	float limit_voltage = 0.0f;
-	float limit_velocity = 0.0f;
+
 	float target_angle = 0.0f;
 	float target_velocity = 0.0f;
 	float target_current = 0.0f;
+
 	float rt_angle = 0.0f;
 	float rt_velocity = 0.0f;
+	float rt_current = 0.0f;
+
 	bool over_current = false;
 	bool voltage_error = false;
 	bool is_shut_down = false;
 	float voltage_power_supply = 24.0f; // 电源电压
-	float v_safe = voltage_power_supply * 0.95f;
+	float v_safe = voltage_power_supply * 1.05f;
 	float duty_a = 0, duty_b = 0, duty_c = 0;
 	float Ua = 0, Ub = 0, Uc = 0;
+
+	float current_loop_p=DEFAULT_current_loop_p;
+
+	float vel_angloop_p=DEFAULT_vel_angloop_p;
+	float vel_loop_p=DEFAULT_vel_loop_p;
+
+	float ang_angloop_p=DEFAULT_ang_angloop_p;	
+	float angle_loop_p=DEFAULT_angle_loop_p;
+	
+	float vel_MIT_p=DEFAULT_vel_MIT_p;
+
 	float velocity_open_loop(void);
 	float angle_control(void);
 	float angle_control_with_velocity_limit(void);
+	float MIT_Mode(void);
 	float velocity_control(void);
 	float current_control(void);
 
-	void update_power(void);
+	void update_power(float voltage);
 	void DFOC_alignSensor(int _PP);
 	void shut_down(void);
 	void resume(void);
 	void drv_stop(void);
 	float drv_hold(void);
-	PIDController vel_loop = PIDController{
-		vel_loop_p * 100, 0.20f, 5e-5, 1000000, voltage_power_supply}; // 12V
-	PIDController angle_loop =
-		PIDController{angle_loop_p * 100, 100.0f, 0.2f, 10000000,
-					  voltage_power_supply}; // 12V
-	PIDController current_loop = PIDController{
-		current_loop_p * 100, 5e-7f, 7e-8, 500, 24}; // 12V
-	PIDController vel_inVelAngloop = PIDController{
-		vel_angloop_p * 100, 3.0f, 5e-4, 1000000, voltage_power_supply};
+
+	// 此处默认值不作数
+	PIDController vel_loop = PIDController{vel_loop_p * 100, 0.0f, 0, 10000,
+										   voltage_power_supply}; // 12V
+	PIDController angle_loop = PIDController{angle_loop_p * 100, 0, 0, 10000000,
+											 voltage_power_supply}; // 12V
+	PIDController current_loop = PIDController{current_loop_p * 100, 0, 0, 1500,
+											   voltage_power_supply}; // 12V
+	PIDController vel_inVelAngloop =	
+		PIDController{vel_angloop_p * 100, 0, 0, 1000000, v_safe};
 	PIDController ang_inVelAngloop =
-		PIDController{ang_angloop_p * 100, 0.0f, 5e-4, 1000000, 999};
+		PIDController{ang_angloop_p * 100, 0, 0, 20000, 100};
 
 	void update_pid_limits() {
 		vel_loop.limit = voltage_power_supply;
@@ -64,12 +80,15 @@ class FOC_Controlroller {
 	float Ualpha, Ubeta = 0;
 	float zero_electric_angle = 0;
 	float v_half = voltage_power_supply * 0.5f; // 电源电压的一半
+	float startup_ramp_val = 0.0f;      // 软启动系数
+	float startup_ramp_step = 0.01f;    // 启动斜坡步长，可调节以控制启动速度
 	void setPhaseVoltage(float Uq, float Ud);
+	void setSVPWM_PhaseVoltage(float Uq, float Ud);
 	float cal_Iq_Id(float current_a, float current_b, float current_c);
 };
 
 // 定义函数指针类型
-typedef float (FOC_Controlroller::*FOCFunctionPtr)(void);
+typedef float (FOC_Controller::*FOCFunctionPtr)(void);
 
 inline float _normalizeAngle(float angle);
 inline float _electricalAngle();