docs(config): 新增软件和硬件版本号定义
- 定义SOFTWARE_VERSION 26.1.19.1 - 定义HARDWARE_VERSION 25.12.01.0 - 定义BOOT_VERSION 26.1.12.0 feat(motor-control): 实现FOC控制算法速度环优化 refactor(can-protocol): 升级CANFD通信协议支持版本管理 - 实现版本号管理和CANFD_VERSION_ID协议 - 扩展CMD_ID消息解析支持PID参数动态配置 - 添加版本管理器实例并实现版本信息发送功能 - 优化CANFD异步发送数据长度判断逻辑 fix(angle-sensor): 修正角度传感器方向和偏移校准 - 添加ANGLESENS_OFFSET补偿参数 - 反转角度传感器读数方向(DIR = -1.0f) - 在角度计算中应用负向因子和偏移量 chore(memory-layout): 调整Flash存储布局和版本配置 - 扩展BOOTLOADER_SIZE从16KB到24KB - 更新APPLATEST_START_ADDR和APPBACK_START_ADDR偏移 - 重新分配OTA配置参数存储区域 - 增加EEP_DEBUG调试开关 refactor(pid-controller): 增强PID控制器功能接口 - 添加set_P_percent方法支持百分比参数设置 - 格式化PID计算代码提升可读性 - 保留积分和微分环节的约束保护机制
This commit is contained in:
+12
-10
@@ -9,12 +9,14 @@
|
||||
#include <arm_math.h>
|
||||
#include <cmath>
|
||||
#include <gd32c10x.h>
|
||||
#include "Filter.h"
|
||||
#include <stdint.h>
|
||||
|
||||
FOC_Controlroller FOC; // FOC控制初始化
|
||||
extern sampler sampler;
|
||||
extern angle_sensor KTH7823;
|
||||
extern CAN_transmiter CAN0_transmiter;
|
||||
extern EncoderVelocityKalman Vel_kalman; // 速度滤波
|
||||
|
||||
// 初始变量及函数定义
|
||||
#define _constrain(amt, low, high) \
|
||||
@@ -54,7 +56,7 @@ inline float FOC_Controlroller::cal_Iq_Id(float current_a, float current_b,
|
||||
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
|
||||
return I_q * 1000.0f - KE_FACTOR * rt_velocity; // 返回Iq值，乘以1000以转换为mA
|
||||
}
|
||||
|
||||
/**
|
||||
@@ -156,17 +158,17 @@ 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 *
|
||||
1.0 / TIMER2_FREQUENCY);
|
||||
shaft_angle = _normalizeAngle(shaft_angle +
|
||||
target_velocity * ANGLE_FACTOR *
|
||||
REDUCTION_RATIO * 1.0 / TIMER2_FREQUENCY);
|
||||
|
||||
// 因为在开环控制中，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'));
|
||||
CAN0_transmiter.testfloat_union.f = shaft_angle;
|
||||
// cal_Iq_Id(sampler.get_current('U'), sampler.get_current('V'),
|
||||
// sampler.get_current('W'));
|
||||
|
||||
return limit_voltage;
|
||||
}
|
||||
@@ -221,8 +223,8 @@ float FOC_Controlroller::angle_control_with_velocity_limit() {
|
||||
_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;
|
||||
float Uq = vel_inVelAngloop(rt_velocity - target_velocity);
|
||||
CAN0_transmiter.testfloat_union.f = shaft_angle;
|
||||
|
||||
// 获取电机轴角度
|
||||
shaft_angle = KTH7823.getcurrent_sensorangle();
|
||||
@@ -241,7 +243,7 @@ float FOC_Controlroller::angle_control_with_velocity_limit() {
|
||||
*/
|
||||
float FOC_Controlroller::velocity_control() {
|
||||
// PID 控制计算
|
||||
float Uq = vel_loop((target_velocity - rt_velocity) * 25);
|
||||
float Uq = vel_loop((rt_velocity - target_velocity) * 25);
|
||||
|
||||
// 获取电机轴角度
|
||||
shaft_angle = KTH7823.getcurrent_sensorangle();
|
||||
|
||||
+11
-13
@@ -37,24 +37,23 @@ class FOC_Controlroller {
|
||||
void resume(void);
|
||||
void drv_stop(void);
|
||||
float drv_hold(void);
|
||||
PIDController vel_loop = PIDController{
|
||||
vel_loop_p * 100, 0.20f, 5e-5, 1000000, voltage_power_supply}; // 12V
|
||||
PIDController angle_loop =
|
||||
PIDController{angle_loop_p * 100, 100.0f, 0.2f, 10000000,
|
||||
voltage_power_supply}; // 12V
|
||||
PIDController current_loop = PIDController{
|
||||
current_loop_p * 100, 5e-7f, 7e-8, 500, 24}; // 12V
|
||||
PIDController vel_inVelAngloop = PIDController{
|
||||
vel_angloop_p * 100, 3.0f, 5e-4, 1000000, voltage_power_supply};
|
||||
PIDController ang_inVelAngloop =
|
||||
PIDController{ang_angloop_p * 100, 0.0f, 5e-4, 1000000, 999};
|
||||
|
||||
protected:
|
||||
float shaft_angle = 0;
|
||||
float Ualpha, Ubeta = 0;
|
||||
float zero_electric_angle = 0;
|
||||
float v_half = voltage_power_supply * 0.5f; // 电源电压的一半
|
||||
|
||||
PIDController vel_loop =
|
||||
PIDController{0.01f, 0.20f, 5e-5, 1000000, voltage_power_supply}; // 12V
|
||||
PIDController angle_loop = PIDController{20.0f, 100.0f, 0.2f, 10000000,
|
||||
voltage_power_supply}; // 12V
|
||||
PIDController current_loop =
|
||||
PIDController{0.006f, 0.02f, 0, 100000, voltage_power_supply}; // 12V
|
||||
PIDController vel_inVelAngloop =
|
||||
PIDController{0.12f, 3.0f, 5e-4, 1000000, voltage_power_supply};
|
||||
PIDController ang_inVelAngloop =
|
||||
PIDController{100.0f, 0.0f, 5e-4, 1000000, 999};
|
||||
|
||||
void setPhaseVoltage(float Uq, float Ud);
|
||||
float cal_Iq_Id(float current_a, float current_b, float current_c);
|
||||
};
|
||||
@@ -65,5 +64,4 @@ typedef float (FOC_Controlroller::*FOCFunctionPtr)(void);
|
||||
inline float _normalizeAngle(float angle);
|
||||
inline float _electricalAngle();
|
||||
|
||||
|
||||
#endif
|
||||
+20
-10
@@ -18,25 +18,27 @@ PIDController::PIDController(float P, float I, float D, float ramp, float limit)
|
||||
float PIDController::operator()(float error) {
|
||||
// P环
|
||||
float proportional = P * error;
|
||||
|
||||
|
||||
// Tustin 散点积分（I环）
|
||||
// 添加积分限幅，防止积分饱和
|
||||
float integral = integral_prev + I * TIME_SLICE * 0.5f * (error + error_prev);
|
||||
float integral =
|
||||
integral_prev + I * TIME_SLICE * 0.5f * (error + error_prev);
|
||||
integral = _constrain(integral, -limit, limit);
|
||||
|
||||
|
||||
// D环（微分环节）
|
||||
float derivative = D * (error - error_prev) / TIME_SLICE;
|
||||
|
||||
|
||||
// 将P,I,D三环的计算值加起来
|
||||
float output = proportional + integral + derivative;
|
||||
output = _constrain(output, -limit, limit);
|
||||
|
||||
// 输出斜坡限制
|
||||
// 简化的斜坡限制（单行逻辑）
|
||||
if (output_ramp > 0) {
|
||||
float max_delta = output_ramp * TIME_SLICE;
|
||||
output = output_prev + fmaxf(-max_delta, fminf(max_delta, output - output_prev));
|
||||
}
|
||||
// 简化的斜坡限制（单行逻辑）
|
||||
if (output_ramp > 0) {
|
||||
float max_delta = output_ramp * TIME_SLICE;
|
||||
output = output_prev +
|
||||
fmaxf(-max_delta, fminf(max_delta, output - output_prev));
|
||||
}
|
||||
// 保存值（为了下一次循环）
|
||||
integral_prev = integral;
|
||||
output_prev = output;
|
||||
@@ -57,4 +59,12 @@ void PIDController::set_PID(float P, float I, float D) {
|
||||
this->P = P;
|
||||
this->I = I;
|
||||
this->D = D;
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 设置P参数
|
||||
*
|
||||
* @param P: P参数
|
||||
* @return void
|
||||
*/
|
||||
void PIDController::set_P_percent(float pp) { this->P = pp; }
|
||||
@@ -9,6 +9,7 @@ public:
|
||||
|
||||
float operator() (float error);
|
||||
void set_PID(float P, float I, float D);
|
||||
void set_P_percent(float pp);
|
||||
|
||||
float P; //!< 比例增益(P环增益)
|
||||
float I; //!< 积分增益（I环增益）
|
||||
|
||||
Reference in New Issue
Block a user