feat(FOC): 重构FOC控制器实现并修复SVPWM算法支持
- 修复类名拼写错误：FOC_Controlroller -> FOC_Controller - 移除不必要的角度归一化函数，直接使用fmod优化性能 - 添加完整的SVPWM算法实现，提高电机控制效率 - 重构PID参数配置，使用动态参数调整机制 - 增加电流反馈功能，实时监测电机运行状态 - 优化电压安全限制，提高系统稳定性 - 更新控制模式映射，支持更多控制策略 - 改进CAN通信协议，增加电流反馈传输
This commit is contained in:
+38
-77
@@ -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];
|
||||
|
||||
Reference in New Issue
Block a user