From 3f24292ac1ad5921b9ae794a018745e20b9a2a3b Mon Sep 17 00:00:00 2001 From: cbb <2625274797@qq.com> Date: Wed, 21 Jan 2026 11:48:31 +0800 Subject: [PATCH] =?UTF-8?q?feat(FOC):=20=E6=B7=BB=E5=8A=A0FOC=E6=8E=A7?= =?UTF-8?q?=E5=88=B6=E5=99=A8=E5=88=9D=E5=A7=8B=E5=8C=96=E5=87=BD=E6=95=B0?= =?UTF-8?q?=E3=80=81=E6=94=AF=E6=8C=81=E7=94=B5=E5=8E=8B=E8=87=AA=E9=80=82?= =?UTF-8?q?=E5=BA=94PID=E8=B0=83=E6=95=B4?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 添加了FOC控制器的init()方法，用于统一初始化电机控制参数， 包括速度环、角度环和电流环的PID参数设置。同时优化了CAN通信 中的PID参数计算逻辑，引入电压补偿机制，提高了电机控制精度。 --- Robot_V2/Algorithm/FOC.cpp | 35 +++++++++++++++++++++++++++++------ Robot_V2/Algorithm/FOC.h | 1 + Robot_V2/device/Can.cpp | 9 +++++---- Robot_V2/main.cpp | 8 ++++---- Robot_V2/sys/config.h | 14 +++++++------- 5 files changed, 46 insertions(+), 21 deletions(-) diff --git a/Robot_V2/Algorithm/FOC.cpp b/Robot_V2/Algorithm/FOC.cpp index 7fa485c..d82f9a9 100644 --- a/Robot_V2/Algorithm/FOC.cpp +++ b/Robot_V2/Algorithm/FOC.cpp @@ -1,5 +1,6 @@ #include "FOC.h" #include "Can.h" +#include "Filter.h" #include "Timer.h" #include "adc.h" #include "ang_sensor.h" @@ -9,14 +10,14 @@ #include #include #include -#include "Filter.h" #include + FOC_Controlroller FOC; // FOC控制初始化 extern sampler sampler; extern angle_sensor KTH7823; extern CAN_transmiter CAN0_transmiter; -extern EncoderVelocityKalman Vel_kalman; // 速度滤波 +extern EncoderVelocityKalman Vel_kalman; // 速度滤波 // 初始变量及函数定义 #define _constrain(amt, low, high) \ @@ -56,7 +57,29 @@ 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 - KE_FACTOR * rt_velocity; // 返回Iq值，乘以1000以转换为mA + return I_q * 1000.0f - + KE_FACTOR * rt_velocity; // 返回Iq值，乘以1000以转换为mA +} + +/** + * @brief 电机控制类初始化函数 + * + * @return u8 0表示初始化成功，1表示初始化失败 + */ +u8 FOC_Controlroller::init() { + motor_init(); // 电机驱动初始化 + vel_loop.set_PID(vel_loop_p * 100 / limit_voltage, 0.20f, 5e-5); // 设置电机速度控制参数 + angle_loop.set_PID(angle_loop_p * 100 / limit_voltage, 100.0f, + 0.2f); // 设置电机角度控制参数 + current_loop.set_PID(current_loop_p * 100 / limit_voltage, 5e-7f, + 7e-8); // 设置电机电流控制参数 + + vel_inVelAngloop.set_PID(vel_angloop_p * 100 / limit_voltage, 3.0f, + 5e-4); // 设置电机速度控制参数 + ang_inVelAngloop.set_PID(ang_angloop_p * 100 / limit_voltage, 0.0f, + 5e-4); // 设置电机角度控制参数 + + return 0; } /** @@ -158,9 +181,9 @@ void FOC_Controlroller::update_power(void) { */ float FOC_Controlroller::velocity_open_loop() { // 通过时间间隔和目标速度计算需要转动的机械角度，存储在shaft_angle中，在此之前，还需要对轴角度归一化 - shaft_angle = _normalizeAngle(shaft_angle + - target_velocity * ANGLE_FACTOR * - REDUCTION_RATIO * 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; diff --git a/Robot_V2/Algorithm/FOC.h b/Robot_V2/Algorithm/FOC.h index 7e83c68..27204a5 100644 --- a/Robot_V2/Algorithm/FOC.h +++ b/Robot_V2/Algorithm/FOC.h @@ -25,6 +25,7 @@ class FOC_Controlroller { float v_safe = voltage_power_supply * 1.05f; float duty_a = 0, duty_b = 0, duty_c = 0; float Ua = 0, Ub = 0, Uc = 0; + u8 init(void); float velocity_open_loop(void); float angle_control(void); float angle_control_with_velocity_limit(void); diff --git a/Robot_V2/device/Can.cpp b/Robot_V2/device/Can.cpp index 693538b..4e191c4 100644 --- a/Robot_V2/device/Can.cpp +++ b/Robot_V2/device/Can.cpp @@ -597,13 +597,14 @@ void CAN_transmiter::communication_check() { // 解析PID参数 u16 P_config = unpack_bytes_to_u16(receive_message.rx_data, 15); - FOC.current_loop.set_P_percent(current_loop_p * P_config); + float voltage = FOC.limit_voltage; + FOC.current_loop.set_P_percent(current_loop_p * P_config / voltage); P_config = unpack_bytes_to_u16(receive_message.rx_data, 17); - FOC.vel_loop.set_P_percent(vel_loop_p * P_config); + FOC.vel_loop.set_P_percent(vel_loop_p * P_config / voltage); P_config = unpack_bytes_to_u16(receive_message.rx_data, 19); - FOC.angle_loop.set_P_percent(angle_loop_p * P_config); + FOC.angle_loop.set_P_percent(angle_loop_p * P_config / voltage); P_config = unpack_bytes_to_u16(receive_message.rx_data, 21); - FOC.vel_inVelAngloop.set_P_percent(vel_angloop_p * P_config); + FOC.vel_inVelAngloop.set_P_percent(vel_angloop_p * P_config / voltage); } else if (receive_message.rx_efid == CANFD_RESET_ID) { u8 first_byte = receive_message.rx_data[0]; u8 eigth_byte = receive_message.rx_data[7]; diff --git a/Robot_V2/main.cpp b/Robot_V2/main.cpp index 38392ee..2a54fd6 100644 --- a/Robot_V2/main.cpp +++ b/Robot_V2/main.cpp @@ -40,7 +40,7 @@ int main(void) { angsens_MG_check(); // 角度传感器磁感应强度测试 watchdog_init(50); // 看门狗初始化 - + while (1) { __disable_irq(); u32 currentTasks = @@ -58,9 +58,8 @@ u8 hardware_init(void) { systick_config(); // 系统时钟初始化 unused_pin_init(); // 未使用引脚初始化 USART0_init(); // 调试串口初始化 - motor_init(); // 电机驱动初始化 User_timer_init(); // 任务Timer初始化 - + u8 init_status = 0; init_status += KTH7823.init(); // 角度传感器初始化 init_status += sampler.init(); // 采样器初始化 @@ -69,6 +68,7 @@ u8 hardware_init(void) { init_status += Angle_kalman.init(KTH7823.getAngle()); // 角度滤波初始化 init_status += Vel_kalman.init(0.0f); // 速度滤波初始化 init_status += CAN0_transmiter.init(); // CAN初始化 + init_status += FOC.init(); // FOC初始化 return init_status; } @@ -120,7 +120,7 @@ void do_task(u32 currentTasks) { if (currentTasks & (1 << TASK_VBUS_UPDATE)) { // 更新vbus电压值 power_manage(); } - if (currentTasks & (1 << TASK_CAN_TRANSMIT)) { // CAN通信 + if (currentTasks & (1 << TASK_CAN_TRANSMIT)) { // CAN通信 // CAN0_transmiter.testfloat_union.f = cnt_test; // 负载检测 cnt_test = 0; CAN0_transmiter.can0_send_flag = SET; diff --git a/Robot_V2/sys/config.h b/Robot_V2/sys/config.h index 5307ed3..fd15d78 100644 --- a/Robot_V2/sys/config.h +++ b/Robot_V2/sys/config.h @@ -12,7 +12,7 @@ // 版本号定义 - 可根据实际项目进行调整 constexpr u16 SOFTWARE_VERSION_MAJOR = 26; constexpr u16 SOFTWARE_VERSION_MINOR = 1; -constexpr u16 SOFTWARE_VERSION_PATCH = 19; +constexpr u16 SOFTWARE_VERSION_PATCH = 20; constexpr u16 SOFTWARE_VERSION_BUILD = 1; constexpr u16 HARDWARE_VERSION_MAJOR = 25; @@ -53,16 +53,16 @@ constexpr u32 OTA_AVAL = 0x00000000; // 全零表示可用 constexpr u8 PP = 11; constexpr float DIR = -1.0f; constexpr u16 REDUCTION_RATIO = 25; -constexpr float CURRENT_OFFSET = -1.615f; // 当前电流传感器的零点偏移 +constexpr float CURRENT_OFFSET = -1.62f; // 当前电流传感器的零点偏移 constexpr float HOLD_PWM_DUTY = 0.95f; constexpr u16 ANGLESENS_OFFSET = +0; constexpr float KE_FACTOR = 14.0f; // PID配置 -constexpr float current_loop_p = 6e-2f / 100; // 电流P -constexpr float vel_loop_p = 0.01f / 100; // 速度P -constexpr float angle_loop_p = 20.0f / 100; // 角度P -constexpr float vel_angloop_p = 0.12f / 100; // 速度 速度-角度P -constexpr float ang_angloop_p = 100.0f / 100; // 角度 速度-角度P +constexpr float current_loop_p = 6e-2f / 100 * 12; // 电流P +constexpr float vel_loop_p = 0.01f / 100 * 12; // 速度P +constexpr float angle_loop_p = 20.0f / 100 * 12; // 角度P +constexpr float vel_angloop_p = 0.12f / 100 * 12; // 速度 速度-角度P +constexpr float ang_angloop_p = 100.0f / 100 * 12; // 角度 速度-角度P // 常量定义 constexpr u8 SIZE_FLOAT = sizeof(float);