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

feat(FOC): 集成ARM数学库优化FOC算法实现

Browse Source 
- 引入arm_math.h库以使用ARM优化的数学函数
- 将角度归一化从弧度制改为角度制，使用360.0f替代2*PI
- 使用arm_sin_cos_f32替代查找表实现正弦余弦计算
- 移除注释中的ARM Clark和Park变换函数调用
- 修正机械角度计算中的时间常量为TIMER2_FREQUENCY倒数
- 移除传感器角度到弧度的转换，保持原始角度值
- 激活电流环控制中的Iq/Id计算并发送CAN数据
This commit is contained in:
cbb
2026-01-07 23:13:21 +08:00
parent fccb93dd41
commit 72506ce87d
14 changed files with 133 additions and 452 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Robot_V2/Algorithm/FOC.cpp
+24 -22
View File
@@ -6,11 +6,11 @@
#include "config.h"
#include "motor.h"
#include "pid.h"
#include <arm_math.h>
#include <cmath>
#include <gd32c10x.h>
#include <stdint.h>
FOC_Controlroller FOC; // FOC控制初始化
extern sampler sampler;
extern angle_sensor KTH7823;
@@ -22,16 +22,16 @@ extern CAN_transmiter CAN0_transmiter;
#define constrain(value, min, max, target_min, target_max) \
((value - min) / (max - min) * (target_max - target_min))
/**
/**@
* @brief 归一化角度
*
* @param angle
* @return float
*/
inline float _normalizeAngle(float angle) {
float a = fmod(angle, 2 * PI);
float a = fmod(angle, 360.0f);
// 取余运算可以用于归一化，列出特殊值例子算便知
return a >= 0 ? a : (a + 2 * PI);
return a >= 0 ? a : (a + 360.0f);
}
// 通过Ia,Ib,Ic计算Iq,Id(目前仅输出Iq)
@@ -40,16 +40,19 @@ inline float FOC_Controlroller::cal_Iq_Id(float current_a, float current_b,
// 计算电气角度
float angle_el = _normalizeAngle(shaft_angle * PP);
float I_alpha, I_beta;
// Clarke变换 (使用三相电流)
float I_alpha = (2.0f / 3.0f) * current_a - (1.0f / 3.0f) * current_b -
(1.0f / 3.0f) * current_c;
float I_beta = _1_SQRT3 * current_b - _1_SQRT3 * current_c;
// arm_clarke_f32(current_a, current_b, &I_alpha, &I_beta);
I_alpha = (2.0f / 3.0f) * current_a - (1.0f / 3.0f) * current_b -
(1.0f / 3.0f) * current_c;
I_beta = _1_SQRT3 * current_b - _1_SQRT3 * current_c;
// Park变换
float sin_el, cos_el;
cos_el = cos_lut[(u16)(angle_el * LUT_SCALE) & LUT_MASK];
sin_el = sin_lut[(u16)(angle_el * LUT_SCALE) & LUT_MASK];
float I_q = I_beta * cos_el - I_alpha * sin_el;
arm_sin_cos_f32(angle_el, &sin_el, &cos_el);
float I_q, I_d;
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; // 返回Iq值，乘以1000以转换为mA
}
@@ -73,8 +76,7 @@ inline void FOC_Controlroller::setPhaseVoltage(float Uq, float Ud) {
// 帕克逆变换 (直接计算，避免函数调用开销)
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];
arm_sin_cos_f32(angle_el, &sin_el, &cos_el);
float Ualpha = Ud * cos_el - Uq * sin_el;
float Ubeta = Ud * sin_el + Uq * cos_el;
@@ -93,7 +95,6 @@ inline void FOC_Controlroller::setPhaseVoltage(float Uq, float Ud) {
duty_b = duty_b * voltage_exchange;
duty_c = duty_c * voltage_exchange;
Set_3Pwm(duty_a, duty_b, duty_c);
// std::printf("PWM:%f,%f,%f\n", dc_a, dc_b, dc_c);
}
/**
@@ -156,8 +157,8 @@ void FOC_Controlroller::update_power(void) {
float FOC_Controlroller::velocity_open_loop() {
// 通过时间间隔和目标速度计算需要转动的机械角度，存储在shaft_angle中，在此之前，还需要对轴角度归一化
// 预计算常量 1e-4 = 100微秒控制周期
shaft_angle =
_normalizeAngle(shaft_angle + target_velocity * ANGLE_FACTOR * 1e-4f);
shaft_angle = _normalizeAngle(shaft_angle + target_velocity * ANGLE_FACTOR *
1.0 / TIMER2_FREQUENCY);
// 因为在开环控制中，Ud始终为0，只控制Uq
const float Uq = voltage_power_supply / 2.0f;
@@ -195,7 +196,7 @@ float FOC_Controlroller::angle_control() {
float Uq = angle_loop(target_angle - rt_angle);
// 获取电机轴角度
shaft_angle = KTH7823.getcurrent_sensorangle() * DEG2RAD;
shaft_angle = KTH7823.getcurrent_sensorangle();
// 设置相电压
setPhaseVoltage(Uq, 0.0f);
return shaft_angle;
@@ -224,7 +225,7 @@ float FOC_Controlroller::angle_control_with_velocity_limit() {
// CAN0_transmiter.testfloat_union.f = target_velocity - rt_velocity;
// 获取电机轴角度
shaft_angle = KTH7823.getcurrent_sensorangle() * DEG2RAD;
shaft_angle = KTH7823.getcurrent_sensorangle();
// 设置相电压
setPhaseVoltage(Uq, 0.0f);
@@ -243,12 +244,13 @@ float FOC_Controlroller::velocity_control() {
float Uq = vel_loop((target_velocity - rt_velocity) * 25);
// 获取电机轴角度
shaft_angle = KTH7823.getcurrent_sensorangle() * DEG2RAD;
shaft_angle = KTH7823.getcurrent_sensorangle();
// 设置相电压
setPhaseVoltage(Uq, 0.0f);
// CAN0_transmiter.testfloat_union.f =
cal_Iq_Id(sampler.get_current('U'), sampler.get_current('V'),
sampler.get_current('W'));
CAN0_transmiter.testfloat_union.f =
cal_Iq_Id(sampler.get_current('U'), sampler.get_current('V'),
sampler.get_current('W'));
return rt_velocity;
}
@@ -259,7 +261,7 @@ float FOC_Controlroller::current_control() {
float Uq = current_loop(target_current - Iq);
CAN0_transmiter.testfloat_union.f = Iq;
// 获取电机轴角度
shaft_angle = KTH7823.getcurrent_sensorangle() * DEG2RAD;
shaft_angle = KTH7823.getcurrent_sensorangle();
// 设置相电压
setPhaseVoltage(Uq, 0.0f);
return 1;