feat(FOC): 添加FOC控制器初始化函数、支持电压自适应PID调整

添加了FOC控制器的init()方法，用于统一初始化电机控制参数，
包括速度环、角度环和电流环的PID参数设置。同时优化了CAN通信
中的PID参数计算逻辑，引入电压补偿机制，提高了电机控制精度。

Robot_V2/main.cpp

@@ -40,7 +40,7 @@ int main(void) {
 
 	angsens_MG_check(); // 角度传感器磁感应强度测试
 	watchdog_init(50);	// 看门狗初始化
-	
+
 	while (1) {
 		__disable_irq();
 		u32 currentTasks =
@@ -58,9 +58,8 @@ u8 hardware_init(void) {
 	systick_config();  // 系统时钟初始化
 	unused_pin_init(); // 未使用引脚初始化
 	USART0_init();	   // 调试串口初始化
-	motor_init();	   // 电机驱动初始化
 	User_timer_init(); // 任务Timer初始化
-	
+
 	u8 init_status = 0;
 	init_status += KTH7823.init(); // 角度传感器初始化
 	init_status += sampler.init(); // 采样器初始化
@@ -69,6 +68,7 @@ u8 hardware_init(void) {
 	init_status += Angle_kalman.init(KTH7823.getAngle()); // 角度滤波初始化
 	init_status += Vel_kalman.init(0.0f);				  // 速度滤波初始化
 	init_status += CAN0_transmiter.init();				  // CAN初始化
+	init_status += FOC.init(); // FOC初始化
 	return init_status;
 }
 
@@ -120,7 +120,7 @@ void do_task(u32 currentTasks) {
 	if (currentTasks & (1 << TASK_VBUS_UPDATE)) { // 更新vbus电压值
 		power_manage();
 	}
-	if (currentTasks & (1 << TASK_CAN_TRANSMIT)) {	  // CAN通信
+	if (currentTasks & (1 << TASK_CAN_TRANSMIT)) { // CAN通信
 		// CAN0_transmiter.testfloat_union.f = cnt_test; // 负载检测
 		cnt_test = 0;
 		CAN0_transmiter.can0_send_flag = SET;