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

feat(FOC): 重构FOC控制器实现并修复SVPWM算法支持

Browse Source 
- 修复类名拼写错误：FOC_Controlroller -> FOC_Controller
- 移除不必要的角度归一化函数，直接使用fmod优化性能
- 添加完整的SVPWM算法实现，提高电机控制效率
- 重构PID参数配置，使用动态参数调整机制
- 增加电流反馈功能，实时监测电机运行状态
- 优化电压安全限制，提高系统稳定性
- 更新控制模式映射，支持更多控制策略
- 改进CAN通信协议，增加电流反馈传输
This commit is contained in:
cbb
2026-02-02 02:12:14 +08:00
parent 25e2edfd0c
commit c9934849c5
14 changed files with 386 additions and 233 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Robot_V2/device/ADC.cpp
+2 -2
View File
@@ -15,7 +15,7 @@ constexpr float VCC_FACTOR = 11.0f;
constexpr float CURRENT_FACTOR =
1.0f / 50.0f * 100.0f; // 50倍放大，100倍电流采样电阻, 单位mA
extern CAN_transmiter CAN0_transmiter;
extern FOC_Controlroller FOC;
extern FOC_Controller FOC;
sampler sampler; // 采样器初始化
u16 adc_value[6] = {0}; // 显式初始化数组元素为0
/*!
@@ -240,7 +240,7 @@ void sampler::update_drv_temp() {
*/
void sampler::update_current() {
u8 idx = current_sample_index;
extern FOC_Controlroller FOC;
extern FOC_Controller FOC;
// 优化计算
const float scale = ADC_SCALE * CURRENT_FACTOR;
const float offset = CURRENT_OFFSET * CURRENT_FACTOR;
Robot_V2/device/Can.cpp
+38 -77
View File
@@ -21,7 +21,7 @@
constexpr u16 CANX = CAN0;
CAN_transmiter CAN0_transmiter; // CAN初始化
extern FOC_Controlroller FOC;
extern FOC_Controller FOC;
extern sampler sampler;
extern angle_sensor KTH7823;
extern EncoderAngleKalman Angle_kalman; // 角度滤波
@@ -447,13 +447,15 @@ void CAN_transmiter::update(void) {
target_angle_union.f = FOC.target_angle;
target_velocity_union.f = FOC.target_velocity;
target_current_union.f = FOC.target_current;
limit_velocity_union.f = FOC.limit_velocity;
rt_angle_union.f = FOC.rt_angle;
rt_velocity_union.f = FOC.rt_velocity;
rt_current_union.f = FOC.rt_current;
limit_voltage = FOC.limit_voltage * 1000;
mcu_temp = sampler.get_mcu_temp() * 100;
motor_temp = sampler.get_drv_temp() * 100;
Power_Voltage = sampler.get_vbus_voltage() * 1000;
rt_angle_union.f = FOC.rt_angle;
rt_velocity_union.f = FOC.rt_velocity;
MotorStatus = gpio_input_bit_get(GPIOB, GPIO_PIN_2) == RESET;
EEPROMStatus = flashEEP.EEPROMStatus;
Target_Control_Mode = FOC.control_mode;
@@ -465,57 +467,6 @@ void CAN_transmiter::update(void) {
v_current_union.f = sampler.get_current('V') * 1000;
}
/**
* @brief CAN发送函数
* @retval 0: 发送失败
* 1: 发送成功
*/
u8 CAN_transmiter::CAN_all_Send() {
u32 success_flag = 0;
constexpr u32 RT_Vel_Ang_FEED = 0x101;
constexpr u32 SET_Vel_Ang_FEED = 0x110;
constexpr u32 Current_FEED = 0x117;
constexpr u32 Status_FEED = 0x118;
constexpr u32 Limit_Vel_Vol_FEED = 0x119;
// 打包RT_Vel_Ang_FEED - 只清零实际使用的8字节
memset(transmit_data, 0, 8);
pack_float_to_bytes(transmit_data, 0, rt_angle_union);
pack_float_to_bytes(transmit_data, 4, rt_velocity_union);
success_flag += CAN_AsyncSend(RT_Vel_Ang_FEED, 8, transmit_data);
// 打包SET_Vel_Ang_FEED - 只清零实际使用的8字节
memset(transmit_data, 0, 8);
pack_float_to_bytes(transmit_data, 0, target_angle_union);
pack_float_to_bytes(transmit_data, 4, target_velocity_union);
success_flag += CAN_AsyncSend(SET_Vel_Ang_FEED, 8, transmit_data);
// 打包Current_FEED - 只清零实际使用的6字节
memset(transmit_data, 0, 6);
pack_u16_to_bytes(transmit_data, 0, u16(u_current_union.f));
pack_u16_to_bytes(transmit_data, 2, u16(w_current_union.f));
pack_u16_to_bytes(transmit_data, 4, u16(v_current_union.f));
success_flag += CAN_AsyncSend(Current_FEED, 6, transmit_data);
// 打包Status_FEED - 只清零实际使用的8字节
memset(transmit_data, 0, 8);
transmit_data[0] = MotorStatus;
transmit_data[1] = controller_Mode;
pack_u16_to_bytes(transmit_data, 2, motor_temp);
pack_u16_to_bytes(transmit_data, 4, mcu_temp);
pack_u16_to_bytes(transmit_data, 6, Power_Voltage);
success_flag += CAN_AsyncSend(Status_FEED, 8, transmit_data);
// 打包Limit_Vel_Vol_FEED - 只清零实际使用的6字节
memset(transmit_data, 0, 6);
pack_float_to_bytes(transmit_data, 0, limit_velocity_union);
pack_u16_to_bytes(transmit_data, 4, limit_voltage);
success_flag += CAN_AsyncSend(Limit_Vel_Vol_FEED, 6, transmit_data);
return success_flag;
}
/**
* @brief CANFD发送函数
* @retval 0: 发送失败
@@ -538,17 +489,19 @@ u8 CAN_transmiter::CANFD_all_Send() {
pack_u16_to_bytes(transmit_data, 11, Power_Voltage);
pack_float_to_bytes(transmit_data, 13, rt_angle_union);
pack_float_to_bytes(transmit_data, 17, rt_velocity_union);
pack_float_to_bytes(transmit_data, 21, testfloat_union);
pack_float_to_bytes(transmit_data, 21, rt_current_union);
pack_float_to_bytes(transmit_data, 25, testfloat_union);
pack_u16_to_bytes(transmit_data, 29, test_u16);
success_flag += CANFD_AsyncSend(CANFD_STATUS_ID, 32, transmit_data);
// 打包config数据 - 只清零实际使用的16字节
memset(transmit_data, 0, 16);
// 打包config数据 - 只清零实际使用的24字节
memset(transmit_data, 0, 24);
transmit_data[0] = controller_Mode;
pack_u16_to_bytes(transmit_data, 1, limit_voltage);
pack_float_to_bytes(transmit_data, 3, limit_velocity_union);
pack_float_to_bytes(transmit_data, 7, target_angle_union);
pack_float_to_bytes(transmit_data, 11, target_velocity_union);
success_flag += CANFD_AsyncSend(CANFD_CONFIG_ID, 16, transmit_data);
pack_float_to_bytes(transmit_data, 3, target_current_union);
pack_float_to_bytes(transmit_data, 7, target_velocity_union);
pack_float_to_bytes(transmit_data, 11, target_angle_union);
success_flag += CANFD_AsyncSend(CANFD_CONFIG_ID, 24, transmit_data);
return success_flag;
}
@@ -577,34 +530,42 @@ void CAN_transmiter::communication_check() {
if (receive_message.rx_efid == CANFD_CMD_ID) {
// 控制模式
FOC.control_mode = receive_message.rx_data[0];
// 目标角度
// 目标电压限制
FOC.limit_voltage =
float(unpack_bytes_to_u16(receive_message.rx_data, 1)) / 1000;
// 目标电流&目标速度
FOC.limit_velocity =
// 目标电流
target_current_union.f =
unpack_bytes_to_float(receive_message.rx_data, 3).f;
FOC.target_current = FOC.limit_velocity;
// 目标角度
target_angle_union.f =
unpack_bytes_to_float(receive_message.rx_data, 7).f;
FOC.target_angle = target_angle_union.f;
FOC.current_loop.set_limit(target_current_union.f);
// 目标速度
target_velocity_union.f =
unpack_bytes_to_float(receive_message.rx_data, 7).f;
FOC.ang_inVelAngloop.set_limit(target_velocity_union.f);
// 目标角度
target_angle_union.f =
unpack_bytes_to_float(receive_message.rx_data, 11).f;
// 赋值
FOC.target_angle = target_angle_union.f;
FOC.target_velocity = target_velocity_union.f;
FOC.target_current = FOC.target_velocity * 10.0f;
FOC.target_current = target_current_union.f;
// 解析PID参数
u16 P_config = unpack_bytes_to_u16(receive_message.rx_data, 15);
float voltage = FOC.limit_voltage;
FOC.current_loop.set_P_percent(current_loop_p * P_config / voltage);
// PID参数
u16 P_config = unpack_bytes_to_u16(receive_message.rx_data, 15);
FOC.current_loop_p = DEFAULT_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 / voltage);
FOC.vel_loop_p = DEFAULT_vel_loop_p * P_config / voltage;
FOC.vel_MIT_p = DEFAULT_vel_MIT_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 / voltage);
FOC.angle_loop_p = DEFAULT_angle_loop_p * P_config / voltage;
FOC.ang_angloop_p = DEFAULT_ang_angloop_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 /
voltage);
FOC.vel_angloop_p = DEFAULT_vel_angloop_p * P_config;
} 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/device/Can.h
+19 -12
View File
@@ -6,6 +6,7 @@
extern "C" {
#endif
#include <cstdint>
#include "gd32c10x_can.h"
// 定义一个联合体类型用于float和byte数组之间的转换
@@ -72,24 +73,29 @@ class CAN_transmiter {
private:
bool is_sleep = false; // 是否休眠
u8 controller_Mode = 0; // 电机模式
float_union_t limit_velocity_union = {0}; // 限制速度
u16 limit_voltage = 0; // 限制电压
short mcu_temp = 0; // MCU温度
short motor_temp = 0; // 电机温度
u16 Power_Voltage = 0; // 电源电压
float_union_t rt_angle_union = {0}; // 实时角度
float_union_t rt_velocity_union = {0}; // 实时速度
bool MotorStatus = 0; // 电机状态码
u8 Target_Control_Mode = 0; // 目标控制模式
u8 controller_Mode = 0; // 当前模式
short mcu_temp = 0; // MCU温度
short motor_temp = 0; // 电机温度
u16 Power_Voltage = 0; // 电源电压
u16 limit_voltage = 0; // 限制电压
float_union_t rt_angle_union = {0}; // 实时角度
float_union_t rt_velocity_union = {0}; // 实时速度
float_union_t rt_current_union = {0}; // 实时电流
float_union_t target_angle_union = {0}; // 目标角度
u8 Target_Control_Mode = 0; // 目标控制模式
u8 EEPROMStatus = 0; // EEPROM状态码
float_union_t target_velocity_union = {0}; // 目标速度
float_union_t target_current_union = {0};
float_union_t target_current_union = {0}; // 目标电流
float_union_t u_current_union = {0}; // U相电流
float_union_t w_current_union = {0}; // W相电流
float_union_t v_current_union = {0}; // V相电流
bool MotorStatus = 0; // 电机状态码
u8 EEPROMStatus = 0; // EEPROM状态码
can_trasnmit_message_struct transmit_message; // 发送消息结构体
u8 transmit_mailbox = CAN_NOMAILBOX; // 初始化为无mailbox状态
u8 transmit_data[64]; // CANFD最大64字节
@@ -106,6 +112,7 @@ class CAN_transmiter {
FlagStatus can0_receive_flag; // CAN0接收标志
FlagStatus can0_send_flag; // CAN0发送标志
float_union_t testfloat_union = {0}; // 测试用
u16 test_u16 = 0;
bool MagStatus = 0; // 磁状态码
bool ThermalStatus = 0; // 热能状态码
Robot_V2/device/Timer.cpp
+2 -2
View File
@@ -11,7 +11,7 @@
volatile u32 total_us100_counter = 0; // 总计数器，每100微秒增加1
extern u16 adc_value[7];
extern FOC_Controlroller FOC;
extern FOC_Controller FOC;
extern TaskScheduler scheduler;
extern CAN_transmiter CAN0_transmiter;
// 添加系统运行时间计数器
@@ -94,9 +94,9 @@ void TIMER2_IRQHandler(void) {
timer_interrupt_flag_clear(TIMER2, TIMER_INT_UP);
// 每50us执行一次的操作 - 电机控制
scheduler.taskFlags |= (1 << TASK_MOTOR_CONTROL);
scheduler.taskFlags |=
(1 << TASK_ANGLE_UPDATE) | (1 << TASK_VELOCITY_UPDATE);
scheduler.taskFlags |= (1 << TASK_MOTOR_CONTROL);
// 使用标志位控制，每100us执行一次其他任务
static bool isEvenCall = false;