cbb
abd64da360
新增angle_control_with_velocity_limit函数，实现带速度限制的角度控制， 包含角度PID和速度PID双环控制，防止电机转速过快导致的不稳定。 - 新增带速度限制的角度控制函数 - 配置新的PID控制器用于速度内环和角度外环
290 lines
8.0 KiB
C++
290 lines
8.0 KiB
C++
#include "DengFOC.h"
|
|
#include "Can.h"
|
|
#include "Timer.h"
|
|
#include "adc.h"
|
|
#include "ang_sensor.h"
|
|
#include "initial.h"
|
|
#include "pid.h"
|
|
#include <cmath>
|
|
#include <gd32c10x.h>
|
|
#include <stdint.h>
|
|
#include "motor.h"
|
|
|
|
constexpr double PI = 3.1415926535897932384626433832795f;
|
|
constexpr float ANGLE_FACTOR = 25.0f / 360.0f * 2.0f * 3.14159265358979323846f;
|
|
constexpr float _1_SQRT3 = 0.57735026919f;
|
|
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 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;
|
|
extern angle_sensor KTH7823;
|
|
extern CAN_transmiter CAN0_transmiter;
|
|
|
|
// 初始变量及函数定义
|
|
#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))
|
|
|
|
/**
|
|
* @brief 归一化角度
|
|
*
|
|
* @param angle
|
|
* @return float
|
|
*/
|
|
inline float _normalizeAngle(float angle) {
|
|
float a = fmod(angle, 2 * PI);
|
|
// 取余运算可以用于归一化，列出特殊值例子算便知
|
|
return a >= 0 ? a : (a + 2 * PI);
|
|
}
|
|
|
|
// 通过Ia,Ib,Ic计算Iq,Id(目前仅输出Iq)
|
|
inline float FOC_Controlroller::cal_Iq_Id(float current_a, float current_b,
|
|
float current_c) {
|
|
// 计算电气角度
|
|
float angle_el = _normalizeAngle(shaft_angle * PP);
|
|
|
|
// 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;
|
|
|
|
// 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;
|
|
|
|
return I_q*1000.0f; // 返回Iq值，乘以1000以转换为mA
|
|
}
|
|
|
|
/**
|
|
* @brief 电机控制类构造函数
|
|
*
|
|
*/
|
|
FOC_Controlroller::FOC_Controlroller() { ; }
|
|
|
|
/**
|
|
* @brief 设置电机输出，使用优化的SVPWM算法
|
|
*
|
|
* @param Uq: q轴电压
|
|
* @param Ud: d轴电压
|
|
* @param angle_el: 电机轴角度
|
|
*/
|
|
inline void FOC_Controlroller::setPhaseVoltage(float Uq, float Ud) {
|
|
// 计算电气角度
|
|
float angle_el = _normalizeAngle(shaft_angle * PP);
|
|
|
|
// 帕克逆变换 (直接计算，避免函数调用开销)
|
|
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 = sqrt3dv2 * Ubeta - 0.5f * Ualpha + v_half;
|
|
Uc = -0.5f * Ualpha - sqrt3dv2 * Ubeta + v_half;
|
|
|
|
// 计算占空比并限制范围
|
|
duty_a = _constrain(Ua / v_safe, 0.0f, 1.0f);
|
|
duty_b = _constrain(Ub / v_safe, 0.0f, 1.0f);
|
|
duty_c = _constrain(Uc / v_safe, 0.0f, 1.0f);
|
|
// 设置PWM输出
|
|
float voltage_exchange = limit_voltage / v_safe;
|
|
duty_a = duty_a * voltage_exchange;
|
|
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);
|
|
}
|
|
|
|
/**
|
|
* @brief 电机关机
|
|
*
|
|
*/
|
|
void FOC_Controlroller::shut_down() {
|
|
is_shut_down = 1;
|
|
motor_sleep();
|
|
}
|
|
/**
|
|
* @brief 占空比置0
|
|
*
|
|
*/
|
|
void FOC_Controlroller::drv_stop() { Set_3Pwm(0.0f, 0.0f, 0.0f); }
|
|
/**
|
|
* @brief 电机锁止
|
|
*/
|
|
void FOC_Controlroller::drv_hold(float duty) {
|
|
if (duty > 0.95f) {
|
|
duty = 0.95f;
|
|
} else if (duty < 0.0f) {
|
|
duty = 0.0f;
|
|
}
|
|
Set_3Pwm(duty, duty, duty);
|
|
}
|
|
/**
|
|
* @brief 电机恢复
|
|
*
|
|
*/
|
|
void FOC_Controlroller::resume() {
|
|
over_current = false;
|
|
voltage_error = false;
|
|
is_shut_down = false;
|
|
motor_resume();
|
|
}
|
|
/**
|
|
* @brief 更新电源电压
|
|
*
|
|
*/
|
|
void FOC_Controlroller::update_power(void) {
|
|
voltage_power_supply = sampler.get_vbus_voltage();
|
|
if (voltage_power_supply < 36.0f and voltage_power_supply > 8.0f) {
|
|
voltage_error = false;
|
|
v_half = voltage_power_supply * 0.5f;
|
|
v_safe = voltage_power_supply * 0.95f;
|
|
} else {
|
|
voltage_error = true;
|
|
}
|
|
}
|
|
|
|
/**
|
|
* @brief 速度开环控制
|
|
*
|
|
* @param target_velocity: 目标速度
|
|
* @return float: q轴电压
|
|
*/
|
|
float FOC_Controlroller::velocity_open_loop() {
|
|
// 通过时间间隔和目标速度计算需要转动的机械角度，存储在shaft_angle中，在此之前，还需要对轴角度归一化
|
|
// 预计算常量 1e-4 = 100微秒控制周期
|
|
shaft_angle =
|
|
_normalizeAngle(shaft_angle + target_velocity * ANGLE_FACTOR * 1e-4f);
|
|
|
|
// 因为在开环控制中，Ud始终为0，只控制Uq
|
|
const float Uq = voltage_power_supply / 2.0f;
|
|
|
|
setPhaseVoltage(Uq, 0.0f);
|
|
// CAN0_transmiter.testfloat_union.f =
|
|
cal_Iq_Id(sampler.get_current('U'), sampler.get_current('V'), sampler.get_current('W'));
|
|
|
|
return limit_voltage;
|
|
}
|
|
|
|
/**
|
|
* @brief 电机初始化
|
|
*
|
|
* @param _PP: 电机极队数
|
|
*/
|
|
// 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);
|
|
// }
|
|
|
|
/**
|
|
* @brief 角度闭环控制
|
|
*
|
|
* @param target_angle: 目标角度
|
|
* @return float: 当前角度
|
|
*/
|
|
float FOC_Controlroller::angle_control(float target_angle) {
|
|
// PID 控制计算
|
|
float Uq = angle_loop(target_angle - rt_angle);
|
|
|
|
// 获取电机轴角度
|
|
shaft_angle = KTH7823.getcurrent_sensorangle() * DEG2RAD;
|
|
// 设置相电压
|
|
setPhaseVoltage(Uq, 0.0f);
|
|
return shaft_angle;
|
|
}
|
|
|
|
/**
|
|
* @brief 带速度限制的角度闭环控制
|
|
*
|
|
* @param target_angle: 目标角度
|
|
* @param max_velocity: 最大速度限制 (单位: rad/s)
|
|
* @return float: 当前角度
|
|
*/
|
|
float FOC_Controlroller::angle_control_with_velocity_limit(float target_angle) {
|
|
// 计算角度误差
|
|
float angle_error = target_angle - rt_angle;
|
|
|
|
// 使用角度PID控制器计算目标速度
|
|
float target_velocity = ang_inVelAngloop(angle_error);
|
|
|
|
// 对目标速度进行限幅
|
|
target_velocity = _constrain(target_velocity, -limit_velocity, limit_velocity);
|
|
|
|
// 使用速度PID控制器计算最终输出电压
|
|
float Uq = vel_inVelAngloop(target_velocity - rt_velocity);
|
|
// CAN0_transmiter.testfloat_union.f = target_velocity - rt_velocity;
|
|
|
|
// 获取电机轴角度
|
|
shaft_angle = KTH7823.getcurrent_sensorangle() * DEG2RAD;
|
|
|
|
// 设置相电压
|
|
setPhaseVoltage(Uq, 0.0f);
|
|
|
|
return shaft_angle;
|
|
}
|
|
|
|
/**
|
|
* @brief 角度闭环控制
|
|
*
|
|
* @param target_angle: 目标角度
|
|
* @return float: 当前角度
|
|
*/
|
|
float FOC_Controlroller::angle_control_NPID(float target_angle) {
|
|
static float Kp = 20.0f;
|
|
// 控制力矩计算
|
|
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;
|
|
}
|
|
|
|
/**
|
|
* @brief 速度闭环控制
|
|
*
|
|
* @param target_velocity: 目标速度
|
|
* @return float: 当前速度
|
|
*/
|
|
float FOC_Controlroller::velocity_control(float target_velocity) {
|
|
// PID 控制计算
|
|
float Uq = vel_loop((target_velocity - rt_velocity)*25);
|
|
|
|
// 获取电机轴角度
|
|
shaft_angle = KTH7823.getcurrent_sensorangle() * DEG2RAD;
|
|
// 设置相电压
|
|
setPhaseVoltage(Uq, 0.0f);
|
|
// CAN0_transmiter.testfloat_union.f =
|
|
cal_Iq_Id(sampler.get_current('U'), sampler.get_current('V'), sampler.get_current('W'));
|
|
return rt_velocity;
|
|
}
|
|
|
|
float FOC_Controlroller::current_control(float target_current) {
|
|
// PID 控制计算
|
|
float Iq = cal_Iq_Id(sampler.get_current('U'), sampler.get_current('V'), sampler.get_current('W'));
|
|
float Uq = current_loop(target_current - Iq);
|
|
CAN0_transmiter.testfloat_union.f = Iq;
|
|
// 获取电机轴角度
|
|
shaft_angle = KTH7823.getcurrent_sensorangle() * DEG2RAD;
|
|
// 设置相电压
|
|
setPhaseVoltage(Uq, 0.0f);
|
|
return 1;
|
|
} |