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

```

Browse Source 
feat(DengFOC):
新增了基于三相电流采样的dq轴电流计算函数`cal_Iq_Id`
调整了开环控制中的电压限制逻辑
更新了默认电源电压值为24V。
KHT7823测试代码
```
This commit is contained in:
cbb
2025-10-30 20:20:14 +08:00
parent 5d14731af5
commit 180a46271c
10 changed files with 318 additions and 364 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Robot_V2/Algorithm/DengFOC.cpp
+81 -7
View File
@@ -1,23 +1,41 @@
#include "DengFOC.h"
#include "Filter.h"
#include "Timer.h"
#include "adc.h"
#include "pid.h"
#include "systick.h"
#include <cmath>
#include <cstdio>
#include <gd32c10x.h>
#include <stdint.h>
constexpr double PI = 3.1415926535897932384626433832795f;
constexpr float _1_SQRT3 = 0.57735026919f;
constexpr float _2_SQRT3 = 1.15470053838f;
constexpr float _3PI_2 = 4.71238898038f;
constexpr u8 PP = 11, DIR = 1;
constexpr float sqrt3 = 1.73205080757f; // sqrt(3)的预计算值
constexpr float zero_electric_angle = 0.0f;
extern sampler sampler;
CurrentKalman curU_kalman(0.0f, 0.0026f, 4.8f, 0.0001f, 1e-6f, 0.02f);
CurrentKalman curV_kalman(0.0f, 0.0026f, 4.8f, 0.0001f, 1e-6f, 0.02f);
CurrentKalman curW_kalman(0.0f, 0.0026f, 4.8f, 0.0001f, 1e-6f, 0.02f);
PIDController vel_loop_M0 = PIDController{2, 0, 0, 100000, 24.0 / 2.0};
PIDController angle_loop_M0 = PIDController{2, 0, 0, 100000, 100};
PIDController current_loop_M0 = PIDController{2, 0, 0, 100000, 800};
// 初始变量及函数定义
#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))
inline float _electricalAngle(float shaft_angle) { return shaft_angle * PP; }
inline float _electricalAngle(float shaft_angle) {
return _normalizeAngle((float)(DIR * PP) * shaft_angle -
zero_electric_angle);
}
/**
* @brief 归一化角度
@@ -90,7 +108,7 @@ float FOC_Controlroller::velocity_open_loop(float target_velocity) {
angle_el = _normalizeAngle(angle_el);
// 因为在开环控制中，Ud始终为0，只控制Uq
const float Uq = limit_voltage;
const float Uq = voltage_power_supply / 2.0f;
const float Ud = 0.0f;
// 帕克逆变换 (直接计算，避免函数调用开销)
@@ -106,9 +124,9 @@ float FOC_Controlroller::velocity_open_loop(float target_velocity) {
Uc = (-Ualpha - sqrt3 * Ubeta) * 0.5f + v_half;
// 计算占空比
Ua = constrain(Ua, 0.0f, voltage_power_supply, 0.0f, limit_voltage);
Ub = constrain(Ub, 0.0f, voltage_power_supply, 0.0f, limit_voltage);
Uc = constrain(Uc, 0.0f, voltage_power_supply, 0.0f, limit_voltage);
Ua = constrain(Ua, 0.0f, voltage_power_supply, 0.0f, limit_voltage * 2);
Ub = constrain(Ub, 0.0f, voltage_power_supply, 0.0f, limit_voltage * 2);
Uc = constrain(Uc, 0.0f, voltage_power_supply, 0.0f, limit_voltage * 2);
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);
@@ -141,4 +159,60 @@ void FOC_Controlroller::shut_down() {
void FOC_Controlroller::resume() {
is_shut_down = 0;
gpio_bit_set(GPIOB, GPIO_PIN_12);
}
}
// 通过Ia,Ib,Ic计算Iq,Id(目前仅输出Iq)
float cal_Iq_Id(float current_a, float current_b, float angle_el) {
float I_alpha = current_a;
float I_beta = _1_SQRT3 * current_a + _2_SQRT3 * current_b;
float ct = cos(angle_el);
float st = sin(angle_el);
// float I_d = I_alpha * ct + I_beta * st;
float I_q = I_beta * ct - I_alpha * st;
return I_q;
}
void FOC_Controlroller::setTorque(float Uq, float angle_el) {
Uq =
_constrain(Uq, -(voltage_power_supply) / 2, (voltage_power_supply) / 2);
float Ud = 0;
angle_el = _normalizeAngle(angle_el);
// 帕克逆变换
Ualpha = -Uq * sin(angle_el);
Ubeta = Uq * cos(angle_el);
// 克拉克逆变换
Ua = Ualpha + voltage_power_supply / 2;
Ub = (sqrt(3) * Ubeta - Ualpha) / 2 + voltage_power_supply / 2;
Uc = (-Ualpha - sqrt(3) * Ubeta) / 2 + voltage_power_supply / 2;
driver_output(Ua, Ub, Uc);
}
float FOC_Controlroller::DFOC_M0_Current() {
// 使用卡尔曼滤波后的三相电流计算Iq
float Iu_filtered = curU_kalman.update(sampler.get_current('U'), Ua);
float Iv_filtered = curV_kalman.update(sampler.get_current('V'), Ub);
float Iw_filtered = curW_kalman.update(sampler.get_current('W'), Uc);
// 根据基尔霍夫电流定律验证三相电流总和是否接近0
// float current_sum = Iu_filtered + Iv_filtered + Iw_filtered;
// 用三相滤波后的电流计算Iq（通过Clarke和Park变换）
// 方法1: 使用U和V相计算
float I_q_M0_ori =
cal_Iq_Id(Iu_filtered, Iv_filtered, _electricalAngle(shaft_angle));
// 方法2: 也可以使用其他两相的组合计算，然后取平均值以提高精度
// float I_q_UW = cal_Iq_Id(Iu_filtered, Iw_filtered,
// _electricalAngle(shaft_angle)); float I_q_VW = cal_Iq_Id(Iv_filtered,
// Iw_filtered, _electricalAngle(shaft_angle)); float I_q_M0_ori = (I_q_UV +
// I_q_UW + I_q_VW) / 3.0f;
return I_q_M0_ori;
// return 0;
}
void FOC_Controlroller::DFOC_M0_setTorque(float Target) // 电流力矩环
{
// setTorque(current_loop_M0(Target-DFOC_M0_Current()),_electricalAngle());
}
Robot_V2/Algorithm/DengFOC.h
+6 -9
View File
@@ -1,9 +1,3 @@
// 灯哥开源，遵循GNU协议，转载请著名版权！
// GNU开源协议（GNU General Public License,
// GPL）是一种自由软件许可协议，保障用户能够自由地使用、研究、分享和修改软件。
// 该协议的主要特点是，要求任何修改或衍生的作品必须以相同的方式公开发布，即必须开源。此外，该协议也要求在使用或分发软件时，必须保留版权信息和许可协议。GNU开源协议是自由软件基金会（FSF）制定和维护的一种协议，常用于GNU计划的软件和其他自由软件中。
// 仅在DengFOC官方硬件上测试过，欢迎硬件购买/支持作者，淘宝搜索店铺：灯哥开源
// 你的支持将是接下来做视频和持续开源的经费，灯哥在这里先谢谢大家了
// 函数声明
#ifndef _DENGFOC_H
#define _DENGFOC_H
@@ -13,7 +7,7 @@ class FOC_Controlroller {
FOC_Controlroller();
float control_mode = 0;
float limit_voltage = 2.0f;
float limit_voltage = 0.0f;
float limit_velocity = 0.0f;
float target_angle = 0.0f;
float target_velocity = 0.0f;
@@ -30,10 +24,13 @@ class FOC_Controlroller {
float shaft_angle = 0;
float Ualpha, Ubeta = 0, Ua = 0, Ub = 0, Uc = 0;
float zero_electric_angle = 0;
float voltage_power_supply = 12.0f; // 电源电压
float voltage_power_supply = 24.0f; // 电源电压
void driver_output(float Ua, float Ub, float Uc);
void setPhaseVoltage(float Uq, float Ud, float angle_el);
void setTorque(float Uq,float angle_el);
float DFOC_M0_Current();
void DFOC_M0_setTorque(float Target);
};
float _normalizeAngle(float angle);
@@ -42,4 +39,4 @@ float _electricalAngle(float shaft_angle);
float DFOC_M0_Angle();
float serial_motor_target();
#endif
#endif
Robot_V2/Algorithm/pid.cpp
+52
View File
@@ -0,0 +1,52 @@
#include "pid.h"
#include "Timer.h"
#define _constrain(amt,low,high) ((amt)<(low)?(low):((amt)>(high)?(high):(amt)))
PIDController::PIDController(float P, float I, float D, float ramp, float limit)
: P(P)
, I(I)
, D(D)
, output_ramp(ramp) // PID控制器加速度限幅
, limit(limit) // PID控制器输出限幅
, error_prev(0.0f)
, output_prev(0.0f)
, integral_prev(0.0f)
{
timestamp_prev = get_system_runtime_us() ;
}
// PID 控制器函数
float PIDController::operator() (float error){
// 计算两次循环中间的间隔时间
unsigned long timestamp_now = get_system_runtime_us();
float Ts = (timestamp_now - timestamp_prev) * 1e-6f;
if(Ts <= 0 || Ts > 0.5f) Ts = 1e-3f;
// P环
float proportional = P * error;
// Tustin 散点积分（I环）
float integral = integral_prev + I*Ts*0.5f*(error + error_prev);
integral = _constrain(integral, -limit, limit);
// D环（微分环节）
float derivative = D*(error - error_prev)/Ts;
// 将P,I,D三环的计算值加起来
float output = proportional + integral + derivative;
output = _constrain(output, -limit, limit);
if(output_ramp > 0){
// 对PID的变化速率进行限制
float output_rate = (output - output_prev)/Ts;
if (output_rate > output_ramp)
output = output_prev + output_ramp*Ts;
else if (output_rate < -output_ramp)
output = output_prev - output_ramp*Ts;
}
// 保存值（为了下一次循环）
integral_prev = integral;
output_prev = output;
error_prev = error;
timestamp_prev = timestamp_now;
return output;
}