refactor(foc): 重构FOC控制算法与角度传感器接口
- 移除冗余的电机方向和极对数参数传递逻辑 - 简化电气角度计算函数，直接读取角度传感器数据 - 调整电压设置函数，优化PWM输出逻辑 - 注释掉未使用的速度开环控制调用，启用角度闭环控制 - 增加角度和速度获取接口，支持原始值和输出级值区分 feat(sensor): 完善角度传感器数据处理与速度估算 - 增加速度滤波缓冲区，提高速度估算精度 - 改进角度绕回检测阈值，增强鲁棒性 - 添加获取原始角度和速度的接口 - 增加SPI DMA传输超时打印信息 fix(can): 修正CAN通信中的数据打包与解析问题 - 正确发送电机实时速度数据 - 修正CAN FD数据包长度以匹配实际内容 - 角度目标值增加缩放因子适配外部控制指令
This commit is contained in:
@@ -2,6 +2,8 @@
|
||||
#include "Filter.h"
|
||||
#include "Timer.h"
|
||||
#include "adc.h"
|
||||
#include "ang_sensor.h"
|
||||
#include "gd32c10x_dbg.h"
|
||||
#include "pid.h"
|
||||
#include "systick.h"
|
||||
#include <cmath>
|
||||
@@ -10,21 +12,22 @@
|
||||
#include <stdint.h>
|
||||
|
||||
constexpr double PI = 3.1415926535897932384626433832795f;
|
||||
constexpr float DEG2RAD = 1.0f / 360.0f * 6.28318530718f;
|
||||
constexpr float RAD2DEG = 360.0f / 6.28318530718f;
|
||||
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 u8 PP = 11;
|
||||
constexpr float DIR = -1.0;
|
||||
constexpr float sqrt3 = 1.73205080757f; // sqrt(3)的预计算值
|
||||
constexpr float zero_electric_angle = 0.0f;
|
||||
|
||||
extern sampler sampler;
|
||||
extern angle_sensor KTH7823;
|
||||
|
||||
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) \
|
||||
@@ -32,11 +35,6 @@ PIDController current_loop_M0 = PIDController{2, 0, 0, 100000, 800};
|
||||
#define constrain(value, min, max, target_min, target_max) \
|
||||
((value - min) / (max - min) * (target_max - target_min))
|
||||
|
||||
inline float _electricalAngle(float shaft_angle) {
|
||||
return _normalizeAngle((float)(DIR * PP) * shaft_angle -
|
||||
zero_electric_angle);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 归一化角度
|
||||
*
|
||||
@@ -48,18 +46,34 @@ 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 电机控制类构造函数
|
||||
*
|
||||
*/
|
||||
FOC_Controlroller::FOC_Controlroller() { ; }
|
||||
/**
|
||||
* @brief 获取电机 shaft_angle
|
||||
*
|
||||
* @return void
|
||||
*/
|
||||
void FOC_Controlroller::driver_output(float Ua, float Ub, float Uc) {
|
||||
|
||||
/**
|
||||
* @brief 设置电机输出
|
||||
*
|
||||
* @param Uq: q轴电压
|
||||
* @param Ud: d轴电压
|
||||
* @param angle_el: 电机轴角度
|
||||
*/
|
||||
inline void FOC_Controlroller::setPhaseVoltage(float Uq, float Ud,
|
||||
float angle_el) {
|
||||
angle_el = _normalizeAngle(angle_el);
|
||||
// 帕克逆变换
|
||||
Ualpha = -Uq * sin(angle_el);
|
||||
Ubeta = Uq * cos(angle_el);
|
||||
|
||||
// 克拉克逆变换
|
||||
Ua = Ualpha + voltage_power_supply / 2;
|
||||
Ub = (sqrt3 * Ubeta - Ualpha) / 2 + voltage_power_supply / 2;
|
||||
Uc = (-Ualpha - sqrt3 * Ubeta) / 2 + voltage_power_supply / 2;
|
||||
// 计算占空比
|
||||
Ua = constrain(Ua, 0.0f, voltage_power_supply, 0.0f, limit_voltage);
|
||||
Ub = constrain(Ub, 0.0f, voltage_power_supply, 0.0f, limit_voltage);
|
||||
@@ -71,34 +85,13 @@ void FOC_Controlroller::driver_output(float Ua, float Ub, float Uc) {
|
||||
// std::printf("PWM:%f,%f,%f\n", dc_a, dc_b, dc_c);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 设置电机输出
|
||||
*
|
||||
* @param Uq: q轴电压
|
||||
* @param Ud: d轴电压
|
||||
* @param angle_el: 电机轴角度
|
||||
*/
|
||||
void FOC_Controlroller::setPhaseVoltage(float Uq, float Ud, float angle_el) {
|
||||
Uq = _constrain(Uq, -voltage_power_supply / 2, voltage_power_supply / 2);
|
||||
angle_el = _normalizeAngle(angle_el + zero_electric_angle);
|
||||
// 帕克逆变换
|
||||
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);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 速度开环控制
|
||||
*
|
||||
* @param target_velocity: 目标速度
|
||||
* @return float: q轴电压
|
||||
*/
|
||||
float FOC_Controlroller::velocity_open_loop(float target_velocity) {
|
||||
float FOC_Controlroller::velocity_open_loop() {
|
||||
// 通过时间间隔和目标速度计算需要转动的机械角度，存储在shaft_angle中，在此之前，还需要对轴角度归一化
|
||||
// 预计算常量 1e-4 = 100微秒控制周期
|
||||
shaft_angle = _normalizeAngle(shaft_angle + target_velocity * 1e-4f);
|
||||
@@ -139,17 +132,15 @@ float FOC_Controlroller::velocity_open_loop(float target_velocity) {
|
||||
* @brief 电机初始化
|
||||
*
|
||||
* @param _PP: 电机极队数
|
||||
* @param _DIR: 电机极性
|
||||
*/
|
||||
void FOC_Controlroller::DFOC_alignSensor(int _PP, int _DIR) {
|
||||
u8 PP = _PP;
|
||||
u8 DIR = _DIR;
|
||||
setPhaseVoltage(3, 0, _3PI_2);
|
||||
delay_1ms(3000);
|
||||
zero_electric_angle = _electricalAngle(0);
|
||||
setPhaseVoltage(0, 0, _3PI_2);
|
||||
std::printf("0电角度:%f", zero_electric_angle);
|
||||
}
|
||||
// void FOC_Controlroller::DFOC_alignSensor(int _PP) {
|
||||
// u8 PP = _PP;
|
||||
// setPhaseVoltage(3, 0, _3PI_2);
|
||||
// delay_1ms(3000);
|
||||
// zero_electric_angle = _electricalAngle();
|
||||
// setPhaseVoltage(0, 0, _3PI_2);
|
||||
// std::printf("0电角度:%f", zero_electric_angle);
|
||||
// }
|
||||
|
||||
void FOC_Controlroller::shut_down() {
|
||||
is_shut_down = 100;
|
||||
@@ -164,62 +155,42 @@ void FOC_Controlroller::resume() {
|
||||
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::update_power(void) {
|
||||
voltage_power_supply = sampler.get_vbus_voltage();
|
||||
}
|
||||
|
||||
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);
|
||||
/**
|
||||
* @brief 角度闭环控制
|
||||
*
|
||||
* @param target_velocity: 目标速度
|
||||
* @return float: q轴电压
|
||||
*/
|
||||
float FOC_Controlroller::angle_control(float target_angle) {
|
||||
float Sensor_angel = KTH7823.getRawAngle() * DEG2RAD;
|
||||
float Kp = 2.0f;
|
||||
|
||||
float Uq = _constrain(Kp * (target_angle - DIR * Sensor_angel) * RAD2DEG,
|
||||
-voltage_power_supply, voltage_power_supply);
|
||||
shaft_angle = _normalizeAngle(KTH7823.read_sensorAngle() * DEG2RAD);
|
||||
float Ud = 0.0f;
|
||||
// 直接计算电气角度，避免函数调用开销
|
||||
float angle_el = _normalizeAngle(shaft_angle * PP + zero_electric_angle);
|
||||
|
||||
// 帕克逆变换
|
||||
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());
|
||||
}
|
||||
|
||||
void FOC_Controlroller::update_power(void) {
|
||||
voltage_power_supply = sampler.get_vbus_voltage();
|
||||
Ub = (sqrt3 * Ubeta - Ualpha) / 2 + voltage_power_supply / 2;
|
||||
Uc = (-Ualpha - sqrt3 * Ubeta) / 2 + voltage_power_supply / 2;
|
||||
// 计算占空比
|
||||
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);
|
||||
float dc_a = _constrain(Ua / voltage_power_supply, 0.0f, 1.0f);
|
||||
float dc_b = _constrain(Ub / voltage_power_supply, 0.0f, 1.0f);
|
||||
float dc_c = _constrain(Uc / voltage_power_supply, 0.0f, 1.0f);
|
||||
Set_3Pwm(dc_a, dc_b, dc_c);
|
||||
return Sensor_angel;
|
||||
}
|
||||
Reference in New Issue
Block a user