fix(algorithm): 修改FOC控制器中的角度计算和校准参数
修正了FOC控制器中电气角度计算的符号错误（减法改为加法）， 移除了不必要的空行，并优化了velocity_open_loop函数中的注释 格式。同时修正了角度控制中PID计算的目标角度差值顺序。 BREAKING CHANGE: 角度校准参数从硬编码值改为配置常量。
This commit is contained in:
@@ -64,7 +64,7 @@ inline float FOC_Controller::cal_Iq_Id(float current_a, float current_b,
|
||||
current_c -= current_avg;
|
||||
|
||||
// 计算电气角度
|
||||
float angle_el = fmod(shaft_angle * PP - KTH7823.calibration_angle, 360.0f);
|
||||
float angle_el = fmod(shaft_angle * PP + KTH7823.calibration_angle, 360.0f);
|
||||
|
||||
// Clarke变换 (使用三相电流)
|
||||
float I_alpha, I_beta;
|
||||
@@ -249,7 +249,6 @@ inline void FOC_Controller::setSVPWM_PhaseVoltage(float Uq, float Ud) {
|
||||
duty_c = 0.5f;
|
||||
break;
|
||||
}
|
||||
|
||||
// 设置PWM输出
|
||||
Set_3Pwm(duty_a, duty_b, duty_c);
|
||||
}
|
||||
@@ -409,13 +408,10 @@ void FOC_Controller::fast_voltage_check(void) {
|
||||
* @return float: q轴电压
|
||||
*/
|
||||
float FOC_Controller::velocity_open_loop() {
|
||||
// 通过时间间隔和目标速度计算需要转动的机械角度，存储在shaft_angle中，在此之前，还需要对轴角度归一化
|
||||
shaft_angle = fmod(shaft_angle + target_velocity * REDUCTION_RATIO/
|
||||
MOTOR_FREQUENCY,
|
||||
// 通过时间间隔和目标速度计算需要转动的机械角度，存储在 shaft_angle 中
|
||||
shaft_angle =
|
||||
fmod(shaft_angle + target_velocity * REDUCTION_RATIO / MOTOR_FREQUENCY,
|
||||
360.0f);
|
||||
// 获取电机轴角度
|
||||
// float shaft_angle = KTH7823.getcurrent_sensorangle();
|
||||
CAN0_transmiter.testfloat_union.f = shaft_angle;
|
||||
rt_current = cal_Iq_Id(sampler.get_current('U'), sampler.get_current('V'),
|
||||
sampler.get_current('W'));
|
||||
|
||||
@@ -434,7 +430,7 @@ float FOC_Controller::velocity_open_loop() {
|
||||
*/
|
||||
float FOC_Controller::angle_control() {
|
||||
// PID 控制计算
|
||||
float Uq = angle_loop(rt_angle - target_angle);
|
||||
float Uq = angle_loop(target_angle - rt_angle);
|
||||
|
||||
// 获取电机轴角度
|
||||
shaft_angle = KTH7823.getcurrent_sensorangle();
|
||||
|
||||
@@ -14,7 +14,7 @@ int spi_rw_half_word_dma(uint32_t spi_periph, uint16_t tx_data,
|
||||
|
||||
class angle_sensor {
|
||||
public:
|
||||
float calibration_angle = 195.0f;
|
||||
float calibration_angle = ANGLESENS_CALIBRATION_ANGLE;
|
||||
u8 init(void);
|
||||
float read_sensorAngle(void);
|
||||
float update(void);
|
||||
|
||||
@@ -89,7 +89,8 @@ constexpr float DIR = 1.0f; // 默认方向：-1.0f 反转，1.0f 正转
|
||||
constexpr u16 REDUCTION_RATIO = 25;
|
||||
constexpr float CURRENT_OFFSET = -3.31f / 2.0f; // 当前电流传感器的零点偏移
|
||||
constexpr float HOLD_PWM_DUTY = 0.1f;
|
||||
constexpr u16 ANGLESENS_OFFSET = +15;
|
||||
constexpr u16 ANGLESENS_OFFSET = 0;
|
||||
constexpr float ANGLESENS_CALIBRATION_ANGLE = 83.0f;
|
||||
constexpr float KE_FACTOR = 0.0f;
|
||||
|
||||
// 角度卡尔曼滤波器参数
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
@@ -1,169 +0,0 @@
|
||||
#ifndef _CAN_H
|
||||
#define _CAN_H
|
||||
|
||||
#include <stdint.h>
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
#include <cstdint>
|
||||
#include "gd32c10x_can.h"
|
||||
#include "config.h"
|
||||
|
||||
// 定义一个联合体类型用于 float 和 byte 数组之间的转换
|
||||
typedef union {
|
||||
float f;
|
||||
u8 b[4];
|
||||
} float_union_t;
|
||||
|
||||
// 添加辅助函数用于将 u16 类型数据打包到 u8 数组中
|
||||
static inline void pack_u16_to_bytes(u8 *buffer, u16 offset, u16 value) {
|
||||
buffer[offset] = (u8)(value & 0xFF);
|
||||
buffer[offset + 1] = (u8)((value >> 8) & 0xFF);
|
||||
}
|
||||
|
||||
// 添加辅助函数用于将 float 类型数据打包到 u8 数组中
|
||||
static inline void pack_float_to_bytes(u8 *buffer, u16 offset,
|
||||
float_union_t &float_union) {
|
||||
buffer[offset] = float_union.b[0];
|
||||
buffer[offset + 1] = float_union.b[1];
|
||||
buffer[offset + 2] = float_union.b[2];
|
||||
buffer[offset + 3] = float_union.b[3];
|
||||
}
|
||||
|
||||
// 添加辅助函数用于从 u8 数组中解包 short 类型数据
|
||||
static inline short unpack_bytes_to_short(u8 *buffer, u16 offset) {
|
||||
short value = (short)(buffer[offset] | (buffer[offset + 1] << 8));
|
||||
return value;
|
||||
}
|
||||
|
||||
// 添加辅助函数用于从 u8 数组中解包 u16 类型数据
|
||||
static inline u16 unpack_bytes_to_u16(u8 *buffer, u16 offset) {
|
||||
u16 value = (u16)(buffer[offset] | (buffer[offset + 1] << 8));
|
||||
return value;
|
||||
}
|
||||
|
||||
// 添加辅助函数用于从 u8 数组中解包 float 类型数据
|
||||
static inline float_union_t unpack_bytes_to_float(u8 *buffer, u16 offset) {
|
||||
float_union_t float_union;
|
||||
float_union.b[0] = buffer[offset];
|
||||
float_union.b[1] = buffer[offset + 1];
|
||||
float_union.b[2] = buffer[offset + 2];
|
||||
float_union.b[3] = buffer[offset + 3];
|
||||
return float_union;
|
||||
}
|
||||
|
||||
u8 can_gpio_init(void); // CAN 引脚初始化
|
||||
void can_config(void); // CAN 初始化
|
||||
void canfd_config(void); // CANFD 初始化
|
||||
void can_dma_init(void); // CAN DMA 初始化
|
||||
void CAN0_RX0_IRQHandler(void); // CAN 接收中断处理函数
|
||||
void DMA0_Channel4_IRQHandler(void); // DMA 通道 4 中断处理函数 (CAN 接收)
|
||||
void DMA0_Channel5_IRQHandler(void); // DMA 通道 5 中断处理函数 (CAN 发送)
|
||||
void jumpToBOOT();
|
||||
|
||||
void can_loopback_init(void);
|
||||
void can_loopback_test(void);
|
||||
|
||||
/*!
|
||||
\brief CAN 通信类
|
||||
\note 封装 CAN 发送和接收功能，支持多电机节点 ID 隔离
|
||||
*/
|
||||
class CAN_transmiter {
|
||||
|
||||
private:
|
||||
static const uint8_t CAN_RX_BUFFER_SIZE = 8; // CAN 接收缓冲区大小
|
||||
static const u32 CAN_SILENT_DURATION_MS = 10000; // CAN 静默时长 10 秒
|
||||
|
||||
bool is_sleep = false; // 是否休眠
|
||||
bool is_silent = false; // 是否静默（收到 base_id=0x0000 后进入静默）
|
||||
u32 silent_start_time = 0; // 静默开始时间（ms）
|
||||
|
||||
u16 node_id; // 本节点 ID(0-254)，使用 CAN ID 高 8 位存储 (在 init() 中初始化为 DEFAULT_NODE_ID)
|
||||
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}; // 目标角度
|
||||
float_union_t target_velocity_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 字节
|
||||
|
||||
// CAN 接收消息队列
|
||||
can_receive_message_struct rx_message_buffer[CAN_RX_BUFFER_SIZE];
|
||||
volatile uint8_t rx_buffer_head = 0;
|
||||
volatile uint8_t rx_buffer_tail = 0;
|
||||
|
||||
void update(); // 更新数据
|
||||
u8 CAN_Receive(); // CAN 接收
|
||||
u8 CANFD_Receive(); // CANFD 接收
|
||||
bool is_rx_buffer_full();
|
||||
bool is_rx_buffer_empty();
|
||||
bool dequeue_rx_message(can_receive_message_struct& message);
|
||||
void check_silent_mode(); // 检查静默模式是否结束
|
||||
|
||||
public:
|
||||
// 节点 ID 相关辅助函数 (使用高 16 位存储节点 ID)
|
||||
u32 build_can_id(u32 base_id); // 根据节点 ID 构建 CAN ID，格式：[节点 ID(16 位)][基础 ID(16 位)]
|
||||
bool is_message_for_me(u32 received_id); // 判断消息是否发给本节点
|
||||
|
||||
public:
|
||||
// 添加构造函数声明
|
||||
u8 init(void);
|
||||
|
||||
can_receive_message_struct receive_message; // 接收消息结构体
|
||||
FlagStatus can0_send_flag; // CAN0 发送标志
|
||||
float_union_t testfloat_union = {0}; // 测试用
|
||||
u16 test_u16 = 0;
|
||||
|
||||
bool MagStatus = 0; // 磁状态码
|
||||
bool ThermalStatus = 0; // 热能状态码
|
||||
void communication_check(); // 通信检查
|
||||
u8 CAN_Sleep(); // CAN 进入睡眠
|
||||
u8 CAN_Wakeup(); // CAN 唤醒
|
||||
u8 CAN_AsyncSend(u32 ID, u8 length, u8 *data); // CAN 异步发送 (使用 DMA)
|
||||
u8 CANFD_AsyncSend(u32 ID, u8 length, u8 *data); // CANFD 异步发送 (使用 DMA)
|
||||
u8 CAN_all_Send(); // CAN 发送
|
||||
u8 CANFD_all_Send(); // CANFD 发送
|
||||
void enter_silent_mode(); // 进入静默模式（停发 10 秒）
|
||||
bool is_in_silent_mode(); // 检查是否在静默模式中
|
||||
|
||||
// 节点 ID 管理接口
|
||||
u16 get_node_id(); // 获取当前节点 ID
|
||||
void set_node_id(u16 new_id); // 设置节点 ID
|
||||
u8 load_node_id_from_flash(); // 从 Flash 加载节点 ID
|
||||
u8 save_node_id_to_flash(u16 new_id); // 保存节点 ID 到 Flash
|
||||
|
||||
// 中断处理函数需要访问的公共接口
|
||||
void enqueue_rx_message_from_isr(const can_receive_message_struct& message) {
|
||||
if (!is_rx_buffer_full()) {
|
||||
rx_message_buffer[rx_buffer_head] = message;
|
||||
rx_buffer_head = (rx_buffer_head + 1) % CAN_RX_BUFFER_SIZE;
|
||||
}
|
||||
// 如果缓冲区满，丢弃新消息
|
||||
}
|
||||
};
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif
|
||||
Reference in New Issue
Block a user