feat(FOC): 优化开环速度控制逻辑并提升性能

refactor(ADC): 重构采样逻辑并封装为 sampler 类
refactor(CAN): 更新 CAN 发送器以使用新的sampler 接口
refactor(timer): 调整定时器中断结构并优化任务调度

Robot_V2/Algorithm/DengFOC.cpp

@@ -8,13 +8,15 @@
 constexpr double PI = 3.1415926535897932384626433832795f;
 constexpr float _3PI_2 = 4.71238898038f;
 constexpr u8 PP = 11, DIR = 1;
+constexpr float sqrt3 = 1.73205080757f; // sqrt(3)的预计算值
 // 初始变量及函数定义
 #define _constrain(amt, low, high)                                             \
 	((amt) < (low) ? (low) : ((amt) > (high) ? (high) : (amt)))
 #define constrain(value, min, max, target_min, target_max)                     \
 	((value - min) / (max - min) * (target_max - target_min))
 
-float _electricalAngle(float shaft_angle) { return shaft_angle * PP; }
+
+inline float _electricalAngle(float shaft_angle) { return shaft_angle * PP; }
 
 /**
  * @brief 归一化角度
@@ -22,7 +24,7 @@ float _electricalAngle(float shaft_angle) { return shaft_angle * PP; }
  * @param angle
  * @return float
  */
-float _normalizeAngle(float angle) {
+inline float _normalizeAngle(float angle) {
 	float a = fmod(angle, 2 * PI);
 	// 取余运算可以用于归一化，列出特殊值例子算便知
 	return a >= 0 ? a : (a + 2 * PI);
@@ -78,18 +80,37 @@ void FOC_Controlroller::setPhaseVoltage(float Uq, float Ud, float angle_el) {
  * @return float: q轴电压
  */
 float FOC_Controlroller::velocity_open_loop(float target_velocity) {
-	now_us = get_system_runtime_us();
-	float dt = (now_us - last_time_us) * 1e-6f;
-	// 限制时间间隔
-	if (dt <= 0.0f || dt > 0.5f)
-		dt = 1e-3f;
 	// 通过时间间隔和目标速度计算需要转动的机械角度，存储在shaft_angle中，在此之前，还需要对轴角度归一化
-	shaft_angle = _normalizeAngle(shaft_angle + target_velocity * dt);
+	// 预计算常量 1e-4 = 100微秒控制周期 / 1秒 = 100e-6
+	shaft_angle = _normalizeAngle(shaft_angle + target_velocity * 1e-4f);
+	
+	// 直接计算电气角度，避免函数调用开销
+	float angle_el = shaft_angle * PP + zero_electric_angle;
+	angle_el = _normalizeAngle(angle_el);
+	
+	// 因为在开环控制中，Ud始终为0，只控制Uq
+	const float Uq = limit_voltage;
+	const float Ud = 0.0f;
+	
+	// 帕克逆变换 (直接计算，避免函数调用开销)
+	const float sin_el = sin(angle_el);
+	const float cos_el = cos(angle_el);
+	Ualpha = -Uq * sin_el;
+	Ubeta = Uq * cos_el;
 
-	float uq = limit_voltage; // 设定一个固定的q轴电压
-	setPhaseVoltage(uq, 0, _electricalAngle(shaft_angle));
-	last_time_us = now_us;
-	return uq;
+	// 克拉克逆变换 (预计算常量值)
+	const float v_half = voltage_power_supply * 0.5f; // 电源电压的一半
+	Ua = Ualpha + v_half;
+	Ub = (sqrt3 * Ubeta - Ualpha) * 0.5f + v_half;
+	Uc = (-Ualpha - sqrt3 * Ubeta) * 0.5f + v_half;
+	
+	// 计算占空比
+	const float dc_a = _constrain(Ua / voltage_power_supply, 0.0f, 1.0f);
+	const float dc_b = _constrain(Ub / voltage_power_supply, 0.0f, 1.0f);
+	const float dc_c = _constrain(Uc / voltage_power_supply, 0.0f, 1.0f);
+	Set_3Pwm(dc_a, dc_b, dc_c);
+	
+	return limit_voltage;
 }
 
 /**