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

fix(FOC): 修正大角度下会导致角度控制失控的问题

Browse Source 
移除冗余的 `_electricalAngle` 函数，修正 `setPhaseVoltage` 中电气角度的计算方式
在定时器中断中增加实时角度更新，支持新增的无 PID 角度控制模式。
This commit is contained in:
cbb
2025-11-10 02:08:03 +08:00
parent b33574d392
commit 7a602dda48
6 changed files with 35 additions and 28 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Robot_V2/Algorithm/DengFOC.cpp
+16 -15
View File
@@ -52,10 +52,6 @@ inline float _normalizeAngle(float angle) {
// 取余运算可以用于归一化，列出特殊值例子算便知
return a >= 0 ? a : (a + 2 * PI);
}
inline float _electricalAngle() {
return _normalizeAngle((float)(DIR * PP) * KTH7823.read_sensorAngle() *
-zero_electric_angle);
}
/**
* @brief 电机控制类构造函数
*
@@ -71,7 +67,8 @@ FOC_Controlroller::FOC_Controlroller() { ; }
*/
inline void FOC_Controlroller::setPhaseVoltage(float Uq, float Ud) {
// 计算电气角度
float angle_el = _normalizeAngle(shaft_angle * PP + zero_electric_angle);
float angle_el = _normalizeAngle(shaft_angle * PP);
// 帕克逆变换 (直接计算，避免函数调用开销)
const float sin_el = sin(angle_el);
const float cos_el = cos(angle_el);
@@ -164,9 +161,11 @@ float FOC_Controlroller::velocity_open_loop() {
* @return float: 当前角度
*/
float FOC_Controlroller::angle_control(float target_angle) {
shaft_angle = KTH7823.getRawAngle() * DEG2RAD;
// PID 控制计算
float Uq = angle_loop(shaft_angle - target_angle * ANGLE_FACTOR);
float Uq = angle_loop(target_angle - rt_angle);
// 获取电机轴角度
shaft_angle = KTH7823.getcurrent_sensorangle() * DEG2RAD;
// 设置相电压
setPhaseVoltage(Uq, 0.0f);
return shaft_angle;
@@ -179,13 +178,13 @@ float FOC_Controlroller::angle_control(float target_angle) {
* @return float: 当前角度
*/
float FOC_Controlroller::angle_control_NPID(float target_angle) {
shaft_angle = KTH7823.getRawAngle() * DEG2RAD;
float Kp = 20.0f;
static float Kp = 20.0f;
// 控制力矩计算
float Uq =
1.0f -
_constrain(Kp * (target_angle * ANGLE_FACTOR - DIR * shaft_angle),
-voltage_power_supply, voltage_power_supply);
float Uq = _constrain(Kp * (target_angle - rt_angle), -voltage_power_supply,
voltage_power_supply);
// 获取电机轴角度
shaft_angle = KTH7823.getcurrent_sensorangle() * DEG2RAD;
// 设置相电压
setPhaseVoltage(Uq, 0.0f);
return shaft_angle;
@@ -198,9 +197,11 @@ float FOC_Controlroller::angle_control_NPID(float target_angle) {
* @return float: 当前速度
*/
float FOC_Controlroller::velocity_control(float target_velocity) {
shaft_angle = KTH7823.getcurrent_sensorangle() * DEG2RAD;
// PID 控制计算
float Uq = vel_loop(rt_velocity - target_velocity);
float Uq = vel_loop(target_velocity - rt_velocity);
// 获取电机轴角度
shaft_angle = KTH7823.getcurrent_sensorangle() * DEG2RAD;
// 设置相电压
setPhaseVoltage(Uq, 0.0f);
return rt_velocity;