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

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

Robot_V2/Algorithm/FOC.cpp

@@ -1,5 +1,6 @@
 #include "FOC.h"
 #include "Can.h"
+#include "Filter.h"
 #include "Timer.h"
 #include "adc.h"
 #include "ang_sensor.h"
@@ -9,14 +10,14 @@
 #include <arm_math.h>
 #include <cmath>
 #include <gd32c10x.h>
-#include "Filter.h"
 #include <stdint.h>
 
+
 FOC_Controlroller FOC; // FOC控制初始化
 extern sampler sampler;
 extern angle_sensor KTH7823;
 extern CAN_transmiter CAN0_transmiter;
-extern EncoderVelocityKalman Vel_kalman;	// 速度滤波
+extern EncoderVelocityKalman Vel_kalman; // 速度滤波
 
 // 初始变量及函数定义
 #define _constrain(amt, low, high)                                             \
@@ -56,7 +57,29 @@ inline float FOC_Controlroller::cal_Iq_Id(float current_a, float current_b,
 	I_q = I_beta * cos_el - I_alpha * sin_el;
 	// arm_park_f32(I_alpha, I_beta, &I_d, &I_q, sin_el, cos_el);
 
-	return I_q * 1000.0f - KE_FACTOR * rt_velocity; // 返回Iq值，乘以1000以转换为mA
+	return I_q * 1000.0f -
+		   KE_FACTOR * rt_velocity; // 返回Iq值，乘以1000以转换为mA
+}
+
+/**
+ * @brief 电机控制类初始化函数
+ *
+ * @return u8 0表示初始化成功，1表示初始化失败
+ */
+u8 FOC_Controlroller::init() {
+	motor_init();									 // 电机驱动初始化
+	vel_loop.set_PID(vel_loop_p * 100 / limit_voltage, 0.20f, 5e-5); // 设置电机速度控制参数
+	angle_loop.set_PID(angle_loop_p * 100 / limit_voltage, 100.0f,
+					   0.2f); // 设置电机角度控制参数
+	current_loop.set_PID(current_loop_p * 100 / limit_voltage, 5e-7f,
+						 7e-8); // 设置电机电流控制参数
+
+	vel_inVelAngloop.set_PID(vel_angloop_p * 100 / limit_voltage, 3.0f,
+							 5e-4); // 设置电机速度控制参数
+	ang_inVelAngloop.set_PID(ang_angloop_p * 100 / limit_voltage, 0.0f,
+							 5e-4); // 设置电机角度控制参数
+
+	return 0;
 }
 
 /**
@@ -158,9 +181,9 @@ void FOC_Controlroller::update_power(void) {
  */
 float FOC_Controlroller::velocity_open_loop() {
 	// 通过时间间隔和目标速度计算需要转动的机械角度，存储在shaft_angle中，在此之前，还需要对轴角度归一化
-	shaft_angle = _normalizeAngle(shaft_angle +
-								  target_velocity * ANGLE_FACTOR *
-									  REDUCTION_RATIO * 1.0 / TIMER2_FREQUENCY);
+	shaft_angle = _normalizeAngle(shaft_angle + target_velocity * ANGLE_FACTOR *
+													REDUCTION_RATIO * 1.0 /
+													TIMER2_FREQUENCY);
 
 	// 因为在开环控制中，Ud始终为0，只控制Uq
 	const float Uq = voltage_power_supply / 2.0f;