cbb/Motor_v2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

feat(FOC): 添加FOC控制器初始化函数、支持电压自适应PID调整

Browse Source 
添加了FOC控制器的init()方法，用于统一初始化电机控制参数，
包括速度环、角度环和电流环的PID参数设置。同时优化了CAN通信
中的PID参数计算逻辑，引入电压补偿机制，提高了电机控制精度。
This commit is contained in:
cbb
2026-01-21 11:48:31 +08:00
parent c0b5f6902b
commit 3f24292ac1
5 changed files with 46 additions and 21 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Robot_V2/Algorithm/FOC.cpp
+29 -6
View File
@@ -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 <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; // 速度滤波
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;
Robot_V2/Algorithm/FOC.h
+1
View File
@@ -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);
Robot_V2/device/Can.cpp
+5 -4
View File
@@ -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];
Robot_V2/main.cpp
+2 -2
View File
@@ -58,7 +58,6 @@ u8 hardware_init(void) {
systick_config(); // 系统时钟初始化
unused_pin_init(); // 未使用引脚初始化
USART0_init(); // 调试串口初始化
motor_init(); // 电机驱动初始化
User_timer_init(); // 任务Timer初始化
u8 init_status = 0;
@@ -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;
Robot_V2/sys/config.h
+7 -7
View File
@@ -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);