cbb/Motor_v2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

fix:修复任务调度不准确的问题，

Browse Source 
feat:优化电流采样速率
feat:优化PIDramp效率
feat:优化角度采样效率
fix:回滚CAN打包逻辑
feat:去除timer3
feat(FOC): 优化帕克逆变换与克拉克逆变换计算方式
将原有的三角函数计算替换为查表法，提高运算效率。同时添加了 sqrt(3)/2 的预计算常量，并优化了电压限制部分的计算逻辑，减少重复运算。
This commit is contained in:
cbb
2025-11-23 18:31:47 +08:00
parent 59f161bd73
commit 871e33e10f
14 changed files with 503 additions and 189 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Robot_V2/Algorithm/DengFOC.cpp
+16 -12
View File
@@ -17,8 +17,10 @@ constexpr float _2_SQRT3 = 1.15470053838f;
constexpr float _3PI_2 = 4.71238898038f;
constexpr u8 PP = 11;
constexpr float DIR = 1.0;
constexpr float sqrt3 = 1.73205080757f; // sqrt(3)的预计算值
constexpr float sqrt3 = 1.73205080757f; // sqrt(3)的预计算值
constexpr float sqrt3dv2 = 0.86602540378f; // sqrt(3)/2的预计算值
constexpr float zero_electric_angle = 0.0f;
constexpr u16 LUT_SCALE = (LUT_SIZE / (2.0f * PI));
FOC_Controlroller FOC; // FOC控制初始化
extern sampler sampler;
@@ -63,24 +65,26 @@ inline void FOC_Controlroller::setPhaseVoltage(float Uq, float Ud) {
float angle_el = _normalizeAngle(shaft_angle * PP);
// 帕克逆变换 (直接计算，避免函数调用开销)
const float sin_el = sin(angle_el);
const float cos_el = cos(angle_el);
Ualpha = -Uq * sin_el;
Ubeta = Uq * cos_el;
float sin_el, cos_el;
sin_el = sin_lut[(u16)(angle_el * LUT_SCALE) & LUT_MASK];
cos_el = cos_lut[(u16)(angle_el * LUT_SCALE) & LUT_MASK];
float Ualpha = Ud * cos_el - Uq * sin_el;
float Ubeta = Ud * sin_el + Uq * cos_el;
// 克拉克逆变换 (预计算常量值)
// 克拉克逆变换 (使用预计算常量优化)
Ua = Ualpha + v_half;
Ub = (sqrt3 * Ubeta - Ualpha) * 0.5f + v_half;
Uc = (-Ualpha - sqrt3 * Ubeta) * 0.5f + v_half;
Ub = sqrt3dv2 * Ubeta - 0.5f * Ualpha + v_half;
Uc = -0.5f * Ualpha - sqrt3dv2 * Ubeta + v_half;
// 计算占空比
// 计算占空比并限制范围
float dc_a = _constrain(Ua / v_safe, 0.0f, 1.0f);
float dc_b = _constrain(Ub / v_safe, 0.0f, 1.0f);
float dc_c = _constrain(Uc / v_safe, 0.0f, 1.0f);
// 设置PWM输出
dc_a = dc_a * limit_voltage / v_safe;
dc_b = dc_b * limit_voltage / v_safe;
dc_c = dc_c * limit_voltage / v_safe;
float voltage_exchange = limit_voltage / v_safe;
dc_a = dc_a * voltage_exchange;
dc_b = dc_b * voltage_exchange;
dc_c = dc_c * voltage_exchange;
Set_3Pwm(dc_a, dc_b, dc_c);
// std::printf("PWM:%f,%f,%f\n", dc_a, dc_b, dc_c);
}