From bdf03517b31de208ce811fa5cc0daea1ba22f698 Mon Sep 17 00:00:00 2001 From: cbb <2625274797@qq.com> Date: Thu, 19 Mar 2026 23:49:08 +0800 Subject: [PATCH] =?UTF-8?q?feat(CAN):=20=E6=B7=BB=E5=8A=A0=E9=9D=99?= =?UTF-8?q?=E9=BB=98=E6=A8=A1=E5=BC=8F=E6=94=AF=E6=8C=81=E5=B9=B6=E4=BC=98?= =?UTF-8?q?=E5=8C=96=E8=8A=82=E7=82=B9ID=E5=A4=84=E7=90=86=20-=20=E6=B7=BB?= =?UTF-8?q?=E5=8A=A0=E9=9D=99=E9=BB=98=E6=A8=A1=E5=BC=8F=E5=8A=9F=E8=83=BD?= =?UTF-8?q?=EF=BC=8C=E5=9C=A8=E6=8E=A5=E6=94=B6=E5=88=B0base=5Fid=3D0x0000?= =?UTF-8?q?=E6=B6=88=E6=81=AF=E6=97=B6=E5=81=9C=E6=AD=A2=E5=8F=91=E9=80=81?= =?UTF-8?q?10=E7=A7=92=20-=20=E4=BF=AE=E6=AD=A3=E8=8A=82=E7=82=B9ID?= =?UTF-8?q?=E5=8A=A0=E8=BD=BD=E9=80=BB=E8=BE=91=EF=BC=8C=E5=8C=BA=E5=88=86?= =?UTF-8?q?=E6=9C=89=E6=95=88=E8=8A=82=E7=82=B9ID(0-65534)=E5=92=8CFlash?= =?UTF-8?q?=E6=93=A6=E9=99=A4=E7=8A=B6=E6=80=81(0xFFFF)=20-=20=E6=B7=BB?= =?UTF-8?q?=E5=8A=A0=E9=9D=99=E9=BB=98=E6=A8=A1=E5=BC=8F=E7=9B=B8=E5=85=B3?= =?UTF-8?q?=E7=9A=84=E7=8A=B6=E6=80=81=E6=A3=80=E6=9F=A5=E5=92=8C=E6=97=B6?= =?UTF-8?q?=E9=97=B4=E7=AE=A1=E7=90=86=E5=8A=9F=E8=83=BD=20-=20=E5=9C=A8CA?= =?UTF-8?q?N=E9=80=9A=E4=BF=A1=E6=A3=80=E6=9F=A5=E4=B8=AD=E9=9B=86?= =?UTF-8?q?=E6=88=90=E9=9D=99=E9=BB=98=E6=A8=A1=E5=BC=8F=E5=A4=84=E7=90=86?= =?UTF-8?q?=E6=B5=81=E7=A8=8B?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit fix(FOC): 修复速度控制中的方向参数应用 - 在velocity_control、angle_control_with_velocity_limit和MIT_Mode函数中 正确应用DIR参数调整速度误差的符号 - 添加详细注释说明DIR参数对速度误差计算的影响 refactor(systick): 添加毫秒级系统时间功能 - 实现get_systick_ms()函数提供毫秒级系统时间 - 添加SysTick中断处理程序更新系统时间计数器 chore(init): 调整初始化顺序优化系统启动流程 - 将EEPROM初始化移至CAN初始化之前，确保正确的依赖关系 --- Robot_V2/Algorithm/FOC.cpp | 10 ++-- Robot_V2/device/ADC.cpp | 2 +- Robot_V2/device/Can.cpp | 101 +++++++++++++++++++++++++++++-------- Robot_V2/device/Can.h | 6 +++ Robot_V2/main.cpp | 9 ++-- Robot_V2/sys/config.h | 6 +-- Robot_V2/sys/flash.cpp | 4 +- Robot_V2/sys/systick.cpp | 22 ++++++++ Robot_V2/sys/systick.h | 2 + 9 files changed, 126 insertions(+), 36 deletions(-) diff --git a/Robot_V2/Algorithm/FOC.cpp b/Robot_V2/Algorithm/FOC.cpp index f7c167c..1a46ccd 100644 --- a/Robot_V2/Algorithm/FOC.cpp +++ b/Robot_V2/Algorithm/FOC.cpp @@ -367,8 +367,10 @@ float FOC_Controller::angle_control() { * @return float: 当前速度 */ float FOC_Controller::velocity_control() { - // PID 控制计算 - float Uq = vel_loop((target_velocity - rt_velocity) * REDUCTION_RATIO); + // PID 控制计算，使用 DIR 参数调整速度误差的符号 + // DIR = -1.0f 时：速度误差 = -(target_velocity - rt_velocity) = rt_velocity - target_velocity + // DIR = 1.0f 时：速度误差 = target_velocity - rt_velocity + float Uq = vel_loop((target_velocity - rt_velocity) * DIR * REDUCTION_RATIO); CAN0_transmiter.testfloat_union.f = Uq; // 获取电机轴角度 @@ -424,7 +426,7 @@ float FOC_Controller::angle_control_with_velocity_limit() { } else { vel_inVelAngloop.set_P(vel_angloop_p); } - float Uq = vel_inVelAngloop(target_velocity - rt_velocity); + float Uq = vel_inVelAngloop(target_velocity - rt_velocity)* DIR; CAN0_transmiter.testfloat_union.f = Uq; // // 实现力度控制：如果电流超过目标电流，则限制输出 @@ -465,7 +467,7 @@ float FOC_Controller::MIT_Mode() { // 第二层：速度环（中环） // 根据目标速度和实际速度的差值，通过速度环PID控制器计算目标电流 - float velocity_error = (rt_velocity - target_velocity); + float velocity_error = (target_velocity - rt_velocity)* DIR; vel_inMITloop.set_P(vel_MIT_p); float target_current = vel_inMITloop(velocity_error); diff --git a/Robot_V2/device/ADC.cpp b/Robot_V2/device/ADC.cpp index da04452..6c9deff 100644 --- a/Robot_V2/device/ADC.cpp +++ b/Robot_V2/device/ADC.cpp @@ -80,7 +80,7 @@ void adc_config(void) { adc_channel_length_config(ADC0, ADC_REGULAR_CHANNEL, 6); /* 配置ADC规则通道 */ adc_regular_channel_config(ADC0, 0, ADC_CHANNEL_16, - ADC_SAMPLETIME_55POINT5); + ADC_SAMPLETIME_41POINT5); adc_regular_channel_config(ADC0, 1, ADC_CHANNEL_9, ADC_SAMPLETIME_28POINT5); adc_regular_channel_config(ADC0, 2, ADC_CHANNEL_3, ADC_SAMPLETIME_28POINT5); adc_regular_channel_config(ADC0, 3, ADC_CHANNEL_4, ADC_SAMPLETIME_28POINT5); diff --git a/Robot_V2/device/Can.cpp b/Robot_V2/device/Can.cpp index 260d7b2..f6df0bb 100644 --- a/Robot_V2/device/Can.cpp +++ b/Robot_V2/device/Can.cpp @@ -33,6 +33,7 @@ extern EncoderAngleKalman Angle_kalman; // 角度滤波 extern EncoderVelocityKalman Vel_kalman; // 速度滤波 extern FlashEEPROM flashEEP; extern VersionManager Version; // 声明外部版本管理器实例 +extern u32 motor_runtime_50us; // 电机驱动计算轮次（用于静默模式计时） // 添加 DMA 相关变量 dma_parameter_struct dma_rx_parameter; @@ -105,16 +106,17 @@ void CAN_transmiter::set_node_id(u16 new_id) { \retval FLASH_EEPROM_OK: 加载成功 其他：加载失败，使用默认值 \note Flash 擦除状态为 0xFFFF，需要特殊处理避免误判 + 注意：0 是有效的节点 ID，不应被视为无效值 */ u8 CAN_transmiter::load_node_id_from_flash() { u16 node_id_temp = 0xFFFF; // 初始化为 Flash 擦除值 FlashEepromStatus status = flashEEP.read(FlashEEPROM::ADDR_NODE_ID, &node_id_temp); - // 检查 Flash 是否为擦除状态 (0xFFFF 或 0) 或读取失败 - // 注意：Flash 未写入时读到的值是 0xFFFF，但有些情况可能读到 0 - if (status != FLASH_EEPROM_OK || node_id_temp == 0xFFFF || - node_id_temp == 0) { + // 检查 Flash 是否为擦除状态 (0xFFFF) 或读取失败 + // 注意：Flash 未写入时读到的值是 0xFFFF + // 0 是有效的节点 ID，不应被视为无效值 + if (status != FLASH_EEPROM_OK || node_id_temp == 0xFFFF) { node_id = DEFAULT_NODE_ID; CAN_DEBUG_PRINT( "Node ID not found in Flash (status=%d, val=0x%04X), using " @@ -123,7 +125,7 @@ u8 CAN_transmiter::load_node_id_from_flash() { return FLASH_EEPROM_DATA_NOT_FOUND; } - // 检查节点 ID 是否有效 + // 检查节点 ID 是否有效 (0-65534 都是有效值) if (node_id_temp <= MAX_NODE_ID) { node_id = node_id_temp; CAN_DEBUG_PRINT("Node ID loaded from Flash: %d\r\n", node_id); @@ -203,6 +205,37 @@ bool CAN_transmiter::dequeue_rx_message(can_receive_message_struct &message) { return false; } +// 静默模式相关函数实现 +// 使用 systick 获取系统时间（ms） +extern u32 get_systick_ms(void); + +/*! + \brief 进入静默模式（停发 10 秒） +*/ +void CAN_transmiter::enter_silent_mode() { + is_silent = true; + silent_start_time = get_systick_ms(); // 记录当前时间（ms） + CAN_DEBUG_PRINT("Enter silent mode for %d ms, start_time=%d\r\n", CAN_SILENT_DURATION_MS, silent_start_time); +} + +/*! + \brief 检查是否在静默模式中 +*/ +bool CAN_transmiter::is_in_silent_mode() { return is_silent; } + +/*! + \brief 检查静默模式是否结束 +*/ +void CAN_transmiter::check_silent_mode() { + if (is_silent) { + u32 current_time = get_systick_ms(); // 当前时间（ms） + if (current_time - silent_start_time >= CAN_SILENT_DURATION_MS) { + is_silent = false; + CAN_DEBUG_PRINT("Exit silent mode, elapsed=%d ms\r\n", current_time - silent_start_time); + } + } +} + /*! \brief initialize CAN GPIO \param[in] none @@ -645,10 +678,16 @@ void CAN_transmiter::update(void) { /** * @brief CANFD 发送函数 - * @retval 0: 发送失败 + * @retval 0: 发送失败（或处于静默模式） * 1: 发送成功 */ u8 CAN_transmiter::CANFD_all_Send() { + // 检查是否在静默模式中，如果是则不发送 + if (is_in_silent_mode()) { + check_silent_mode(); // 检查是否已退出静默 + return 0; + } + u32 success_flag = 0; // 打包 status 数据 - 只清零实际使用的 32 字节 @@ -694,11 +733,13 @@ u8 CAN_transmiter::CANFD_all_Send() { void CAN_transmiter::communication_check() { // CAN0 发送数据标志 if (SET == can0_send_flag) { - // CAN_DEBUG_PRINT("\r\n can0 %X receive data:", - // receive_message.rx_efid); - update(); - // CAN0_transmiter.CAN_all_Send(); - CANFD_all_Send(); + // 检查是否在静默模式中 + if (!is_in_silent_mode()) { + update(); + CANFD_all_Send(); + } else { + check_silent_mode(); // 检查静默是否结束 + } can0_send_flag = RESET; } @@ -709,14 +750,37 @@ void CAN_transmiter::communication_check() { // current_message.rx_efid); for (i = 0; i < current_message.rx_dlen; // i++) { CAN_DEBUG_PRINT(" %02x", current_message.rx_data[i]); // } - // 检查消息是否是发给本节点的 (检查高 16 位) + // 提取基础 ID (低 16 位，去除高 16 位节点 ID 部分) + u32 base_id = current_message.rx_efid & 0xFFFF; + + // 提取发送方的节点 ID + u16 sender_node_id = (u16)((current_message.rx_efid >> 16) & 0xFFFF); + + // 特殊处理 base_id=0x0000（BOOT 命令）：即使是其他节点的广播消息也要处理 + if (base_id == CANFD_BOOT_ID) { + // 判断是广播消息还是其他节点的命令 + if (sender_node_id == BROADCAST_NODE_ID || sender_node_id != node_id) { + // 广播消息或其他节点的命令 → 进入静默模式（停发 10 秒） + CAN_DEBUG_PRINT("Received base_id=0x0000 from node %d, entering silent mode for 10s\r\n", sender_node_id); + enter_silent_mode(); + } else { + // 自己节点的命令 → 执行 BOOT 功能 + CAN_DEBUG_PRINT("Received base_id=0x0000 from self, jumping to BOOT\r\n"); + motor_sleep(); + flashEEP.save_system_config(false); + fmc_unlock(); + OTA_SET(n_APP_LATEST_EN_ADDR, OTA_NOT_AVAL); // 设置 APP 为不激活 + fmc_lock(); + jumpToBOOT(); + } + continue; // BOOT 命令处理完跳过后续逻辑 + } + + // 其他 base_id 的消息，检查是否是发给本节点的 if (!is_message_for_me(current_message.rx_efid)) { continue; // 不是发给本节点的消息，跳过 } - // 提取基础 ID (低 16 位，去除高 16 位节点 ID 部分) - u32 base_id = current_message.rx_efid & 0xFFFF; - if (base_id == CANFD_CMD_ID) { // 控制模式 FOC.control_mode = current_message.rx_data[0]; @@ -787,13 +851,6 @@ void CAN_transmiter::communication_check() { } else if (base_id == CANFD_VERSION_ID) { // 处理版本号请求，发送当前系统版本信息 Version.sendVersionInfoViaCan(); - } else if (base_id == CANFD_BOOT_ID) { - motor_sleep(); - flashEEP.save_system_config(false); - fmc_unlock(); - OTA_SET(n_APP_LATEST_EN_ADDR, OTA_NOT_AVAL); // 设置 APP 为不激活 - fmc_lock(); - jumpToBOOT(); } else if (base_id == CANFD_OTA_ACK_ID) { volatile u32 *flash_ptr = (volatile u32 *)n_APP_LATEST_EN_ADDR; u32 app_enable = *flash_ptr; diff --git a/Robot_V2/device/Can.h b/Robot_V2/device/Can.h index 195b40c..769f9af 100644 --- a/Robot_V2/device/Can.h +++ b/Robot_V2/device/Can.h @@ -73,8 +73,11 @@ 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; // 目标控制模式 @@ -115,6 +118,7 @@ class CAN_transmiter { 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) @@ -139,6 +143,8 @@ class CAN_transmiter { 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 diff --git a/Robot_V2/main.cpp b/Robot_V2/main.cpp index 5bd063c..488d078 100644 --- a/Robot_V2/main.cpp +++ b/Robot_V2/main.cpp @@ -78,13 +78,12 @@ u8 hardware_init(void) { init_status += KTH7823.init(); // 角度传感器初始化 init_status += sampler.init(); // 采样器初始化 - // 先初始化 CAN，确保节点 ID 被正确设置 - // CAN 初始化会先设置默认节点 ID，然后从 Flash 加载 - init_status += CAN0_transmiter.init(); // CAN 初始化 - - // EEPROM 初始化 (必须在 CAN 初始化之后，因为会读取节点 ID) + // EEPROM 初始化 (必须在 CAN 初始化之前，因为 CAN 会从 Flash 读取节点 ID) flashEEP.init(); // EEPROM 初始化 + // CAN 初始化 (会先设置默认节点 ID，然后从 Flash 加载已保存的节点 ID) + init_status += CAN0_transmiter.init(); // CAN 初始化 + init_status += Angle_kalman.init(KTH7823.getAngle()); // 角度滤波初始化 init_status += Vel_kalman.init(0.0f); // 速度滤波初始化 init_status += FOC.init(); // FOC 初始化 diff --git a/Robot_V2/sys/config.h b/Robot_V2/sys/config.h index 357b6a2..6647874 100644 --- a/Robot_V2/sys/config.h +++ b/Robot_V2/sys/config.h @@ -17,8 +17,8 @@ // 版本号定义 - 可根据实际项目进行调整 constexpr u16 SOFTWARE_VERSION_MAJOR = 26; constexpr u16 SOFTWARE_VERSION_MINOR = 3; -constexpr u16 SOFTWARE_VERSION_PATCH = 15; -constexpr u16 SOFTWARE_VERSION_BUILD = 4; +constexpr u16 SOFTWARE_VERSION_PATCH = 16; +constexpr u16 SOFTWARE_VERSION_BUILD = 2; constexpr u16 HARDWARE_VERSION_MAJOR = 25; constexpr u16 HARDWARE_VERSION_MINOR = 12; @@ -64,7 +64,7 @@ constexpr u32 OTA_AVAL = 0x00000000; // 全零表示可用 // 参数配置 constexpr u8 PP = 11; -constexpr float DIR = -1.0f; +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; diff --git a/Robot_V2/sys/flash.cpp b/Robot_V2/sys/flash.cpp index 13c7afa..8151865 100644 --- a/Robot_V2/sys/flash.cpp +++ b/Robot_V2/sys/flash.cpp @@ -165,7 +165,9 @@ FlashEepromStatus FlashEEPROM::read(u8 virtualAddr, u16 *data) const { u8 addr = (u8)(data_word & DATA_ADDR_MASK); // 找到匹配的虚拟地址 if (addr == virtualAddr) { - *data = (u16)(data_word & DATA_U16_MASK); + // 右移 16 位以提取高 16 位的 u16 数据 + *data = (u16)((data_word & DATA_U16_MASK) >> 16); + EEP_DEBUG_PRINT("read u16: addr=0x%x, val=%d\r\n", address, *data); return FLASH_EEPROM_OK; } } diff --git a/Robot_V2/sys/systick.cpp b/Robot_V2/sys/systick.cpp index d723152..aa5277f 100644 --- a/Robot_V2/sys/systick.cpp +++ b/Robot_V2/sys/systick.cpp @@ -52,9 +52,20 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #endif volatile static uint32_t delay; +volatile static uint32_t systick_ms = 0; // 系统运行时间（毫秒） extern CAN_transmiter CAN0_transmiter; extern FlashEEPROM flashEEP; +/*! + \brief get system tick time in milliseconds + \param[in] none + \param[out] none + \retval system time in ms +*/ +uint32_t get_systick_ms(void) { + return systick_ms; +} + /*! \brief configure systick \param[in] none @@ -71,6 +82,17 @@ void systick_config(void) { /* configure the systick handler priority */ NVIC_SetPriority(SysTick_IRQn, 0x00U); } + +/*! + \brief SysTick handler - increment millisecond counter + \param[in] none + \param[out] none + \retval none +*/ +extern "C" void SysTick_Handler(void) { + systick_ms++; // 递增系统时间 + delay_decrement(); // 递减延时计数器 +} /*! \brief delay a time in seconds \param[in] count: count in seconds diff --git a/Robot_V2/sys/systick.h b/Robot_V2/sys/systick.h index 494cf61..52ced95 100644 --- a/Robot_V2/sys/systick.h +++ b/Robot_V2/sys/systick.h @@ -42,6 +42,8 @@ extern "C" { /* configure systick */ void systick_config(void); +/* get system tick time in milliseconds */ +uint32_t get_systick_ms(void); /* delay a time in milliseconds */ void delay_1ms(uint32_t count); /* delay a time in seconds */