From abd64da36044363e9ca7bccf978f95527b52877f Mon Sep 17 00:00:00 2001 From: cbb <2625274797@qq.com> Date: Tue, 23 Dec 2025 11:55:09 +0800 Subject: [PATCH] =?UTF-8?q?feat(FOC):=20=E6=B7=BB=E5=8A=A0=E5=B8=A6?= =?UTF-8?q?=E9=80=9F=E5=BA=A6=E9=99=90=E5=88=B6=E7=9A=84=E8=A7=92=E5=BA=A6?= =?UTF-8?q?=E9=97=AD=E7=8E=AF=E6=8E=A7=E5=88=B6=E5=8A=9F=E8=83=BD?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 新增angle_control_with_velocity_limit函数，实现带速度限制的角度控制， 包含角度PID和速度PID双环控制，防止电机转速过快导致的不稳定。 - 新增带速度限制的角度控制函数 - 配置新的PID控制器用于速度内环和角度外环 --- Robot_V2/Algorithm/DengFOC.cpp | 30 ++++++++++++++++++++++++++++++ Robot_V2/Algorithm/DengFOC.h | 10 +++++++--- Robot_V2/device/Can.cpp | 2 +- Robot_V2/main.cpp | 9 ++++----- Robot_V2/sys/flash.cpp | 7 ++----- 5 files changed, 44 insertions(+), 14 deletions(-) diff --git a/Robot_V2/Algorithm/DengFOC.cpp b/Robot_V2/Algorithm/DengFOC.cpp index 4d6ece9..b824d00 100644 --- a/Robot_V2/Algorithm/DengFOC.cpp +++ b/Robot_V2/Algorithm/DengFOC.cpp @@ -209,6 +209,36 @@ float FOC_Controlroller::angle_control(float target_angle) { 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 角度闭环控制 * diff --git a/Robot_V2/Algorithm/DengFOC.h b/Robot_V2/Algorithm/DengFOC.h index cc967e6..3a616aa 100644 --- a/Robot_V2/Algorithm/DengFOC.h +++ b/Robot_V2/Algorithm/DengFOC.h @@ -25,6 +25,7 @@ class FOC_Controlroller { float Ua = 0, Ub = 0, Uc = 0; float velocity_open_loop(); float angle_control(float target_angle); + float angle_control_with_velocity_limit(float target_angle); float velocity_control(float target_velocity); float current_control(float target_current); float angle_control_NPID(float target_angle); @@ -42,13 +43,16 @@ class FOC_Controlroller { float zero_electric_angle = 0; float v_half = voltage_power_supply * 0.5f; // 电源电压的一半 - - PIDController vel_loop = PIDController{0.028f, 0.2f, 1e-4, 1000000, - voltage_power_supply}; // 12V + PIDController vel_loop = + PIDController{0.028f, 0.2f, 1e-4, 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.35f, 5.0f,1e-3, 1000000, voltage_power_supply}; + PIDController ang_inVelAngloop = + PIDController{30.0f, 0.0f, 1e-3, 1000000, 999}; void setPhaseVoltage(float Uq, float Ud); float cal_Iq_Id(float current_a, float current_b, float current_c); diff --git a/Robot_V2/device/Can.cpp b/Robot_V2/device/Can.cpp index 76059c3..39df10c 100644 --- a/Robot_V2/device/Can.cpp +++ b/Robot_V2/device/Can.cpp @@ -716,7 +716,7 @@ void CAN_transmiter::communication_check() { if (first_byte >> 3 & 0x01) { KTH7823.angle_set(0); Angle_kalman.init(0); // 角度滤波 - Vel_kalman.init(0); // 角度滤波 + Vel_kalman.init(0); // 角度滤波 } if (eigth_byte & 0x80 >> 7) { prepare_deep_sleep(true); diff --git a/Robot_V2/main.cpp b/Robot_V2/main.cpp index 42b4855..435ba1b 100644 --- a/Robot_V2/main.cpp +++ b/Robot_V2/main.cpp @@ -25,8 +25,8 @@ extern FOC_Controlroller FOC; // FOC控制 extern FlashEEPROM flashEEP; // EEPROM类 TaskScheduler scheduler = {0}; -volatile u32 system_runtime_us = 0; -volatile u32 cnt_test = 0; +volatile u32 system_runtime_us = 0; // 电机驱动计算轮次 +volatile u32 cnt_test = 0; // 负载检测 int main(void) { systick_config(); // 系统时钟初始化 SystemCoreClockUpdate(); // 更新系统时钟变量 @@ -89,9 +89,8 @@ void do_task(u32 currentTasks) { break; } case 1: { - FOC.angle_control(FOC.target_angle); // 角度闭环控制 - // FOC.angle_control_with_velocity_limit(FOC.target_angle, - // FOC.limit_velocity); // 角度闭环控制，带速度限制 + // FOC.angle_control(FOC.target_angle); // 角度闭环控制 + FOC.angle_control_with_velocity_limit(FOC.target_angle); // 角度闭环控制，带速度限制 break; } case 2: { diff --git a/Robot_V2/sys/flash.cpp b/Robot_V2/sys/flash.cpp index 0a6ebfb..a3f9ac6 100644 --- a/Robot_V2/sys/flash.cpp +++ b/Robot_V2/sys/flash.cpp @@ -146,10 +146,10 @@ FlashEepromStatus FlashEEPROM::read(u8 virtualAddr, u16 *data) const { u32 data_word = *((u32 *)address); // 检查是否是有效数据（非擦除状态） if (data_word != 0xFFFFFFFF) { - u8 addr = (u8)(data_word & 0xFF); + u8 addr = (u8)(data_word & DATA_ADDR_MASK); // 找到匹配的虚拟地址 if (addr == virtualAddr) { - *data = (u16)((data_word >> 16) & 0xFFFF); + *data = (u16)(data_word & DATA_U16_MASK); return FLASH_EEPROM_OK; } } @@ -504,10 +504,7 @@ FlashEepromStatus FlashEEPROM::writeCRC(u32 CRC_NUM, bool saved) { fmc_unlock(); fmc_flag_clear(FMC_FLAG_PGERR | FMC_FLAG_PGAERR | FMC_FLAG_WPERR); fmc_state_enum fmc_status = fmc_word_program(next_address, CRC_NUM); - // printf("write address:0x%X,status:0x%x \r\n", next_address, - // fmc_status); fmc_lock(); - #if EEPDEBUG printf("CRC cmp1,0x%X_0x%x \r\n", CRC_NUM, *((u32 *)next_address)); #endif