From 527d2e9be4bcb29a70ce81a7eb5a6b1b8161124e Mon Sep 17 00:00:00 2001 From: Robofish <1683502971@qq.com> Date: Thu, 12 Mar 2026 00:14:36 +0800 Subject: [PATCH 01/34] =?UTF-8?q?=E4=BF=AE?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .github/copilot-instructions.md | 6 ++++++ User/module/config.c | 4 ++-- 2 files changed, 8 insertions(+), 2 deletions(-) create mode 100644 .github/copilot-instructions.md diff --git a/.github/copilot-instructions.md b/.github/copilot-instructions.md new file mode 100644 index 0000000..139e11a --- /dev/null +++ b/.github/copilot-instructions.md @@ -0,0 +1,6 @@ +--- +description: 当助手已完成当前请求的工作、准备结束回复或不再有明确下一步可执行内容时，必须先调用提问工具 askQuestions 获取进一步指示；不得直接结束对话。 / When the assistant has completed the current task and is about to end the reply or has no clear next action, it must call the askQuestions tool to obtain further instructions instead of ending the conversation. +applyTo: ‘当检测到回复将以收尾语句结束或无后续动作时 / When a reply is about to end with a closing statement or has no follow-up action’ +--- +当你完成工作时，请调用提问工具（askQuestions）来获取进一步的指示，而不是直接结束对话。 +When you finish the task, call the askQuestions tool to request further instructions instead of ending the conversation. \ No newline at end of file diff --git a/User/module/config.c b/User/module/config.c index 6fa9df7..0aaff69 100644 --- a/User/module/config.c +++ b/User/module/config.c @@ -66,7 +66,7 @@ Config_RobotParam_t robot_config = { }, .mech_zero = { .yaw = 0.0f, - .pit = 0.137291431f, + .pit = 3.23056364f, }, .travel = { .yaw = -1.0f, @@ -312,7 +312,7 @@ Config_RobotParam_t robot_config = { .wheel_radius = 0.068f, .wheel_gear_ratio = 4.5f, .hip_width = 0.423f, /* 髋宽，即两腿间距 (m)，用于Roll几何补偿 */ - .joint_zero = {0.0f, 0.0f, 1.32422018f, 1.16195965f}, /* 关节电机零点偏移 */ + .joint_zero = {0.0f, 0.0f, 1.33022018f, 1.13195965f}, /* 关节电机零点偏移 */ .mech_zero_yaw = 2.96925735f, /* 机械零点 */ }, From 66e6d298089b5ddba67d280b4539e9df28d5609f Mon Sep 17 00:00:00 2001 From: Robofish <1683502971@qq.com> Date: Thu, 12 Mar 2026 00:51:15 +0800 Subject: [PATCH 02/34] =?UTF-8?q?=E6=B7=BB=E5=8A=A0ref?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- CMakeLists.txt | 3 +++ Core/Inc/stm32h7xx_it.h | 2 ++ Core/Src/dma.c | 6 ++++++ Core/Src/stm32h7xx_it.c | 30 +++++++++++++++++++++++++++ Core/Src/usart.c | 45 ++++++++++++++++++++++++++++++++++++++++- CtrBoard-H7_ALL.ioc | 44 ++++++++++++++++++++++++++++++++++++++-- User/bsp/uart.c | 4 ++++ User/bsp/uart.h | 1 + 8 files changed, 132 insertions(+), 3 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index cef6493..2a9f9f3 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -69,6 +69,7 @@ target_sources(${CMAKE_PROJECT_NAME} PRIVATE User/component/pid.c User/component/user_math.c User/component/vmc.c + User/component/ui.c # User/device sources User/device/bmi088.c @@ -83,6 +84,8 @@ target_sources(${CMAKE_PROJECT_NAME} PRIVATE User/device/vision_bridge.c User/device/vofa.c User/device/mrobot.c + User/device/referee.c + User/device/supercap.c # User/module sources User/module/balance_chassis.c diff --git a/Core/Inc/stm32h7xx_it.h b/Core/Inc/stm32h7xx_it.h index bcc30a9..60e44fa 100644 --- a/Core/Inc/stm32h7xx_it.h +++ b/Core/Inc/stm32h7xx_it.h @@ -58,6 +58,7 @@ void DMA1_Stream2_IRQHandler(void); void DMA1_Stream3_IRQHandler(void); void DMA1_Stream4_IRQHandler(void); void DMA1_Stream5_IRQHandler(void); +void DMA1_Stream6_IRQHandler(void); void FDCAN1_IT0_IRQHandler(void); void FDCAN2_IT0_IRQHandler(void); void FDCAN1_IT1_IRQHandler(void); @@ -67,6 +68,7 @@ void USART1_IRQHandler(void); void USART2_IRQHandler(void); void USART3_IRQHandler(void); void EXTI15_10_IRQHandler(void); +void DMA1_Stream7_IRQHandler(void); void UART5_IRQHandler(void); void UART7_IRQHandler(void); void ADC3_IRQHandler(void); diff --git a/Core/Src/dma.c b/Core/Src/dma.c index a079425..8cd4c9c 100644 --- a/Core/Src/dma.c +++ b/Core/Src/dma.c @@ -61,6 +61,12 @@ void MX_DMA_Init(void) /* DMA1_Stream5_IRQn interrupt configuration */ HAL_NVIC_SetPriority(DMA1_Stream5_IRQn, 5, 0); HAL_NVIC_EnableIRQ(DMA1_Stream5_IRQn); + /* DMA1_Stream6_IRQn interrupt configuration */ + HAL_NVIC_SetPriority(DMA1_Stream6_IRQn, 5, 0); + HAL_NVIC_EnableIRQ(DMA1_Stream6_IRQn); + /* DMA1_Stream7_IRQn interrupt configuration */ + HAL_NVIC_SetPriority(DMA1_Stream7_IRQn, 5, 0); + HAL_NVIC_EnableIRQ(DMA1_Stream7_IRQn); } diff --git a/Core/Src/stm32h7xx_it.c b/Core/Src/stm32h7xx_it.c index b2c4ae3..7243b2d 100644 --- a/Core/Src/stm32h7xx_it.c +++ b/Core/Src/stm32h7xx_it.c @@ -68,6 +68,8 @@ extern SPI_HandleTypeDef hspi2; extern DMA_HandleTypeDef hdma_uart5_rx; extern DMA_HandleTypeDef hdma_usart1_tx; extern DMA_HandleTypeDef hdma_usart1_rx; +extern DMA_HandleTypeDef hdma_usart10_rx; +extern DMA_HandleTypeDef hdma_usart10_tx; extern UART_HandleTypeDef huart5; extern UART_HandleTypeDef huart7; extern UART_HandleTypeDef huart1; @@ -262,6 +264,20 @@ void DMA1_Stream5_IRQHandler(void) /* USER CODE END DMA1_Stream5_IRQn 1 */ } +/** + * @brief This function handles DMA1 stream6 global interrupt. + */ +void DMA1_Stream6_IRQHandler(void) +{ + /* USER CODE BEGIN DMA1_Stream6_IRQn 0 */ + + /* USER CODE END DMA1_Stream6_IRQn 0 */ + HAL_DMA_IRQHandler(&hdma_usart10_rx); + /* USER CODE BEGIN DMA1_Stream6_IRQn 1 */ + + /* USER CODE END DMA1_Stream6_IRQn 1 */ +} + /** * @brief This function handles FDCAN1 interrupt 0. */ @@ -390,6 +406,20 @@ void EXTI15_10_IRQHandler(void) /* USER CODE END EXTI15_10_IRQn 1 */ } +/** + * @brief This function handles DMA1 stream7 global interrupt. + */ +void DMA1_Stream7_IRQHandler(void) +{ + /* USER CODE BEGIN DMA1_Stream7_IRQn 0 */ + + /* USER CODE END DMA1_Stream7_IRQn 0 */ + HAL_DMA_IRQHandler(&hdma_usart10_tx); + /* USER CODE BEGIN DMA1_Stream7_IRQn 1 */ + + /* USER CODE END DMA1_Stream7_IRQn 1 */ +} + /** * @brief This function handles UART5 global interrupt. */ diff --git a/Core/Src/usart.c b/Core/Src/usart.c index bdff729..1c68df4 100644 --- a/Core/Src/usart.c +++ b/Core/Src/usart.c @@ -33,6 +33,8 @@ UART_HandleTypeDef huart10; DMA_HandleTypeDef hdma_uart5_rx; DMA_HandleTypeDef hdma_usart1_tx; DMA_HandleTypeDef hdma_usart1_rx; +DMA_HandleTypeDef hdma_usart10_rx; +DMA_HandleTypeDef hdma_usart10_tx; /* UART5 init function */ void MX_UART5_Init(void) @@ -265,7 +267,7 @@ void MX_USART10_UART_Init(void) /* USER CODE END USART10_Init 1 */ huart10.Instance = USART10; - huart10.Init.BaudRate = 921600; + huart10.Init.BaudRate = 115200; huart10.Init.WordLength = UART_WORDLENGTH_8B; huart10.Init.StopBits = UART_STOPBITS_1; huart10.Init.Parity = UART_PARITY_NONE; @@ -598,6 +600,43 @@ void HAL_UART_MspInit(UART_HandleTypeDef* uartHandle) GPIO_InitStruct.Alternate = GPIO_AF11_USART10; HAL_GPIO_Init(GPIOE, &GPIO_InitStruct); + /* USART10 DMA Init */ + /* USART10_RX Init */ + hdma_usart10_rx.Instance = DMA1_Stream6; + hdma_usart10_rx.Init.Request = DMA_REQUEST_USART10_RX; + hdma_usart10_rx.Init.Direction = DMA_PERIPH_TO_MEMORY; + hdma_usart10_rx.Init.PeriphInc = DMA_PINC_DISABLE; + hdma_usart10_rx.Init.MemInc = DMA_MINC_ENABLE; + hdma_usart10_rx.Init.PeriphDataAlignment = DMA_PDATAALIGN_BYTE; + hdma_usart10_rx.Init.MemDataAlignment = DMA_MDATAALIGN_BYTE; + hdma_usart10_rx.Init.Mode = DMA_NORMAL; + hdma_usart10_rx.Init.Priority = DMA_PRIORITY_LOW; + hdma_usart10_rx.Init.FIFOMode = DMA_FIFOMODE_DISABLE; + if (HAL_DMA_Init(&hdma_usart10_rx) != HAL_OK) + { + Error_Handler(); + } + + __HAL_LINKDMA(uartHandle,hdmarx,hdma_usart10_rx); + + /* USART10_TX Init */ + hdma_usart10_tx.Instance = DMA1_Stream7; + hdma_usart10_tx.Init.Request = DMA_REQUEST_USART10_TX; + hdma_usart10_tx.Init.Direction = DMA_MEMORY_TO_PERIPH; + hdma_usart10_tx.Init.PeriphInc = DMA_PINC_DISABLE; + hdma_usart10_tx.Init.MemInc = DMA_MINC_ENABLE; + hdma_usart10_tx.Init.PeriphDataAlignment = DMA_PDATAALIGN_BYTE; + hdma_usart10_tx.Init.MemDataAlignment = DMA_MDATAALIGN_BYTE; + hdma_usart10_tx.Init.Mode = DMA_NORMAL; + hdma_usart10_tx.Init.Priority = DMA_PRIORITY_LOW; + hdma_usart10_tx.Init.FIFOMode = DMA_FIFOMODE_DISABLE; + if (HAL_DMA_Init(&hdma_usart10_tx) != HAL_OK) + { + Error_Handler(); + } + + __HAL_LINKDMA(uartHandle,hdmatx,hdma_usart10_tx); + /* USART10 interrupt Init */ HAL_NVIC_SetPriority(USART10_IRQn, 5, 0); HAL_NVIC_EnableIRQ(USART10_IRQn); @@ -737,6 +776,10 @@ void HAL_UART_MspDeInit(UART_HandleTypeDef* uartHandle) */ HAL_GPIO_DeInit(GPIOE, GPIO_PIN_2|GPIO_PIN_3); + /* USART10 DMA DeInit */ + HAL_DMA_DeInit(uartHandle->hdmarx); + HAL_DMA_DeInit(uartHandle->hdmatx); + /* USART10 interrupt Deinit */ HAL_NVIC_DisableIRQ(USART10_IRQn); /* USER CODE BEGIN USART10_MspDeInit 1 */ diff --git a/CtrBoard-H7_ALL.ioc b/CtrBoard-H7_ALL.ioc index fd2f482..f3759af 100644 --- a/CtrBoard-H7_ALL.ioc +++ b/CtrBoard-H7_ALL.ioc @@ -80,7 +80,9 @@ Dma.Request2=SPI2_TX Dma.Request3=UART5_RX Dma.Request4=USART1_TX Dma.Request5=USART1_RX -Dma.RequestsNb=6 +Dma.Request6=USART10_RX +Dma.Request7=USART10_TX +Dma.RequestsNb=8 Dma.SPI2_RX.1.Direction=DMA_PERIPH_TO_MEMORY Dma.SPI2_RX.1.EventEnable=DISABLE Dma.SPI2_RX.1.FIFOMode=DMA_FIFOMODE_DISABLE @@ -135,6 +137,42 @@ Dma.UART5_RX.3.SyncEnable=DISABLE Dma.UART5_RX.3.SyncPolarity=HAL_DMAMUX_SYNC_NO_EVENT Dma.UART5_RX.3.SyncRequestNumber=1 Dma.UART5_RX.3.SyncSignalID=NONE +Dma.USART10_RX.6.Direction=DMA_PERIPH_TO_MEMORY +Dma.USART10_RX.6.EventEnable=DISABLE +Dma.USART10_RX.6.FIFOMode=DMA_FIFOMODE_DISABLE +Dma.USART10_RX.6.Instance=DMA1_Stream6 +Dma.USART10_RX.6.MemDataAlignment=DMA_MDATAALIGN_BYTE +Dma.USART10_RX.6.MemInc=DMA_MINC_ENABLE +Dma.USART10_RX.6.Mode=DMA_NORMAL +Dma.USART10_RX.6.PeriphDataAlignment=DMA_PDATAALIGN_BYTE +Dma.USART10_RX.6.PeriphInc=DMA_PINC_DISABLE +Dma.USART10_RX.6.Polarity=HAL_DMAMUX_REQ_GEN_RISING +Dma.USART10_RX.6.Priority=DMA_PRIORITY_LOW +Dma.USART10_RX.6.RequestNumber=1 +Dma.USART10_RX.6.RequestParameters=Instance,Direction,PeriphInc,MemInc,PeriphDataAlignment,MemDataAlignment,Mode,Priority,FIFOMode,SignalID,Polarity,RequestNumber,SyncSignalID,SyncPolarity,SyncEnable,EventEnable,SyncRequestNumber +Dma.USART10_RX.6.SignalID=NONE +Dma.USART10_RX.6.SyncEnable=DISABLE +Dma.USART10_RX.6.SyncPolarity=HAL_DMAMUX_SYNC_NO_EVENT +Dma.USART10_RX.6.SyncRequestNumber=1 +Dma.USART10_RX.6.SyncSignalID=NONE +Dma.USART10_TX.7.Direction=DMA_MEMORY_TO_PERIPH +Dma.USART10_TX.7.EventEnable=DISABLE +Dma.USART10_TX.7.FIFOMode=DMA_FIFOMODE_DISABLE +Dma.USART10_TX.7.Instance=DMA1_Stream7 +Dma.USART10_TX.7.MemDataAlignment=DMA_MDATAALIGN_BYTE +Dma.USART10_TX.7.MemInc=DMA_MINC_ENABLE +Dma.USART10_TX.7.Mode=DMA_NORMAL +Dma.USART10_TX.7.PeriphDataAlignment=DMA_PDATAALIGN_BYTE +Dma.USART10_TX.7.PeriphInc=DMA_PINC_DISABLE +Dma.USART10_TX.7.Polarity=HAL_DMAMUX_REQ_GEN_RISING +Dma.USART10_TX.7.Priority=DMA_PRIORITY_LOW +Dma.USART10_TX.7.RequestNumber=1 +Dma.USART10_TX.7.RequestParameters=Instance,Direction,PeriphInc,MemInc,PeriphDataAlignment,MemDataAlignment,Mode,Priority,FIFOMode,SignalID,Polarity,RequestNumber,SyncSignalID,SyncPolarity,SyncEnable,EventEnable,SyncRequestNumber +Dma.USART10_TX.7.SignalID=NONE +Dma.USART10_TX.7.SyncEnable=DISABLE +Dma.USART10_TX.7.SyncPolarity=HAL_DMAMUX_SYNC_NO_EVENT +Dma.USART10_TX.7.SyncRequestNumber=1 +Dma.USART10_TX.7.SyncSignalID=NONE Dma.USART1_RX.5.Direction=DMA_PERIPH_TO_MEMORY Dma.USART1_RX.5.EventEnable=DISABLE Dma.USART1_RX.5.FIFOMode=DMA_FIFOMODE_DISABLE @@ -334,6 +372,8 @@ NVIC.DMA1_Stream2_IRQn=true\:5\:0\:false\:false\:true\:true\:false\:true\:true NVIC.DMA1_Stream3_IRQn=true\:5\:0\:false\:false\:true\:true\:false\:true\:true NVIC.DMA1_Stream4_IRQn=true\:5\:0\:false\:false\:true\:true\:false\:true\:true NVIC.DMA1_Stream5_IRQn=true\:5\:0\:false\:false\:true\:true\:false\:true\:true +NVIC.DMA1_Stream6_IRQn=true\:5\:0\:false\:false\:true\:true\:false\:true\:true +NVIC.DMA1_Stream7_IRQn=true\:5\:0\:false\:false\:true\:true\:false\:true\:true NVIC.DebugMonitor_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false\:false NVIC.EXTI15_10_IRQn=true\:5\:0\:false\:false\:true\:true\:true\:true\:true NVIC.FDCAN1_IT0_IRQn=true\:5\:0\:false\:false\:true\:true\:true\:true\:true @@ -758,7 +798,7 @@ UART7.IPParameters=BaudRate USART1.BaudRate=921600 USART1.IPParameters=VirtualMode-Asynchronous,BaudRate USART1.VirtualMode-Asynchronous=VM_ASYNC -USART10.BaudRate=921600 +USART10.BaudRate=115200 USART10.IPParameters=VirtualMode,BaudRate USART10.VirtualMode=VM_ASYNC USART2.BaudRate=921600 diff --git a/User/bsp/uart.c b/User/bsp/uart.c index 5afaa39..b1008f0 100644 --- a/User/bsp/uart.c +++ b/User/bsp/uart.c @@ -27,6 +27,8 @@ static BSP_UART_t UART_Get(UART_HandleTypeDef *huart) { return BSP_UART_DR16; else if (huart->Instance == USART1) return BSP_UART_VOFA; + else if (huart->Instance == USART10) + return BSP_UART_REF; else return BSP_UART_ERR; } @@ -119,6 +121,8 @@ UART_HandleTypeDef *BSP_UART_GetHandle(BSP_UART_t uart) { return &huart5; case BSP_UART_VOFA: return &huart1; + case BSP_UART_REF: + return &huart10; default: return NULL; } diff --git a/User/bsp/uart.h b/User/bsp/uart.h index 669379a..80ffe91 100644 --- a/User/bsp/uart.h +++ b/User/bsp/uart.h @@ -29,6 +29,7 @@ extern "C" { typedef enum { BSP_UART_DR16, BSP_UART_VOFA, + BSP_UART_REF, BSP_UART_NUM, BSP_UART_ERR, } BSP_UART_t; From abc8885795e98947a548878e4211cd900d59b5db Mon Sep 17 00:00:00 2001 From: xxxxm <2389287465@qq.com> Date: Sat, 14 Mar 2026 12:41:14 +0800 Subject: [PATCH 03/34] bc --- User/device/referee.h | 5 -- User/module/cmd/cmd.c | 19 ++--- User/module/config.c | 24 +++--- User/module/shoot.c | 183 ++++++++++++++++++++++++------------------ User/module/shoot.h | 7 +- 5 files changed, 133 insertions(+), 105 deletions(-) diff --git a/User/device/referee.h b/User/device/referee.h index e74582b..a771bb5 100644 --- a/User/device/referee.h +++ b/User/device/referee.h @@ -574,11 +574,6 @@ typedef struct { osTimerId_t ui_slow_timer_id; } Referee_t; -/* Referee_ChassisUI_t 已移至 module/chassis.h */ -/* Referee_CapUI_t 已移至 device/supercap.h */ -/* Referee_GimbalUI_t 已移至 module/gimbal.h */ -/* Referee_ShootUI_t 已移至 module/shoot.h */ - typedef struct __packed { /* UI缓冲数据 */ UI_Ele_t grapic[REF_UI_MAX_GRAPIC_NUM]; diff --git a/User/module/cmd/cmd.c b/User/module/cmd/cmd.c index 9219749..c13f9e5 100644 --- a/User/module/cmd/cmd.c +++ b/User/module/cmd/cmd.c @@ -59,8 +59,8 @@ static void CMD_RC_BuildGimbalCmd(CMD_t *ctx) { } /* 左摇杆控制云台 */ - ctx->output.gimbal.cmd.delta_yaw = -ctx->input.rc.joy_left.x * 4.0f; - ctx->output.gimbal.cmd.delta_pit = -ctx->input.rc.joy_left.y * 2.5f; + ctx->output.gimbal.cmd.delta_yaw = -ctx->input.rc.joy_right.x * 5.0f; + ctx->output.gimbal.cmd.delta_pit = ctx->input.rc.joy_right.y * 5.0f; } #endif /* CMD_ENABLE_SRC_RC && CMD_ENABLE_MODULE_GIMBAL */ @@ -68,7 +68,7 @@ static void CMD_RC_BuildGimbalCmd(CMD_t *ctx) { #if CMD_ENABLE_SRC_RC && CMD_ENABLE_MODULE_SHOOT static void CMD_RC_BuildShootCmd(CMD_t *ctx) { if (ctx->input.online[CMD_SRC_RC]) { - ctx->output.shoot.cmd.mode = SHOOT_MODE_SINGLE; + ctx->output.shoot.cmd.mode = SHOOT_MODE_BURST; } else { ctx->output.shoot.cmd.mode = SHOOT_MODE_SAFE; } @@ -393,12 +393,13 @@ static void CMD_PC_BuildBalanceChassisCmd(CMD_t *ctx) { float vx = 0.0f, wz = 0.0f; uint32_t kb = ctx->input.pc.keyboard.bitmap; - if (kb & CMD_KEY_W) vx += sens->move_sens; - if (kb & CMD_KEY_S) vx -= sens->move_sens; - if (kb & CMD_KEY_A) wz += sens->move_sens; - if (kb & CMD_KEY_D) wz -= sens->move_sens; - if (kb & CMD_KEY_SHIFT) { vx *= sens->move_fast_mult; wz *= sens->move_fast_mult; } - if (kb & CMD_KEY_CTRL) { vx *= sens->move_slow_mult; wz *= sens->move_slow_mult; } + //案件的命令要放到behavior里， + // if (kb & CMD_KEY_W) vx += sens->move_sens; + // if (kb & CMD_KEY_S) vx -= sens->move_sens; + // if (kb & CMD_KEY_A) wz += sens->move_sens; + // if (kb & CMD_KEY_D) wz -= sens->move_sens; + // if (kb & CMD_KEY_SHIFT) { vx *= sens->move_fast_mult; wz *= sens->move_fast_mult; } + // if (kb & CMD_KEY_CTRL) { vx *= sens->move_slow_mult; wz *= sens->move_slow_mult; } ctx->output.balance_chassis.cmd.move_vec.vx = vx; ctx->output.balance_chassis.cmd.move_vec.wz = wz; diff --git a/User/module/config.c b/User/module/config.c index 0ee13be..35e95c3 100644 --- a/User/module/config.c +++ b/User/module/config.c @@ -110,19 +110,19 @@ Config_RobotParam_t robot_config = { .projectileType=SHOOT_PROJECTILE_17MM, .fric_num=2, .extra_deceleration_ratio=1.0f, - .num_trig_tooth=5, + .num_trig_tooth=8, .shot_freq=1.0f, .shot_burst_num=3, .ratio_multilevel = {1.0f}, }, .jamDetection={ .enable=true, - .threshold=310.0f, + .threshold=200.0f, .suspectedTime=0.5f, }, .heatControl={ .enable=true, - .nmax=2.0f, // 最大射频 Hz + .nmax=18.0f, // 最大射频 Hz .Hwarn=200.0f, // 热量预警值 .Hsatu=100.0f, // 热量饱和值 .Hthres=50.0f, // 热量阈值 @@ -151,10 +151,10 @@ Config_RobotParam_t robot_config = { } }, .trig = { - .can = BSP_FDCAN_2, - .id = 0x205, - .module = MOTOR_M3508, - .reverse = false, + .can = BSP_CAN_1, + .id = 0x203, + .module = MOTOR_M2006, + .reverse = true, .gear=true, }, }, @@ -180,12 +180,12 @@ Config_RobotParam_t robot_config = { .range=-1.0f, }, .trig_2006 = { - .k=2.5f, + .k=1.0f, .p=1.0f, .i=0.1f, - .d=0.04f, - .i_limit=0.4f, - .out_limit=1.0f, + .d=0.05f, + .i_limit=0.8f, + .out_limit=0.5f, .d_cutoff_freq=-1.0f, .range=M_2PI, }, @@ -195,7 +195,7 @@ Config_RobotParam_t robot_config = { .i=0.3f, .d=0.5f, .i_limit=0.2f, - .out_limit=1.0f, + .out_limit=0.9f, .d_cutoff_freq=-1.0f, .range=-1.0f, }, diff --git a/User/module/shoot.c b/User/module/shoot.c index f03bc53..1774165 100644 --- a/User/module/shoot.c +++ b/User/module/shoot.c @@ -46,7 +46,6 @@ void Task(void *argument) { /* 发射检测参数 */ #define SHOT_DETECT_RPM_DROP_THRESHOLD 100.0f /* 摩擦轮转速下降阈值，单位rpm */ #define SHOT_DETECT_SUSPECTED_TIME 0.0005f /* 发射嫌疑持续时间阈值，单位秒 */ -#define FRIC_READY_RPM_RATIO 0.95f /* 摩擦轮准备好的转速比例 */ /* Private macro ------------------------------------------------------------ */ /* Private variables -------------------------------------------------------- */ @@ -236,40 +235,6 @@ static float Shoot_CalcAvgFricRpm(Shoot_t *s) return (fric_num > 0) ? (sum / fric_num) : 0.0f; } -/** - * \brief 发射检测：通过摩擦轮掉速检测发射 - * - * \param s 包含发射数据的结构体 - * - * \return 是否检测到掉速超过阈值 - */ -static bool Shoot_DetectShotByRpmDrop(Shoot_t *s) -{ - if (s == NULL) { - return false; - } - - /* 只在READY和FIRE状态下检测，避免停机时误判 */ - if (s->running_state != SHOOT_STATE_READY && - s->running_state != SHOOT_STATE_FIRE) { - return false; - } - - /* 计算当前摩擦轮平均转速 */ - s->heatcontrol.avgFricRpm = Shoot_CalcAvgFricRpm(s); - - /* 计算当前转速与目标转速的差值（掉速量） */ - s->heatcontrol.rpmDrop = s->target_variable.fric_rpm - s->heatcontrol.avgFricRpm; - - /* 判断是否发生明显掉速 */ - if (s->heatcontrol.rpmDrop > SHOT_DETECT_RPM_DROP_THRESHOLD) { - return true; - } - - return false; -} - - /** * \brief 热量数据融合：用裁判系统数据修正自主估计 * @@ -353,18 +318,29 @@ static int8_t Shoot_HeatDetectionFSM(Shoot_t *s) /* 计算当前平均转速 */ s->heatcontrol.avgFricRpm = Shoot_CalcAvgFricRpm(s); - /* 检查摩擦轮是否达到目标转速的85%以上 */ - if (s->heatcontrol.avgFricRpm >= s->target_variable.fric_rpm * FRIC_READY_RPM_RATIO) { + /* 检查摩擦轮是否初步达到设定速度 */ + if (s->target_variable.fric_rpm - s->heatcontrol.avgFricRpm < SHOT_DETECT_RPM_DROP_THRESHOLD * 0.8f) { s->heatcontrol.detectState = SHOOT_HEAT_DETECT_READY; - s->heatcontrol.lastAvgFricRpm = s->heatcontrol.avgFricRpm; + s->heatcontrol.baselineRpm = s->heatcontrol.avgFricRpm; // 记录初始基准转速 } } break; case SHOOT_HEAT_DETECT_READY: - /* 准备检测状态：监测摩擦轮掉速 */ - /* 检测掉速（当前转速低于目标转速超过阈值） */ - if (Shoot_DetectShotByRpmDrop(s)) { + /* 准备检测状态：监测摩擦轮相对当前的基准发生掉速 */ + s->heatcontrol.avgFricRpm = Shoot_CalcAvgFricRpm(s); + + /* 动态更新平稳时的基准转速：若当前转速比基准高，迅速成为新基准；若更低，基准非常缓慢地衰减，以适应正常波动 */ + if (s->heatcontrol.avgFricRpm > s->heatcontrol.baselineRpm) { + s->heatcontrol.baselineRpm = s->heatcontrol.avgFricRpm; + } else { + s->heatcontrol.baselineRpm = s->heatcontrol.baselineRpm * 0.999f + s->heatcontrol.avgFricRpm * 0.001f; + } + + s->heatcontrol.rpmDrop = s->heatcontrol.baselineRpm - s->heatcontrol.avgFricRpm; + + /* 检测掉速（当前转速低于动态基准超过阈值） */ + if (s->heatcontrol.rpmDrop > SHOT_DETECT_RPM_DROP_THRESHOLD) { s->heatcontrol.detectState = SHOOT_HEAT_DETECT_SUSPECTED; s->heatcontrol.detectTime = s->timer.now; /* 记录进入嫌疑状态的时间 */ } @@ -377,45 +353,47 @@ static int8_t Shoot_HeatDetectionFSM(Shoot_t *s) case SHOOT_HEAT_DETECT_SUSPECTED: /* 发射嫌疑状态：持续检测掉速 */ - /* 更新掉速量 */ s->heatcontrol.avgFricRpm = Shoot_CalcAvgFricRpm(s); - s->heatcontrol.rpmDrop = s->target_variable.fric_rpm - s->heatcontrol.avgFricRpm; + s->heatcontrol.rpmDrop = s->heatcontrol.baselineRpm - s->heatcontrol.avgFricRpm; - /* 若掉速消失（转速恢复正常），回到准备状态 */ - if (s->heatcontrol.rpmDrop < SHOT_DETECT_RPM_DROP_THRESHOLD) { + /* 若掉速消失（转速刚掉下又马上弹回基准附近），回到准备状态 (阈值的 60%) */ + if (s->heatcontrol.rpmDrop < SHOT_DETECT_RPM_DROP_THRESHOLD * 0.6f) { s->heatcontrol.detectState = SHOOT_HEAT_DETECT_READY; } - /* 若嫌疑状态持续超过阈值时间，确认发射 */ + /* 若掉速状态持续超过设定的嫌疑时间，确认这是一次发射 */ else if ((s->timer.now - s->heatcontrol.detectTime) >= SHOT_DETECT_SUSPECTED_TIME) { s->heatcontrol.detectState = SHOOT_HEAT_DETECT_CONFIRMED; } break; case SHOOT_HEAT_DETECT_CONFIRMED: - /* 确认发射状态：增加热量并返回准备状态 */ - /* 根据弹丸类型增加估计热量 */ - switch (s->param->basic.projectileType) { - case SHOOT_PROJECTILE_17MM: - s->heatcontrol.Hnow_estimated += 10.0f; - break; - case SHOOT_PROJECTILE_42MM: - s->heatcontrol.Hnow_estimated += 100.0f; - break; - default: - s->heatcontrol.Hnow_estimated += s->heatcontrol.Hgen; - break; + if (s->heatcontrol.unverified_shots > 0) { + s->heatcontrol.unverified_shots--; + } + s->heatcontrol.detectState = SHOOT_HEAT_DETECT_RECOVERING; + s->heatcontrol.valleyRpm = s->heatcontrol.avgFricRpm; /* 记录进入此时的转速为谷底初值 */ + break; + + case SHOOT_HEAT_DETECT_RECOVERING: + + s->heatcontrol.avgFricRpm = Shoot_CalcAvgFricRpm(s); + + if (s->heatcontrol.avgFricRpm < s->heatcontrol.valleyRpm) { + s->heatcontrol.valleyRpm = s->heatcontrol.avgFricRpm; } - /* 限制估计热量不超过最大值 */ - if (s->heatcontrol.Hnow_estimated > s->heatcontrol.Hmax) { - s->heatcontrol.Hnow_estimated = s->heatcontrol.Hmax; + if ((s->heatcontrol.avgFricRpm - s->heatcontrol.valleyRpm > 30.0f) || + (s->target_variable.fric_rpm - s->heatcontrol.avgFricRpm < SHOT_DETECT_RPM_DROP_THRESHOLD * 0.8f)) { + + s->heatcontrol.detectState = SHOOT_HEAT_DETECT_READY; + + s->heatcontrol.baselineRpm = s->heatcontrol.avgFricRpm; } - /* 增加发射计数 */ - s->heatcontrol.shots_detected++; - - /* 返回准备检测状态 */ - s->heatcontrol.detectState = SHOOT_HEAT_DETECT_READY; + /* 如果摩擦轮停止 */ + if (s->running_state == SHOOT_STATE_IDLE) { + s->heatcontrol.detectState = SHOOT_HEAT_DETECT_IDLE; + } break; default: @@ -423,7 +401,7 @@ static int8_t Shoot_HeatDetectionFSM(Shoot_t *s) break; } - /* 善后工作：热量冷却（每个周期都执行） */ + /* 热量冷却*/ if (s->heatcontrol.Hnow_estimated > 0.0f && s->heatcontrol.Hcd > 0.0f) { s->heatcontrol.Hnow_estimated -= s->heatcontrol.Hcd * s->timer.dt; if (s->heatcontrol.Hnow_estimated < 0.0f) { @@ -431,6 +409,36 @@ static int8_t Shoot_HeatDetectionFSM(Shoot_t *s) } } + /* 空弹链检测与修正：拨盘触发后超过0.4秒内仍未有对应的摩擦轮掉速确认 */ + if (s->heatcontrol.unverified_shots > 0 && (s->timer.now - s->var_trig.time_lastShoot) > 0.4f) { + /* 这些弹丸确定未射出（空发） */ + float refund_heat = 0.0f; + switch (s->param->basic.projectileType) { + case SHOOT_PROJECTILE_17MM: refund_heat = 10.0f; break; + case SHOOT_PROJECTILE_42MM: refund_heat = 100.0f; break; + default: refund_heat = s->heatcontrol.Hgen; break; + } + refund_heat *= s->heatcontrol.unverified_shots; + + s->heatcontrol.Hnow_estimated -= refund_heat; + if (s->heatcontrol.Hnow_estimated < 0.0f) { + s->heatcontrol.Hnow_estimated = 0.0f; + } + + if (s->var_trig.num_shooted >= s->heatcontrol.unverified_shots) { + s->var_trig.num_shooted -= s->heatcontrol.unverified_shots; + } else { + s->var_trig.num_shooted = 0; + } + + if (s->heatcontrol.shots_detected >= s->heatcontrol.unverified_shots) { + s->heatcontrol.shots_detected -= s->heatcontrol.unverified_shots; + } else { + s->heatcontrol.shots_detected = 0; + } + + s->heatcontrol.unverified_shots = 0; + } /* 数据融合 */ Shoot_FuseHeatData(s); @@ -500,18 +508,22 @@ static float Shoot_CaluFreqByHeat(Shoot_t *s) */ int8_t Shoot_CaluTargetAngle(Shoot_t *s, Shoot_CMD_t *cmd) { - if (s == NULL || s->var_trig.num_toShoot == 0) { + if (s == NULL) { return SHOOT_ERR_NULL; } /* 更新热量控制数据 */ Shoot_UpdateHeatControl(s); + if (s->var_trig.num_toShoot == 0) { + return SHOOT_OK; /* 即使没有弹丸待发，热量更新照常执行 */ + } + /* 根据热量控制计算实际射频 */ float actual_freq = Shoot_CaluFreqByHeat(s); float dt = s->timer.now - s->var_trig.time_lastShoot; - float dpos; + float dpos; dpos = CircleError(s->target_variable.trig_angle, s->var_trig.trig_agl, M_2PI); /* 使用热量控制计算出的射频，而不是配置的固定射频 */ @@ -519,7 +531,24 @@ int8_t Shoot_CaluTargetAngle(Shoot_t *s, Shoot_CMD_t *cmd) { s->var_trig.time_lastShoot=s->timer.now; CircleAdd(&s->target_variable.trig_angle, M_2PI/s->param->basic.num_trig_tooth, M_2PI); - s->var_trig.num_toShoot--; + s->var_trig.num_toShoot--; + s->var_trig.num_shooted++; + + /* 预测性发射：拨盘拨动即认为弹丸会射出，立即增加热量与发射计数 */ + float add_heat = 0.0f; + switch (s->param->basic.projectileType) { + case SHOOT_PROJECTILE_17MM: add_heat = 10.0f; break; + case SHOOT_PROJECTILE_42MM: add_heat = 100.0f; break; + default: add_heat = s->heatcontrol.Hgen; break; + } + s->heatcontrol.Hnow_estimated += add_heat; + if (s->heatcontrol.Hnow_estimated > s->heatcontrol.Hmax) { + s->heatcontrol.Hnow_estimated = s->heatcontrol.Hmax; + } + s->heatcontrol.shots_detected++; + + /* 增加等待摩擦轮掉速确认的弹丸数 */ + s->heatcontrol.unverified_shots++; } return SHOOT_OK; } @@ -623,16 +652,17 @@ int8_t Shoot_RunningFSM(Shoot_t *s, Shoot_CMD_t *cmd) return SHOOT_ERR_NULL; // 参数错误 } uint8_t fric_num = s->param->basic.fric_num; - static float pos; + if(s->mode==SHOOT_MODE_SAFE){ for(int i=0;iparam->motor.fric[i].param); } - MOTOR_RM_Relax(&s->param->motor.trig);\ - pos=s->target_variable.trig_angle=s->var_trig.trig_agl; + MOTOR_RM_Relax(&s->param->motor.trig); + s->target_variable.trig_angle=s->var_trig.trig_agl; } else{ + Shoot_CaluTargetAngle(s, cmd); switch(s->running_state) { case SHOOT_STATE_IDLE:/*熄火等待*/ @@ -645,7 +675,7 @@ int8_t Shoot_RunningFSM(Shoot_t *s, Shoot_CMD_t *cmd) MOTOR_RM_SetOutput(&s->param->motor.fric[i].param, s->output.lpfout_fric[i]); } - s->output.outagl_trig =PID_Calc(&s->pid.trig,pos,s->var_trig.trig_agl,0,s->timer.dt); + s->output.outagl_trig =PID_Calc(&s->pid.trig,s->target_variable.trig_angle,s->var_trig.trig_agl,0,s->timer.dt); s->output.outomg_trig =PID_Calc(&s->pid.trig_omg,s->output.outagl_trig,s->var_trig.trig_rpm,0,s->timer.dt); s->output.outlpf_trig =LowPassFilter2p_Apply(&s->filter.trig.out, s->output.outomg_trig); MOTOR_RM_SetOutput(&s->param->motor.trig, s->output.outlpf_trig); @@ -689,7 +719,7 @@ int8_t Shoot_RunningFSM(Shoot_t *s, Shoot_CMD_t *cmd) } /* 设置拨弹电机输出 */ s->output.outagl_trig =PID_Calc(&s->pid.trig, - pos, + s->target_variable.trig_angle, s->var_trig.trig_agl, 0, s->timer.dt); @@ -731,7 +761,6 @@ int8_t Shoot_RunningFSM(Shoot_t *s, Shoot_CMD_t *cmd) break; case SHOOT_STATE_FIRE:/*射击*/ - Shoot_CaluTargetAngle(s, cmd); for(int i=0;iparam->motor.fric[i].level-1; @@ -776,7 +805,7 @@ int8_t Shoot_RunningFSM(Shoot_t *s, Shoot_CMD_t *cmd) if(!cmd->firecmd) { s->running_state=SHOOT_STATE_READY; - pos=s->var_trig.trig_agl; + s->target_variable.trig_angle=s->var_trig.trig_agl; s->var_trig.num_toShoot=0; } break; diff --git a/User/module/shoot.h b/User/module/shoot.h index 59e42e4..cd50342 100644 --- a/User/module/shoot.h +++ b/User/module/shoot.h @@ -35,7 +35,8 @@ typedef enum { SHOOT_HEAT_DETECT_IDLE = 0, /* 停机状态 */ SHOOT_HEAT_DETECT_READY, /* 准备检测状态 */ SHOOT_HEAT_DETECT_SUSPECTED, /* 发射嫌疑状态 */ - SHOOT_HEAT_DETECT_CONFIRMED /* 确认发射状态 */ + SHOOT_HEAT_DETECT_CONFIRMED, /* 确认发射状态 */ + SHOOT_HEAT_DETECT_RECOVERING /* 发射恢复状态 */ }Shoot_HeatDetectionFSM_State_t; typedef enum { SHOOT_STATE_IDLE = 0,/* 熄火 */ @@ -114,7 +115,8 @@ typedef struct { Shoot_HeatDetectionFSM_State_t detectState; // 检测状态 float detectTime; // 检测计时器 float avgFricRpm; // 摩擦轮平均转速 - float lastAvgFricRpm; // 上次摩擦轮平均转速 + float baselineRpm; // 动态基准转速，用于连发掉速检测的参照 + float valleyRpm; // 掉速谷底转速，用于判断掉速回升 float rpmDrop; // 转速下降量 bool shotDetected; // 检测到发射标志 @@ -123,6 +125,7 @@ typedef struct { float Hnow_fused; // 融合后的热量值 uint16_t shots_detected; // 检测到的发射数 uint16_t shots_available;// 当前热量可发射弹数 + uint16_t unverified_shots; // 已经拨弹但等待摩擦轮掉速确认的弹丸数 }Shoot_HeatControl_t; typedef struct { From a3b4381f7e126b9334e8d57ba49b2612a9ca5244 Mon Sep 17 00:00:00 2001 From: Robofish <1683502971@qq.com> Date: Sat, 14 Mar 2026 16:49:35 +0800 Subject: [PATCH 04/34] =?UTF-8?q?=E4=BF=AE=E5=A4=8D=E8=A3=81=E5=88=A4?= =?UTF-8?q?=E7=B3=BB=E7=BB=9F=E6=BA=90=E7=A0=81=E7=9A=84=E8=B7=A8=E5=B9=B3?= =?UTF-8?q?=E5=8F=B0=20include=20=E8=B7=AF=E5=BE=84?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- User/device/referee.c | 14 +++++++------- User/device/referee.h | 16 ++++++++-------- 2 files changed, 15 insertions(+), 15 deletions(-) diff --git a/User/device/referee.c b/User/device/referee.c index fb3b8b8..43e41a0 100644 --- a/User/device/referee.c +++ b/User/device/referee.c @@ -6,13 +6,13 @@ #include "device.h" #include -//#include "bsp\delay.h" -#include "bsp\uart.h" -#include "component\crc16.h" -#include "component\crc8.h" -#include "component\user_math.h" -#include "device\referee.h" -// #include "module\cmd\cmd.h" +//#include "bsp/delay.h" +#include "bsp/uart.h" +#include "component/crc16.h" +#include "component/crc8.h" +#include "component/user_math.h" +#include "device/referee.h" +// #include "module/cmd/cmd.h" /* Private define ----------------------------------------------------------- */ #define REF_HEADER_SOF (0xA5) diff --git a/User/device/referee.h b/User/device/referee.h index a771bb5..0e484f6 100644 --- a/User/device/referee.h +++ b/User/device/referee.h @@ -11,14 +11,14 @@ extern "C" { /* Includes ----------------------------------------------------------------- */ #include #include -#include "component\ui.h" -#include "component\user_math.h" -#include "device\device.h" -#include "device\referee_proto_types.h" -#include "device\supercap.h" -#include "module\balance_chassis.h" -#include "module\gimbal.h" -#include "module\shoot.h" +#include "component/ui.h" +#include "component/user_math.h" +#include "device/device.h" +#include "device/referee_proto_types.h" +#include "device/supercap.h" +#include "module/balance_chassis.h" +#include "module/gimbal.h" +#include "module/shoot.h" /* Exported constants ------------------------------------------------------- */ /* Exported macro ----------------------------------------------------------- */ From 511b9809c9d5f50404ed167719659a9bf17cf6a5 Mon Sep 17 00:00:00 2001 From: Robofish <1683502971@qq.com> Date: Sat, 14 Mar 2026 17:24:48 +0800 Subject: [PATCH 05/34] fix dr16 --- User/device/dr16.c | 53 ++++++++++++++++++++++++++++++++++++++++++++-- 1 file changed, 51 insertions(+), 2 deletions(-) diff --git a/User/device/dr16.c b/User/device/dr16.c index 120997b..650f748 100644 --- a/User/device/dr16.c +++ b/User/device/dr16.c @@ -41,12 +41,26 @@ static osThreadId_t thread_alert; static bool inited = false; +static DR16_t *dr16_instance = NULL; /* 用于空闲中断回调中访问实例 */ +static uint8_t sync_buf[32]; /* 帧同步时的丢弃缓冲区 */ /* Private function -------------------------------------------------------- */ static void DR16_RxCpltCallback(void) { osThreadFlagsSet(thread_alert, SIGNAL_DR16_RAW_REDY); } +/** + * @brief 空闲中断回调 - 用于帧同步 + * 空闲中断表示一帧传输结束（总线空闲），此时停止当前DMA接收， + * 丢弃不完整的数据，这样下一次 StartDmaRecv 就能从帧头开始。 + */ +static void DR16_IdleCallback(void) { + /* 停止当前DMA接收（无论收了多少字节） */ + HAL_UART_AbortReceive(BSP_UART_GetHandle(BSP_UART_DR16)); + /* 通知任务：可以启动下一次对齐的接收了 */ + osThreadFlagsSet(thread_alert, SIGNAL_DR16_RAW_REDY); +} + static bool DR16_DataCorrupted(const DR16_t *dr16) { if (dr16 == NULL) return DEVICE_ERR_NULL; @@ -79,16 +93,49 @@ int8_t DR16_Init(DR16_t *dr16) { if (inited) return DEVICE_ERR_INITED; if ((thread_alert = osThreadGetId()) == NULL) return DEVICE_ERR_NULL; + dr16_instance = dr16; + + /* 注册 DMA 接收完成回调 */ BSP_UART_RegisterCallback(BSP_UART_DR16, BSP_UART_RX_CPLT_CB, DR16_RxCpltCallback); + /* 注册空闲中断回调并使能空闲中断，用于帧同步 */ + BSP_UART_RegisterCallback(BSP_UART_DR16, BSP_UART_IDLE_LINE_CB, + DR16_IdleCallback); + __HAL_UART_ENABLE_IT(BSP_UART_GetHandle(BSP_UART_DR16), UART_IT_IDLE); + + /* + * 首次帧同步：启动一次丢弃式DMA接收。 + * 如果遥控器已经在发送，DMA会从帧中间开始收，空闲中断到来时 + * IdleCallback 会 Abort 这次接收并通知任务，下一次 StartDmaRecv + * 就能从完整帧头开始。 + */ + HAL_UART_Receive_DMA(BSP_UART_GetHandle(BSP_UART_DR16), + sync_buf, sizeof(sync_buf)); + /* 等待空闲中断完成首次同步（最多50ms，足够等一帧） */ + osThreadFlagsWait(SIGNAL_DR16_RAW_REDY, osFlagsWaitAll, 50); + inited = true; return DEVICE_OK; } int8_t DR16_Restart(void) { - __HAL_UART_DISABLE(BSP_UART_GetHandle(BSP_UART_DR16)); - __HAL_UART_ENABLE(BSP_UART_GetHandle(BSP_UART_DR16)); + UART_HandleTypeDef *huart = BSP_UART_GetHandle(BSP_UART_DR16); + + /* 先终止当前DMA接收 */ + HAL_UART_AbortReceive(huart); + + /* 重置串口 */ + __HAL_UART_DISABLE(huart); + __HAL_UART_ENABLE(huart); + + /* 重新使能空闲中断 */ + __HAL_UART_ENABLE_IT(huart, UART_IT_IDLE); + + /* 重新做帧同步：丢弃式接收，等空闲中断对齐 */ + HAL_UART_Receive_DMA(huart, sync_buf, sizeof(sync_buf)); + osThreadFlagsWait(SIGNAL_DR16_RAW_REDY, osFlagsWaitAll, 50); + return DEVICE_OK; } @@ -109,6 +156,8 @@ int8_t DR16_ParseData(DR16_t *dr16){ if (dr16 == NULL) return DEVICE_ERR_NULL; if (DR16_DataCorrupted(dr16)) { + /* 数据损坏说明帧错位了，重启串口并重新同步 */ + DR16_Restart(); return DEVICE_ERR; } From bc9801a60a49335f67805f8f08411282ef169c41 Mon Sep 17 00:00:00 2001 From: Robofish <1683502971@qq.com> Date: Sun, 15 Mar 2026 09:55:19 +0800 Subject: [PATCH 06/34] =?UTF-8?q?=E6=8B=BF=E5=87=BAvmc?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- User/module/balance_chassis.c | 148 +++++----------------------------- User/module/balance_chassis.h | 10 +++ 2 files changed, 30 insertions(+), 128 deletions(-) diff --git a/User/module/balance_chassis.c b/User/module/balance_chassis.c index 60d1fb6..90d6289 100644 --- a/User/module/balance_chassis.c +++ b/User/module/balance_chassis.c @@ -411,70 +411,6 @@ static void Chassis_JumpControl(Chassis_t *c, const Chassis_CMD_t *c_cmd, float } } -/** - * @brief 倒地自起控制 - * @note 自起流程（左右腿独立状态机）： - * STRETCH(伸腿) -> BACKLEG(后甩腿) -> COMPLETE(完成) - * 两腿均 COMPLETE 后自动切换到 WHELL_LEG_BALANCE 模式 - * - * @param c 底盘结构体指针 - */ -static void Chassis_RecoverControl(Chassis_t *c) { - float theta_l = c->vmc_[0].leg.theta; - float theta_r = c->vmc_[1].leg.theta; - - /* ===== 左腿状态机 ===== */ - switch (c->recover.state[0]) { - case RECOVER_FLIP: - /* 翻身阶段：theta朝上（|theta| > π/2），收腿慢慢翻转 */ - /* 等待 theta 转到腿朝下（|theta| < π/2） */ - if (fabsf(theta_l) < (float)M_PI_2) { - c->recover.state[0] = RECOVER_STRETCH; - } - break; - - case RECOVER_STRETCH: - /* 伸腿：等待腿伸到位 */ - if (c->vmc_[0].leg.L0 > 0.28f) { - c->recover.state[0] = RECOVER_BACKLEG; - } - break; - - case RECOVER_BACKLEG: - /* 后甩腿：等待 phi0 趋近目标角度 */ - if (fabsf(c->vmc_[0].leg.phi0 - 0.5f) < 0.1f) { - c->recover.state[0] = RECOVER_COMPLETE; - } - break; - - case RECOVER_COMPLETE: - break; - } - - /* ===== 右腿状态机 ===== */ - switch (c->recover.state[1]) { - case RECOVER_FLIP: - if (fabsf(theta_r) < (float)M_PI_2) { - c->recover.state[1] = RECOVER_STRETCH; - } - break; - - case RECOVER_STRETCH: - if (c->vmc_[1].leg.L0 > 0.28f) { - c->recover.state[1] = RECOVER_BACKLEG; - } - break; - - case RECOVER_BACKLEG: - if (fabsf(c->vmc_[1].leg.phi0 - 0.5f) < 0.1f) { - c->recover.state[1] = RECOVER_COMPLETE; - } - break; - - case RECOVER_COMPLETE: - break; - } -} /** * @brief 初始化底盘 @@ -716,81 +652,23 @@ int8_t Chassis_Control(Chassis_t *c, const Chassis_CMD_t *c_cmd) { case CHASSIS_MODE_RELAX: Chassis_MotorRelax(c); Chassis_LQRControl(c, c_cmd); + Chassis_VMCControl(c, c_cmd); break; - case CHASSIS_MODE_RECOVER: { - /* 运行自起状态机 */ - Chassis_RecoverControl(c); - - /* 根据状态决定输出力 */ - float fn_left, fn_right; - float spring_left = Chassis_CalcSpringForce(c->vmc_[0].leg.L0); - float spring_right = Chassis_CalcSpringForce(c->vmc_[1].leg.L0); - if (isnan(spring_left)) spring_left = 0.0f; - if (isnan(spring_right)) spring_right = 0.0f; - - /* 左腿 */ - if (c->recover.state[0] == RECOVER_FLIP) { - fn_left = 30.0f - spring_left; /* 收腿，借重力慢慢翻转 */ - } else if (c->recover.state[0] == RECOVER_STRETCH) { - fn_left = -80.0f - spring_left; /* 大力伸腿 */ - } else if (c->recover.state[0] == RECOVER_BACKLEG) { - fn_left = -40.0f - spring_left; /* 维持伸腿 */ - } else { - fn_left = -20.0f - spring_left; - } - - /* 右腿 */ - if (c->recover.state[1] == RECOVER_FLIP) { - fn_right = 30.0f - spring_right; - } else if (c->recover.state[1] == RECOVER_STRETCH) { - fn_right = -80.0f - spring_right; - } else if (c->recover.state[1] == RECOVER_BACKLEG) { - fn_right = -40.0f - spring_right; - } else { - fn_right = -20.0f - spring_right; - } - - VMC_InverseSolve(&c->vmc_[0], fn_left, 0.0f); - VMC_GetJointTorques(&c->vmc_[0], &c->output.joint[0], &c->output.joint[1]); - VMC_InverseSolve(&c->vmc_[1], fn_right, 0.0f); - VMC_GetJointTorques(&c->vmc_[1], &c->output.joint[3], &c->output.joint[2]); - c->output.wheel[0] = 0.0f; - c->output.wheel[1] = 0.0f; - Chassis_Output(c); - - /* 两腿均完成，自动切换到平衡模式 */ - if (c->recover.state[0] == RECOVER_COMPLETE && - c->recover.state[1] == RECOVER_COMPLETE) { - Chassis_ResetControllers(c); - Chassis_SelectYawZeroPoint(c); - c->mode = CHASSIS_MODE_WHELL_LEG_BALANCE; - } - } break; - - // case CHASSIS_MODE_CALIBRATE: { - // Chassis_LQRControl(c, c_cmd); - // LF= Chassis_CalcSpringForce(c->vmc_[0].leg.L0); - // RF= Chassis_CalcSpringForce(c->vmc_[1].leg.L0); - // L_fn = -LF; - // VMC_InverseSolve(&c->vmc_[0], L_fn, L_tp); - // VMC_GetJointTorques(&c->vmc_[0], &c->output.joint[0], &c->output.joint[1]); - // VMC_InverseSolve(&c->vmc_[1], R_fn, R_tp); - // VMC_GetJointTorques(&c->vmc_[1], &c->output.joint[3], &c->output.joint[2]); - - // c->output.wheel[0] = 0.0f; - // c->output.wheel[1] = 0.0f; - // Chassis_Output(c); - // } break; + // case CHASSIS_MODE_RECOVER: + // Chassis_RecoverControl(c); + // break; case CHASSIS_MODE_WHELL_LEG_BALANCE: Chassis_LQRControl(c, c_cmd); + Chassis_VMCControl(c, c_cmd); Chassis_Output(c); break; case CHASSIS_MODE_BALANCE_ROTOR: Chassis_LQRControl(c, c_cmd); c->output.wheel[0] += c_cmd->move_vec.vy * 0.2f; c->output.wheel[1] -= c_cmd->move_vec.vy * 0.2f; + Chassis_VMCControl(c, c_cmd); Chassis_Output(c); break; @@ -952,6 +830,20 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) { c->output.wheel[0] = c->lqr[0].control_output.T / c->param->mech.wheel_gear_ratio + scaled_yaw_force; c->output.wheel[1] = c->lqr[1].control_output.T / c->param->mech.wheel_gear_ratio - scaled_yaw_force; + return CHASSIS_OK; +} + +/** + * @brief VMC控制（虚拟模型控制） + * @param c 底盘结构体指针 + * @param c_cmd 控制指令 + * @return 操作结果 + * @note 将LQR输出的力矩通过VMC逆解算转换为关节力矩， + * 同时处理横滚补偿、腿长控制、跳跃控制和摆角同步 + */ +int8_t Chassis_VMCControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) { + if (c == NULL || c_cmd == NULL) return CHASSIS_ERR_NULL; + /* ==================== 横滚角补偿 ==================== */ /* 方法1: 几何前馈腿长补偿 (参考底盘文件夹算法) */ float Rl = c->param->mech.hip_width / 2.0f; /* 髋宽的一半 */ diff --git a/User/module/balance_chassis.h b/User/module/balance_chassis.h index 363a487..e0c498a 100644 --- a/User/module/balance_chassis.h +++ b/User/module/balance_chassis.h @@ -291,6 +291,16 @@ int8_t Chassis_Control(Chassis_t *c, const Chassis_CMD_t *c_cmd); */ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd); +/** + * \brief VMC控制（虚拟模型控制） + * + * \param c 包含底盘数据的结构体 + * \param c_cmd 底盘控制指令 + * + * \return 函数运行结果 + */ +int8_t Chassis_VMCControl(Chassis_t *c, const Chassis_CMD_t *c_cmd); + /** * \brief 底盘输出值 * From 0ab60f81ddf52128e7a7bd7e13d2fee52705583a Mon Sep 17 00:00:00 2001 From: Robofish <1683502971@qq.com> Date: Sun, 15 Mar 2026 10:00:03 +0800 Subject: [PATCH 07/34] =?UTF-8?q?=E6=B5=8B=E4=BD=8D=E7=A7=BB?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- User/module/balance_chassis.c | 6 +++--- utils/Simulation-master/balance/series_legs/get_k_length.m | 4 ++-- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/User/module/balance_chassis.c b/User/module/balance_chassis.c index 90d6289..8d08335 100644 --- a/User/module/balance_chassis.c +++ b/User/module/balance_chassis.c @@ -762,11 +762,11 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) { /* ==================== 目标状态 ==================== */ LQR_State_t target_state = { - .theta = 0.0f, + .theta = 0.0f+c->param->lqr_offset.theta, .d_theta = 0.0f, - .phi = 0.0f, + .phi = 0.0f+c->param->lqr_offset.phi, .d_phi = 0.0f, - .x = c->chassis_state.target_x, + .x = c->chassis_state.target_x+c->param->lqr_offset.x, .d_x = c->chassis_state.target_velocity_x, }; diff --git a/utils/Simulation-master/balance/series_legs/get_k_length.m b/utils/Simulation-master/balance/series_legs/get_k_length.m index 03d19c3..d46203b 100644 --- a/utils/Simulation-master/balance/series_legs/get_k_length.m +++ b/utils/Simulation-master/balance/series_legs/get_k_length.m @@ -48,8 +48,8 @@ function K = get_k_length(leg_length) B=subs(B,[R,L,LM,l,mw,mp,M,Iw,Ip,IM,g],[R1,L1,LM1,l1,mw1,mp1,M1,Iw1,Ip1,IM1,9.8]); B=double(B); - Q=diag([4000 200 500 50 50000 10]);%theta d_theta x d_x phi d_phi%700 1 600 200 1000 1 - R=[150 0;0 5]; %T Tp + Q=diag([4000 200 1000 500 50000 10]);%theta d_theta x d_x phi d_phi%700 1 600 200 1000 1 + R=[150 0;0 25]; %T Tp K=lqr(A,B,Q,R); From 5ad9ef8f4975023f186dd9d59c5ad88a9c74539c Mon Sep 17 00:00:00 2001 From: Robofish <1683502971@qq.com> Date: Sun, 15 Mar 2026 10:23:54 +0800 Subject: [PATCH 08/34] add offline --- User/module/balance_chassis.c | 32 +++++++++++++++++++++++++++++++- User/module/balance_chassis.h | 5 +++++ 2 files changed, 36 insertions(+), 1 deletion(-) diff --git a/User/module/balance_chassis.c b/User/module/balance_chassis.c index 8d08335..c1b7c55 100644 --- a/User/module/balance_chassis.c +++ b/User/module/balance_chassis.c @@ -31,7 +31,6 @@ static void Chassis_UpdateChassisState(Chassis_t *c); static void Chassis_ResetControllers(Chassis_t *c); static void Chassis_SelectYawZeroPoint(Chassis_t *c); static void Chassis_JumpControl(Chassis_t *c, const Chassis_CMD_t *c_cmd, float *target_L0, float *extra_force); -static void Chassis_RecoverControl(Chassis_t *c); /* Private functions -------------------------------------------------------- */ @@ -543,6 +542,9 @@ int8_t Chassis_Init(Chassis_t *c, Chassis_Params_t *param, float target_freq) { c->jump.launch_force = c->param->jump.launch_force; c->jump.retract_leg_length = c->param->jump.retract_leg_length; + /* 初始化电机离线检测 */ + c->motor_status.any_offline = false; + return CHASSIS_OK; } @@ -559,10 +561,19 @@ int8_t Chassis_UpdateFeedback(Chassis_t *c) { MOTOR_LK_UpdateAll(); /* 更新关节电机反馈 */ + uint64_t now_us = BSP_TIME_Get_us(); + bool any_offline = false; + for (int i = 0; i < 4; i++) { MOTOR_LZ_t *joint_motor = MOTOR_LZ_GetMotor(&c->param->joint_param[i]); if (joint_motor != NULL) { c->feedback.joint[i] = joint_motor->motor.feedback; + /* 检测关节电机离线：500ms 无更新 */ + if (now_us - joint_motor->motor.header.last_online_time > 500000u) { + any_offline = true; + } + } else { + any_offline = true; } /* 机械零点调整 */ @@ -580,9 +591,17 @@ int8_t Chassis_UpdateFeedback(Chassis_t *c) { MOTOR_LK_t *wheel_motor = MOTOR_LK_GetMotor(&c->param->wheel_param[i]); if (wheel_motor != NULL) { c->feedback.wheel[i] = wheel_motor->motor.feedback; + /* 检测轮子电机离线：500ms 无更新 */ + if (now_us - wheel_motor->motor.header.last_online_time > 500000u) { + any_offline = true; + } + } else { + any_offline = true; } } + c->motor_status.any_offline = any_offline; + /* 更新机体状态估计 */ Chassis_UpdateChassisState(c); @@ -615,6 +634,17 @@ int8_t Chassis_Control(Chassis_t *c, const Chassis_CMD_t *c_cmd) { c->dt = (BSP_TIME_Get_us() - c->lask_wakeup) / 1000000.0f; c->lask_wakeup = BSP_TIME_Get_us(); + /* 电机离线保护：有电机离线时强制 RELAX 并持续使能 */ + if (c->motor_status.any_offline) { + if (c->mode != CHASSIS_MODE_RELAX) { + c->mode = CHASSIS_MODE_RELAX; + Chassis_ResetControllers(c); + } + Chassis_MotorRelax(c); + Chassis_MotorEnable(c); + return CHASSIS_OK; + } + /* 设置底盘模式 */ if (Chassis_SetMode(c, c_cmd->mode) != CHASSIS_OK) { return CHASSIS_ERR_MODE; diff --git a/User/module/balance_chassis.h b/User/module/balance_chassis.h index e0c498a..5fd3f0e 100644 --- a/User/module/balance_chassis.h +++ b/User/module/balance_chassis.h @@ -215,6 +215,11 @@ typedef struct { Recover_State_t state[2]; /* 左右腿自起状态 */ } recover; + /* 电机离线检测 */ + struct { + bool any_offline; /* 是否有电机离线 */ + } motor_status; + /* PID计算的目标值 */ struct { AHRS_Eulr_t chassis; From 9992ffba2dfaacae027048b81b985b50967c2347 Mon Sep 17 00:00:00 2001 From: Robofish <1683502971@qq.com> Date: Sun, 15 Mar 2026 11:26:01 +0800 Subject: [PATCH 09/34] fix rotor --- .DS_Store | Bin 8196 -> 12292 bytes User/module/balance_chassis.c | 97 +++++++++++++++++++++++++++------- User/module/balance_chassis.h | 17 ++++++ User/module/config.c | 19 +++++++ 4 files changed, 115 insertions(+), 18 deletions(-) diff --git a/.DS_Store b/.DS_Store index 1fed1b622b6f443fc2ff7a4544e9e83ebacb753a..49bb25a6296f3160217e20210e3afbfdf44bc507 100644 GIT binary patch literal 12292 zcmeHNU5pb|6h61RxGf92-9P?GDr+Kb!N{dhgEA zh&~=NBF1R8FOu2L?}l_e87*8#qFYsEeAwT*M%Rppswo*kpvD!T?07=e;`(@>KB~pD zf;MD!czs^quzyA_w|M!ojzCXW_u-B}Zu#P_jzI6y-ouA|-nRBdk8ByvYJ%eVC@JZY?odomrOjt}KU~&MNYQWXNy`}+c=kPlA`+H3d zuDq@WTn$vv0QV0892^EMm^Yzt9XRn2n)mY%S~OgXI1@0y9{>g|m^UGwpfQsa&?IG6 zior~BJP%lSK?~+hXp$2XGh-ZUW}dk4#>DJ^0our3tSVm7K2lcFKsJgaohnU?^QUlr^9W!zcHm7-4cB_)f0+!K)w?#6U zh?Y?L4(?1wqDpeJ)~6eWo(ijwf^#mW45cG6RZFz5x@xM`*B<1`MDtU-d4ak~YvIa* zto@6`NpU@->6!wfL9?elxWPj4(gIpdA=*LvDNiTp9r}R2q>J<`{Xy5*R5p#xV+&XZ zTf&yHAY0A0uWy~py9G}ju>0xXLQ$E!{zN(F+(DT&X^F;aSezf}nFoggt1Dr}{{@y@j%Zvd9JPa=u~0@!>RJLba4G6&6Us6hXd(2mg*FW&^_ayAC+-8% zQ`bbpT^V+Ce1Z~=_ZF6Ld}ax!yJsm%IK5prRl+Hg>PC!n*6dl8jZW22vb=` zEK$0SaoU!3I1PmyNUTqGX)5aZ-B8IICmH#z~_A&aT7!9L8A>EvOX7FNvD( zI7LuDRVZsWQg#gEye%1N6)Kyov-WdWRs~a*d9eRj!68+}F3EmP$<5`P0 z-VbTlqIR01xLraz>u^JDltp!wYr~*XO4G2oPf-sorV(0(a}OQBud_^7OCWWpq^nkJ z0z;UWQ?9ot!?X?~K1v(GQwUExp?wEOo;JrgUV0q3t3g>&0yfVfK`rpJypC^|lpO({ zPvWnLC%M{B3K7F?tCr8qRktXYedKg(R*cxW&0pp1%kBPjVsgf*r;vxVe{3 ziy&+rqKFv$PlPml8+Jq!BF*|;vhWi2C}Kaw?hgcZLv?JM-y*ijHoF*4_FZk4yZ=67 zdM3xLvj44)W%FTXVX^rA+rc-F;$$g{crwSer`kKfaj{S4HNWay=1gIiu)eX&T(*1~ zPCNT${%S9M%hw&3)BHX^f_))s=aRkb*byAqGOzec#z6UaGRJd&9Dlmgu~4MUdVQttKdYED^Y90yiOg6ddy?gO0rNfgCu(l(|wRdU=&r#*jI!pPt7A;53HS7D4vzT$k^nx6{UH5mV-rXwd`qab!+E zo;zAtA<|4TrT^NSg(hdI4k1WbmaN-Vc-?VkTtY7yyiFST}u6>byun1rk$Fr z7@AtZp8?7Gz$MSM<0umH9C;Xd?neANp)s9eb}5A&HAqrA%8zzz`Wjj-Vc%u>KXf>W zD-!?xpEz`yb1XToJNoW3Q|Bp3h1}`0~QQgFmFPW{6_u+AnnR_Z_nKQ Zzv8}b*OI&cmqoZdbCf?r-XZn>e*i<+)jt3L delta 253 zcmZokXmOBWU|?W$DortDU;r^WfEYvza8E20o2Vx_*+7Ry637P&G6KaJ81fj>8B#oR z@{^NtHVZO+W8JL4w3cx*I|mB~qr_w$1;fd|6*MMCD$SZaSy^v#wF>v-7!}3Ie^oRl zFIClMV>k*lYu@B*s-=?;s})VYA|$wZyU=^4$!21%o3+Kc8Tpu)fbOaL4+abjle?sK zCyOyX7Gwsw0q6>DAmIvf9?pid.leg_length[0]); PID_Reset(&c->pid.leg_length[1]); PID_Reset(&c->pid.yaw); + PID_Reset(&c->pid.rotor); PID_Reset(&c->pid.roll); PID_Reset(&c->pid.roll_force); PID_Reset(&c->pid.tp[0]); @@ -143,6 +144,13 @@ static int8_t Chassis_SetMode(Chassis_t *c, Chassis_Mode_t mode) { return CHASSIS_OK; } + /* ROTOR -> BALANCE：启动退出过渡（减速→对齐），不立即切换 */ + if (c->mode == CHASSIS_MODE_BALANCE_ROTOR && + mode == CHASSIS_MODE_WHELL_LEG_BALANCE) { + c->rotor_state.exiting = true; + return CHASSIS_OK; /* 保持在 ROTOR 模式，由控制循环处理退出 */ + } + Chassis_MotorEnable(c); Chassis_ResetControllers(c); Chassis_SelectYawZeroPoint(c); @@ -171,6 +179,12 @@ static int8_t Chassis_SetMode(Chassis_t *c, Chassis_Mode_t mode) { c->recover.state[1] = (fabsf(theta_r) > (float)M_PI_2) ? RECOVER_FLIP : RECOVER_STRETCH; } + /* 进入小陀螺时重置旋转状态 */ + if (mode == CHASSIS_MODE_BALANCE_ROTOR) { + c->rotor_state.current_spin_speed = 0.0f; + c->rotor_state.exiting = false; + } + c->mode = mode; return CHASSIS_OK; } @@ -454,6 +468,7 @@ int8_t Chassis_Init(Chassis_t *c, Chassis_Params_t *param, float target_freq) { PID_Init(&c->pid.leg_length[0], KPID_MODE_CALC_D, target_freq, ¶m->leg_length); PID_Init(&c->pid.leg_length[1], KPID_MODE_CALC_D, target_freq, ¶m->leg_length); PID_Init(&c->pid.yaw, KPID_MODE_CALC_D, target_freq, ¶m->yaw); + PID_Init(&c->pid.rotor, KPID_MODE_CALC_D, target_freq, ¶m->rotor); PID_Init(&c->pid.roll, KPID_MODE_CALC_D, target_freq, ¶m->roll); PID_Init(&c->pid.roll_force, KPID_MODE_CALC_D, target_freq, ¶m->roll_force); PID_Init(&c->pid.tp[0], KPID_MODE_CALC_D, target_freq, ¶m->tp); @@ -545,6 +560,10 @@ int8_t Chassis_Init(Chassis_t *c, Chassis_Params_t *param, float target_freq) { /* 初始化电机离线检测 */ c->motor_status.any_offline = false; + /* 初始化小陀螺状态 */ + c->rotor_state.current_spin_speed = 0.0f; + c->rotor_state.exiting = false; + return CHASSIS_OK; } @@ -696,8 +715,6 @@ int8_t Chassis_Control(Chassis_t *c, const Chassis_CMD_t *c_cmd) { break; case CHASSIS_MODE_BALANCE_ROTOR: Chassis_LQRControl(c, c_cmd); - c->output.wheel[0] += c_cmd->move_vec.vy * 0.2f; - c->output.wheel[1] -= c_cmd->move_vec.vy * 0.2f; Chassis_VMCControl(c, c_cmd); Chassis_Output(c); break; @@ -753,21 +770,18 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) { if (c == NULL || c_cmd == NULL) return CHASSIS_ERR_NULL; /* ==================== 速度控制 ==================== */ - // float raw_vx = c_cmd->move_vec.vx * 2.0f; - // float desired_velocity = c->yaw_control.is_reversed ? -raw_vx : raw_vx; - - // /* 加速度限制 */ - // float velocity_change = desired_velocity - c->chassis_state.last_target_velocity_x; - // float max_velocity_change = c->param->motion.max_acceleration * c->dt; - // velocity_change = LIMIT(velocity_change, -max_velocity_change, max_velocity_change); - - // float target_dot_x = c->chassis_state.last_target_velocity_x + velocity_change; - // c->chassis_state.target_velocity_x = target_dot_x; - // c->chassis_state.last_target_velocity_x = target_dot_x; - // c->chassis_state.target_x += target_dot_x * c->dt; - c->chassis_state.target_velocity_x = c_cmd->move_vec.vx * 2.0f; - c->chassis_state.last_target_velocity_x = c->chassis_state.target_velocity_x; - c->chassis_state.target_x += c->chassis_state.target_velocity_x * c->dt; + if (c_cmd->mode == CHASSIS_MODE_BALANCE_ROTOR) { + /* 小陀螺平移：将世界坐标系期望速度投影到机体坐标系 */ + float yaw_angle = c->feedback.imu.euler.yaw; + float world_vx = c_cmd->move_vec.vx * c->param->rotor_ctrl.max_trans_speed; + c->chassis_state.target_velocity_x = world_vx * cosf(yaw_angle); + c->chassis_state.last_target_velocity_x = c->chassis_state.target_velocity_x; + c->chassis_state.target_x += c->chassis_state.target_velocity_x * c->dt; + } else { + c->chassis_state.target_velocity_x = c_cmd->move_vec.vx * 2.0f; + c->chassis_state.last_target_velocity_x = c->chassis_state.target_velocity_x; + c->chassis_state.target_x += c->chassis_state.target_velocity_x * c->dt; + } /* ==================== 状态更新 ==================== */ @@ -801,7 +815,54 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) { }; /* ==================== Yaw轴控制 ==================== */ - if (c_cmd->mode!= CHASSIS_MODE_BALANCE_ROTOR || c_cmd->move_vec.vy == 0.0f) { + if (c_cmd->mode == CHASSIS_MODE_BALANCE_ROTOR || c->mode == CHASSIS_MODE_BALANCE_ROTOR) { + /* 小陀螺模式：PID跟踪目标角速度，带速度斜坡 */ + float target_spin = c_cmd->move_vec.vy * c->param->rotor_ctrl.max_spin_speed; + + if (c->rotor_state.exiting) { + /* 退出过渡：目标角速度为0，用减速斜坡 */ + target_spin = 0.0f; + float decel_step = c->param->rotor_ctrl.spin_decel * c->dt; + if (c->rotor_state.current_spin_speed > decel_step) { + c->rotor_state.current_spin_speed -= decel_step; + } else if (c->rotor_state.current_spin_speed < -decel_step) { + c->rotor_state.current_spin_speed += decel_step; + } else { + /* 减速完成，检查yaw对齐 */ + c->rotor_state.current_spin_speed = 0.0f; + Chassis_SelectYawZeroPoint(c); + float yaw_error = CircleError(c->yaw_control.target_yaw, + c->feedback.yaw.rotor_abs_angle, M_2PI); + if (fabsf(yaw_error) < c->param->rotor_ctrl.align_threshold) { + /* 对齐完成，切换到平衡模式 */ + c->rotor_state.exiting = false; + c->mode = CHASSIS_MODE_WHELL_LEG_BALANCE; + Chassis_ResetControllers(c); + } + } + } else { + /* 正常旋转：平滑加速斜坡 */ + float accel_step = c->param->rotor_ctrl.spin_accel * c->dt; + if (c->rotor_state.current_spin_speed < target_spin - accel_step) { + c->rotor_state.current_spin_speed += accel_step; + } else if (c->rotor_state.current_spin_speed > target_spin + accel_step) { + c->rotor_state.current_spin_speed -= accel_step; + } else { + c->rotor_state.current_spin_speed = target_spin; + } + } + + /* 退出过渡减速完成后，用yaw PID对齐零点 */ + if (c->rotor_state.exiting && c->rotor_state.current_spin_speed == 0.0f) { + c->yaw_control.yaw_force = PID_Calc(&c->pid.yaw, c->yaw_control.target_yaw, + c->feedback.yaw.rotor_abs_angle, 0.0f, c->dt); + } else { + float current_yaw_omega = c->feedback.imu.gyro.z; + c->yaw_control.yaw_force = PID_Calc(&c->pid.rotor, c->rotor_state.current_spin_speed, + current_yaw_omega, 0.0f, c->dt); + } + c->yaw_control.is_reversed = false; + } else { c->yaw_control.current_yaw = c->feedback.yaw.rotor_abs_angle; float manual_offset = c_cmd->move_vec.vy * M_PI_2; diff --git a/User/module/balance_chassis.h b/User/module/balance_chassis.h index 5fd3f0e..51c7a0a 100644 --- a/User/module/balance_chassis.h +++ b/User/module/balance_chassis.h @@ -103,6 +103,7 @@ typedef struct { LQR_GainMatrix_t lqr_gains; /* LQR增益矩阵参数 */ KPID_Params_t yaw; /* yaw轴PID控制参数 */ + KPID_Params_t rotor; /* 小陀螺角速度PID */ KPID_Params_t roll; /* roll轴腿长补偿PID */ KPID_Params_t roll_force; /* roll轴力补偿PID */ KPID_Params_t tp; /* 摆力矩PID */ @@ -146,6 +147,15 @@ typedef struct { float non_contact_theta; /* 离地时摆角目标 (rad) */ } motion; + /* 小陀螺参数 */ + struct { + float max_spin_speed; /* 最大旋转角速度 (rad/s) */ + float max_trans_speed; /* 小陀螺时最大平移速度 (m/s) */ + float spin_accel; /* 旋转加速度 (rad/s²) */ + float spin_decel; /* 退出减速度 (rad/s²) */ + float align_threshold; /* 退出对齐阈值 (rad)，yaw误差小于此值时回到BALANCE模式 */ + } rotor_ctrl; + /* LQR 目标状态偏移 */ struct { float theta; @@ -200,6 +210,12 @@ typedef struct { bool is_reversed; /* 是否使用反转的零点（180度零点），影响前后方向 */ } yaw_control; + /* 小陀螺状态 */ + struct { + float current_spin_speed; /* 当前旋转速度 (rad/s)，含斜坡 */ + bool exiting; /* 正在退出小陀螺 */ + } rotor_state; + /* 跳跃控制相关 */ struct { Jump_State_t state; /* 跳跃状态 */ @@ -228,6 +244,7 @@ typedef struct { /* 反馈控制用的PID */ struct { KPID_t yaw; /* 跟随云台用的PID */ + KPID_t rotor; /* 小陀螺角速度PID */ KPID_t roll; /* 横滚角腿长补偿PID */ KPID_t roll_force; /* 横滚角力补偿PID */ KPID_t tp[2]; diff --git a/User/module/config.c b/User/module/config.c index 35e95c3..cddf13f 100644 --- a/User/module/config.c +++ b/User/module/config.c @@ -243,6 +243,17 @@ Config_RobotParam_t robot_config = { .range=M_2PI, /* 2*PI，用于处理角度循环误差 */ }, + .rotor={ + .k=1.0f, + .p=0.5f, + .i=0.0f, + .d=0.01f, + .i_limit=0.0f, + .out_limit=1.0f, + .d_cutoff_freq=30.0f, + .range=-1.0f, /* 角速度不需要循环误差处理 */ + }, + .roll={ .k=0.15f, .p=0.3f, /* 横滚角腿长补偿比例系数 (降低避免抖动) */ @@ -383,6 +394,14 @@ Config_RobotParam_t robot_config = { .non_contact_theta = -0.01f, /* 离地摆角目标 (rad) */ }, + .rotor_ctrl = { + .max_spin_speed = 0.05236f, /* 最大旋转角速度 (rad/s)，0.5rpm */ + .max_trans_speed = 1.5f, /* 小陀螺时最大平移速度 (m/s) */ + .spin_accel = 5.0f, /* 旋转加速度 (rad/s²) */ + .spin_decel = 8.0f, /* 退出减速度 (rad/s²)，比加速更快 */ + .align_threshold = 0.15f, /* 退出对齐阈值 (rad)，约8.6° */ + }, + .vmc_param = { { // 左腿 .leg_1 = 0.215f, // 后髋连杆长度 (L1) (m) From bef9394790cc57f55af2627723d0236b5cbe6389 Mon Sep 17 00:00:00 2001 From: Robofish <1683502971@qq.com> Date: Sun, 15 Mar 2026 13:32:15 +0800 Subject: [PATCH 10/34] =?UTF-8?q?addx=E8=A1=A5=E5=81=BF?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- User/module/balance_chassis.c | 10 ++++++++-- 1 file changed, 8 insertions(+), 2 deletions(-) diff --git a/User/module/balance_chassis.c b/User/module/balance_chassis.c index 13bc948..53766dc 100644 --- a/User/module/balance_chassis.c +++ b/User/module/balance_chassis.c @@ -805,12 +805,19 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) { LQR_CalculateGainMatrix(&c->lqr[1], c->vmc_[1].leg.L0); /* ==================== 目标状态 ==================== */ + /* 腿长-位移补偿：根据腿长多项式拟合补偿位移偏移 */ + float avg_L0 = (c->vmc_[0].leg.L0 + c->vmc_[1].leg.L0) * 0.5f; + float leg_x_comp = -45.406f * avg_L0 * avg_L0 * avg_L0 + + 7.06585f * avg_L0 * avg_L0 + + 5.98465f * avg_L0 + - 1.14975f; + LQR_State_t target_state = { .theta = 0.0f+c->param->lqr_offset.theta, .d_theta = 0.0f, .phi = 0.0f+c->param->lqr_offset.phi, .d_phi = 0.0f, - .x = c->chassis_state.target_x+c->param->lqr_offset.x, + .x = c->chassis_state.target_x+c->param->lqr_offset.x + leg_x_comp, .d_x = c->chassis_state.target_velocity_x, }; @@ -914,7 +921,6 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) { /* ==================== 轮毂输出 ==================== */ /* 腿长增加时有效轮距增大，等比例缩小yaw_force防止过冲 */ - float avg_L0 = (c->vmc_[0].leg.L0 + c->vmc_[1].leg.L0) * 0.5f; float yaw_scale = c->param->leg.base_length / avg_L0; /* 以基础腿长为基准归一化 */ float scaled_yaw_force = c->yaw_control.yaw_force * yaw_scale; From 2ba7d879b0b55e2e0f9183972d7204a48e8c4cd2 Mon Sep 17 00:00:00 2001 From: Robofish <1683502971@qq.com> Date: Sun, 15 Mar 2026 14:01:48 +0800 Subject: [PATCH 11/34] =?UTF-8?q?fix=20x=E8=A1=A5=E5=81=BF?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- User/module/balance_chassis.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/User/module/balance_chassis.c b/User/module/balance_chassis.c index 53766dc..bebb653 100644 --- a/User/module/balance_chassis.c +++ b/User/module/balance_chassis.c @@ -817,7 +817,7 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) { .d_theta = 0.0f, .phi = 0.0f+c->param->lqr_offset.phi, .d_phi = 0.0f, - .x = c->chassis_state.target_x+c->param->lqr_offset.x + leg_x_comp, + .x = c->chassis_state.target_x-c->param->lqr_offset.x + leg_x_comp, .d_x = c->chassis_state.target_velocity_x, }; From d5029eefdfbf135913bab13150e9e0a79b078405 Mon Sep 17 00:00:00 2001 From: Robofish <1683502971@qq.com> Date: Sun, 15 Mar 2026 14:30:46 +0800 Subject: [PATCH 12/34] fix cmd --- User/module/config.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/User/module/config.c b/User/module/config.c index cddf13f..3ec8fe2 100644 --- a/User/module/config.c +++ b/User/module/config.c @@ -481,8 +481,8 @@ Config_RobotParam_t robot_config = { #endif #if CMD_ENABLE_MODULE_BALANCE_CHASSIS .balance_sw_up = CHASSIS_MODE_RELAX, - .balance_sw_mid = CHASSIS_MODE_RELAX, - .balance_sw_down = CHASSIS_MODE_WHELL_LEG_BALANCE, + .balance_sw_mid = CHASSIS_MODE_WHELL_LEG_BALANCE, + .balance_sw_down = CHASSIS_MODE_BALANCE_ROTOR, #endif }, }, From c522e563afadf7758b5986e7736184169c0be2da Mon Sep 17 00:00:00 2001 From: Robofish <1683502971@qq.com> Date: Sun, 15 Mar 2026 15:10:55 +0800 Subject: [PATCH 13/34] fix --- User/module/balance_chassis.c | 20 ++++++++------------ 1 file changed, 8 insertions(+), 12 deletions(-) diff --git a/User/module/balance_chassis.c b/User/module/balance_chassis.c index bebb653..7a2efce 100644 --- a/User/module/balance_chassis.c +++ b/User/module/balance_chassis.c @@ -155,17 +155,12 @@ static int8_t Chassis_SetMode(Chassis_t *c, Chassis_Mode_t mode) { Chassis_ResetControllers(c); Chassis_SelectYawZeroPoint(c); - /* RELAX -> BALANCE：根据 pitch 角度判断是否需要自起 */ + /* RELAX -> BALANCE：pitch过大时保持RELAX，避免进入未实现的RECOVER */ if (mode == CHASSIS_MODE_WHELL_LEG_BALANCE && c->mode == CHASSIS_MODE_RELAX) { float pitch = c->feedback.imu.euler.pit; - /* pitch 绝对值大于 0.8rad（约46度）认为是倒地，需要自起 */ - if (fabsf(pitch) > 0.2f) { - /* 根据 theta 判断腿朝上还是朝下 */ - float theta_l = c->vmc_[0].leg.theta; - float theta_r = c->vmc_[1].leg.theta; - c->recover.state[0] = (fabsf(theta_l) > (float)M_PI_2) ? RECOVER_FLIP : RECOVER_STRETCH; - c->recover.state[1] = (fabsf(theta_r) > (float)M_PI_2) ? RECOVER_FLIP : RECOVER_STRETCH; - c->mode = CHASSIS_MODE_RECOVER; + if (fabsf(pitch) > 0.8f) { + /* pitch过大，机体未扶正，保持RELAX */ + Chassis_MotorRelax(c); return CHASSIS_OK; } /* 否则直接进入平衡模式 */ @@ -704,9 +699,10 @@ int8_t Chassis_Control(Chassis_t *c, const Chassis_CMD_t *c_cmd) { Chassis_VMCControl(c, c_cmd); break; - // case CHASSIS_MODE_RECOVER: - // Chassis_RecoverControl(c); - // break; + case CHASSIS_MODE_RECOVER: + /* 自起功能未实现，安全放松电机 */ + Chassis_MotorRelax(c); + break; case CHASSIS_MODE_WHELL_LEG_BALANCE: Chassis_LQRControl(c, c_cmd); From 7e98e574bd4ecf20b6b2b34bd729db485b9da712 Mon Sep 17 00:00:00 2001 From: Robofish <1683502971@qq.com> Date: Sun, 15 Mar 2026 15:29:57 +0800 Subject: [PATCH 14/34] FIX --- User/module/balance_chassis.c | 14 ++++++++++++++ 1 file changed, 14 insertions(+) diff --git a/User/module/balance_chassis.c b/User/module/balance_chassis.c index 7a2efce..fbcdc88 100644 --- a/User/module/balance_chassis.c +++ b/User/module/balance_chassis.c @@ -800,6 +800,20 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) { LQR_CalculateGainMatrix(&c->lqr[0], c->vmc_[0].leg.L0); LQR_CalculateGainMatrix(&c->lqr[1], c->vmc_[1].leg.L0); + /* 速度误差大、高速运动或双腿离地时，禁用x位移跟踪防止轮子失控 */ + { + float vel_error = c->chassis_state.target_velocity_x - c->chassis_state.velocity_x; + bool both_off_ground = !c->vmc_[0].leg.is_ground_contact && !c->vmc_[1].leg.is_ground_contact; + if (fabsf(c->chassis_state.target_velocity_x) > 0.5f || + fabsf(vel_error) > 0.7f || + both_off_ground) { + c->lqr[0].K_matrix[0][2] = 0.0f; /* 禁用T的x位移增益 */ + c->lqr[1].K_matrix[0][2] = 0.0f; + c->chassis_state.position_x = 0.0f; + c->chassis_state.target_x = 0.0f; + } + } + /* ==================== 目标状态 ==================== */ /* 腿长-位移补偿：根据腿长多项式拟合补偿位移偏移 */ float avg_L0 = (c->vmc_[0].leg.L0 + c->vmc_[1].leg.L0) * 0.5f; From 39e18ab745a7982273a11a714164303fd1d69f4c Mon Sep 17 00:00:00 2001 From: Robofish <1683502971@qq.com> Date: Sun, 15 Mar 2026 15:46:44 +0800 Subject: [PATCH 15/34] fix xxxx --- User/module/balance_chassis.c | 14 -------------- 1 file changed, 14 deletions(-) diff --git a/User/module/balance_chassis.c b/User/module/balance_chassis.c index fbcdc88..7a2efce 100644 --- a/User/module/balance_chassis.c +++ b/User/module/balance_chassis.c @@ -800,20 +800,6 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) { LQR_CalculateGainMatrix(&c->lqr[0], c->vmc_[0].leg.L0); LQR_CalculateGainMatrix(&c->lqr[1], c->vmc_[1].leg.L0); - /* 速度误差大、高速运动或双腿离地时，禁用x位移跟踪防止轮子失控 */ - { - float vel_error = c->chassis_state.target_velocity_x - c->chassis_state.velocity_x; - bool both_off_ground = !c->vmc_[0].leg.is_ground_contact && !c->vmc_[1].leg.is_ground_contact; - if (fabsf(c->chassis_state.target_velocity_x) > 0.5f || - fabsf(vel_error) > 0.7f || - both_off_ground) { - c->lqr[0].K_matrix[0][2] = 0.0f; /* 禁用T的x位移增益 */ - c->lqr[1].K_matrix[0][2] = 0.0f; - c->chassis_state.position_x = 0.0f; - c->chassis_state.target_x = 0.0f; - } - } - /* ==================== 目标状态 ==================== */ /* 腿长-位移补偿：根据腿长多项式拟合补偿位移偏移 */ float avg_L0 = (c->vmc_[0].leg.L0 + c->vmc_[1].leg.L0) * 0.5f; From 557a41e8b6ba61164050b606ede5510d1abc0a1e Mon Sep 17 00:00:00 2001 From: Robofish <1683502971@qq.com> Date: Sun, 15 Mar 2026 16:20:58 +0800 Subject: [PATCH 16/34] =?UTF-8?q?=E4=BF=AE=E5=A4=8D?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- User/module/balance_chassis.c | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/User/module/balance_chassis.c b/User/module/balance_chassis.c index 7a2efce..ea7b3ef 100644 --- a/User/module/balance_chassis.c +++ b/User/module/balance_chassis.c @@ -770,7 +770,8 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) { /* 小陀螺平移：将世界坐标系期望速度投影到机体坐标系 */ float yaw_angle = c->feedback.imu.euler.yaw; float world_vx = c_cmd->move_vec.vx * c->param->rotor_ctrl.max_trans_speed; - c->chassis_state.target_velocity_x = world_vx * cosf(yaw_angle); + float world_vy = c_cmd->move_vec.vy * c->param->rotor_ctrl.max_trans_speed; + c->chassis_state.target_velocity_x = world_vx * cosf(yaw_angle) + world_vy * sinf(yaw_angle); c->chassis_state.last_target_velocity_x = c->chassis_state.target_velocity_x; c->chassis_state.target_x += c->chassis_state.target_velocity_x * c->dt; } else { @@ -819,8 +820,8 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) { /* ==================== Yaw轴控制 ==================== */ if (c_cmd->mode == CHASSIS_MODE_BALANCE_ROTOR || c->mode == CHASSIS_MODE_BALANCE_ROTOR) { - /* 小陀螺模式：PID跟踪目标角速度，带速度斜坡 */ - float target_spin = c_cmd->move_vec.vy * c->param->rotor_ctrl.max_spin_speed; + /* 小陀螺模式：自动以max_spin_speed恒速旋转 */ + float target_spin = c->param->rotor_ctrl.max_spin_speed; if (c->rotor_state.exiting) { /* 退出过渡：目标角速度为0，用减速斜坡 */ @@ -860,7 +861,7 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) { c->yaw_control.yaw_force = PID_Calc(&c->pid.yaw, c->yaw_control.target_yaw, c->feedback.yaw.rotor_abs_angle, 0.0f, c->dt); } else { - float current_yaw_omega = c->feedback.imu.gyro.z; + float current_yaw_omega = -c->feedback.imu.gyro.z; /* 取反匹配yaw_force→轮子旋转方向 */ c->yaw_control.yaw_force = PID_Calc(&c->pid.rotor, c->rotor_state.current_spin_speed, current_yaw_omega, 0.0f, c->dt); } From a8d01e7f749e374289c9f67de062f7471d59da47 Mon Sep 17 00:00:00 2001 From: Robofish <1683502971@qq.com> Date: Sun, 15 Mar 2026 17:12:53 +0800 Subject: [PATCH 17/34] =?UTF-8?q?=E4=BF=AE=E9=99=80=E8=9E=BA=E7=A7=BB?= =?UTF-8?q?=E5=8A=A8=E6=96=B9=E5=90=91?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- User/module/balance_chassis.c | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/User/module/balance_chassis.c b/User/module/balance_chassis.c index ea7b3ef..f35a311 100644 --- a/User/module/balance_chassis.c +++ b/User/module/balance_chassis.c @@ -767,11 +767,11 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) { /* ==================== 速度控制 ==================== */ if (c_cmd->mode == CHASSIS_MODE_BALANCE_ROTOR) { - /* 小陀螺平移：将世界坐标系期望速度投影到机体坐标系 */ - float yaw_angle = c->feedback.imu.euler.yaw; + /* 小陀螺平移：将云台坐标系（用户视角）速度投影到机体坐标系 */ + float gimbal_offset = c->feedback.yaw.rotor_abs_angle - c->param->mech.mech_zero_yaw; float world_vx = c_cmd->move_vec.vx * c->param->rotor_ctrl.max_trans_speed; float world_vy = c_cmd->move_vec.vy * c->param->rotor_ctrl.max_trans_speed; - c->chassis_state.target_velocity_x = world_vx * cosf(yaw_angle) + world_vy * sinf(yaw_angle); + c->chassis_state.target_velocity_x = world_vx * cosf(gimbal_offset) + world_vy * sinf(gimbal_offset); c->chassis_state.last_target_velocity_x = c->chassis_state.target_velocity_x; c->chassis_state.target_x += c->chassis_state.target_velocity_x * c->dt; } else { From a195d14e009160e1b45464aea1a0834ee1e22d52 Mon Sep 17 00:00:00 2001 From: Robofish <1683502971@qq.com> Date: Sun, 15 Mar 2026 17:15:21 +0800 Subject: [PATCH 18/34] =?UTF-8?q?=E4=BF=AE=E4=BF=AE=E5=B9=B3=E7=A7=BB?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- User/module/config.c | 912 ++++++++++++++++++++++--------------------- 1 file changed, 459 insertions(+), 453 deletions(-) diff --git a/User/module/config.c b/User/module/config.c index 3ec8fe2..b62cdc5 100644 --- a/User/module/config.c +++ b/User/module/config.c @@ -1,6 +1,6 @@ /* - * 配置相关 - */ + * 配置相关 + */ /* Includes ----------------------------------------------------------------- */ #include "module/config.h" @@ -15,488 +15,494 @@ /* Exported variables ------------------------------------------------------- */ /** - * @brief 机器人参数配置 - * @note 在此配置机器人参数 - */ + * @brief 机器人参数配置 + * @note 在此配置机器人参数 + */ Config_RobotParam_t robot_config = { - /* USER CODE BEGIN robot_config */ - .ref_screen={ - .width=1920, - .height=1080, - }, - .gimbal_param = { - .pid = { - .yaw_omega = { - .k = 0.25f, - .p = 1.0f, - .i = 0.0f, - .d = 0.0f, - .i_limit = 1.0f, - .out_limit = 1.0f, - .d_cutoff_freq = -1.0f, - .range = -1.0f, - }, - .yaw_angle = { - .k = 7.0f, - .p = 3.5f, - .i = 0.0f, - .d = 0.0f, - .i_limit = 0.0f, - .out_limit = 10.0f, - .d_cutoff_freq = -1.0f, - .range = M_2PI, - }, - .pit_omega = { - .k = 0.25f, - .p = 1.0f, - .i = 0.0f, - .d = 0.0f, - .i_limit = 1.0f, - .out_limit = 1.0f, - .d_cutoff_freq = -1.0f, - .range = -1.0f, - }, - .pit_angle = { - .k = 2.5f, - .p = 5.0f, - .i = 0.2f, - .d = 0.01f, - .i_limit = 0.0f, - .out_limit = 10.0f, - .d_cutoff_freq = -1.0f, - .range = M_2PI, - }, - }, - .mech_zero = { - .yaw = 0.0f, - .pit = 3.23056364f, - }, - .travel = { - .yaw = -1.0f, - .pit = 0.85f, - }, - .low_pass_cutoff_freq = { - .out = -1.0f, - .gyro = 1000.0f, - }, - .pit_motor ={ - .can = BSP_CAN_1, - .id = 0x206, - .gear = false, - .module = MOTOR_GM6020, - .reverse = true, - }, - .yaw_motor = { - .can = BSP_CAN_1, - .can_id = 0x1, - .master_id = 0x11, - .module = MOTOR_DM_J4310, - .reverse = false, - }, - .imu = { - .can = BSP_CAN_2, - .accl_id = 0x100, // 加速度计 (十进制256) - .gyro_id = 0x101, // 陀螺仪 (十进制257) - .eulr_id = 0x102, // 欧拉角 (十进制258) - .quat_id = 0x103 // 四元数 (十进制259) - }, - - }, +    /* USER CODE BEGIN robot_config */ +        .ref_screen={ +                    .width=1920, +                    .height=1080, +      }, +    .gimbal_param = { +        .pid = { +            .yaw_omega = { +                .k = 0.25f, +                .p = 1.0f, +                .i = 0.0f, +                .d = 0.0f, +                .i_limit = 1.0f, +                .out_limit = 1.0f, +                .d_cutoff_freq = -1.0f, +                .range = -1.0f, +            }, +            .yaw_angle = { +                .k = 7.0f, +                .p = 3.5f, +                .i = 0.0f, +                .d = 0.0f, +                .i_limit = 0.0f, +                .out_limit = 10.0f, +                .d_cutoff_freq = -1.0f, +                .range = M_2PI, +            }, +            .pit_omega = { +                .k = 0.25f, +                .p = 1.0f, +                .i = 0.0f, +                .d = 0.0f, +                .i_limit = 1.0f, +                .out_limit = 1.0f, +                .d_cutoff_freq = -1.0f, +                .range = -1.0f, +            }, +            .pit_angle = { +                .k = 2.5f, +                .p = 5.0f, +                .i = 0.2f, +                .d = 0.01f, +                .i_limit = 0.0f, +                .out_limit = 10.0f, +                .d_cutoff_freq = -1.0f, +                .range = M_2PI, +            }, +        }, +        .mech_zero = { +            .yaw = 0.0f, +            .pit = 3.23056364f, +        }, +        .travel = { +            .yaw = -1.0f, +            .pit = 0.85f, +        }, +        .low_pass_cutoff_freq = { +            .out = -1.0f, +            .gyro = 1000.0f, +        }, +        .pit_motor ={ +            .can = BSP_CAN_1, +            .id = 0x206, +            .gear = false, +            .module = MOTOR_GM6020, +            .reverse = true, +        }, +        .yaw_motor = { +            .can = BSP_CAN_1, +            .can_id = 0x1, +            .master_id = 0x11, +            .module = MOTOR_DM_J4310, +            .reverse = false, +        }, +        .imu = { +            .can = BSP_CAN_2, +            .accl_id = 0x100,  // 加速度计 (十进制256) +            .gyro_id = 0x101,  // 陀螺仪 (十进制257) +            .eulr_id = 0x102,  // 欧拉角 (十进制258) +            .quat_id = 0x103   // 四元数 (十进制259) +        }, +        +    }, - .shoot_param = { - .basic={ - .projectileType=SHOOT_PROJECTILE_17MM, - .fric_num=2, - .extra_deceleration_ratio=1.0f, - .num_trig_tooth=8, - .shot_freq=1.0f, - .shot_burst_num=3, - .ratio_multilevel = {1.0f}, - }, - .jamDetection={ - .enable=true, - .threshold=200.0f, - .suspectedTime=0.5f, - }, - .heatControl={ - .enable=true, - .nmax=18.0f, // 最大射频 Hz - .Hwarn=200.0f, // 热量预警值 - .Hsatu=100.0f, // 热量饱和值 - .Hthres=50.0f, // 热量阈值 - }, - .motor={ - .fric = { - { - .param = { - .can = BSP_CAN_1, - .id = 0x201, - .module = MOTOR_M3508, - .reverse = false, - .gear=false, - }, - .level=1, - }, - { - .param = { - .can = BSP_CAN_1, - .id = 0x202, - .module = MOTOR_M3508, - .reverse = true, - .gear=false, - }, - .level=1, - } - }, - .trig = { - .can = BSP_CAN_1, - .id = 0x203, - .module = MOTOR_M2006, - .reverse = true, - .gear=true, - }, - }, - .pid={ - .fric_follow = { - .k=1.0f, - .p=1.5f, - .i=0.0f, - .d=0.0f, - .i_limit=0.0f, - .out_limit=0.9f, - .d_cutoff_freq=30.0f, - .range=-1.0f, - }, - .fric_err = { - .k=0.0f, - .p=0.0f, - .i=0.0f, - .d=0.0f, - .i_limit=0.0f, - .out_limit=0.0f, - .d_cutoff_freq=0.0f, - .range=-1.0f, - }, - .trig_2006 = { - .k=1.0f, - .p=1.0f, - .i=0.1f, - .d=0.05f, - .i_limit=0.8f, - .out_limit=0.5f, - .d_cutoff_freq=-1.0f, - .range=M_2PI, - }, - .trig_omg_2006 = { - .k=1.0f, - .p=1.5f, - .i=0.3f, - .d=0.5f, - .i_limit=0.2f, - .out_limit=0.9f, - .d_cutoff_freq=-1.0f, - .range=-1.0f, - }, - .trig_3508 = { - .k=0.5f, - .p=1.8f, - .i=0.3f, - .d=0.1f, - .i_limit=0.15f, - .out_limit=1.0f, - .d_cutoff_freq=-1.0f, - .range=M_2PI, - }, - .trig_omg_3508 = { - .k=1.0f, - .p=1.0f, - .i=0.0f, - .d=0.0f, - .i_limit=0.0f, - .out_limit=1.0f, - .d_cutoff_freq=-1.0f, - .range=-1.0f, - }, - }, - .filter={ - .fric = { - .in = 30.0f, - .out = 30.0f, - }, - .trig = { - .in = 30.0f, - .out = 30.0f, - }, - }, - }, - .chassis_param = { - .yaw={ - .k=1.0f, - .p=1.0f, - .i=0.0f, - .d=0.3f, - .i_limit=0.0f, - .out_limit=1.0f, - .d_cutoff_freq=30.0f, - .range=M_2PI, /* 2*PI，用于处理角度循环误差 */ - }, - - .rotor={ - .k=1.0f, - .p=0.5f, - .i=0.0f, - .d=0.01f, - .i_limit=0.0f, - .out_limit=1.0f, - .d_cutoff_freq=30.0f, - .range=-1.0f, /* 角速度不需要循环误差处理 */ - }, - - .roll={ - .k=0.15f, - .p=0.3f, /* 横滚角腿长补偿比例系数 (降低避免抖动) */ - .i=0.0f, /* 横滚角腿长补偿积分系数 (关闭避免震荡) */ - .d=0.01f, /* 横滚角腿长补偿微分系数 (关闭避免噪声放大) */ - .i_limit=0.0f, /* 积分限幅 */ - .out_limit=0.03f, /* 输出限幅，腿长差值限制(m) 降低避免过度补偿 */ - .d_cutoff_freq=30.0f, /* 微分截止频率 */ - .range=-1.0f, /* 不使用循环误差处理 */ - }, - - .roll_force={ - .k=5.0f, - .p=10.0f, /* 横滚角力补偿比例系数 (大幅降低避免抖动) */ - .i=0.0f, /* 横滚角力补偿积分系数 */ - .d=5.0f, /* 横滚角力补偿微分系数 (降低避免高频抖动) */ - .i_limit=0.0f, /* 积分限幅 */ - .out_limit=20.0f, /* 输出限幅，力补偿限制(N) 降低避免过度补偿 */ - .d_cutoff_freq=30.0f, /* 微分截止频率 */ - .range=-1.0f, /* 不使用循环误差处理 */ - }, +    .shoot_param = { +        .basic={ +          .projectileType=SHOOT_PROJECTILE_17MM, +              .fric_num=2, +              .extra_deceleration_ratio=1.0f, +          .num_trig_tooth=8, +          .shot_freq=1.0f, +          .shot_burst_num=3, +          .ratio_multilevel = {1.0f}, +        },   +        .jamDetection={ +            .enable=true, +            .threshold=200.0f, +            .suspectedTime=0.5f, +        }, +        .heatControl={ +            .enable=true, +            .nmax=18.0f,      // 最大射频 Hz +            .Hwarn=200.0f,    // 热量预警值 +            .Hsatu=100.0f,    // 热量饱和值 +            .Hthres=50.0f,    // 热量阈值 +        }, +        .motor={ +            .fric = { +              { +                .param = { +                  .can = BSP_CAN_1, +                  .id = 0x201, +                  .module = MOTOR_M3508, +                  .reverse = false, +                  .gear=false, +                }, +              .level=1, +            }, +              { +                .param = { +                  .can = BSP_CAN_1, +                  .id = 0x202, +                  .module = MOTOR_M3508, +                  .reverse = true, +                  .gear=false, +                }, +              .level=1, +            } +            }, +            .trig = { +                .can = BSP_CAN_1, +                .id = 0x203, +                .module = MOTOR_M2006, +                .reverse = true, +                .gear=true, +            }, +        }, +        .pid={       +            .fric_follow = { +                .k=1.0f, +                .p=1.5f, +                .i=0.0f, +                .d=0.0f, +                .i_limit=0.0f, +                .out_limit=0.9f, +                .d_cutoff_freq=30.0f, +                .range=-1.0f, +            }, +            .fric_err = { +                .k=0.0f, +                .p=0.0f, +                .i=0.0f, +                .d=0.0f, +                .i_limit=0.0f, +                .out_limit=0.0f, +                .d_cutoff_freq=0.0f, +                .range=-1.0f, +            }, +            .trig_2006 = { +                .k=1.0f, +                .p=1.0f, +                .i=0.1f, +                .d=0.05f, +                .i_limit=0.8f, +                .out_limit=0.5f, +                .d_cutoff_freq=-1.0f, +                .range=M_2PI, +            }, +            .trig_omg_2006 = { +                .k=1.0f, +                .p=1.5f, +                .i=0.3f, +                .d=0.5f, +                .i_limit=0.2f, +                .out_limit=0.9f, +                .d_cutoff_freq=-1.0f, +                .range=-1.0f, +            }, +            .trig_3508 = { +                .k=0.5f, +                .p=1.8f, +                .i=0.3f, +                .d=0.1f, +                .i_limit=0.15f, +                .out_limit=1.0f, +                .d_cutoff_freq=-1.0f, +                .range=M_2PI, +            }, +            .trig_omg_3508 = { +                .k=1.0f, +                .p=1.0f, +                .i=0.0f, +                .d=0.0f, +                .i_limit=0.0f, +                .out_limit=1.0f, +                .d_cutoff_freq=-1.0f, +                .range=-1.0f, +            }, +        }, +        .filter={ +            .fric = { +                .in = 30.0f, +                .out = 30.0f, +            }, +            .trig = { +                .in = 30.0f, +                .out = 30.0f, +            }, +        },   +    }, +    .chassis_param = { +        .yaw={ +            .k=1.0f, +            .p=1.0f, +            .i=0.0f, +            .d=0.3f, +            .i_limit=0.0f, +            .out_limit=1.0f, +            .d_cutoff_freq=30.0f, +            .range=M_2PI, /* 2*PI，用于处理角度循环误差 */ +        }, +        +        .rotor={ +            .k=1.0f, +            .p=0.5f, +            .i=0.0f, +            .d=0.01f, +            .i_limit=0.0f, +            .out_limit=1.0f, +            .d_cutoff_freq=30.0f, +            .range=-1.0f, /* 角速度不需要循环误差处理 */ +        }, +        +        .roll={ +            .k=0.15f, +            .p=0.3f,           /* 横滚角腿长补偿比例系数 (降低避免抖动) */ +            .i=0.0f,           /* 横滚角腿长补偿积分系数 (关闭避免震荡) */ +            .d=0.01f,           /* 横滚角腿长补偿微分系数 (关闭避免噪声放大) */ +            .i_limit=0.0f,     /* 积分限幅 */ +            .out_limit=0.03f,  /* 输出限幅，腿长差值限制(m) 降低避免过度补偿 */ +            .d_cutoff_freq=30.0f, /* 微分截止频率 */ +            .range=-1.0f,      /* 不使用循环误差处理 */ +        }, +        +        .roll_force={ +            .k=5.0f, +            .p=10.0f,          /* 横滚角力补偿比例系数 (大幅降低避免抖动) */ +            .i=0.0f,           /* 横滚角力补偿积分系数 */ +            .d=5.0f,           /* 横滚角力补偿微分系数 (降低避免高频抖动) */ +            .i_limit=0.0f,     /* 积分限幅 */ +            .out_limit=20.0f,  /* 输出限幅，力补偿限制(N) 降低避免过度补偿 */ +            .d_cutoff_freq=30.0f, /* 微分截止频率 */ +            .range=-1.0f,      /* 不使用循环误差处理 */ +        }, - .leg_length={ - .k = 40.0f, - .p = 50.0f, - .i = 0.01f, - .d = 2.0f, - .i_limit = 0.0f, - .out_limit = 500.0f, - .d_cutoff_freq = -1.0f, - .range = -1.0f, - }, +        .leg_length={ +            .k = 40.0f, +            .p = 50.0f, +            .i = 0.01f, +            .d = 2.0f, +            .i_limit = 0.0f, +            .out_limit = 500.0f, +            .d_cutoff_freq = -1.0f, +            .range = -1.0f, +        }, - .leg_theta={ - .k=5.0f, - .p=2.0f, /* 摆角比例系数 */ - .i=0.0f, /* 摆角积分系数 */ - .d=0.0f, /* 摆角微分系数 */ - .i_limit=0.0f, /* 积分限幅 */ - .out_limit=0.05f, /* 输出限幅，腿长差值限制 */ - .d_cutoff_freq=30.0f, /* 微分截止频率 */ - .range=-1.0f, /* 不使用循环误差处理 */ - }, +        .leg_theta={ +            .k=5.0f, +            .p=2.0f,           /* 摆角比例系数 */ +            .i=0.0f,           /* 摆角积分系数 */ +            .d=0.0f,           /* 摆角微分系数 */ +            .i_limit=0.0f,     /* 积分限幅 */ +            .out_limit=0.05f,  /* 输出限幅，腿长差值限制 */ +            .d_cutoff_freq=30.0f, /* 微分截止频率 */ +            .range=-1.0f,      /* 不使用循环误差处理 */ +        }, - .tp={ - .k=4.0f, - .p=3.0f, /* 俯仰角比例系数 */ - .i=0.0f, /* 俯仰角积分系数 */ - .d=0.0f, /* 俯仰角微分系数 */ - .i_limit=0.0f, /* 积分限幅 */ - .out_limit=10.0f, /* 输出限幅，腿长差值限制 */ - .d_cutoff_freq=30.0f, /* 微分截止频率 */ - .range=-1.0f, /* 不使用循环误差处理 */ - }, - - .low_pass_cutoff_freq = { - .in = 30.0f, - .out = 30.0f, - }, +        .tp={ +            .k=4.0f, +            .p=3.0f,          /* 俯仰角比例系数 */ +            .i=0.0f,           /* 俯仰角积分系数 */ +            .d=0.0f,           /* 俯仰角微分系数 */ +            .i_limit=0.0f,     /* 积分限幅 */ +            .out_limit=10.0f,   /* 输出限幅，腿长差值限制 */ +            .d_cutoff_freq=30.0f, /* 微分截止频率 */ +            .range=-1.0f,      /* 不使用循环误差处理 */ +        }, +        +        .low_pass_cutoff_freq = { +            .in = 30.0f, +            .out = 30.0f, +        }, - .yaw_motor = { // 云台Yaw轴电机 - .can = BSP_CAN_1, - .can_id = 0x1, - .master_id = 0x11, - .module = MOTOR_DM_J4310, - .reverse = false, - }, - - .joint_param = { - { // 左髋关节 - .can = BSP_CAN_3, - .motor_id = 124, - .host_id = 0xFF, - .module = MOTOR_LZ_RSO3, - .reverse = false, - .mode = MOTOR_LZ_MODE_MOTION, - }, - { // 右髋关节 - .can = BSP_CAN_3, - .motor_id = 125, - .host_id = 0xFF, - .module = MOTOR_LZ_RSO3, - .reverse = false, - .mode = MOTOR_LZ_MODE_MOTION, - }, - { // 左膝关节 - .can = BSP_CAN_3, - .motor_id = 126, - .host_id = 0xFF, - .module = MOTOR_LZ_RSO3, - .reverse = true, - .mode = MOTOR_LZ_MODE_MOTION, - }, - { // 右膝关节 - .can = BSP_CAN_3, - .motor_id = 127, - .host_id = 0xFF, - .module = MOTOR_LZ_RSO3, - .reverse = true, - .mode = MOTOR_LZ_MODE_MOTION, - }, - }, - - .wheel_param = { - { // 左轮电机 - .can = BSP_CAN_3, - .id = 0x142, - .module = MOTOR_LK_MF9025, - .reverse = false, - }, - { // 右轮电机 - .can = BSP_CAN_3, - .id = 0x141, - .module = MOTOR_LK_MF9025, - .reverse = true, - }, - }, +        .yaw_motor = { // 云台Yaw轴电机 +            .can = BSP_CAN_1, +            .can_id = 0x1, +            .master_id = 0x11, +            .module = MOTOR_DM_J4310, +            .reverse = false, +        }, +        +        .joint_param = { +            { // 左髋关节 +                .can = BSP_CAN_3, +                .motor_id = 124, +                .host_id = 0xFF, +                .module = MOTOR_LZ_RSO3, +                .reverse = false, +                .mode = MOTOR_LZ_MODE_MOTION, +                }, +            { // 右髋关节 +                .can = BSP_CAN_3, +                .motor_id = 125, +                .host_id = 0xFF, +                .module = MOTOR_LZ_RSO3, +                .reverse = false, +                .mode = MOTOR_LZ_MODE_MOTION, +                }, +            { // 左膝关节 +                 .can = BSP_CAN_3, +                .motor_id = 126, +                .host_id = 0xFF, +                .module = MOTOR_LZ_RSO3, +                .reverse = true, +                .mode = MOTOR_LZ_MODE_MOTION, +                }, +            { // 右膝关节 +                .can = BSP_CAN_3, +                .motor_id = 127, +                .host_id = 0xFF, +                .module = MOTOR_LZ_RSO3, +                .reverse = true, +                .mode = MOTOR_LZ_MODE_MOTION, +                }, +        }, +        +        .wheel_param = { +            { // 左轮电机 +                .can = BSP_CAN_3, +                .id = 0x142, +                .module = MOTOR_LK_MF9025, +                .reverse = false, +            }, +            { // 右轮电机 +                .can = BSP_CAN_3, +                .id = 0x141, +                .module = MOTOR_LK_MF9025, +                .reverse = true, +            }, +        }, - .mech = { - .wheel_radius = 0.068f, - .wheel_gear_ratio = 4.5f, - .hip_width = 0.423f, /* 髋宽，即两腿间距 (m)，用于Roll几何补偿 */ - .joint_zero = {0.0f, 0.0f, 1.33022018f, 1.13195965f}, /* 关节电机零点偏移 */ - .mech_zero_yaw = 2.96925735f, /* 机械零点 */ - }, +        .mech = { +            .wheel_radius = 0.068f, +            .wheel_gear_ratio = 4.5f, +            .hip_width = 0.423f,           /* 髋宽，即两腿间距 (m)，用于Roll几何补偿 */ +            .joint_zero = {0.0f, 0.0f, 1.33022018f, 1.13195965f}, /* 关节电机零点偏移 */ +            .mech_zero_yaw = 2.96925735f,  /* 机械零点 */ +        }, - .leg = { - .base_length = 0.16f, /* 基础腿长 (m) */ - .length_range = 0.20f, /* 腿长调节范围 (m) */ - .min_length = 0.14f, /* 最小腿长 (m) */ - .max_length = 0.36f, /* 最大腿长 (m) */ - .left_base_force = 65.0f, /* 左腿基础支撑力 (N) */ - .right_base_force = 65.0f, /* 右腿基础支撑力 (N) */ - }, +        .leg = { +            .base_length = 0.16f,          /* 基础腿长 (m) */ +            .length_range = 0.20f,         /* 腿长调节范围 (m) */ +            .min_length = 0.14f,           /* 最小腿长 (m) */ +            .max_length = 0.36f,           /* 最大腿长 (m) */ +            .left_base_force = 65.0f,      /* 左腿基础支撑力 (N) */ +            .right_base_force = 65.0f,     /* 右腿基础支撑力 (N) */ +        }, - .motion = { - .max_acceleration = 5.0f, /* 最大加速度 (m/s^2) */ - .non_contact_theta = -0.01f, /* 离地摆角目标 (rad) */ - }, +        .motion = { +            .max_acceleration = 5.0f,      /* 最大加速度 (m/s^2) */ +            .non_contact_theta = -0.01f,    /* 离地摆角目标 (rad) */ +        }, - .rotor_ctrl = { - .max_spin_speed = 0.05236f, /* 最大旋转角速度 (rad/s)，0.5rpm */ - .max_trans_speed = 1.5f, /* 小陀螺时最大平移速度 (m/s) */ - .spin_accel = 5.0f, /* 旋转加速度 (rad/s²) */ - .spin_decel = 8.0f, /* 退出减速度 (rad/s²)，比加速更快 */ - .align_threshold = 0.15f, /* 退出对齐阈值 (rad)，约8.6° */ - }, +        .rotor_ctrl = { +            .max_spin_speed = 5.0236f,    /* 最大旋转角速度 (rad/s)，0.5rpm */ +            .max_trans_speed = 1.5f,       /* 小陀螺时最大平移速度 (m/s) */ +            .spin_accel = 5.0f,             +            +            +            +            +            +            /* 旋转加速度 (rad/s²) */ +            .spin_decel = 8.0f,            /* 退出减速度 (rad/s²)，比加速更快 */ +            .align_threshold = 0.15f,      /* 退出对齐阈值 (rad)，约8.6° */ +        }, - .vmc_param = { - { // 左腿 - .leg_1 = 0.215f, // 后髋连杆长度 (L1) (m) - .leg_2 = 0.258f, // 后膝连杆长度 (L2) (m) - .leg_3 = 0.258f, // 前膝连杆长度 (L3) (m) - .leg_4 = 0.215f, // 前髋连杆长度 (L4) (m) - .hip_length = 0.0f // 髋宽 (L5) (m) - }, - { // 右腿 - .leg_1 = 0.215f, // 后髋连杆长度 (L1) (m) - .leg_2 = 0.258f, // 后膝连杆长度 (L2) (m) - .leg_3 = 0.258f, // 前膝连杆长度 (L3) (m) - .leg_4 = 0.215f, // 前髋连杆长度 (L4) (m) - .hip_length = 0.0f // 髋宽 (L5) (m) - } - }, - .lqr_gains = { - .k11_coeff = { -1.997928012118380e+02f, 2.358970856133577e+02f, -1.221432656066952e+02f, -3.660884265594530e+00f }, // theta - .k12_coeff = { 1.144642140244768e+00f, -6.781143989723293e-01f, -8.286867905694857e+00f, -3.233743818654922e-01f }, // d_theta - .k13_coeff = { -2.920908857554099e+01f, 3.294053235814564e+01f, -1.331127106026891e+01f, -2.036633181450379e+00f }, // x - .k14_coeff = { -2.202395151966686e+01f, 2.624258394298977e+01f, -1.238392622003398e+01f, -2.516594294742349e+00f }, // d_x - .k15_coeff = { -5.531234648285231e+01f, 7.666196018790744e+01f, -4.254301644877548e+01f, 1.191958597928930e+01f }, // phi - .k16_coeff = { -8.603393557876432e+00f, 1.181868681749069e+01f, -6.519358011847959e+00f, 2.002836931783026e+00f }, // d_phi - .k21_coeff = { 3.121841964126664e+02f, -2.644789946321499e+02f, 5.653973783920610e+01f, 1.306067415926613e+01f }, // theta - .k22_coeff = { 9.751578045310433e+00f, -8.174866581419979e+00f, 1.060040492386880e+00f, 1.945460420856203e+00f }, // d_theta - .k23_coeff = { -1.589159892148102e+01f, 3.771040781828523e+01f, -2.789168930865428e+01f, 8.354369470743295e+00f }, // x - .k24_coeff = { -3.304075197263637e+01f, 5.411818719230526e+01f, -3.342861600971858e+01f, 9.256855471266778e+00f }, // d_x - .k25_coeff = { 2.574491871362636e+02f, -2.771545105998671e+02f, 1.074894289176479e+02f, 3.130550754680395e+00f }, // phi - .k26_coeff = { 4.162978395880715e+01f, -4.533986766238992e+01f, 1.794589550749570e+01f, 2.032213740678163e-01f }, // d_phi - }, +        .vmc_param = { +            { // 左腿 +                .leg_1 = 0.215f,      // 后髋连杆长度 (L1) (m) +                .leg_2 = 0.258f,      // 后膝连杆长度 (L2) (m) +                .leg_3 = 0.258f,      // 前膝连杆长度 (L3) (m) +                .leg_4 = 0.215f,      // 前髋连杆长度 (L4) (m) +                .hip_length = 0.0f    // 髋宽 (L5) (m) +            }, +            { // 右腿 +                .leg_1 = 0.215f,      // 后髋连杆长度 (L1) (m) +                .leg_2 = 0.258f,      // 后膝连杆长度 (L2) (m) +                .leg_3 = 0.258f,      // 前膝连杆长度 (L3) (m) +                .leg_4 = 0.215f,      // 前髋连杆长度 (L4) (m) +                .hip_length = 0.0f    // 髋宽 (L5) (m) +            } +        }, +    .lqr_gains = { +        .k11_coeff = { -1.997928012118380e+02f, 2.358970856133577e+02f, -1.221432656066952e+02f, -3.660884265594530e+00f }, // theta +        .k12_coeff = { 1.144642140244768e+00f, -6.781143989723293e-01f, -8.286867905694857e+00f, -3.233743818654922e-01f },  // d_theta +        .k13_coeff = { -2.920908857554099e+01f, 3.294053235814564e+01f, -1.331127106026891e+01f, -2.036633181450379e+00f },  // x +        .k14_coeff = { -2.202395151966686e+01f, 2.624258394298977e+01f, -1.238392622003398e+01f, -2.516594294742349e+00f },  // d_x +        .k15_coeff = { -5.531234648285231e+01f, 7.666196018790744e+01f, -4.254301644877548e+01f, 1.191958597928930e+01f }, // phi +        .k16_coeff = { -8.603393557876432e+00f, 1.181868681749069e+01f, -6.519358011847959e+00f, 2.002836931783026e+00f },  // d_phi +        .k21_coeff = { 3.121841964126664e+02f, -2.644789946321499e+02f, 5.653973783920610e+01f, 1.306067415926613e+01f }, // theta +        .k22_coeff = { 9.751578045310433e+00f, -8.174866581419979e+00f, 1.060040492386880e+00f, 1.945460420856203e+00f },  // d_theta +        .k23_coeff = { -1.589159892148102e+01f, 3.771040781828523e+01f, -2.789168930865428e+01f, 8.354369470743295e+00f },  // x +        .k24_coeff = { -3.304075197263637e+01f, 5.411818719230526e+01f, -3.342861600971858e+01f, 9.256855471266778e+00f },  // d_x +        .k25_coeff = { 2.574491871362636e+02f, -2.771545105998671e+02f, 1.074894289176479e+02f, 3.130550754680395e+00f }, // phi +        .k26_coeff = { 4.162978395880715e+01f, -4.533986766238992e+01f, 1.794589550749570e+01f, 2.032213740678163e-01f },  // d_phi +    }, - .jump = { - .crouch_time_ms = 500, - .launch_time_ms = 300, - .retract_time_ms = 500, - .land_time_ms = 500, - .crouch_leg_length = 0.13f, /* 不用蹲太低 */ - .launch_force = 20.0f, /* 起跳力减小 */ - .retract_leg_length = 0.12f, /* 收腿目标 */ - .retract_force = -20.0f, /* 收腿前馈力减小 */ - }, +    .jump = { +        .crouch_time_ms = 500, +        .launch_time_ms = 300, +        .retract_time_ms = 500, +        .land_time_ms = 500, +        .crouch_leg_length = 0.13f,    /* 不用蹲太低 */ +        .launch_force = 20.0f,         /* 起跳力减小 */ +        .retract_leg_length = 0.12f,   /* 收腿目标 */ +        .retract_force = -20.0f,       /* 收腿前馈力减小 */ +    }, - .lqr_offset = { - .theta = 0.0f, - .x = 0.0f, - .phi = 0.0f, - }, - - }, +    .lqr_offset = { +        .theta = 0.0f, +        .x = 0.0f, +        .phi = 0.0f, +    }, +    +    }, - .ai_param = { - .can = BSP_FDCAN_2, - .vision_id = 0x104, - }, +    .ai_param = { +        .can = BSP_FDCAN_2, +        .vision_id = 0x104, +    }, - .cmd_param = { - .source_priority = { +    .cmd_param = { +        .source_priority = { #if CMD_ENABLE_SRC_RC - [0] = CMD_SRC_RC, +            [0] = CMD_SRC_RC, #endif #if CMD_ENABLE_SRC_PC - [1] = CMD_SRC_PC, +            [1] = CMD_SRC_PC, #endif - }, - .sensitivity = { - .mouse_sens = 60.0f, - .move_sens = 1.0f, - .move_fast_mult = 2.0f, - .move_slow_mult = 0.4f, - }, - .rc_mode_map = { +        }, +        .sensitivity = { +            .mouse_sens     = 60.0f, +            .move_sens      = 1.0f, +            .move_fast_mult = 2.0f, +            .move_slow_mult = 0.4f, +        }, +        .rc_mode_map = { #if CMD_ENABLE_MODULE_GIMBAL - .gimbal_sw_up = GIMBAL_MODE_RELAX, - .gimbal_sw_mid = GIMBAL_MODE_RELATIVE, - .gimbal_sw_down = GIMBAL_MODE_RELATIVE, +            .gimbal_sw_up   = GIMBAL_MODE_RELAX, +            .gimbal_sw_mid  = GIMBAL_MODE_RELATIVE, +            .gimbal_sw_down = GIMBAL_MODE_RELATIVE, #endif #if CMD_ENABLE_MODULE_BALANCE_CHASSIS - .balance_sw_up = CHASSIS_MODE_RELAX, - .balance_sw_mid = CHASSIS_MODE_WHELL_LEG_BALANCE, - .balance_sw_down = CHASSIS_MODE_BALANCE_ROTOR, +            .balance_sw_up   = CHASSIS_MODE_RELAX, +            .balance_sw_mid  = CHASSIS_MODE_WHELL_LEG_BALANCE, +            .balance_sw_down = CHASSIS_MODE_BALANCE_ROTOR, #endif - }, - }, +        }, +    }, - /* USER CODE END robot_config */ +    /* USER CODE END robot_config */ }; /* Private function prototypes ---------------------------------------------- */ /* Exported functions ------------------------------------------------------- */ /** - * @brief 获取机器人配置参数 - * @return 机器人配置参数指针 - */ + * @brief 获取机器人配置参数 + * @return 机器人配置参数指针 + */ Config_RobotParam_t* Config_GetRobotParam(void) { - return &robot_config; -} \ No newline at end of file +    return &robot_config; +} From 9ee45df660fbfed332ed478b71faec4556d9ea62 Mon Sep 17 00:00:00 2001 From: Robofish <1683502971@qq.com> Date: Sun, 15 Mar 2026 17:20:57 +0800 Subject: [PATCH 19/34] fix config --- User/module/config.c | 916 +++++++++++++++++++++---------------------- 1 file changed, 458 insertions(+), 458 deletions(-) diff --git a/User/module/config.c b/User/module/config.c index b62cdc5..c8a9591 100644 --- a/User/module/config.c +++ b/User/module/config.c @@ -1,6 +1,6 @@ /* - * 配置相关 - */ + * 配置相关 + */ /* Includes ----------------------------------------------------------------- */ #include "module/config.h" @@ -15,494 +15,494 @@ /* Exported variables ------------------------------------------------------- */ /** - * @brief 机器人参数配置 - * @note 在此配置机器人参数 - */ + * @brief 机器人参数配置 + * @note 在此配置机器人参数 + */ Config_RobotParam_t robot_config = { -    /* USER CODE BEGIN robot_config */ -        .ref_screen={ -                    .width=1920, -                    .height=1080, -      }, -    .gimbal_param = { -        .pid = { -            .yaw_omega = { -                .k = 0.25f, -                .p = 1.0f, -                .i = 0.0f, -                .d = 0.0f, -                .i_limit = 1.0f, -                .out_limit = 1.0f, -                .d_cutoff_freq = -1.0f, -                .range = -1.0f, -            }, -            .yaw_angle = { -                .k = 7.0f, -                .p = 3.5f, -                .i = 0.0f, -                .d = 0.0f, -                .i_limit = 0.0f, -                .out_limit = 10.0f, -                .d_cutoff_freq = -1.0f, -                .range = M_2PI, -            }, -            .pit_omega = { -                .k = 0.25f, -                .p = 1.0f, -                .i = 0.0f, -                .d = 0.0f, -                .i_limit = 1.0f, -                .out_limit = 1.0f, -                .d_cutoff_freq = -1.0f, -                .range = -1.0f, -            }, -            .pit_angle = { -                .k = 2.5f, -                .p = 5.0f, -                .i = 0.2f, -                .d = 0.01f, -                .i_limit = 0.0f, -                .out_limit = 10.0f, -                .d_cutoff_freq = -1.0f, -                .range = M_2PI, -            }, -        }, -        .mech_zero = { -            .yaw = 0.0f, -            .pit = 3.23056364f, -        }, -        .travel = { -            .yaw = -1.0f, -            .pit = 0.85f, -        }, -        .low_pass_cutoff_freq = { -            .out = -1.0f, -            .gyro = 1000.0f, -        }, -        .pit_motor ={ -            .can = BSP_CAN_1, -            .id = 0x206, -            .gear = false, -            .module = MOTOR_GM6020, -            .reverse = true, -        }, -        .yaw_motor = { -            .can = BSP_CAN_1, -            .can_id = 0x1, -            .master_id = 0x11, -            .module = MOTOR_DM_J4310, -            .reverse = false, -        }, -        .imu = { -            .can = BSP_CAN_2, -            .accl_id = 0x100,  // 加速度计 (十进制256) -            .gyro_id = 0x101,  // 陀螺仪 (十进制257) -            .eulr_id = 0x102,  // 欧拉角 (十进制258) -            .quat_id = 0x103   // 四元数 (十进制259) -        }, -        -    }, + /* USER CODE BEGIN robot_config */ + .ref_screen={ + .width=1920, + .height=1080, + }, + .gimbal_param = { + .pid = { + .yaw_omega = { + .k = 0.25f, + .p = 1.0f, + .i = 0.0f, + .d = 0.0f, + .i_limit = 1.0f, + .out_limit = 1.0f, + .d_cutoff_freq = -1.0f, + .range = -1.0f, + }, + .yaw_angle = { + .k = 7.0f, + .p = 3.5f, + .i = 0.0f, + .d = 0.0f, + .i_limit = 0.0f, + .out_limit = 10.0f, + .d_cutoff_freq = -1.0f, + .range = M_2PI, + }, + .pit_omega = { + .k = 0.25f, + .p = 1.0f, + .i = 0.0f, + .d = 0.0f, + .i_limit = 1.0f, + .out_limit = 1.0f, + .d_cutoff_freq = -1.0f, + .range = -1.0f, + }, + .pit_angle = { + .k = 2.5f, + .p = 5.0f, + .i = 0.2f, + .d = 0.01f, + .i_limit = 0.0f, + .out_limit = 10.0f, + .d_cutoff_freq = -1.0f, + .range = M_2PI, + }, + }, + .mech_zero = { + .yaw = 0.0f, + .pit = 3.23056364f, + }, + .travel = { + .yaw = -1.0f, + .pit = 0.85f, + }, + .low_pass_cutoff_freq = { + .out = -1.0f, + .gyro = 1000.0f, + }, + .pit_motor ={ + .can = BSP_CAN_1, + .id = 0x206, + .gear = false, + .module = MOTOR_GM6020, + .reverse = true, + }, + .yaw_motor = { + .can = BSP_CAN_1, + .can_id = 0x1, + .master_id = 0x11, + .module = MOTOR_DM_J4310, + .reverse = false, + }, + .imu = { + .can = BSP_CAN_2, + .accl_id = 0x100, // 加速度计 (十进制256) + .gyro_id = 0x101, // 陀螺仪 (十进制257) + .eulr_id = 0x102, // 欧拉角 (十进制258) + .quat_id = 0x103 // 四元数 (十进制259) + }, + + }, -    .shoot_param = { -        .basic={ -          .projectileType=SHOOT_PROJECTILE_17MM, -              .fric_num=2, -              .extra_deceleration_ratio=1.0f, -          .num_trig_tooth=8, -          .shot_freq=1.0f, -          .shot_burst_num=3, -          .ratio_multilevel = {1.0f}, -        },   -        .jamDetection={ -            .enable=true, -            .threshold=200.0f, -            .suspectedTime=0.5f, -        }, -        .heatControl={ -            .enable=true, -            .nmax=18.0f,      // 最大射频 Hz -            .Hwarn=200.0f,    // 热量预警值 -            .Hsatu=100.0f,    // 热量饱和值 -            .Hthres=50.0f,    // 热量阈值 -        }, -        .motor={ -            .fric = { -              { -                .param = { -                  .can = BSP_CAN_1, -                  .id = 0x201, -                  .module = MOTOR_M3508, -                  .reverse = false, -                  .gear=false, -                }, -              .level=1, -            }, -              { -                .param = { -                  .can = BSP_CAN_1, -                  .id = 0x202, -                  .module = MOTOR_M3508, -                  .reverse = true, -                  .gear=false, -                }, -              .level=1, -            } -            }, -            .trig = { -                .can = BSP_CAN_1, -                .id = 0x203, -                .module = MOTOR_M2006, -                .reverse = true, -                .gear=true, -            }, -        }, -        .pid={       -            .fric_follow = { -                .k=1.0f, -                .p=1.5f, -                .i=0.0f, -                .d=0.0f, -                .i_limit=0.0f, -                .out_limit=0.9f, -                .d_cutoff_freq=30.0f, -                .range=-1.0f, -            }, -            .fric_err = { -                .k=0.0f, -                .p=0.0f, -                .i=0.0f, -                .d=0.0f, -                .i_limit=0.0f, -                .out_limit=0.0f, -                .d_cutoff_freq=0.0f, -                .range=-1.0f, -            }, -            .trig_2006 = { -                .k=1.0f, -                .p=1.0f, -                .i=0.1f, -                .d=0.05f, -                .i_limit=0.8f, -                .out_limit=0.5f, -                .d_cutoff_freq=-1.0f, -                .range=M_2PI, -            }, -            .trig_omg_2006 = { -                .k=1.0f, -                .p=1.5f, -                .i=0.3f, -                .d=0.5f, -                .i_limit=0.2f, -                .out_limit=0.9f, -                .d_cutoff_freq=-1.0f, -                .range=-1.0f, -            }, -            .trig_3508 = { -                .k=0.5f, -                .p=1.8f, -                .i=0.3f, -                .d=0.1f, -                .i_limit=0.15f, -                .out_limit=1.0f, -                .d_cutoff_freq=-1.0f, -                .range=M_2PI, -            }, -            .trig_omg_3508 = { -                .k=1.0f, -                .p=1.0f, -                .i=0.0f, -                .d=0.0f, -                .i_limit=0.0f, -                .out_limit=1.0f, -                .d_cutoff_freq=-1.0f, -                .range=-1.0f, -            }, -        }, -        .filter={ -            .fric = { -                .in = 30.0f, -                .out = 30.0f, -            }, -            .trig = { -                .in = 30.0f, -                .out = 30.0f, -            }, -        },   -    }, -    .chassis_param = { -        .yaw={ -            .k=1.0f, -            .p=1.0f, -            .i=0.0f, -            .d=0.3f, -            .i_limit=0.0f, -            .out_limit=1.0f, -            .d_cutoff_freq=30.0f, -            .range=M_2PI, /* 2*PI，用于处理角度循环误差 */ -        }, -        -        .rotor={ -            .k=1.0f, -            .p=0.5f, -            .i=0.0f, -            .d=0.01f, -            .i_limit=0.0f, -            .out_limit=1.0f, -            .d_cutoff_freq=30.0f, -            .range=-1.0f, /* 角速度不需要循环误差处理 */ -        }, -        -        .roll={ -            .k=0.15f, -            .p=0.3f,           /* 横滚角腿长补偿比例系数 (降低避免抖动) */ -            .i=0.0f,           /* 横滚角腿长补偿积分系数 (关闭避免震荡) */ -            .d=0.01f,           /* 横滚角腿长补偿微分系数 (关闭避免噪声放大) */ -            .i_limit=0.0f,     /* 积分限幅 */ -            .out_limit=0.03f,  /* 输出限幅，腿长差值限制(m) 降低避免过度补偿 */ -            .d_cutoff_freq=30.0f, /* 微分截止频率 */ -            .range=-1.0f,      /* 不使用循环误差处理 */ -        }, -        -        .roll_force={ -            .k=5.0f, -            .p=10.0f,          /* 横滚角力补偿比例系数 (大幅降低避免抖动) */ -            .i=0.0f,           /* 横滚角力补偿积分系数 */ -            .d=5.0f,           /* 横滚角力补偿微分系数 (降低避免高频抖动) */ -            .i_limit=0.0f,     /* 积分限幅 */ -            .out_limit=20.0f,  /* 输出限幅，力补偿限制(N) 降低避免过度补偿 */ -            .d_cutoff_freq=30.0f, /* 微分截止频率 */ -            .range=-1.0f,      /* 不使用循环误差处理 */ -        }, + .shoot_param = { + .basic={ + .projectileType=SHOOT_PROJECTILE_17MM, + .fric_num=2, + .extra_deceleration_ratio=1.0f, + .num_trig_tooth=8, + .shot_freq=1.0f, + .shot_burst_num=3, + .ratio_multilevel = {1.0f}, + }, + .jamDetection={ + .enable=true, + .threshold=200.0f, + .suspectedTime=0.5f, + }, + .heatControl={ + .enable=true, + .nmax=18.0f, // 最大射频 Hz + .Hwarn=200.0f, // 热量预警值 + .Hsatu=100.0f, // 热量饱和值 + .Hthres=50.0f, // 热量阈值 + }, + .motor={ + .fric = { + { + .param = { + .can = BSP_CAN_1, + .id = 0x201, + .module = MOTOR_M3508, + .reverse = false, + .gear=false, + }, + .level=1, + }, + { + .param = { + .can = BSP_CAN_1, + .id = 0x202, + .module = MOTOR_M3508, + .reverse = true, + .gear=false, + }, + .level=1, + } + }, + .trig = { + .can = BSP_CAN_1, + .id = 0x203, + .module = MOTOR_M2006, + .reverse = true, + .gear=true, + }, + }, + .pid={ + .fric_follow = { + .k=1.0f, + .p=1.5f, + .i=0.0f, + .d=0.0f, + .i_limit=0.0f, + .out_limit=0.9f, + .d_cutoff_freq=30.0f, + .range=-1.0f, + }, + .fric_err = { + .k=0.0f, + .p=0.0f, + .i=0.0f, + .d=0.0f, + .i_limit=0.0f, + .out_limit=0.0f, + .d_cutoff_freq=0.0f, + .range=-1.0f, + }, + .trig_2006 = { + .k=1.0f, + .p=1.0f, + .i=0.1f, + .d=0.05f, + .i_limit=0.8f, + .out_limit=0.5f, + .d_cutoff_freq=-1.0f, + .range=M_2PI, + }, + .trig_omg_2006 = { + .k=1.0f, + .p=1.5f, + .i=0.3f, + .d=0.5f, + .i_limit=0.2f, + .out_limit=0.9f, + .d_cutoff_freq=-1.0f, + .range=-1.0f, + }, + .trig_3508 = { + .k=0.5f, + .p=1.8f, + .i=0.3f, + .d=0.1f, + .i_limit=0.15f, + .out_limit=1.0f, + .d_cutoff_freq=-1.0f, + .range=M_2PI, + }, + .trig_omg_3508 = { + .k=1.0f, + .p=1.0f, + .i=0.0f, + .d=0.0f, + .i_limit=0.0f, + .out_limit=1.0f, + .d_cutoff_freq=-1.0f, + .range=-1.0f, + }, + }, + .filter={ + .fric = { + .in = 30.0f, + .out = 30.0f, + }, + .trig = { + .in = 30.0f, + .out = 30.0f, + }, + }, + }, + .chassis_param = { + .yaw={ + .k=1.0f, + .p=1.0f, + .i=0.0f, + .d=0.3f, + .i_limit=0.0f, + .out_limit=1.0f, + .d_cutoff_freq=30.0f, + .range=M_2PI, /* 2*PI，用于处理角度循环误差 */ + }, + + .rotor={ + .k=1.0f, + .p=0.5f, + .i=0.0f, + .d=0.01f, + .i_limit=0.0f, + .out_limit=1.0f, + .d_cutoff_freq=30.0f, + .range=-1.0f, /* 角速度不需要循环误差处理 */ + }, + + .roll={ + .k=0.15f, + .p=0.3f, /* 横滚角腿长补偿比例系数 (降低避免抖动) */ + .i=0.0f, /* 横滚角腿长补偿积分系数 (关闭避免震荡) */ + .d=0.01f, /* 横滚角腿长补偿微分系数 (关闭避免噪声放大) */ + .i_limit=0.0f, /* 积分限幅 */ + .out_limit=0.03f, /* 输出限幅，腿长差值限制(m) 降低避免过度补偿 */ + .d_cutoff_freq=30.0f, /* 微分截止频率 */ + .range=-1.0f, /* 不使用循环误差处理 */ + }, + + .roll_force={ + .k=5.0f, + .p=10.0f, /* 横滚角力补偿比例系数 (大幅降低避免抖动) */ + .i=0.0f, /* 横滚角力补偿积分系数 */ + .d=5.0f, /* 横滚角力补偿微分系数 (降低避免高频抖动) */ + .i_limit=0.0f, /* 积分限幅 */ + .out_limit=20.0f, /* 输出限幅，力补偿限制(N) 降低避免过度补偿 */ + .d_cutoff_freq=30.0f, /* 微分截止频率 */ + .range=-1.0f, /* 不使用循环误差处理 */ + }, -        .leg_length={ -            .k = 40.0f, -            .p = 50.0f, -            .i = 0.01f, -            .d = 2.0f, -            .i_limit = 0.0f, -            .out_limit = 500.0f, -            .d_cutoff_freq = -1.0f, -            .range = -1.0f, -        }, + .leg_length={ + .k = 40.0f, + .p = 50.0f, + .i = 0.01f, + .d = 2.0f, + .i_limit = 0.0f, + .out_limit = 500.0f, + .d_cutoff_freq = -1.0f, + .range = -1.0f, + }, -        .leg_theta={ -            .k=5.0f, -            .p=2.0f,           /* 摆角比例系数 */ -            .i=0.0f,           /* 摆角积分系数 */ -            .d=0.0f,           /* 摆角微分系数 */ -            .i_limit=0.0f,     /* 积分限幅 */ -            .out_limit=0.05f,  /* 输出限幅，腿长差值限制 */ -            .d_cutoff_freq=30.0f, /* 微分截止频率 */ -            .range=-1.0f,      /* 不使用循环误差处理 */ -        }, + .leg_theta={ + .k=5.0f, + .p=2.0f, /* 摆角比例系数 */ + .i=0.0f, /* 摆角积分系数 */ + .d=0.0f, /* 摆角微分系数 */ + .i_limit=0.0f, /* 积分限幅 */ + .out_limit=0.05f, /* 输出限幅，腿长差值限制 */ + .d_cutoff_freq=30.0f, /* 微分截止频率 */ + .range=-1.0f, /* 不使用循环误差处理 */ + }, -        .tp={ -            .k=4.0f, -            .p=3.0f,          /* 俯仰角比例系数 */ -            .i=0.0f,           /* 俯仰角积分系数 */ -            .d=0.0f,           /* 俯仰角微分系数 */ -            .i_limit=0.0f,     /* 积分限幅 */ -            .out_limit=10.0f,   /* 输出限幅，腿长差值限制 */ -            .d_cutoff_freq=30.0f, /* 微分截止频率 */ -            .range=-1.0f,      /* 不使用循环误差处理 */ -        }, -        -        .low_pass_cutoff_freq = { -            .in = 30.0f, -            .out = 30.0f, -        }, + .tp={ + .k=4.0f, + .p=3.0f, /* 俯仰角比例系数 */ + .i=0.0f, /* 俯仰角积分系数 */ + .d=0.0f, /* 俯仰角微分系数 */ + .i_limit=0.0f, /* 积分限幅 */ + .out_limit=10.0f, /* 输出限幅，腿长差值限制 */ + .d_cutoff_freq=30.0f, /* 微分截止频率 */ + .range=-1.0f, /* 不使用循环误差处理 */ + }, + + .low_pass_cutoff_freq = { + .in = 30.0f, + .out = 30.0f, + }, -        .yaw_motor = { // 云台Yaw轴电机 -            .can = BSP_CAN_1, -            .can_id = 0x1, -            .master_id = 0x11, -            .module = MOTOR_DM_J4310, -            .reverse = false, -        }, -        -        .joint_param = { -            { // 左髋关节 -                .can = BSP_CAN_3, -                .motor_id = 124, -                .host_id = 0xFF, -                .module = MOTOR_LZ_RSO3, -                .reverse = false, -                .mode = MOTOR_LZ_MODE_MOTION, -                }, -            { // 右髋关节 -                .can = BSP_CAN_3, -                .motor_id = 125, -                .host_id = 0xFF, -                .module = MOTOR_LZ_RSO3, -                .reverse = false, -                .mode = MOTOR_LZ_MODE_MOTION, -                }, -            { // 左膝关节 -                 .can = BSP_CAN_3, -                .motor_id = 126, -                .host_id = 0xFF, -                .module = MOTOR_LZ_RSO3, -                .reverse = true, -                .mode = MOTOR_LZ_MODE_MOTION, -                }, -            { // 右膝关节 -                .can = BSP_CAN_3, -                .motor_id = 127, -                .host_id = 0xFF, -                .module = MOTOR_LZ_RSO3, -                .reverse = true, -                .mode = MOTOR_LZ_MODE_MOTION, -                }, -        }, -        -        .wheel_param = { -            { // 左轮电机 -                .can = BSP_CAN_3, -                .id = 0x142, -                .module = MOTOR_LK_MF9025, -                .reverse = false, -            }, -            { // 右轮电机 -                .can = BSP_CAN_3, -                .id = 0x141, -                .module = MOTOR_LK_MF9025, -                .reverse = true, -            }, -        }, + .yaw_motor = { // 云台Yaw轴电机 + .can = BSP_CAN_1, + .can_id = 0x1, + .master_id = 0x11, + .module = MOTOR_DM_J4310, + .reverse = false, + }, + + .joint_param = { + { // 左髋关节 + .can = BSP_CAN_3, + .motor_id = 124, + .host_id = 0xFF, + .module = MOTOR_LZ_RSO3, + .reverse = false, + .mode = MOTOR_LZ_MODE_MOTION, + }, + { // 右髋关节 + .can = BSP_CAN_3, + .motor_id = 125, + .host_id = 0xFF, + .module = MOTOR_LZ_RSO3, + .reverse = false, + .mode = MOTOR_LZ_MODE_MOTION, + }, + { // 左膝关节 + .can = BSP_CAN_3, + .motor_id = 126, + .host_id = 0xFF, + .module = MOTOR_LZ_RSO3, + .reverse = true, + .mode = MOTOR_LZ_MODE_MOTION, + }, + { // 右膝关节 + .can = BSP_CAN_3, + .motor_id = 127, + .host_id = 0xFF, + .module = MOTOR_LZ_RSO3, + .reverse = true, + .mode = MOTOR_LZ_MODE_MOTION, + }, + }, + + .wheel_param = { + { // 左轮电机 + .can = BSP_CAN_3, + .id = 0x142, + .module = MOTOR_LK_MF9025, + .reverse = false, + }, + { // 右轮电机 + .can = BSP_CAN_3, + .id = 0x141, + .module = MOTOR_LK_MF9025, + .reverse = true, + }, + }, -        .mech = { -            .wheel_radius = 0.068f, -            .wheel_gear_ratio = 4.5f, -            .hip_width = 0.423f,           /* 髋宽，即两腿间距 (m)，用于Roll几何补偿 */ -            .joint_zero = {0.0f, 0.0f, 1.33022018f, 1.13195965f}, /* 关节电机零点偏移 */ -            .mech_zero_yaw = 2.96925735f,  /* 机械零点 */ -        }, + .mech = { + .wheel_radius = 0.068f, + .wheel_gear_ratio = 4.5f, + .hip_width = 0.423f, /* 髋宽，即两腿间距 (m)，用于Roll几何补偿 */ + .joint_zero = {0.0f, 0.0f, 1.33022018f, 1.13195965f}, /* 关节电机零点偏移 */ + .mech_zero_yaw = 2.96925735f, /* 机械零点 */ + }, -        .leg = { -            .base_length = 0.16f,          /* 基础腿长 (m) */ -            .length_range = 0.20f,         /* 腿长调节范围 (m) */ -            .min_length = 0.14f,           /* 最小腿长 (m) */ -            .max_length = 0.36f,           /* 最大腿长 (m) */ -            .left_base_force = 65.0f,      /* 左腿基础支撑力 (N) */ -            .right_base_force = 65.0f,     /* 右腿基础支撑力 (N) */ -        }, + .leg = { + .base_length = 0.16f, /* 基础腿长 (m) */ + .length_range = 0.20f, /* 腿长调节范围 (m) */ + .min_length = 0.14f, /* 最小腿长 (m) */ + .max_length = 0.36f, /* 最大腿长 (m) */ + .left_base_force = 65.0f, /* 左腿基础支撑力 (N) */ + .right_base_force = 65.0f, /* 右腿基础支撑力 (N) */ + }, -        .motion = { -            .max_acceleration = 5.0f,      /* 最大加速度 (m/s^2) */ -            .non_contact_theta = -0.01f,    /* 离地摆角目标 (rad) */ -        }, + .motion = { + .max_acceleration = 5.0f, /* 最大加速度 (m/s^2) */ + .non_contact_theta = -0.01f, /* 离地摆角目标 (rad) */ + }, -        .rotor_ctrl = { -            .max_spin_speed = 5.0236f,    /* 最大旋转角速度 (rad/s)，0.5rpm */ -            .max_trans_speed = 1.5f,       /* 小陀螺时最大平移速度 (m/s) */ -            .spin_accel = 5.0f,             -            -            -            -            -            -            /* 旋转加速度 (rad/s²) */ -            .spin_decel = 8.0f,            /* 退出减速度 (rad/s²)，比加速更快 */ -            .align_threshold = 0.15f,      /* 退出对齐阈值 (rad)，约8.6° */ -        }, + .rotor_ctrl = { + .max_spin_speed = 5.0236f, /* 最大旋转角速度 (rad/s)，0.5rpm */ + .max_trans_speed = 1.5f, /* 小陀螺时最大平移速度 (m/s) */ + .spin_accel = 5.0f, + + + + + + /* 旋转加速度 (rad/s²) */ + .spin_decel = 8.0f, /* 退出减速度 (rad/s²)，比加速更快 */ + .align_threshold = 0.15f, /* 退出对齐阈值 (rad)，约8.6° */ + }, -        .vmc_param = { -            { // 左腿 -                .leg_1 = 0.215f,      // 后髋连杆长度 (L1) (m) -                .leg_2 = 0.258f,      // 后膝连杆长度 (L2) (m) -                .leg_3 = 0.258f,      // 前膝连杆长度 (L3) (m) -                .leg_4 = 0.215f,      // 前髋连杆长度 (L4) (m) -                .hip_length = 0.0f    // 髋宽 (L5) (m) -            }, -            { // 右腿 -                .leg_1 = 0.215f,      // 后髋连杆长度 (L1) (m) -                .leg_2 = 0.258f,      // 后膝连杆长度 (L2) (m) -                .leg_3 = 0.258f,      // 前膝连杆长度 (L3) (m) -                .leg_4 = 0.215f,      // 前髋连杆长度 (L4) (m) -                .hip_length = 0.0f    // 髋宽 (L5) (m) -            } -        }, -    .lqr_gains = { -        .k11_coeff = { -1.997928012118380e+02f, 2.358970856133577e+02f, -1.221432656066952e+02f, -3.660884265594530e+00f }, // theta -        .k12_coeff = { 1.144642140244768e+00f, -6.781143989723293e-01f, -8.286867905694857e+00f, -3.233743818654922e-01f },  // d_theta -        .k13_coeff = { -2.920908857554099e+01f, 3.294053235814564e+01f, -1.331127106026891e+01f, -2.036633181450379e+00f },  // x -        .k14_coeff = { -2.202395151966686e+01f, 2.624258394298977e+01f, -1.238392622003398e+01f, -2.516594294742349e+00f },  // d_x -        .k15_coeff = { -5.531234648285231e+01f, 7.666196018790744e+01f, -4.254301644877548e+01f, 1.191958597928930e+01f }, // phi -        .k16_coeff = { -8.603393557876432e+00f, 1.181868681749069e+01f, -6.519358011847959e+00f, 2.002836931783026e+00f },  // d_phi -        .k21_coeff = { 3.121841964126664e+02f, -2.644789946321499e+02f, 5.653973783920610e+01f, 1.306067415926613e+01f }, // theta -        .k22_coeff = { 9.751578045310433e+00f, -8.174866581419979e+00f, 1.060040492386880e+00f, 1.945460420856203e+00f },  // d_theta -        .k23_coeff = { -1.589159892148102e+01f, 3.771040781828523e+01f, -2.789168930865428e+01f, 8.354369470743295e+00f },  // x -        .k24_coeff = { -3.304075197263637e+01f, 5.411818719230526e+01f, -3.342861600971858e+01f, 9.256855471266778e+00f },  // d_x -        .k25_coeff = { 2.574491871362636e+02f, -2.771545105998671e+02f, 1.074894289176479e+02f, 3.130550754680395e+00f }, // phi -        .k26_coeff = { 4.162978395880715e+01f, -4.533986766238992e+01f, 1.794589550749570e+01f, 2.032213740678163e-01f },  // d_phi -    }, + .vmc_param = { + { // 左腿 + .leg_1 = 0.215f, // 后髋连杆长度 (L1) (m) + .leg_2 = 0.258f, // 后膝连杆长度 (L2) (m) + .leg_3 = 0.258f, // 前膝连杆长度 (L3) (m) + .leg_4 = 0.215f, // 前髋连杆长度 (L4) (m) + .hip_length = 0.0f // 髋宽 (L5) (m) + }, + { // 右腿 + .leg_1 = 0.215f, // 后髋连杆长度 (L1) (m) + .leg_2 = 0.258f, // 后膝连杆长度 (L2) (m) + .leg_3 = 0.258f, // 前膝连杆长度 (L3) (m) + .leg_4 = 0.215f, // 前髋连杆长度 (L4) (m) + .hip_length = 0.0f // 髋宽 (L5) (m) + } + }, + .lqr_gains = { + .k11_coeff = { -1.997928012118380e+02f, 2.358970856133577e+02f, -1.221432656066952e+02f, -3.660884265594530e+00f }, // theta + .k12_coeff = { 1.144642140244768e+00f, -6.781143989723293e-01f, -8.286867905694857e+00f, -3.233743818654922e-01f }, // d_theta + .k13_coeff = { -2.920908857554099e+01f, 3.294053235814564e+01f, -1.331127106026891e+01f, -2.036633181450379e+00f }, // x + .k14_coeff = { -2.202395151966686e+01f, 2.624258394298977e+01f, -1.238392622003398e+01f, -2.516594294742349e+00f }, // d_x + .k15_coeff = { -5.531234648285231e+01f, 7.666196018790744e+01f, -4.254301644877548e+01f, 1.191958597928930e+01f }, // phi + .k16_coeff = { -8.603393557876432e+00f, 1.181868681749069e+01f, -6.519358011847959e+00f, 2.002836931783026e+00f }, // d_phi + .k21_coeff = { 3.121841964126664e+02f, -2.644789946321499e+02f, 5.653973783920610e+01f, 1.306067415926613e+01f }, // theta + .k22_coeff = { 9.751578045310433e+00f, -8.174866581419979e+00f, 1.060040492386880e+00f, 1.945460420856203e+00f }, // d_theta + .k23_coeff = { -1.589159892148102e+01f, 3.771040781828523e+01f, -2.789168930865428e+01f, 8.354369470743295e+00f }, // x + .k24_coeff = { -3.304075197263637e+01f, 5.411818719230526e+01f, -3.342861600971858e+01f, 9.256855471266778e+00f }, // d_x + .k25_coeff = { 2.574491871362636e+02f, -2.771545105998671e+02f, 1.074894289176479e+02f, 3.130550754680395e+00f }, // phi + .k26_coeff = { 4.162978395880715e+01f, -4.533986766238992e+01f, 1.794589550749570e+01f, 2.032213740678163e-01f }, // d_phi + }, -    .jump = { -        .crouch_time_ms = 500, -        .launch_time_ms = 300, -        .retract_time_ms = 500, -        .land_time_ms = 500, -        .crouch_leg_length = 0.13f,    /* 不用蹲太低 */ -        .launch_force = 20.0f,         /* 起跳力减小 */ -        .retract_leg_length = 0.12f,   /* 收腿目标 */ -        .retract_force = -20.0f,       /* 收腿前馈力减小 */ -    }, + .jump = { + .crouch_time_ms = 500, + .launch_time_ms = 300, + .retract_time_ms = 500, + .land_time_ms = 500, + .crouch_leg_length = 0.13f, /* 不用蹲太低 */ + .launch_force = 20.0f, /* 起跳力减小 */ + .retract_leg_length = 0.12f, /* 收腿目标 */ + .retract_force = -20.0f, /* 收腿前馈力减小 */ + }, -    .lqr_offset = { -        .theta = 0.0f, -        .x = 0.0f, -        .phi = 0.0f, -    }, -    -    }, + .lqr_offset = { + .theta = 0.0f, + .x = 0.0f, + .phi = 0.0f, + }, + + }, -    .ai_param = { -        .can = BSP_FDCAN_2, -        .vision_id = 0x104, -    }, + .ai_param = { + .can = BSP_FDCAN_2, + .vision_id = 0x104, + }, -    .cmd_param = { -        .source_priority = { + .cmd_param = { + .source_priority = { #if CMD_ENABLE_SRC_RC -            [0] = CMD_SRC_RC, + [0] = CMD_SRC_RC, #endif #if CMD_ENABLE_SRC_PC -            [1] = CMD_SRC_PC, + [1] = CMD_SRC_PC, #endif -        }, -        .sensitivity = { -            .mouse_sens     = 60.0f, -            .move_sens      = 1.0f, -            .move_fast_mult = 2.0f, -            .move_slow_mult = 0.4f, -        }, -        .rc_mode_map = { + }, + .sensitivity = { + .mouse_sens = 60.0f, + .move_sens = 1.0f, + .move_fast_mult = 2.0f, + .move_slow_mult = 0.4f, + }, + .rc_mode_map = { #if CMD_ENABLE_MODULE_GIMBAL -            .gimbal_sw_up   = GIMBAL_MODE_RELAX, -            .gimbal_sw_mid  = GIMBAL_MODE_RELATIVE, -            .gimbal_sw_down = GIMBAL_MODE_RELATIVE, + .gimbal_sw_up = GIMBAL_MODE_RELAX, + .gimbal_sw_mid = GIMBAL_MODE_RELATIVE, + .gimbal_sw_down = GIMBAL_MODE_RELATIVE, #endif #if CMD_ENABLE_MODULE_BALANCE_CHASSIS -            .balance_sw_up   = CHASSIS_MODE_RELAX, -            .balance_sw_mid  = CHASSIS_MODE_WHELL_LEG_BALANCE, -            .balance_sw_down = CHASSIS_MODE_BALANCE_ROTOR, + .balance_sw_up = CHASSIS_MODE_RELAX, + .balance_sw_mid = CHASSIS_MODE_WHELL_LEG_BALANCE, + .balance_sw_down = CHASSIS_MODE_BALANCE_ROTOR, #endif -        }, -    }, + }, + }, -    /* USER CODE END robot_config */ + /* USER CODE END robot_config */ }; /* Private function prototypes ---------------------------------------------- */ /* Exported functions ------------------------------------------------------- */ /** - * @brief 获取机器人配置参数 - * @return 机器人配置参数指针 - */ + * @brief 获取机器人配置参数 + * @return 机器人配置参数指针 + */ Config_RobotParam_t* Config_GetRobotParam(void) { -    return &robot_config; + return &robot_config; } From 2427e36bba566706d27cd4d975a111a32cfaa449 Mon Sep 17 00:00:00 2001 From: Robofish <1683502971@qq.com> Date: Sun, 15 Mar 2026 18:37:38 +0800 Subject: [PATCH 20/34] fix 1 --- User/module/balance_chassis.c | 2 +- User/module/config.c | 7 ++----- 2 files changed, 3 insertions(+), 6 deletions(-) diff --git a/User/module/balance_chassis.c b/User/module/balance_chassis.c index f35a311..fe5873e 100644 --- a/User/module/balance_chassis.c +++ b/User/module/balance_chassis.c @@ -771,7 +771,7 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) { float gimbal_offset = c->feedback.yaw.rotor_abs_angle - c->param->mech.mech_zero_yaw; float world_vx = c_cmd->move_vec.vx * c->param->rotor_ctrl.max_trans_speed; float world_vy = c_cmd->move_vec.vy * c->param->rotor_ctrl.max_trans_speed; - c->chassis_state.target_velocity_x = world_vx * cosf(gimbal_offset) + world_vy * sinf(gimbal_offset); + c->chassis_state.target_velocity_x = world_vx * sinf(gimbal_offset) - world_vy * cosf(gimbal_offset); c->chassis_state.last_target_velocity_x = c->chassis_state.target_velocity_x; c->chassis_state.target_x += c->chassis_state.target_velocity_x * c->dt; } else { diff --git a/User/module/config.c b/User/module/config.c index c8a9591..5377bf0 100644 --- a/User/module/config.c +++ b/User/module/config.c @@ -395,14 +395,11 @@ Config_RobotParam_t robot_config = { }, .rotor_ctrl = { - .max_spin_speed = 5.0236f, /* 最大旋转角速度 (rad/s)，0.5rpm */ + .max_spin_speed = 6.0f, /* 最大旋转角速度 (rad/s)，0.5rpm */ .max_trans_speed = 1.5f, /* 小陀螺时最大平移速度 (m/s) */ .spin_accel = 5.0f, - - - /* 旋转加速度 (rad/s²) */ .spin_decel = 8.0f, /* 退出减速度 (rad/s²)，比加速更快 */ .align_threshold = 0.15f, /* 退出对齐阈值 (rad)，约8.6° */ @@ -454,7 +451,7 @@ Config_RobotParam_t robot_config = { .lqr_offset = { .theta = 0.0f, .x = 0.0f, - .phi = 0.0f, + .phi = 0.01f, }, }, From 513254e56fd38f86df484bd79a967f6b1c4ad72a Mon Sep 17 00:00:00 2001 From: Robofish <1683502971@qq.com> Date: Sun, 15 Mar 2026 18:42:50 +0800 Subject: [PATCH 21/34] =?UTF-8?q?fix=20=E6=96=B9=E5=90=91?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- User/module/balance_chassis.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/User/module/balance_chassis.c b/User/module/balance_chassis.c index fe5873e..5c45b69 100644 --- a/User/module/balance_chassis.c +++ b/User/module/balance_chassis.c @@ -771,7 +771,7 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) { float gimbal_offset = c->feedback.yaw.rotor_abs_angle - c->param->mech.mech_zero_yaw; float world_vx = c_cmd->move_vec.vx * c->param->rotor_ctrl.max_trans_speed; float world_vy = c_cmd->move_vec.vy * c->param->rotor_ctrl.max_trans_speed; - c->chassis_state.target_velocity_x = world_vx * sinf(gimbal_offset) - world_vy * cosf(gimbal_offset); + c->chassis_state.target_velocity_x = -world_vx * sinf(gimbal_offset) + world_vy * cosf(gimbal_offset); c->chassis_state.last_target_velocity_x = c->chassis_state.target_velocity_x; c->chassis_state.target_x += c->chassis_state.target_velocity_x * c->dt; } else { From 6b59d5922d58ca18db8885c60b5256e4b7b8f0d0 Mon Sep 17 00:00:00 2001 From: Robofish <1683502971@qq.com> Date: Sun, 15 Mar 2026 18:52:18 +0800 Subject: [PATCH 22/34] =?UTF-8?q?+=E6=AD=A3=E5=BC=A6?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- User/module/balance_chassis.c | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/User/module/balance_chassis.c b/User/module/balance_chassis.c index 5c45b69..758e3c5 100644 --- a/User/module/balance_chassis.c +++ b/User/module/balance_chassis.c @@ -820,8 +820,9 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) { /* ==================== Yaw轴控制 ==================== */ if (c_cmd->mode == CHASSIS_MODE_BALANCE_ROTOR || c->mode == CHASSIS_MODE_BALANCE_ROTOR) { - /* 小陀螺模式：自动以max_spin_speed恒速旋转 */ - float target_spin = c->param->rotor_ctrl.max_spin_speed; + /* 小陀螺模式：旋转速度在5~8 rad/s之间以0.5Hz正弦变化 */ + float t = (float)BSP_TIME_Get_us() / 1000000.0f; /* 秒 */ + float target_spin = 6.5f + 1.5f * sinf(2.0f * M_PI * 0.5f * t); if (c->rotor_state.exiting) { /* 退出过渡：目标角速度为0，用减速斜坡 */ From 1e7fcdc20b0ae7136acd5ca5bdae6e5a4da767ee Mon Sep 17 00:00:00 2001 From: Robofish <1683502971@qq.com> Date: Sun, 15 Mar 2026 19:28:43 +0800 Subject: [PATCH 23/34] =?UTF-8?q?add=E4=B8=8A=E5=8F=B0=E9=98=B6?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- User/module/balance_chassis.c | 117 ++++++++++++++++++++++++++++++++++ User/module/balance_chassis.h | 27 +++++++- User/module/config.c | 9 ++- 3 files changed, 151 insertions(+), 2 deletions(-) diff --git a/User/module/balance_chassis.c b/User/module/balance_chassis.c index 758e3c5..b597465 100644 --- a/User/module/balance_chassis.c +++ b/User/module/balance_chassis.c @@ -31,6 +31,7 @@ static void Chassis_UpdateChassisState(Chassis_t *c); static void Chassis_ResetControllers(Chassis_t *c); static void Chassis_SelectYawZeroPoint(Chassis_t *c); static void Chassis_JumpControl(Chassis_t *c, const Chassis_CMD_t *c_cmd, float *target_L0, float *extra_force); +static void Chassis_ClimbControl(Chassis_t *c, const Chassis_CMD_t *c_cmd); /* Private functions -------------------------------------------------------- */ @@ -138,6 +139,11 @@ static int8_t Chassis_SetMode(Chassis_t *c, Chassis_Mode_t mode) { if (c == NULL) return CHASSIS_ERR_NULL; if (mode == c->mode) return CHASSIS_OK; + /* 上台阶已完成，忽略重复的CLIMB_STEP请求，需先切别的模式再回来 */ + if (mode == CHASSIS_MODE_CLIMB_STEP && c->climb.completed) { + return CHASSIS_OK; + } + /* RECOVER 进行中时，忽略 BALANCE 指令，等自起完成后自动切换 */ if (c->mode == CHASSIS_MODE_RECOVER && mode == CHASSIS_MODE_WHELL_LEG_BALANCE) { @@ -180,6 +186,19 @@ static int8_t Chassis_SetMode(Chassis_t *c, Chassis_Mode_t mode) { c->rotor_state.exiting = false; } + /* 进入上台阶模式：重置状态机 */ + if (mode == CHASSIS_MODE_CLIMB_STEP) { + c->climb.state = CLIMB_EXTEND; + c->climb.completed = false; + c->climb.state_start_time = (uint32_t)(BSP_TIME_Get_us() / 1000); + } + + /* 离开上台阶模式时重置completed标志 */ + if (c->mode == CHASSIS_MODE_CLIMB_STEP && mode != CHASSIS_MODE_CLIMB_STEP) { + c->climb.completed = false; + c->climb.state = CLIMB_IDLE; + } + c->mode = mode; return CHASSIS_OK; } @@ -419,6 +438,100 @@ static void Chassis_JumpControl(Chassis_t *c, const Chassis_CMD_t *c_cmd, float } } +/** + * @brief 上台阶控制 + * @param c 底盘结构体指针 + * @param c_cmd 控制指令 + * + * @note 流程： + * EXTEND(伸腿前进，削弱前摆力) -> RETRACT(腿后摆超阈值，快速收腿) -> SETTLE(稳定) -> 自动退出到BALANCE + * 单次执行：完成后即使CMD继续发CLIMB_STEP也不重复，需切换到其他模式后再回来 + */ +static void Chassis_ClimbControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) { + uint32_t current_time = (uint32_t)(BSP_TIME_Get_us() / 1000); + uint32_t elapsed_time = current_time - c->climb.state_start_time; + + /* 已完成一次上台阶，自动切回BALANCE */ + if (c->climb.completed) { + c->mode = CHASSIS_MODE_WHELL_LEG_BALANCE; + Chassis_ResetControllers(c); + return; + } + + /* 上台阶状态机中始终运行LQR+VMC保持平衡 */ + Chassis_CMD_t climb_cmd = *c_cmd; + + switch (c->climb.state) { + case CLIMB_IDLE: + /* 不应到达此状态，进入CLIMB_STEP时会设为EXTEND */ + c->climb.state = CLIMB_EXTEND; + c->climb.state_start_time = current_time; + break; + + case CLIMB_EXTEND: { + /* 伸腿前进：腿长最大，给定前进速度，削弱摆力矩让腿自由后摆 */ + climb_cmd.move_vec.vx = c->param->climb.forward_speed / 2.0f; + climb_cmd.height = 1.0f; /* 最大腿长 */ + + /* 运行LQR和VMC */ + Chassis_LQRControl(c, &climb_cmd); + + /* 削弱LQR的摆力矩Tp，让腿撞到台阶后能自由后摆 */ + c->lqr[0].control_output.Tp *= c->param->climb.tp_scale; + c->lqr[1].control_output.Tp *= c->param->climb.tp_scale; + + Chassis_VMCControl(c, &climb_cmd); + Chassis_Output(c); + + /* 检查腿后摆角度：任一腿theta超过阈值则收腿 */ + float avg_theta = (c->vmc_[0].leg.theta + c->vmc_[1].leg.theta) / 2.0f; + if (fabsf(avg_theta) > c->param->climb.theta_retract_threshold) { + c->climb.state = CLIMB_RETRACT; + c->climb.state_start_time = current_time; + } + + /* 超时保护：10秒未触发收腿则放弃 */ + if (elapsed_time > 10000) { + c->climb.completed = true; + } + } break; + + case CLIMB_RETRACT: { + /* 快速收腿，继续前进 */ + climb_cmd.move_vec.vx = c->param->climb.forward_speed / 2.0f; + climb_cmd.height = -1.0f; /* 最小腿长 */ + + Chassis_LQRControl(c, &climb_cmd); + Chassis_VMCControl(c, &climb_cmd); + Chassis_Output(c); + + /* 腿收到位或超时，进入稳定阶段 */ + bool legs_retracted = (c->vmc_[0].leg.L0 < c->param->leg.min_length + 0.02f) && + (c->vmc_[1].leg.L0 < c->param->leg.min_length + 0.02f); + if (legs_retracted || elapsed_time > 1000) { + c->climb.state = CLIMB_SETTLE; + c->climb.state_start_time = current_time; + } + } break; + + case CLIMB_SETTLE: + /* 稳定阶段：恢复正常控制，等待稳定 */ + climb_cmd.height = 0.0f; + Chassis_LQRControl(c, &climb_cmd); + Chassis_VMCControl(c, &climb_cmd); + Chassis_Output(c); + + if (elapsed_time > c->param->climb.settle_time_ms) { + c->climb.completed = true; + } + break; + + default: + c->climb.completed = true; + break; + } +} + /** * @brief 初始化底盘 @@ -715,6 +828,10 @@ int8_t Chassis_Control(Chassis_t *c, const Chassis_CMD_t *c_cmd) { Chassis_Output(c); break; + case CHASSIS_MODE_CLIMB_STEP: + Chassis_ClimbControl(c, c_cmd); + break; + default: return CHASSIS_ERR_MODE; } diff --git a/User/module/balance_chassis.h b/User/module/balance_chassis.h index 51c7a0a..98d0ef3 100644 --- a/User/module/balance_chassis.h +++ b/User/module/balance_chassis.h @@ -42,7 +42,8 @@ typedef enum { CHASSIS_MODE_RECOVER, /* 复位模式 */ // CHASSIS_MODE_CALIBRATE, /* 校准模式 */ CHASSIS_MODE_WHELL_LEG_BALANCE, /* 轮子+腿平衡模式，底盘自我平衡，支持跳跃 */ - CHASSIS_MODE_BALANCE_ROTOR /*小陀螺*/ + CHASSIS_MODE_BALANCE_ROTOR, /*小陀螺*/ + CHASSIS_MODE_CLIMB_STEP /* 上台阶模式 */ } Chassis_Mode_t; /* 跳跃状态枚举 */ @@ -62,11 +63,20 @@ typedef enum { RECOVER_COMPLETE /* 完成：切换到平衡模式 */ } Recover_State_t; +/* 上台阶状态枚举 */ +typedef enum { + CLIMB_IDLE, /* 待机，未触发 */ + CLIMB_EXTEND, /* 伸腿前进：腿长最大，削弱前摆力，等待撞到台阶 */ + CLIMB_RETRACT, /* 收腿：腿后摆超过阈值后快速收腿 */ + CLIMB_SETTLE /* 稳定：收腿完成，短暂稳定后退出 */ +} Climb_State_t; + typedef struct { Chassis_Mode_t mode; /* 底盘模式 */ MoveVector_t move_vec; /* 底盘运动向量 */ float height; /* 目标高度 */ bool jump_trigger; /* 跳跃触发标志 */ + bool climb_trigger; /* 上台阶触发标志 */ } Chassis_CMD_t; typedef struct { @@ -156,6 +166,14 @@ typedef struct { float align_threshold; /* 退出对齐阈值 (rad)，yaw误差小于此值时回到BALANCE模式 */ } rotor_ctrl; + /* 上台阶参数 */ + struct { + float forward_speed; /* 前进速度 (m/s) */ + float theta_retract_threshold; /* 腿后摆收腿阈值 (rad)，约30° = 0.52 */ + float tp_scale; /* 摆力矩缩放系数，<1削弱前摆力 */ + uint32_t settle_time_ms; /* 收腿后稳定时间 (ms) */ + } climb; + /* LQR 目标状态偏移 */ struct { float theta; @@ -231,6 +249,13 @@ typedef struct { Recover_State_t state[2]; /* 左右腿自起状态 */ } recover; + /* 上台阶控制相关 */ + struct { + Climb_State_t state; /* 上台阶状态 */ + bool completed; /* 本次已完成标志，防止重复触发 */ + uint32_t state_start_time; /* 当前状态开始时间 (ms) */ + } climb; + /* 电机离线检测 */ struct { bool any_offline; /* 是否有电机离线 */ diff --git a/User/module/config.c b/User/module/config.c index 5377bf0..61cbef5 100644 --- a/User/module/config.c +++ b/User/module/config.c @@ -448,6 +448,13 @@ Config_RobotParam_t robot_config = { .retract_force = -20.0f, /* 收腿前馈力减小 */ }, + .climb = { + .forward_speed = 1.0f, /* 上台阶前进速度 (m/s) */ + .theta_retract_threshold = 0.52f, /* 腿后摆收腿阈值 (rad)，约30° */ + .tp_scale = 0.1f, /* 摆力矩缩放：削弱到10%，让腿自由后摆 */ + .settle_time_ms = 500, /* 收腿后稳定时间 (ms) */ + }, + .lqr_offset = { .theta = 0.0f, .x = 0.0f, @@ -485,7 +492,7 @@ Config_RobotParam_t robot_config = { #if CMD_ENABLE_MODULE_BALANCE_CHASSIS .balance_sw_up = CHASSIS_MODE_RELAX, .balance_sw_mid = CHASSIS_MODE_WHELL_LEG_BALANCE, - .balance_sw_down = CHASSIS_MODE_BALANCE_ROTOR, + .balance_sw_down = CHASSIS_MODE_CLIMB_STEP, #endif }, }, From 9cd6b85e93771ce3716bbe5070f52fa8ec0f95e0 Mon Sep 17 00:00:00 2001 From: Robofish <1683502971@qq.com> Date: Sun, 15 Mar 2026 20:26:28 +0800 Subject: [PATCH 24/34] fix ju --- User/module/balance_chassis.c | 28 +++++++++++++++++++++++----- 1 file changed, 23 insertions(+), 5 deletions(-) diff --git a/User/module/balance_chassis.c b/User/module/balance_chassis.c index b597465..05ba90e 100644 --- a/User/module/balance_chassis.c +++ b/User/module/balance_chassis.c @@ -137,6 +137,14 @@ static void Chassis_SelectYawZeroPoint(Chassis_t *c) { */ static int8_t Chassis_SetMode(Chassis_t *c, Chassis_Mode_t mode) { if (c == NULL) return CHASSIS_ERR_NULL; + + /* 自动退回BALANCE后，只要用户切到非上台阶模式，就允许下次重新触发 */ + if (mode != CHASSIS_MODE_CLIMB_STEP && c->climb.completed) { + c->climb.completed = false; + c->climb.state = CLIMB_IDLE; + c->climb.state_start_time = 0; + } + if (mode == c->mode) return CHASSIS_OK; /* 上台阶已完成，忽略重复的CLIMB_STEP请求，需先切别的模式再回来 */ @@ -197,6 +205,7 @@ static int8_t Chassis_SetMode(Chassis_t *c, Chassis_Mode_t mode) { if (c->mode == CHASSIS_MODE_CLIMB_STEP && mode != CHASSIS_MODE_CLIMB_STEP) { c->climb.completed = false; c->climb.state = CLIMB_IDLE; + c->climb.state_start_time = 0; } c->mode = mode; @@ -469,9 +478,10 @@ static void Chassis_ClimbControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) { break; case CLIMB_EXTEND: { - /* 伸腿前进：腿长最大，给定前进速度，削弱摆力矩让腿自由后摆 */ - climb_cmd.move_vec.vx = c->param->climb.forward_speed / 2.0f; - climb_cmd.height = 1.0f; /* 最大腿长 */ + /* 伸腿前进采用斜坡，避免切换模式时瞬间猛站 */ + float extend_ratio = LIMIT((float)elapsed_time / 500.0f, 0.0f, 1.0f); + climb_cmd.move_vec.vx = (c->param->climb.forward_speed / 2.0f) * extend_ratio; + climb_cmd.height = extend_ratio; /* 运行LQR和VMC */ Chassis_LQRControl(c, &climb_cmd); @@ -483,9 +493,12 @@ static void Chassis_ClimbControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) { Chassis_VMCControl(c, &climb_cmd); Chassis_Output(c); - /* 检查腿后摆角度：任一腿theta超过阈值则收腿 */ + /* 先伸腿到位，再根据摆角触发收腿，避免刚进入模式就立刻缩回 */ + bool legs_extended = (c->vmc_[0].leg.L0 > c->param->leg.max_length - 0.03f) && + (c->vmc_[1].leg.L0 > c->param->leg.max_length - 0.03f); float avg_theta = (c->vmc_[0].leg.theta + c->vmc_[1].leg.theta) / 2.0f; - if (fabsf(avg_theta) > c->param->climb.theta_retract_threshold) { + if (elapsed_time > 300 && legs_extended && + fabsf(avg_theta) > c->param->climb.theta_retract_threshold) { c->climb.state = CLIMB_RETRACT; c->climb.state_start_time = current_time; } @@ -672,6 +685,11 @@ int8_t Chassis_Init(Chassis_t *c, Chassis_Params_t *param, float target_freq) { c->rotor_state.current_spin_speed = 0.0f; c->rotor_state.exiting = false; + /* 初始化上台阶状态 */ + c->climb.state = CLIMB_IDLE; + c->climb.completed = false; + c->climb.state_start_time = 0; + return CHASSIS_OK; } From 0a442e8976f6e7354a757ca16d75c292f950b50b Mon Sep 17 00:00:00 2001 From: Robofish <1683502971@qq.com> Date: Sun, 15 Mar 2026 21:27:17 +0800 Subject: [PATCH 25/34] =?UTF-8?q?=E6=B7=BB=E5=8A=A0=E5=89=8A=E5=BC=B1?= =?UTF-8?q?=E5=87=BD=E6=95=B0?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- User/module/balance_chassis.c | 20 ++++++++++++++++---- 1 file changed, 16 insertions(+), 4 deletions(-) diff --git a/User/module/balance_chassis.c b/User/module/balance_chassis.c index 05ba90e..15961e5 100644 --- a/User/module/balance_chassis.c +++ b/User/module/balance_chassis.c @@ -486,9 +486,22 @@ static void Chassis_ClimbControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) { /* 运行LQR和VMC */ Chassis_LQRControl(c, &climb_cmd); - /* 削弱LQR的摆力矩Tp，让腿撞到台阶后能自由后摆 */ - c->lqr[0].control_output.Tp *= c->param->climb.tp_scale; - c->lqr[1].control_output.Tp *= c->param->climb.tp_scale; + /* 摆力矩Tp随theta非线性缩放：撞到台阶前保持正常，撞到后快速降低 */ + float avg_theta = (c->vmc_[0].leg.theta + c->vmc_[1].leg.theta) / 2.0f; + float theta_abs = fabsf(avg_theta); + float contact_angle = c->param->climb.theta_retract_threshold * 0.5f; + float tp_min = c->param->climb.tp_scale; + float dynamic_scale; + if (theta_abs < contact_angle) { + dynamic_scale = 1.0f; /* 未撞到，保持正常Tp */ + } else { + float t = (theta_abs - contact_angle) / + (c->param->climb.theta_retract_threshold - contact_angle); + t = LIMIT(t, 0.0f, 1.0f); + dynamic_scale = 1.0f + (tp_min - 1.0f) * sqrtf(t); /* sqrt: 撞到后快速衰减 */ + } + c->lqr[0].control_output.Tp *= dynamic_scale; + c->lqr[1].control_output.Tp *= dynamic_scale; Chassis_VMCControl(c, &climb_cmd); Chassis_Output(c); @@ -496,7 +509,6 @@ static void Chassis_ClimbControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) { /* 先伸腿到位，再根据摆角触发收腿，避免刚进入模式就立刻缩回 */ bool legs_extended = (c->vmc_[0].leg.L0 > c->param->leg.max_length - 0.03f) && (c->vmc_[1].leg.L0 > c->param->leg.max_length - 0.03f); - float avg_theta = (c->vmc_[0].leg.theta + c->vmc_[1].leg.theta) / 2.0f; if (elapsed_time > 300 && legs_extended && fabsf(avg_theta) > c->param->climb.theta_retract_threshold) { c->climb.state = CLIMB_RETRACT; From 4773c252f2282186836e410c7d5f8b874448d549 Mon Sep 17 00:00:00 2001 From: Robofish <1683502971@qq.com> Date: Sun, 15 Mar 2026 21:29:41 +0800 Subject: [PATCH 26/34] =?UTF-8?q?=E7=BB=A7=E7=BB=AD=E4=BF=AE?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- User/module/balance_chassis.c | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/User/module/balance_chassis.c b/User/module/balance_chassis.c index 15961e5..dd05833 100644 --- a/User/module/balance_chassis.c +++ b/User/module/balance_chassis.c @@ -491,14 +491,15 @@ static void Chassis_ClimbControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) { float theta_abs = fabsf(avg_theta); float contact_angle = c->param->climb.theta_retract_threshold * 0.5f; float tp_min = c->param->climb.tp_scale; + float tp_initial = 0.8f; /* 初始Tp缩放，刚进入就削弱20% */ float dynamic_scale; if (theta_abs < contact_angle) { - dynamic_scale = 1.0f; /* 未撞到，保持正常Tp */ + dynamic_scale = tp_initial; /* 未撞到，从80%开始 */ } else { float t = (theta_abs - contact_angle) / (c->param->climb.theta_retract_threshold - contact_angle); t = LIMIT(t, 0.0f, 1.0f); - dynamic_scale = 1.0f + (tp_min - 1.0f) * sqrtf(t); /* sqrt: 撞到后快速衰减 */ + dynamic_scale = tp_initial + (tp_min - tp_initial) * sqrtf(t); /* sqrt: 撞到后快速衰减 */ } c->lqr[0].control_output.Tp *= dynamic_scale; c->lqr[1].control_output.Tp *= dynamic_scale; From c468fa59ce063eab38d85e5d3fa5877b92bcd24d Mon Sep 17 00:00:00 2001 From: Robofish <1683502971@qq.com> Date: Sun, 15 Mar 2026 21:32:19 +0800 Subject: [PATCH 27/34] 50 --- User/module/balance_chassis.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/User/module/balance_chassis.c b/User/module/balance_chassis.c index dd05833..09f22eb 100644 --- a/User/module/balance_chassis.c +++ b/User/module/balance_chassis.c @@ -491,7 +491,7 @@ static void Chassis_ClimbControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) { float theta_abs = fabsf(avg_theta); float contact_angle = c->param->climb.theta_retract_threshold * 0.5f; float tp_min = c->param->climb.tp_scale; - float tp_initial = 0.8f; /* 初始Tp缩放，刚进入就削弱20% */ + float tp_initial = 0.5f; /* 初始Tp缩放，刚进入就削弱50% */ float dynamic_scale; if (theta_abs < contact_angle) { dynamic_scale = tp_initial; /* 未撞到，从80%开始 */ From b7851e5e21a96373ddfe99d4b492fcde38738e9c Mon Sep 17 00:00:00 2001 From: Robofish <1683502971@qq.com> Date: Sun, 15 Mar 2026 23:30:49 +0800 Subject: [PATCH 28/34] maybe --- User/module/config.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/User/module/config.c b/User/module/config.c index 61cbef5..ce71565 100644 --- a/User/module/config.c +++ b/User/module/config.c @@ -449,8 +449,8 @@ Config_RobotParam_t robot_config = { }, .climb = { - .forward_speed = 1.0f, /* 上台阶前进速度 (m/s) */ - .theta_retract_threshold = 0.52f, /* 腿后摆收腿阈值 (rad)，约30° */ + .forward_speed = 1.5f, /* 上台阶前进速度 (m/s) */ + .theta_retract_threshold = 0.9f, /* 腿后摆收腿阈值 (rad)，约30° */ .tp_scale = 0.1f, /* 摆力矩缩放：削弱到10%，让腿自由后摆 */ .settle_time_ms = 500, /* 收腿后稳定时间 (ms) */ }, From ec50f074d559ea0225ed95904d81ff11f7ce9cf3 Mon Sep 17 00:00:00 2001 From: Robofish <1683502971@qq.com> Date: Sun, 15 Mar 2026 23:33:26 +0800 Subject: [PATCH 29/34] =?UTF-8?q?=E8=87=AA=E5=8A=A8=E4=B8=8A=E5=8F=B0?= =?UTF-8?q?=E9=98=B6=E5=8F=82=E6=95=B0?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- User/module/config.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/User/module/config.c b/User/module/config.c index ce71565..d945449 100644 --- a/User/module/config.c +++ b/User/module/config.c @@ -451,7 +451,7 @@ Config_RobotParam_t robot_config = { .climb = { .forward_speed = 1.5f, /* 上台阶前进速度 (m/s) */ .theta_retract_threshold = 0.9f, /* 腿后摆收腿阈值 (rad)，约30° */ - .tp_scale = 0.1f, /* 摆力矩缩放：削弱到10%，让腿自由后摆 */ + .tp_scale = 0.08f, /* 摆力矩缩放：削弱到10%，让腿自由后摆 */ .settle_time_ms = 500, /* 收腿后稳定时间 (ms) */ }, From 632cd81ddb0582710b2d0cda6054965d15812cad Mon Sep 17 00:00:00 2001 From: Robofish <1683502971@qq.com> Date: Mon, 16 Mar 2026 00:00:48 +0800 Subject: [PATCH 30/34] =?UTF-8?q?=E6=B5=8B=E6=B5=8Brecover?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- User/module/balance_chassis.c | 130 ++++++++++++++++++++++++++++++++-- User/module/balance_chassis.h | 10 +++ User/module/config.c | 8 ++- 3 files changed, 140 insertions(+), 8 deletions(-) diff --git a/User/module/balance_chassis.c b/User/module/balance_chassis.c index 09f22eb..1257a02 100644 --- a/User/module/balance_chassis.c +++ b/User/module/balance_chassis.c @@ -32,6 +32,7 @@ static void Chassis_ResetControllers(Chassis_t *c); static void Chassis_SelectYawZeroPoint(Chassis_t *c); static void Chassis_JumpControl(Chassis_t *c, const Chassis_CMD_t *c_cmd, float *target_L0, float *extra_force); static void Chassis_ClimbControl(Chassis_t *c, const Chassis_CMD_t *c_cmd); +static void Chassis_RecoverControl(Chassis_t *c); /* Private functions -------------------------------------------------------- */ @@ -169,15 +170,13 @@ static int8_t Chassis_SetMode(Chassis_t *c, Chassis_Mode_t mode) { Chassis_ResetControllers(c); Chassis_SelectYawZeroPoint(c); - /* RELAX -> BALANCE：pitch过大时保持RELAX，避免进入未实现的RECOVER */ + /* RELAX -> BALANCE：pitch过大时自动进入RECOVER自起 */ if (mode == CHASSIS_MODE_WHELL_LEG_BALANCE && c->mode == CHASSIS_MODE_RELAX) { float pitch = c->feedback.imu.euler.pit; if (fabsf(pitch) > 0.8f) { - /* pitch过大，机体未扶正，保持RELAX */ - Chassis_MotorRelax(c); - return CHASSIS_OK; + /* pitch过大，机体倒地，进入自起模式 */ + mode = CHASSIS_MODE_RECOVER; } - /* 否则直接进入平衡模式 */ } /* 直接进入 RECOVER 时也重置状态机 */ @@ -186,6 +185,7 @@ static int8_t Chassis_SetMode(Chassis_t *c, Chassis_Mode_t mode) { float theta_r = c->vmc_[1].leg.theta; c->recover.state[0] = (fabsf(theta_l) > (float)M_PI_2) ? RECOVER_FLIP : RECOVER_STRETCH; c->recover.state[1] = (fabsf(theta_r) > (float)M_PI_2) ? RECOVER_FLIP : RECOVER_STRETCH; + c->recover.state_start_time = (uint32_t)(BSP_TIME_Get_us() / 1000); } /* 进入小陀螺时重置旋转状态 */ @@ -559,6 +559,111 @@ static void Chassis_ClimbControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) { } +/** + * @brief 倒地自起控制 + * @param c 底盘结构体指针 + * @note 三阶段状态机: FLIP→STRETCH→BACKLEG→COMPLETE + * 使用MIT模式(kp=0)做速度控制，轮子全程关闭 + */ +static void Chassis_RecoverControl(Chassis_t *c) { + uint32_t now = (uint32_t)(BSP_TIME_Get_us() / 1000); + uint32_t elapsed = now - c->recover.state_start_time; + + /* 构建MIT速度控制参数模板 */ + MOTOR_LZ_MotionParam_t vel_cmd = { + .torque = 0.0f, + .target_angle = 0.0f, + .target_velocity = 0.0f, + .kp = 0.0f, + .kd = c->param->recover.kd, + }; + + /* 轮子全程关闭 */ + MOTOR_LK_Relax(&c->param->wheel_param[0]); + MOTOR_LK_Relax(&c->param->wheel_param[1]); + + float stretch_vel = c->param->recover.stretch_velocity; + Recover_State_t state = c->recover.state[0]; /* 两腿同步 */ + + switch (state) { + case RECOVER_FLIP: { + /* 翻身阶段：腿朝上（|theta|>π/2）时，转动腿使其朝下 + * 所有关节同向旋转，使腿绕机体转动 */ + vel_cmd.target_velocity = stretch_vel; + MOTOR_LZ_MotionControl(&c->param->joint_param[0], &vel_cmd); + MOTOR_LZ_MotionControl(&c->param->joint_param[1], &vel_cmd); + vel_cmd.target_velocity = -stretch_vel; + MOTOR_LZ_MotionControl(&c->param->joint_param[2], &vel_cmd); + MOTOR_LZ_MotionControl(&c->param->joint_param[3], &vel_cmd); + + /* 腿转到朝下 → 进入伸腿 */ + if ((fabsf(c->vmc_[0].leg.theta) < (float)M_PI_2 && + fabsf(c->vmc_[1].leg.theta) < (float)M_PI_2) || + elapsed > 5000) { + c->recover.state[0] = c->recover.state[1] = RECOVER_STRETCH; + c->recover.state_start_time = now; + } + } break; + + case RECOVER_STRETCH: { + /* 伸腿阶段：两关节反向 → 延伸五连杆 → 增大L0 + * 左腿: J0=+V, J1=-V; 右腿: J2=-V, J3=+V */ + vel_cmd.target_velocity = stretch_vel; + MOTOR_LZ_MotionControl(&c->param->joint_param[0], &vel_cmd); + vel_cmd.target_velocity = -stretch_vel; + MOTOR_LZ_MotionControl(&c->param->joint_param[1], &vel_cmd); + MOTOR_LZ_MotionControl(&c->param->joint_param[2], &vel_cmd); + vel_cmd.target_velocity = stretch_vel; + MOTOR_LZ_MotionControl(&c->param->joint_param[3], &vel_cmd); + + /* 两腿L0都超过目标 → 进入甩腿 */ + if ((c->vmc_[0].leg.L0 > c->param->recover.target_leg_length && + c->vmc_[1].leg.L0 > c->param->recover.target_leg_length) || + elapsed > 3000) { + c->recover.state[0] = c->recover.state[1] = RECOVER_BACKLEG; + c->recover.state_start_time = now; + } + } break; + + case RECOVER_BACKLEG: { + /* 甩腿阶段：两关节同向 → 推动机体立起 + * 左腿: J0=-V, J1=-V; 右腿: J2=+V, J3=+V */ + float back_vel = c->param->recover.backleg_velocity; + vel_cmd.target_velocity = -back_vel; + MOTOR_LZ_MotionControl(&c->param->joint_param[0], &vel_cmd); + MOTOR_LZ_MotionControl(&c->param->joint_param[1], &vel_cmd); + vel_cmd.target_velocity = back_vel; + MOTOR_LZ_MotionControl(&c->param->joint_param[2], &vel_cmd); + MOTOR_LZ_MotionControl(&c->param->joint_param[3], &vel_cmd); + + /* 体姿接近竖直 → 完成 */ + if (fabsf(c->feedback.imu.euler.pit) < c->param->recover.phi_tolerance || + elapsed > 5000) { + c->recover.state[0] = c->recover.state[1] = RECOVER_COMPLETE; + c->recover.state_start_time = now; + } + } break; + + case RECOVER_COMPLETE: { + /* 收腿回中：反向于伸腿，快速缩短 */ + vel_cmd.target_velocity = -stretch_vel; + MOTOR_LZ_MotionControl(&c->param->joint_param[0], &vel_cmd); + vel_cmd.target_velocity = stretch_vel; + MOTOR_LZ_MotionControl(&c->param->joint_param[1], &vel_cmd); + MOTOR_LZ_MotionControl(&c->param->joint_param[2], &vel_cmd); + vel_cmd.target_velocity = -stretch_vel; + MOTOR_LZ_MotionControl(&c->param->joint_param[3], &vel_cmd); + + /* 短暂收腿后切回BALANCE */ + if (elapsed > 500) { + c->mode = CHASSIS_MODE_WHELL_LEG_BALANCE; + Chassis_ResetControllers(c); + } + } break; + } +} + + /** * @brief 初始化底盘 * @param c 底盘结构体指针 @@ -703,6 +808,11 @@ int8_t Chassis_Init(Chassis_t *c, Chassis_Params_t *param, float target_freq) { c->climb.completed = false; c->climb.state_start_time = 0; + /* 初始化自起状态 */ + c->recover.state[0] = RECOVER_STRETCH; + c->recover.state[1] = RECOVER_STRETCH; + c->recover.state_start_time = 0; + return CHASSIS_OK; } @@ -808,6 +918,13 @@ int8_t Chassis_Control(Chassis_t *c, const Chassis_CMD_t *c_cmd) { return CHASSIS_ERR_MODE; } + /* 倒地自动检测：BALANCE/ROTOR模式下pitch过大 → 自动进入RECOVER */ + if ((c->mode == CHASSIS_MODE_WHELL_LEG_BALANCE || + c->mode == CHASSIS_MODE_BALANCE_ROTOR) && + fabsf(c->feedback.imu.euler.pit) > 1.5f) { + Chassis_SetMode(c, CHASSIS_MODE_RECOVER); + } + /* 跳跃触发入口统一放在底盘控制主流程：由chassis_cmd直接驱动 */ if (c_cmd->jump_trigger && (c->mode == CHASSIS_MODE_WHELL_LEG_BALANCE || c->mode == CHASSIS_MODE_BALANCE_ROTOR) && @@ -844,8 +961,7 @@ int8_t Chassis_Control(Chassis_t *c, const Chassis_CMD_t *c_cmd) { break; case CHASSIS_MODE_RECOVER: - /* 自起功能未实现，安全放松电机 */ - Chassis_MotorRelax(c); + Chassis_RecoverControl(c); break; case CHASSIS_MODE_WHELL_LEG_BALANCE: diff --git a/User/module/balance_chassis.h b/User/module/balance_chassis.h index 98d0ef3..d0f07f1 100644 --- a/User/module/balance_chassis.h +++ b/User/module/balance_chassis.h @@ -174,6 +174,15 @@ typedef struct { uint32_t settle_time_ms; /* 收腿后稳定时间 (ms) */ } climb; + /* 倒地自起参数 */ + struct { + float stretch_velocity; /* 伸腿角速度 (rad/s) */ + float backleg_velocity; /* 甩腿角速度 (rad/s) */ + float target_leg_length; /* 伸腿目标长度 (m) */ + float phi_tolerance; /* 体姿角到达容差 (rad) */ + float kd; /* MIT速度模式阻尼 (Nm·s/rad) */ + } recover; + /* LQR 目标状态偏移 */ struct { float theta; @@ -247,6 +256,7 @@ typedef struct { /* 自起控制相关 */ struct { Recover_State_t state[2]; /* 左右腿自起状态 */ + uint32_t state_start_time; /* 当前阶段开始时间 (ms) */ } recover; /* 上台阶控制相关 */ diff --git a/User/module/config.c b/User/module/config.c index d945449..ad087c9 100644 --- a/User/module/config.c +++ b/User/module/config.c @@ -454,7 +454,13 @@ Config_RobotParam_t robot_config = { .tp_scale = 0.08f, /* 摆力矩缩放：削弱到10%，让腿自由后摆 */ .settle_time_ms = 500, /* 收腿后稳定时间 (ms) */ }, - + .recover = { + .stretch_velocity = 2.0f, /* 伸腿角速度 (rad/s) */ + .backleg_velocity = 2.0f, /* 甩腿角速度 (rad/s) */ + .target_leg_length = 0.28f, /* 伸腿目标长度 (m) */ + .phi_tolerance = 0.5f, /* 体姿接近站立容差 (rad)，约28° */ + .kd = 3.0f, /* MIT速度模式阻尼 */ + }, .lqr_offset = { .theta = 0.0f, .x = 0.0f, From 66cc032a45d5cca70df46c0852d27fc8ae0a1bc1 Mon Sep 17 00:00:00 2001 From: xxxxm <2389287465@qq.com> Date: Sun, 15 Mar 2026 13:56:47 +0800 Subject: [PATCH 31/34] =?UTF-8?q?=E8=87=AA=E7=9E=84=E6=8E=A5=E5=8F=A3?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- CMakeLists.txt | 2 +- User/bsp/uart.h | 1 + User/module/aimbot.c | 266 ++++++++++++++++++++++++++++++++++ User/module/aimbot.h | 174 ++++++++++++++++++++++ User/module/cmd/cmd.c | 5 +- User/module/cmd/cmd_feature.h | 6 +- User/module/config.c | 5 + User/module/config.h | 2 + User/module/gimbal.c | 65 ++++++--- User/module/gimbal.h | 19 +++ User/module/shoot.c | 203 ++++++++++++-------------- User/module/shoot.h | 15 +- User/task/ai.c | 73 +++++++--- User/task/cmd.c | 11 +- User/task/init.c | 7 +- User/task/user_task.h | 13 +- 16 files changed, 679 insertions(+), 188 deletions(-) create mode 100644 User/module/aimbot.c create mode 100644 User/module/aimbot.h diff --git a/CMakeLists.txt b/CMakeLists.txt index 530a8dd..6f0b735 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -93,7 +93,7 @@ target_sources(${CMAKE_PROJECT_NAME} PRIVATE User/module/cmd/cmd_behavior.c User/module/cmd/cmd_example.c User/module/vision_bridge.c - + User/module/aimbot.c # User/task sources User/task/ai.c User/task/atti_esit.c diff --git a/User/bsp/uart.h b/User/bsp/uart.h index 80ffe91..f931e9d 100644 --- a/User/bsp/uart.h +++ b/User/bsp/uart.h @@ -30,6 +30,7 @@ typedef enum { BSP_UART_DR16, BSP_UART_VOFA, BSP_UART_REF, + BSP_UART_AI, BSP_UART_NUM, BSP_UART_ERR, } BSP_UART_t; diff --git a/User/module/aimbot.c b/User/module/aimbot.c new file mode 100644 index 0000000..d5745be --- /dev/null +++ b/User/module/aimbot.c @@ -0,0 +1,266 @@ +#include "module/aimbot.h" +#include "device/device.h" +#include "bsp/uart.h" +#include "component/crc16.h" +#include + +/* ===================================================================== + * FDCAN 帧索引（反馈方向：下层板?上层板） + * 注：上层板使用自己的IMU，下层板只发送射击数据与模式 + * ===================================================================== */ +#define FB_FRAME_DATA 0u /* 弹速(4B)+弹数(2B)+模式(1B)+保留(1B) */ + +/* ===================================================================== + * UART 通信接口（上层板 �?上位�?PC�? + * ===================================================================== */ + +int8_t Aimbot_AIStartRecv(Aimbot_AI_t *ai) { + if (BSP_UART_Receive(BSP_UART_AI, (uint8_t *)ai, sizeof(*ai), true) == DEVICE_OK) { + return DEVICE_OK; + } + return DEVICE_ERR; +} + +int8_t Aimbot_AIGetResult(Aimbot_AI_t *ai, Aimbot_AIResult_t *result) { + if (ai->head[0] != 'M' || ai->head[1] != 'R') { + return DEVICE_ERR; + } + if (!CRC16_Verify((const uint8_t *)ai, sizeof(*ai))) { + return DEVICE_ERR; + } + result->mode = ai->mode; + result->gimbal_t.setpoint.yaw = ai->yaw; + result->gimbal_t.vel.yaw = ai->yaw_vel; + result->gimbal_t.accl.yaw = ai->yaw_acc; + result->gimbal_t.setpoint.pit = ai->pitch; + result->gimbal_t.vel.pit = ai->pitch_vel; + result->gimbal_t.accl.pit = ai->pitch_acc; + return DEVICE_OK; +} + +/** + * @brief 打包 MCU 数据（UART 发给上位机格式），修正了原始实现中的字段错误�? + * 四元数来�?quat 参数；欧拉角、角速度来自 gimbal_fb->imu�? + */ +int8_t Aimbot_MCUPack(Aimbot_MCU_t *mcu, const Gimbal_Feedback_t *gimbal_fb, + const AHRS_Quaternion_t *quat, + float bullet_speed, uint16_t bullet_count, uint8_t mode) { + if (mcu == NULL || gimbal_fb == NULL || quat == NULL) { + return DEVICE_ERR_NULL; + } + mcu->head[0] = 'M'; + mcu->head[1] = 'R'; + mcu->mode = mode; + mcu->q[0] = quat->q0; + mcu->q[1] = quat->q1; + mcu->q[2] = quat->q2; + mcu->q[3] = quat->q3; + mcu->yaw = gimbal_fb->imu.eulr.yaw; + mcu->yaw_vel = gimbal_fb->imu.gyro.z; + mcu->pitch = gimbal_fb->imu.eulr.pit; + mcu->pitch_vel = gimbal_fb->imu.gyro.x; + mcu->bullet_speed = bullet_speed; + mcu->bullet_count = bullet_count; + mcu->crc16 = CRC16_Calc((const uint8_t *)mcu, + sizeof(*mcu) - sizeof(uint16_t), CRC16_INIT); + if (!CRC16_Verify((const uint8_t *)mcu, sizeof(*mcu))) { + return DEVICE_ERR; + } + return DEVICE_OK; +} + +int8_t Aimbot_MCUStartSend(Aimbot_MCU_t *mcu) { + if (BSP_UART_Transmit(BSP_UART_AI, (uint8_t *)mcu, sizeof(*mcu), true) == DEVICE_OK) { + return DEVICE_OK; + } + return DEVICE_ERR; +} + +/* ===================================================================== + * CAN 通信接口（下层板 �?上层板） + * ===================================================================== */ + +/** + * @brief 初始�?Aimbot CAN 通信：注册指令接收队列和反馈收发队列�? + * 下层板只需注册 cmd_id；上层板只需注册 fb_base_id �?6 �?ID�? + * 本函数同时注册两侧所需 ID，上/下层板共用同一初始化流程即可�? + */ +int8_t Aimbot_Init(Aimbot_Param_t *param) { + if (param == NULL) return DEVICE_ERR_NULL; + + BSP_FDCAN_Init(); + + /* 注册 AI 指令帧队列（下层板接收／上层板发送） */ + BSP_FDCAN_RegisterId(param->can, param->cmd_id, + BSP_FDCAN_DEFAULT_QUEUE_SIZE); + + /* 注册反馈数据帧队列（上层板接收／下层板发送） */ + for (uint8_t i = 0; i < AIMBOT_FB_FRAME_NUM; i++) { + BSP_FDCAN_RegisterId(param->can, param->fb_base_id + i, + BSP_FDCAN_DEFAULT_QUEUE_SIZE); + } + + return DEVICE_OK; +} + +/** + * @brief �?Gimbal/IMU/Shoot 数据打包 CAN 反馈结构体�? +/** + * @brief 【下层板】打包 CAN 反馈结构体。 + */ +int8_t Aimbot_PackFeedback(Aimbot_FeedbackData_t *fb, + float bullet_speed, uint16_t bullet_count, + uint8_t mode) { + if (fb == NULL) { + return DEVICE_ERR_NULL; + } + fb->mode = mode; + fb->bullet_speed = bullet_speed; + fb->bullet_count = bullet_count; + return DEVICE_OK; +} + +/** + * @brief 【下层板】将反馈数据打成 1 个 CAN 标准帧发给上层板。 + * + * 帧格式（每帧 8 字节）： + * 帧0: bullet_speed(float,4B) bullet_count(uint16,2B) mode(1B) rsv(1B) + */ +void Aimbot_SendFeedbackOnCAN(const Aimbot_Param_t *param, + const Aimbot_FeedbackData_t *fb) { + if (param == NULL || fb == NULL) return; + + BSP_FDCAN_StdDataFrame_t frame; + frame.dlc = AIMBOT_CAN_DLC; + + /* 帧0: 弹速 + 弹数 + 模式 */ + frame.id = param->fb_base_id + FB_FRAME_DATA; + memcpy(&frame.data[0], &fb->bullet_speed, 4); + frame.data[4] = (uint8_t)(fb->bullet_count & 0xFFu); + frame.data[5] = (uint8_t)((fb->bullet_count >> 8u) & 0xFFu); + frame.data[6] = fb->mode; + frame.data[7] = 0u; + BSP_FDCAN_TransmitStdDataFrame(param->can, &frame); +} + +/** + * @brief 【下层板】从 CAN 队列中非阻塞地取出上层板发来�?AI 指令并解析�? + * + * 指令帧格式（8 字节，与 vision_bridge 一致）�? + * data[0] : mode (1B) + * data[1..3.5] : yaw (28bit 有符号定点数�?.1µrad/LSB) + * data[4.5..7] : pit (28bit 有符号定点数�?.1µrad/LSB) + * + * @return DEVICE_OK 成功解析到新指令 + * DEVICE_ERR 队列空，无新数据 + */ +int8_t Aimbot_ParseCmdFromCAN(const Aimbot_Param_t *param, + Aimbot_AIResult_t *result) { + if (param == NULL || result == NULL) return DEVICE_ERR_NULL; + + BSP_FDCAN_Message_t msg; + int8_t ret = DEVICE_ERR; + + if (BSP_FDCAN_GetMessage(param->can, param->cmd_id, &msg, + BSP_FDCAN_TIMEOUT_IMMEDIATE) == 0) { + result->mode = msg.data[0]; + + /* 解析 yaw（高 28 位），符号扩展为 int32 */ + int32_t yaw_raw = (int32_t)(((uint32_t)msg.data[1] << 20u) | + ((uint32_t)msg.data[2] << 12u) | + ((uint32_t)msg.data[3] << 4u) | + ((uint32_t)(msg.data[4] >> 4u) & 0xFu)); + if (yaw_raw & 0x08000000) yaw_raw |= (int32_t)0xF0000000; + result->gimbal_t.setpoint.yaw = (float)yaw_raw / AIMBOT_ANGLE_SCALE; + + /* 解析 pit（低 28 位），符号扩展为 int32 */ + int32_t pit_raw = (int32_t)(((uint32_t)(msg.data[4] & 0xFu) << 24u) | + ((uint32_t)msg.data[5] << 16u) | + ((uint32_t)msg.data[6] << 8u) | + (uint32_t)msg.data[7]); + if (pit_raw & 0x08000000) pit_raw |= (int32_t)0xF0000000; + result->gimbal_t.setpoint.pit = (float)pit_raw / AIMBOT_ANGLE_SCALE; + ret = DEVICE_OK; + } + + if (BSP_FDCAN_GetMessage(param->can, param->cmd_id + 1, &msg, + BSP_FDCAN_TIMEOUT_IMMEDIATE) == 0) { + memcpy(&result->gimbal_t.vel.yaw, &msg.data[0], 4); + memcpy(&result->gimbal_t.vel.pit, &msg.data[4], 4); + ret = DEVICE_OK; + } + + if (BSP_FDCAN_GetMessage(param->can, param->cmd_id + 2, &msg, + BSP_FDCAN_TIMEOUT_IMMEDIATE) == 0) { + memcpy(&result->gimbal_t.accl.yaw, &msg.data[0], 4); + memcpy(&result->gimbal_t.accl.pit, &msg.data[4], 4); + ret = DEVICE_OK; + } + + return ret; +} + +/** + * @brief 【上层板】将 AI 指令通过 CAN 发送给下层板（单帧 8 字节）�? + * + * �?vision_bridge.c �?AI_SendCmdOnFDCAN 编码格式完全一致， + * 上层板可直接调用本函数替�?vision_bridge 中的同名函数�? + */ +void Aimbot_SendCmdOnCAN(const Aimbot_Param_t *param, + const Aimbot_AIResult_t *cmd) { + if (param == NULL || cmd == NULL) return; + + const int32_t yaw = (int32_t)(cmd->gimbal_t.setpoint.yaw * AIMBOT_ANGLE_SCALE); + const int32_t pit = (int32_t)(cmd->gimbal_t.setpoint.pit * AIMBOT_ANGLE_SCALE); + + BSP_FDCAN_StdDataFrame_t frame = {0}; + frame.id = param->cmd_id; + frame.dlc = AIMBOT_CAN_DLC; + + frame.data[0] = cmd->mode; + frame.data[1] = (uint8_t)((yaw >> 20) & 0xFF); + frame.data[2] = (uint8_t)((yaw >> 12) & 0xFF); + frame.data[3] = (uint8_t)((yaw >> 4) & 0xFF); + frame.data[4] = (uint8_t)(((yaw & 0xF) << 4) | ((pit >> 24) & 0xF)); + frame.data[5] = (uint8_t)((pit >> 16) & 0xFF); + frame.data[6] = (uint8_t)((pit >> 8) & 0xFF); + frame.data[7] = (uint8_t)(pit & 0xFF); + + BSP_FDCAN_TransmitStdDataFrame(param->can, &frame); + + /* 第二帧：发速度 */ + frame.id = param->cmd_id + 1; + memcpy(&frame.data[0], &cmd->gimbal_t.vel.yaw, 4); + memcpy(&frame.data[4], &cmd->gimbal_t.vel.pit, 4); + BSP_FDCAN_TransmitStdDataFrame(param->can, &frame); + + /* 第三帧：发加速度 */ + frame.id = param->cmd_id + 2; + memcpy(&frame.data[0], &cmd->gimbal_t.accl.yaw, 4); + memcpy(&frame.data[4], &cmd->gimbal_t.accl.pit, 4); + BSP_FDCAN_TransmitStdDataFrame(param->can, &frame); +} + +/** + * @brief 【上层板】从 CAN 队列中非阻塞地解析下层板发来的反馈数据（共1帧）。 + * + * @return DEVICE_OK + */ +int8_t Aimbot_ParseFeedbackFromCAN(const Aimbot_Param_t *param, + Aimbot_FeedbackData_t *fb) { + if (param == NULL || fb == NULL) return DEVICE_ERR_NULL; + + BSP_FDCAN_Message_t msg; + + /* 帧0: 弹速 + 弹数 + 模式 */ + if (BSP_FDCAN_GetMessage(param->can, + param->fb_base_id + FB_FRAME_DATA, + &msg, BSP_FDCAN_TIMEOUT_IMMEDIATE) == 0) { + memcpy(&fb->bullet_speed, &msg.data[0], 4); + fb->bullet_count = (uint16_t)(msg.data[4] | ((uint16_t)msg.data[5] << 8u)); + fb->mode = msg.data[6]; + } + + return DEVICE_OK; +} + diff --git a/User/module/aimbot.h b/User/module/aimbot.h new file mode 100644 index 0000000..1c87ce4 --- /dev/null +++ b/User/module/aimbot.h @@ -0,0 +1,174 @@ + +#pragma once + +#ifdef __cplusplus +extern "C" { +#endif + +/* Includes ----------------------------------------------------------------- */ +#include "component\user_math.h" +#include "module/gimbal.h" +#include "bsp/fdcan.h" +#include + +// 数据包协议定义（UART 通信使用 head[2]={'M', 'R'} 标识） + +/* ============================ CAN 通信定义 ============================ + * 通路：上位机 <--串口--> 上层板 <--CAN--> 下层板 + * + * 注：上层板使用自己的IMU（atti_esti），下层板只需反馈电机和射击数据 + * + * 下层板 → 上层板：反馈数据，单帧 8 字节 + * 帧0 (fb_base_id+0): 弹速(4B) + 弹数(2B) + 模式(1B) + 保留(1B) + * + * 上层板 → 下层板：AI 指令，单帧 8 字节 + * 帧 (cmd_id): mode(1B) + yaw(28bit) + pit(28bit)，定点数精度 0.1µrad + * ====================================================================== */ +#define AIMBOT_FB_FRAME_NUM 1u /* 反馈数据帧总数（仅下层三字段） */ +#define AIMBOT_CAN_DLC 8u /* CAN 帧数据长度 */ +#define AIMBOT_ANGLE_SCALE 10000000.0f /* 指令角度定点数比例（0.1µrad/LSB） */ + +/* CAN 通信参数结构体 */ +typedef struct { + BSP_FDCAN_t can; /* 使用的 CAN 总线实例 */ + uint32_t cmd_id; /* 上层板→下层板 AI 指令帧 CAN ID */ + uint32_t fb_base_id; /* 下层板→上层板 反馈数据起始 CAN ID */ +} Aimbot_Param_t; + +/* CAN 反馈数据结构体（下层板打包后经 CAN 发给上层板的内容） + * 注：下层板只反馈 bullet_speed / bullet_count / mode */ +typedef struct { + uint8_t mode; /* 下层板当前工作模式 */ + float bullet_speed; /* 弹速（m/s） */ + uint16_t bullet_count; /* 子弹累计发射次数 */ +} Aimbot_FeedbackData_t; + +/*-----------------------------to上位机---------------------------------*/ +typedef struct __attribute__((packed)) +{ + uint8_t head[2]; // 数据包头: {'M', 'R'} + uint8_t mode; // 0: 空闲, 1: 自瞄, 2: 小符, 3: 大符 + float q[4]; // 四元数 wxyz 顺序 + float yaw; // 偏航角 + float yaw_vel; // 偏航角速度 + float pitch; // 俯仰角 + float pitch_vel; // 俯仰角速度 + float bullet_speed; // 弹速 + uint16_t bullet_count; // 子弹累计发送次数 + uint16_t crc16; // CRC16 校验 +} Aimbot_MCU_t; + +// 裁判系统数据结构 +typedef struct __attribute__((packed)) +{ + uint16_t remain_hp; // 剩余血量 + uint8_t game_progress : 4; // 比赛进度 + uint16_t stage_remain_time; // 比赛剩余时间 +} Aimbot_RefereeData_t; + +typedef struct __attribute__((packed)) +{ + uint8_t id; // 数据包 ID: 0xA8 + Aimbot_RefereeData_t data; + uint16_t crc16; +} Aimbot_Referee_t; + +/*------------------------------------上位机back-------------------------------------*/ +typedef struct __attribute__((packed)) +{ + uint8_t head[2]; // 数据包头: {'M', 'R'} + uint8_t mode; // 0: 不控制, 1: 控制云台但不开火，2: 控制云台且开火 + float yaw; // 目标偏航角 + float yaw_vel; // 偏航角速度 + float yaw_acc; // 偏航角加速度 + float pitch; // 目标俯仰角 + float pitch_vel; // 俯仰角速度 + float pitch_acc; // 俯仰角加速度 + uint16_t crc16; // CRC16 校验 +} Aimbot_AI_t; + +typedef struct __attribute__((packed)) { + uint8_t mode; // 0: 不控制, 1: 控制云台但不开火, 2: 控制云台且开火 + struct{ +// Gimbal_CMD_t g_cmd; + struct{ + float yaw; // 目标偏航角 + float pit; // 目标俯仰角 + }setpoint; + + struct{ + float pit; // 俯仰角加速度 + float yaw; // 偏航角加速度 + }accl; + struct{ + float pit; // 俯仰角速度 + float yaw; // 偏航角速度 + }vel; + }gimbal_t; + + struct{ + float Vx; // X 方向速度 + float Vy; // Y 方向速度 + float Vw; // Z 方向角速度 + }chassis_t; + + uint8_t reserved; // 预留字段 +} Aimbot_AIResult_t; // 解析后的AI控制指令 + +/* ---------- UART 通信接口（上层板与上位机 PC 间） ---------- */ +int8_t Aimbot_MCUPack(Aimbot_MCU_t *mcu, const Gimbal_Feedback_t *gimbal_fb, + const AHRS_Quaternion_t *quat, + float bullet_speed, uint16_t bullet_count, uint8_t mode); +int8_t Aimbot_MCUStartSend(Aimbot_MCU_t *mcu); +int8_t Aimbot_AIGetResult(Aimbot_AI_t *ai, Aimbot_AIResult_t *result); +int8_t Aimbot_AIStartRecv(Aimbot_AI_t *ai); + +/* ---------- CAN 通信接口（下层板与上层板间） ---------- */ + +/** + * @brief 初始化 Aimbot CAN 通信，注册所有收发 CAN ID 队列 + * @param param CAN 通信参数 + * @return DEVICE_OK / DEVICE_ERR_NULL + */ +int8_t Aimbot_Init(Aimbot_Param_t *param); + +/** + * @brief 【下层板】打包反馈结构体 + * @param fb 输出：反馈数据 + * @param bullet_speed 弹速（m/s） + * @param bullet_count 已发射子弹数 + * @param mode 当前工作模式 + */ +int8_t Aimbot_PackFeedback(Aimbot_FeedbackData_t *fb, + float bullet_speed, uint16_t bullet_count, + uint8_t mode); + +/** + * @brief 【下层板】将反馈数据经 CAN 发送给上层板（1 帧） + */ +void Aimbot_SendFeedbackOnCAN(const Aimbot_Param_t *param, + const Aimbot_FeedbackData_t *fb); + +/** + * @brief 【下层板】从 CAN 解析上层板发来的 AI 指令 + * @return DEVICE_OK 表示成功取到新数据；DEVICE_ERR 表示队列暂无数据 + */ +int8_t Aimbot_ParseCmdFromCAN(const Aimbot_Param_t *param, + Aimbot_AIResult_t *result); + +/** + * @brief 【上层板】通过 CAN 将 AI 指令发送给下层板（单帧 8 字节） + */ +void Aimbot_SendCmdOnCAN(const Aimbot_Param_t *param, + const Aimbot_AIResult_t *cmd); + +/** + * @brief 【上层板】从 CAN 解析下层板发来的反馈数据（逐帧读取，非阻塞） + * @return DEVICE_OK（部分或全部帧已更新均返回 OK） + */ +int8_t Aimbot_ParseFeedbackFromCAN(const Aimbot_Param_t *param, + Aimbot_FeedbackData_t *fb); + +#ifdef __cplusplus +} +#endif diff --git a/User/module/cmd/cmd.c b/User/module/cmd/cmd.c index c13f9e5..1b96cb0 100644 --- a/User/module/cmd/cmd.c +++ b/User/module/cmd/cmd.c @@ -228,10 +228,7 @@ static void CMD_NUC_BuildShootCmd(CMD_t *ctx) { /* 根据AI模式决定射击行为 */ switch (ctx->input.nuc.mode) { case 0: - ctx->output.shoot.cmd.ready = false; - ctx->output.shoot.cmd.firecmd = false; - break; - case 1: + case 1: ctx->output.shoot.cmd.ready = true; ctx->output.shoot.cmd.firecmd = false; break; diff --git a/User/module/cmd/cmd_feature.h b/User/module/cmd/cmd_feature.h index e76a57a..0f9955e 100644 --- a/User/module/cmd/cmd_feature.h +++ b/User/module/cmd/cmd_feature.h @@ -16,8 +16,8 @@ /** PC 端键鼠输入 (通过 DR16 转发) */ #define CMD_ENABLE_SRC_PC 1 -/** NUC / AI 输入 (需要 vision_bridge 模块) */ -#define CMD_ENABLE_SRC_NUC 0 +/** NUC / AI 输入 */ +#define CMD_ENABLE_SRC_NUC 1 /** * 裁判系统数据中转开关 @@ -46,7 +46,7 @@ #define CMD_ENABLE_MODULE_ARM 0 /** 裁判系统UI命令模块 (需要 device/referee.h) */ -#define CMD_ENABLE_MODULE_REFUI 0 +#define CMD_ENABLE_MODULE_REFUI 1 /** 平衡底盘模块 (需要 module/balance_chassis.h) */ #define CMD_ENABLE_MODULE_BALANCE_CHASSIS 1 diff --git a/User/module/config.c b/User/module/config.c index ad087c9..2bc1baa 100644 --- a/User/module/config.c +++ b/User/module/config.c @@ -24,6 +24,11 @@ Config_RobotParam_t robot_config = { .width=1920, .height=1080, }, + .aimbot_param = { + .can = BSP_FDCAN_2, + .cmd_id = 0x520, /* 上层板→下层板 AI指令帧ID (共3帧: 0x520~0x522) */ + .fb_base_id = 0x524, /* 下层板→上层板 反馈起始ID */ + }, .gimbal_param = { .pid = { .yaw_omega = { diff --git a/User/module/config.h b/User/module/config.h index abdf5b0..cec9f02 100644 --- a/User/module/config.h +++ b/User/module/config.h @@ -16,6 +16,7 @@ extern "C" { #include "module/vision_bridge.h" #include "module/cmd/cmd.h" #include "device/referee_proto_types.h" +#include "module/aimbot.h" /** * @brief 机器人参数配置结构体 * @note 在此添加您的配置参数 @@ -28,6 +29,7 @@ typedef struct { AI_Param_t ai_param; CMD_Config_t cmd_param; Referee_Screen_t ref_screen; + Aimbot_Param_t aimbot_param; /* 下层板 ↔ 上层板 CAN 通信参数 */ /* USER CODE END Config_RobotParam */ } Config_RobotParam_t; diff --git a/User/module/gimbal.c b/User/module/gimbal.c index 47daf19..bca9898 100644 --- a/User/module/gimbal.c +++ b/User/module/gimbal.c @@ -19,7 +19,8 @@ /* Private macro ------------------------------------------------------------ */ /* Private variables -------------------------------------------------------- */ /* Private function -------------------------------------------------------- */ - +#define GIMBAL_YAW_INERTIA 0.05f +#define GIMBAL_PIT_INERTIA 0.03f /** * \brief 设置云台模式 * @@ -41,10 +42,15 @@ static int8_t Gimbal_SetMode(Gimbal_t *g, Gimbal_Mode_t mode) { PID_Reset(&g->pid.yaw_omega); PID_Reset(&g->pid.pit_angle); PID_Reset(&g->pid.pit_omega); + PID_Reset(&g->pid.aimbot.yaw_angle); + PID_Reset(&g->pid.aimbot.yaw_omega); + PID_Reset(&g->pid.aimbot.pit_angle); + PID_Reset(&g->pid.aimbot.pit_omega); LowPassFilter2p_Reset(&g->filter_out.yaw, 0.0f); LowPassFilter2p_Reset(&g->filter_out.pit, 0.0f); MOTOR_DM_Enable(&(g->param->yaw_motor)); + MOTOR_DM_Enable(&(g->param->pit_motor)); AHRS_ResetEulr(&(g->setpoint.eulr)); /* 切换模式后重置设定值 */ @@ -91,6 +97,14 @@ int8_t Gimbal_Init(Gimbal_t *g, const Gimbal_Params_t *param, &(g->param->pid.pit_angle)); PID_Init(&(g->pid.pit_omega), KPID_MODE_CALC_D, target_freq, &(g->param->pid.pit_omega)); + PID_Init(&(g->pid.aimbot.yaw_angle), KPID_MODE_SET_D, target_freq, + &(g->param->pid.aimbot.yaw_angle)); + PID_Init(&(g->pid.aimbot.yaw_omega), KPID_MODE_SET_D, target_freq, + &(g->param->pid.aimbot.yaw_omega)); + PID_Init(&(g->pid.aimbot.pit_angle), KPID_MODE_SET_D, target_freq, + &(g->param->pid.aimbot.pit_angle)); + PID_Init(&(g->pid.aimbot.pit_omega), KPID_MODE_SET_D, target_freq, + &(g->param->pid.aimbot.pit_omega)); LowPassFilter2p_Init(&g->filter_out.yaw, target_freq, g->param->low_pass_cutoff_freq.out); @@ -193,33 +207,18 @@ int8_t Gimbal_Control(Gimbal_t *g, Gimbal_CMD_t *g_cmd, Gimbal_AI_t *g_ai) { g->limit.pit.max, g->limit.pit.min, M_2PI); } CircleAdd(&(g->setpoint.eulr.pit), delta_pit, M_2PI); - + + if (g_cmd->mode == GIMBAL_MODE_AI_CONTROL) { + /* AI 模式：直接从指令获取角度设定值（每周期刷新） */ + g->setpoint.eulr.yaw = g_cmd->setpoint_yaw; + g->setpoint.eulr.pit = g_cmd->setpoint_pit; + } /* 控制相关逻辑 */ float yaw_omega_set_point, pit_omega_set_point; switch (g->mode) { case GIMBAL_MODE_RELAX: g->out.yaw = 0.0f; g->out.pit = 0.0f; - break; - case GIMBAL_MODE_AI_CONTROL: - if (g_ai != NULL && g_ai->ctrl) { - g->setpoint.eulr.yaw = g_ai->yaw; - g->setpoint.eulr.pit = -g_ai->pit; - - /* 限位处理 */ - if (g->param->travel.yaw > 0) { - CircleAngleLimit(&g->setpoint.eulr.yaw, - g->feedback.motor.yaw.rotor_abs_angle, - g->feedback.imu.eulr.yaw, - g->limit.yaw.max, g->limit.yaw.min, M_2PI); - } - if (g->param->travel.pit > 0) { - CircleAngleLimit(&g->setpoint.eulr.pit, - g->feedback.motor.pit.rotor_abs_angle, - g->feedback.imu.eulr.rol, - g->limit.pit.max, g->limit.pit.min, M_2PI); - } - } /* fallthrough - AI控制模式也需要执行PID计算 */ case GIMBAL_MODE_ABSOLUTE: @@ -234,6 +233,28 @@ int8_t Gimbal_Control(Gimbal_t *g, Gimbal_CMD_t *g_cmd, Gimbal_AI_t *g_ai) { g->out.pit = PID_Calc(&(g->pid.pit_omega), pit_omega_set_point, g->feedback.imu.gyro.y, 0.f, g->dt); + break; + break; + case GIMBAL_MODE_AI_CONTROL: + /* --- YAW --- */ + // 位置环: Kp * (pos_ref - pos_fdb) + yaw_omega_set_point = PID_Calc(&(g->pid.aimbot.yaw_angle), + g->setpoint.eulr.yaw, + g->feedback.imu.eulr.yaw, 0.0f, g->dt); + // 速度环: Kd * (vel_ref - vel_fdb)，用上位机下发的 yaw_vel 作为前馈参考 + g->out.yaw = PID_Calc(&(g->pid.aimbot.yaw_omega), + yaw_omega_set_point + g_cmd->ff_vel_yaw, + g->feedback.imu.gyro.z, 0.0f, g->dt) + + g_cmd->ff_accl_yaw * GIMBAL_YAW_INERTIA; // 加速度前馈: J * acc + + /* --- PITCH --- */ + pit_omega_set_point = PID_Calc(&(g->pid.aimbot.pit_angle), + g->setpoint.eulr.pit, + g->feedback.imu.eulr.pit, 0.0f, g->dt); + g->out.pit = PID_Calc(&(g->pid.aimbot.pit_omega), + pit_omega_set_point + g_cmd->ff_vel_pit, + g->feedback.imu.gyro.x, 0.0f, g->dt) + + g_cmd->ff_accl_pit * GIMBAL_PIT_INERTIA; // 加速度前馈: J * acc break; } diff --git a/User/module/gimbal.h b/User/module/gimbal.h index e9a03e9..92ddd5f 100644 --- a/User/module/gimbal.h +++ b/User/module/gimbal.h @@ -42,6 +42,13 @@ typedef struct { Gimbal_Mode_t mode; float delta_yaw; float delta_pit; + /* GIMBAL_MODE_AI_CONTROL 速度/加速度前馈（来自 NUC） */ + float setpoint_yaw; + float setpoint_pit; + float ff_vel_yaw; /* yaw 速度前馈（rad/s），叠加到内环角速度设定值 */ + float ff_vel_pit; /* pit 速度前馈（rad/s） */ + float ff_accl_yaw; /* yaw 加速度前馈（归一化），叠加到力矩输出 */ + float ff_accl_pit; /* pit 加速度前馈（归一化） */ } Gimbal_CMD_t; typedef struct { @@ -66,6 +73,12 @@ typedef struct { KPID_Params_t yaw_angle; /* yaw轴角位置环PID参数 */ KPID_Params_t pit_omega; /* pitch轴角速度环PID参数 */ KPID_Params_t pit_angle; /* pitch轴角位置环PID参数 */ + struct { + KPID_t yaw_angle; /* yaw轴角位置环PID */ + KPID_t yaw_omega; /* yaw轴角速度环PID */ + KPID_t pit_angle; /* pitch轴角位置环PID */ + KPID_t pit_omega; /* pitch轴角速度环PID */ + } aimbot; } pid; /* 低通滤波器截止频率 */ @@ -128,6 +141,12 @@ typedef struct { KPID_t yaw_omega; /* yaw轴角速度环PID */ KPID_t pit_angle; /* pitch轴角位置环PID */ KPID_t pit_omega; /* pitch轴角速度环PID */ + struct { + KPID_t yaw_angle; /* yaw轴角位置环PID */ + KPID_t yaw_omega; /* yaw轴角速度环PID */ + KPID_t pit_angle; /* pitch轴角位置环PID */ + KPID_t pit_omega; /* pitch轴角速度环PID */ + } aimbot; } pid; struct { diff --git a/User/module/shoot.c b/User/module/shoot.c index 1774165..847efa6 100644 --- a/User/module/shoot.c +++ b/User/module/shoot.c @@ -41,15 +41,18 @@ void Task(void *argument) { /* Private typedef ---------------------------------------------------------- */ /* Private define ----------------------------------------------------------- */ #define MAX_FRIC_RPM 7000.0f -#define MAX_TRIG_RPM 7000.0f//这里可能也会影响最高发射频率，待测试 +#define MAX_TRIG_RPM 1500.0f//这里可能也会影响最高发射频率，待测试 /* 发射检测参数 */ #define SHOT_DETECT_RPM_DROP_THRESHOLD 100.0f /* 摩擦轮转速下降阈值，单位rpm */ #define SHOT_DETECT_SUSPECTED_TIME 0.0005f /* 发射嫌疑持续时间阈值，单位秒 */ +#define FRIC_READY_RPM_RATIO 0.95f /* 摩擦轮准备好的转速比例 */ /* Private macro ------------------------------------------------------------ */ /* Private variables -------------------------------------------------------- */ static bool last_firecmd; + +float maxTrigrpm=4000.0f; /* Private function -------------------------------------------------------- */ /** @@ -180,7 +183,7 @@ int8_t Shoot_CaluTargetRPM(Shoot_t *s, float target_speed) s->target_variable.fric_rpm=5000.0f; break; case SHOOT_PROJECTILE_42MM: - s->target_variable.fric_rpm=6500.0f;//6500 + s->target_variable.fric_rpm=5000.0f;//6500 break; } return SHOOT_OK; @@ -207,7 +210,7 @@ static int8_t Shoot_UpdateHeatControl(Shoot_t *s) s->heatcontrol.ncd = s->heatcontrol.Hcd / s->heatcontrol.Hgen; } else { s->heatcontrol.ncd = 0.0f; - } + } return SHOOT_OK; } @@ -235,6 +238,40 @@ static float Shoot_CalcAvgFricRpm(Shoot_t *s) return (fric_num > 0) ? (sum / fric_num) : 0.0f; } +/** + * \brief 发射检测：通过摩擦轮掉速检测发射 + * + * \param s 包含发射数据的结构体 + * + * \return 是否检测到掉速超过阈值 + */ +static bool Shoot_DetectShotByRpmDrop(Shoot_t *s) +{ + if (s == NULL) { + return false; + } + + /* 只在READY和FIRE状态下检测，避免停机时误判 */ + if (s->running_state != SHOOT_STATE_READY && + s->running_state != SHOOT_STATE_FIRE) { + return false; + } + + /* 计算当前摩擦轮平均转速 */ + s->heatcontrol.avgFricRpm = Shoot_CalcAvgFricRpm(s); + + /* 计算当前转速与目标转速的差值（掉速量） */ + s->heatcontrol.rpmDrop = s->target_variable.fric_rpm - s->heatcontrol.avgFricRpm; + + /* 判断是否发生明显掉速 */ + if (s->heatcontrol.rpmDrop > SHOT_DETECT_RPM_DROP_THRESHOLD) { + return true; + } + + return false; +} + + /** * \brief 热量数据融合：用裁判系统数据修正自主估计 * @@ -318,29 +355,18 @@ static int8_t Shoot_HeatDetectionFSM(Shoot_t *s) /* 计算当前平均转速 */ s->heatcontrol.avgFricRpm = Shoot_CalcAvgFricRpm(s); - /* 检查摩擦轮是否初步达到设定速度 */ - if (s->target_variable.fric_rpm - s->heatcontrol.avgFricRpm < SHOT_DETECT_RPM_DROP_THRESHOLD * 0.8f) { + /* 检查摩擦轮是否达到目标转速的85%以上 */ + if (s->heatcontrol.avgFricRpm >= s->target_variable.fric_rpm * FRIC_READY_RPM_RATIO) { s->heatcontrol.detectState = SHOOT_HEAT_DETECT_READY; - s->heatcontrol.baselineRpm = s->heatcontrol.avgFricRpm; // 记录初始基准转速 + s->heatcontrol.lastAvgFricRpm = s->heatcontrol.avgFricRpm; } } break; case SHOOT_HEAT_DETECT_READY: - /* 准备检测状态：监测摩擦轮相对当前的基准发生掉速 */ - s->heatcontrol.avgFricRpm = Shoot_CalcAvgFricRpm(s); - - /* 动态更新平稳时的基准转速：若当前转速比基准高，迅速成为新基准；若更低，基准非常缓慢地衰减，以适应正常波动 */ - if (s->heatcontrol.avgFricRpm > s->heatcontrol.baselineRpm) { - s->heatcontrol.baselineRpm = s->heatcontrol.avgFricRpm; - } else { - s->heatcontrol.baselineRpm = s->heatcontrol.baselineRpm * 0.999f + s->heatcontrol.avgFricRpm * 0.001f; - } - - s->heatcontrol.rpmDrop = s->heatcontrol.baselineRpm - s->heatcontrol.avgFricRpm; - - /* 检测掉速（当前转速低于动态基准超过阈值） */ - if (s->heatcontrol.rpmDrop > SHOT_DETECT_RPM_DROP_THRESHOLD) { + /* 准备检测状态：监测摩擦轮掉速 */ + /* 检测掉速（当前转速低于目标转速超过阈值） */ + if (Shoot_DetectShotByRpmDrop(s)) { s->heatcontrol.detectState = SHOOT_HEAT_DETECT_SUSPECTED; s->heatcontrol.detectTime = s->timer.now; /* 记录进入嫌疑状态的时间 */ } @@ -353,47 +379,45 @@ static int8_t Shoot_HeatDetectionFSM(Shoot_t *s) case SHOOT_HEAT_DETECT_SUSPECTED: /* 发射嫌疑状态：持续检测掉速 */ + /* 更新掉速量 */ s->heatcontrol.avgFricRpm = Shoot_CalcAvgFricRpm(s); - s->heatcontrol.rpmDrop = s->heatcontrol.baselineRpm - s->heatcontrol.avgFricRpm; + s->heatcontrol.rpmDrop = s->target_variable.fric_rpm - s->heatcontrol.avgFricRpm; - /* 若掉速消失（转速刚掉下又马上弹回基准附近），回到准备状态 (阈值的 60%) */ - if (s->heatcontrol.rpmDrop < SHOT_DETECT_RPM_DROP_THRESHOLD * 0.6f) { + /* 若掉速消失（转速恢复正常），回到准备状态 */ + if (s->heatcontrol.rpmDrop < SHOT_DETECT_RPM_DROP_THRESHOLD) { s->heatcontrol.detectState = SHOOT_HEAT_DETECT_READY; } - /* 若掉速状态持续超过设定的嫌疑时间，确认这是一次发射 */ + /* 若嫌疑状态持续超过阈值时间，确认发射 */ else if ((s->timer.now - s->heatcontrol.detectTime) >= SHOT_DETECT_SUSPECTED_TIME) { s->heatcontrol.detectState = SHOOT_HEAT_DETECT_CONFIRMED; } break; case SHOOT_HEAT_DETECT_CONFIRMED: - if (s->heatcontrol.unverified_shots > 0) { - s->heatcontrol.unverified_shots--; - } - s->heatcontrol.detectState = SHOOT_HEAT_DETECT_RECOVERING; - s->heatcontrol.valleyRpm = s->heatcontrol.avgFricRpm; /* 记录进入此时的转速为谷底初值 */ - break; - - case SHOOT_HEAT_DETECT_RECOVERING: - - s->heatcontrol.avgFricRpm = Shoot_CalcAvgFricRpm(s); - - if (s->heatcontrol.avgFricRpm < s->heatcontrol.valleyRpm) { - s->heatcontrol.valleyRpm = s->heatcontrol.avgFricRpm; + /* 确认发射状态：增加热量并返回准备状态 */ + /* 根据弹丸类型增加估计热量 */ + switch (s->param->basic.projectileType) { + case SHOOT_PROJECTILE_17MM: + s->heatcontrol.Hnow_estimated += 10.0f; + break; + case SHOOT_PROJECTILE_42MM: + s->heatcontrol.Hnow_estimated += 100.0f; + break; + default: + s->heatcontrol.Hnow_estimated += s->heatcontrol.Hgen; + break; } - if ((s->heatcontrol.avgFricRpm - s->heatcontrol.valleyRpm > 30.0f) || - (s->target_variable.fric_rpm - s->heatcontrol.avgFricRpm < SHOT_DETECT_RPM_DROP_THRESHOLD * 0.8f)) { - - s->heatcontrol.detectState = SHOOT_HEAT_DETECT_READY; - - s->heatcontrol.baselineRpm = s->heatcontrol.avgFricRpm; + /* 限制估计热量不超过最大值 */ + if (s->heatcontrol.Hnow_estimated > s->heatcontrol.Hmax) { + s->heatcontrol.Hnow_estimated = s->heatcontrol.Hmax; } - /* 如果摩擦轮停止 */ - if (s->running_state == SHOOT_STATE_IDLE) { - s->heatcontrol.detectState = SHOOT_HEAT_DETECT_IDLE; - } + /* 增加发射计数 */ + s->heatcontrol.shots_detected++; + + /* 返回准备检测状态 */ + s->heatcontrol.detectState = SHOOT_HEAT_DETECT_READY; break; default: @@ -401,7 +425,7 @@ static int8_t Shoot_HeatDetectionFSM(Shoot_t *s) break; } - /* 热量冷却*/ + /* 善后工作：热量冷却（每个周期都执行） */ if (s->heatcontrol.Hnow_estimated > 0.0f && s->heatcontrol.Hcd > 0.0f) { s->heatcontrol.Hnow_estimated -= s->heatcontrol.Hcd * s->timer.dt; if (s->heatcontrol.Hnow_estimated < 0.0f) { @@ -409,36 +433,6 @@ static int8_t Shoot_HeatDetectionFSM(Shoot_t *s) } } - /* 空弹链检测与修正：拨盘触发后超过0.4秒内仍未有对应的摩擦轮掉速确认 */ - if (s->heatcontrol.unverified_shots > 0 && (s->timer.now - s->var_trig.time_lastShoot) > 0.4f) { - /* 这些弹丸确定未射出（空发） */ - float refund_heat = 0.0f; - switch (s->param->basic.projectileType) { - case SHOOT_PROJECTILE_17MM: refund_heat = 10.0f; break; - case SHOOT_PROJECTILE_42MM: refund_heat = 100.0f; break; - default: refund_heat = s->heatcontrol.Hgen; break; - } - refund_heat *= s->heatcontrol.unverified_shots; - - s->heatcontrol.Hnow_estimated -= refund_heat; - if (s->heatcontrol.Hnow_estimated < 0.0f) { - s->heatcontrol.Hnow_estimated = 0.0f; - } - - if (s->var_trig.num_shooted >= s->heatcontrol.unverified_shots) { - s->var_trig.num_shooted -= s->heatcontrol.unverified_shots; - } else { - s->var_trig.num_shooted = 0; - } - - if (s->heatcontrol.shots_detected >= s->heatcontrol.unverified_shots) { - s->heatcontrol.shots_detected -= s->heatcontrol.unverified_shots; - } else { - s->heatcontrol.shots_detected = 0; - } - - s->heatcontrol.unverified_shots = 0; - } /* 数据融合 */ Shoot_FuseHeatData(s); @@ -508,22 +502,18 @@ static float Shoot_CaluFreqByHeat(Shoot_t *s) */ int8_t Shoot_CaluTargetAngle(Shoot_t *s, Shoot_CMD_t *cmd) { - if (s == NULL) { + if (s == NULL || s->var_trig.num_toShoot == 0) { return SHOOT_ERR_NULL; } /* 更新热量控制数据 */ Shoot_UpdateHeatControl(s); - if (s->var_trig.num_toShoot == 0) { - return SHOOT_OK; /* 即使没有弹丸待发，热量更新照常执行 */ - } - /* 根据热量控制计算实际射频 */ float actual_freq = Shoot_CaluFreqByHeat(s); float dt = s->timer.now - s->var_trig.time_lastShoot; - float dpos; + float dpos; dpos = CircleError(s->target_variable.trig_angle, s->var_trig.trig_agl, M_2PI); /* 使用热量控制计算出的射频，而不是配置的固定射频 */ @@ -531,25 +521,10 @@ int8_t Shoot_CaluTargetAngle(Shoot_t *s, Shoot_CMD_t *cmd) { s->var_trig.time_lastShoot=s->timer.now; CircleAdd(&s->target_variable.trig_angle, M_2PI/s->param->basic.num_trig_tooth, M_2PI); - s->var_trig.num_toShoot--; - s->var_trig.num_shooted++; - - /* 预测性发射：拨盘拨动即认为弹丸会射出，立即增加热量与发射计数 */ - float add_heat = 0.0f; - switch (s->param->basic.projectileType) { - case SHOOT_PROJECTILE_17MM: add_heat = 10.0f; break; - case SHOOT_PROJECTILE_42MM: add_heat = 100.0f; break; - default: add_heat = s->heatcontrol.Hgen; break; - } - s->heatcontrol.Hnow_estimated += add_heat; - if (s->heatcontrol.Hnow_estimated > s->heatcontrol.Hmax) { - s->heatcontrol.Hnow_estimated = s->heatcontrol.Hmax; - } - s->heatcontrol.shots_detected++; - - /* 增加等待摩擦轮掉速确认的弹丸数 */ - s->heatcontrol.unverified_shots++; + s->var_trig.num_toShoot--; + s->var_trig.num_shooted++; } + return SHOOT_OK; } @@ -630,7 +605,7 @@ int8_t Shoot_UpdateFeedback(Shoot_t *s) s->var_trig.trig_agl = M_2PI - s->var_trig.trig_agl; } s->var_trig.fil_trig_rpm = LowPassFilter2p_Apply(&s->filter.trig.in, s->feedback.trig.feedback.rotor_speed); - s->var_trig.trig_rpm = s->feedback.trig.feedback.rotor_speed / MAX_TRIG_RPM; + s->var_trig.trig_rpm = s->feedback.trig.feedback.rotor_speed / maxTrigrpm; if(s->var_trig.trig_rpm>1.0f)s->var_trig.trig_rpm=1.0f; if(s->var_trig.trig_rpm<-1.0f)s->var_trig.trig_rpm=-1.0f; @@ -652,17 +627,16 @@ int8_t Shoot_RunningFSM(Shoot_t *s, Shoot_CMD_t *cmd) return SHOOT_ERR_NULL; // 参数错误 } uint8_t fric_num = s->param->basic.fric_num; - + static float pos; if(s->mode==SHOOT_MODE_SAFE){ for(int i=0;iparam->motor.fric[i].param); } - MOTOR_RM_Relax(&s->param->motor.trig); - s->target_variable.trig_angle=s->var_trig.trig_agl; + MOTOR_RM_Relax(&s->param->motor.trig);\ + pos=s->target_variable.trig_angle=s->var_trig.trig_agl; } else{ - Shoot_CaluTargetAngle(s, cmd); switch(s->running_state) { case SHOOT_STATE_IDLE:/*熄火等待*/ @@ -675,7 +649,7 @@ int8_t Shoot_RunningFSM(Shoot_t *s, Shoot_CMD_t *cmd) MOTOR_RM_SetOutput(&s->param->motor.fric[i].param, s->output.lpfout_fric[i]); } - s->output.outagl_trig =PID_Calc(&s->pid.trig,s->target_variable.trig_angle,s->var_trig.trig_agl,0,s->timer.dt); + s->output.outagl_trig =PID_Calc(&s->pid.trig,pos,s->var_trig.trig_agl,0,s->timer.dt); s->output.outomg_trig =PID_Calc(&s->pid.trig_omg,s->output.outagl_trig,s->var_trig.trig_rpm,0,s->timer.dt); s->output.outlpf_trig =LowPassFilter2p_Apply(&s->filter.trig.out, s->output.outomg_trig); MOTOR_RM_SetOutput(&s->param->motor.trig, s->output.outlpf_trig); @@ -719,7 +693,7 @@ int8_t Shoot_RunningFSM(Shoot_t *s, Shoot_CMD_t *cmd) } /* 设置拨弹电机输出 */ s->output.outagl_trig =PID_Calc(&s->pid.trig, - s->target_variable.trig_angle, + pos, s->var_trig.trig_agl, 0, s->timer.dt); @@ -761,6 +735,7 @@ int8_t Shoot_RunningFSM(Shoot_t *s, Shoot_CMD_t *cmd) break; case SHOOT_STATE_FIRE:/*射击*/ + Shoot_CaluTargetAngle(s, cmd); for(int i=0;iparam->motor.fric[i].level-1; @@ -805,7 +780,7 @@ int8_t Shoot_RunningFSM(Shoot_t *s, Shoot_CMD_t *cmd) if(!cmd->firecmd) { s->running_state=SHOOT_STATE_READY; - s->target_variable.trig_angle=s->var_trig.trig_agl; + pos=s->var_trig.trig_agl; s->var_trig.num_toShoot=0; } break; @@ -991,10 +966,10 @@ int8_t Shoot_Control(Shoot_t *s, Shoot_CMD_t *cmd) s->timer.now = BSP_TIME_Get_us() / 1000000.0f; s->timer.dt = (BSP_TIME_Get_us() - s->timer.lask_wakeup) / 1000000.0f; s->timer.lask_wakeup = BSP_TIME_Get_us(); - Shoot_CaluTargetRPM(s,233); + // Shoot_CaluTargetRPM(s,233); /* 运行热量检测状态机 */ - Shoot_HeatDetectionFSM(s); + // Shoot_HeatDetectionFSM(s); /* 运行卡弹检测状态机 */ Shoot_JamDetectionFSM(s, cmd); @@ -1013,3 +988,7 @@ void Shoot_DumpUI(Shoot_t *s, Shoot_RefereeUI_t *ui) { ui->fire = s->running_state; } +void Shoot_DumpAI(Shoot_t *s, Shoot_AI_t *ai) { + ai->num_shooted=s->var_trig.num_shooted; + ai->bullet_speed=12.0f; +} diff --git a/User/module/shoot.h b/User/module/shoot.h index cd50342..0f14916 100644 --- a/User/module/shoot.h +++ b/User/module/shoot.h @@ -35,8 +35,7 @@ typedef enum { SHOOT_HEAT_DETECT_IDLE = 0, /* 停机状态 */ SHOOT_HEAT_DETECT_READY, /* 准备检测状态 */ SHOOT_HEAT_DETECT_SUSPECTED, /* 发射嫌疑状态 */ - SHOOT_HEAT_DETECT_CONFIRMED, /* 确认发射状态 */ - SHOOT_HEAT_DETECT_RECOVERING /* 发射恢复状态 */ + SHOOT_HEAT_DETECT_CONFIRMED /* 确认发射状态 */ }Shoot_HeatDetectionFSM_State_t; typedef enum { SHOOT_STATE_IDLE = 0,/* 熄火 */ @@ -52,6 +51,11 @@ typedef enum { SHOOT_MODE_NUM }Shoot_Mode_t; +typedef struct { + uint16_t num_shooted;/* 已发射弹数 */ + float bullet_speed;/* 目标弹速 */ +}Shoot_AI_t; + /* UI 导出结构（供 referee 系统绘制） */ typedef struct { Shoot_Mode_t mode; @@ -115,8 +119,7 @@ typedef struct { Shoot_HeatDetectionFSM_State_t detectState; // 检测状态 float detectTime; // 检测计时器 float avgFricRpm; // 摩擦轮平均转速 - float baselineRpm; // 动态基准转速，用于连发掉速检测的参照 - float valleyRpm; // 掉速谷底转速，用于判断掉速回升 + float lastAvgFricRpm; // 上次摩擦轮平均转速 float rpmDrop; // 转速下降量 bool shotDetected; // 检测到发射标志 @@ -125,7 +128,6 @@ typedef struct { float Hnow_fused; // 融合后的热量值 uint16_t shots_detected; // 检测到的发射数 uint16_t shots_available;// 当前热量可发射弹数 - uint16_t unverified_shots; // 已经拨弹但等待摩擦轮掉速确认的弹丸数 }Shoot_HeatControl_t; typedef struct { @@ -219,6 +221,7 @@ typedef struct { struct { float fric_rpm; /* 目标摩擦轮转速 */ float trig_angle;/* 目标拨弹位置 */ + float bullet_speed;/* 目标弹速 */ }target_variable; /* 反馈控制用的PID */ @@ -295,7 +298,7 @@ int8_t Shoot_Control(Shoot_t *s, Shoot_CMD_t *cmd); */ void Shoot_DumpUI(Shoot_t *s, Shoot_RefereeUI_t *ui); - +void Shoot_DumpAI(Shoot_t *s, Shoot_AI_t *ai) ; #ifdef __cplusplus } #endif diff --git a/User/task/ai.c b/User/task/ai.c index ea31f72..3929bf7 100644 --- a/User/task/ai.c +++ b/User/task/ai.c @@ -1,15 +1,16 @@ /* - ai Task - + AI Task - 下层板 */ /* Includes ----------------------------------------------------------------- */ #include "task/user_task.h" /* USER INCLUDE BEGIN */ -#include "bsp/fdcan.h" +#include "module/aimbot.h" #include "module/config.h" #include "module/gimbal.h" -#include "module/vision_bridge.h" +#include "module/shoot.h" +#include "device/referee_proto_types.h" +#include /* USER INCLUDE END */ /* Private typedef ---------------------------------------------------------- */ @@ -17,20 +18,20 @@ /* Private macro ------------------------------------------------------------ */ /* Private variables -------------------------------------------------------- */ /* USER STRUCT BEGIN */ -AI_cmd_t cmd_ai; -AI_t ai; -Gimbal_AI_t ai_for_gimbal; +static Aimbot_Param_t aimbot_can; /* CAN 通信参数（从 config 拷贝） */ +static Aimbot_FeedbackData_t lower_board_fb; /* 打包好发给上层板的数据 */ +static Aimbot_AIResult_t ai_cmd_from_can; /* 上层板下发并传到底板的指令 */ +static AI_cmd_t for_cmdnuc; /* 转换后发往 cmd.nuc 的指令 */ + +static Referee_ForAI_t ai_ref; /* 裁判系统转发给 AI 的数据 */ +static Shoot_AI_t raw_shoot; /* 发射机构反馈 */ /* USER STRUCT END */ /* Private function --------------------------------------------------------- */ -/* USER PRIVATE CODE BEGIN */ - -/* USER PRIVATE CODE END */ /* Exported functions ------------------------------------------------------- */ void Task_ai(void *argument) { (void)argument; /* 未使用argument，消除警告 */ - /* 计算任务运行到指定频率需要等待的tick数 */ const uint32_t delay_tick = osKernelGetTickFreq() / AI_FREQ; @@ -38,25 +39,49 @@ void Task_ai(void *argument) { uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计时 */ /* USER CODE INIT BEGIN */ - AI_Init(&ai, &Config_GetRobotParam()->ai_param); - /* 注册CAN接收ID */ + aimbot_can = Config_GetRobotParam()->aimbot_param; + Aimbot_Init(&aimbot_can); /* USER CODE INIT END */ - + while (1) { tick += delay_tick; /* 计算下一个唤醒时刻 */ /* USER CODE BEGIN */ - AI_ParseCmdFromCan( &ai,&cmd_ai); - if (cmd_ai.mode != 0){ - ai_for_gimbal.ctrl = true; - } else { - ai_for_gimbal.ctrl = false; + + /* 1. 读取裁判系统数据（获知机器人系统状态/弹速等） */ + osMessageQueueGet(task_runtime.msgq.ai.ref, &ai_ref, NULL, 0); + + /* 2. 读取发射机构反馈数据（非阻塞），以获取弹速、弹数等信息 + todo: 检查是否有单独发弹速消息队列 + */ + osMessageQueueGet(task_runtime.msgq.ai.rawdata_FromShoot, &raw_shoot, NULL, 0); + + /* 3. 打包当前底层板的数据（弹速、弹数、模式），通过CAN发送给上层板 + 由于现在只能拿到一部分，预留测试值，实车时接入射击模块返回的信息 + */ + // TODO: 使用实际读取到的 bullet_speed 和 bullet_count + float current_bullet_speed = raw_shoot.bullet_speed; + uint16_t current_bullet_count = raw_shoot.num_shooted; + uint8_t current_mode = 1; + + Aimbot_PackFeedback(&lower_board_fb, current_bullet_speed, current_bullet_count, current_mode); + Aimbot_SendFeedbackOnCAN(&aimbot_can, &lower_board_fb); + + /* 4. 解析上层板发来的 AI 指令（非阻塞提取） */ + if (Aimbot_ParseCmdFromCAN(&aimbot_can, &ai_cmd_from_can) == DEVICE_OK) { + /* 将 Aimbot_AIResult_t 转换为 AI_cmd_t 并推送到 cmd.nuc 队列 */ + for_cmdnuc.mode = ai_cmd_from_can.mode; + for_cmdnuc.gimbal.setpoint.yaw = ai_cmd_from_can.gimbal_t.setpoint.yaw; + for_cmdnuc.gimbal.setpoint.pit = ai_cmd_from_can.gimbal_t.setpoint.pit; + for_cmdnuc.gimbal.vel.yaw = ai_cmd_from_can.gimbal_t.vel.yaw; + for_cmdnuc.gimbal.vel.pit = ai_cmd_from_can.gimbal_t.vel.pit; + for_cmdnuc.gimbal.accl.yaw = ai_cmd_from_can.gimbal_t.accl.yaw; + for_cmdnuc.gimbal.accl.pit = ai_cmd_from_can.gimbal_t.accl.pit; + + osMessageQueueReset(task_runtime.msgq.cmd.nuc); + osMessageQueuePut(task_runtime.msgq.cmd.nuc, &for_cmdnuc, 0, 0); } - ai_for_gimbal.yaw = cmd_ai.gimbal.setpoint.yaw; - ai_for_gimbal.pit = cmd_ai.gimbal.setpoint.pit; - osMessageQueueReset(task_runtime.msgq.gimbal.ai_cmd); - osMessageQueuePut(task_runtime.msgq.gimbal.ai_cmd, &ai_for_gimbal, 0, 0); + /* USER CODE END */ osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */ } - } \ No newline at end of file diff --git a/User/task/cmd.c b/User/task/cmd.c index 58f338f..9f2193c 100644 --- a/User/task/cmd.c +++ b/User/task/cmd.c @@ -13,7 +13,7 @@ #include "module/shoot.h" #include "module/cmd/cmd.h" #include "bsp/fdcan.h" -#include "module/vision_bridge.h" + //#define DEAD_AREA 0.05f /* USER INCLUDE END */ @@ -28,7 +28,7 @@ DR16_t cmd_dr16; #elif CMD_RCTypeTable_Index == 1 AT9S_t cmd_at9s; #endif -// AI_cmd_t cmd_ai; +AI_cmd_t cmd_ai; #if CMD_ENABLE_SRC_REF CMD_RawInput_REF_t cmd_ref; @@ -53,8 +53,6 @@ void Task_cmd(void *argument) { uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计时 */ /* USER CODE INIT BEGIN */ CMD_Init(&cmd, &Config_GetRobotParam()->cmd_param); - // AI_Init(&ai, &Config_GetRobotParam()->ai_param); - /* 注册CAN接收ID */ /* USER CODE INIT END */ while (1) { @@ -65,10 +63,7 @@ void Task_cmd(void *argument) { #elif CMD_RCTypeTable_Index == 1 osMessageQueueGet(task_runtime.msgq.cmd.rc, &cmd_at9s, NULL, 0); #endif - - // /* 从CAN2接收AI命令 */ - // AI_ParseCmdFromCan( &ai,&cmd_ai); - // #error "弄好自瞄之后统一改" + osMessageQueueGet(task_runtime.msgq.cmd.nuc, &cmd_ai, NULL, 0); #if CMD_ENABLE_SRC_REF /* 从裁判系统读取最新数据（非阻塞） */ diff --git a/User/task/init.c b/User/task/init.c index a58c4e2..6093a57 100644 --- a/User/task/init.c +++ b/User/task/init.c @@ -61,7 +61,12 @@ void Task_Init(void *argument) { task_runtime.msgq.gimbal.ai_cmd = osMessageQueueNew(2u, sizeof(Gimbal_AI_t), NULL); task_runtime.msgq.cmd.rc= osMessageQueueNew(3u, sizeof(DR16_t), NULL); - /* 裁判系统 */ + + /* AI */ + task_runtime.msgq.ai.rawdata_FromGimbal = osMessageQueueNew(2u, sizeof(Gimbal_Feedback_t), NULL); + task_runtime.msgq.ai.rawdata_FromIMU = osMessageQueueNew(2u, sizeof(AHRS_Quaternion_t), NULL); + task_runtime.msgq.ai.rawdata_FromShoot = osMessageQueueNew(2u, sizeof(Shoot_AI_t), NULL); + /* 裁判系统 */ task_runtime.msgq.referee.tocmd.ai= osMessageQueueNew(2u, sizeof(Referee_ForAI_t), NULL); task_runtime.msgq.referee.tocmd.cap= osMessageQueueNew(2u, sizeof(Referee_ForCap_t), NULL); task_runtime.msgq.referee.tocmd.chassis= osMessageQueueNew(2u, sizeof(Referee_ForChassis_t), NULL); diff --git a/User/task/user_task.h b/User/task/user_task.h index 082c1e9..5ada786 100644 --- a/User/task/user_task.h +++ b/User/task/user_task.h @@ -87,14 +87,13 @@ typedef struct { struct { osMessageQueueId_t rc; osMessageQueueId_t ref; - osMessageQueueId_t ai; + osMessageQueueId_t nuc; }cmd; - struct { - osMessageQueueId_t quat; - osMessageQueueId_t move_vec; - osMessageQueueId_t eulr; - osMessageQueueId_t fire; - osMessageQueueId_t ref; + struct{ + osMessageQueueId_t rawdata_FromGimbal; + osMessageQueueId_t rawdata_FromShoot; + osMessageQueueId_t rawdata_FromIMU; + osMessageQueueId_t ref; /* Referee_ForAI_t, cmd转发 */ }ai; struct{ osMessageQueueId_t ref; /* Referee_ForCap_t, cmd转发 */ From 8ab6208ff1fa775408f0284b992a8551f8c08d41 Mon Sep 17 00:00:00 2001 From: xxxxm <2389287465@qq.com> Date: Mon, 16 Mar 2026 11:36:31 +0800 Subject: [PATCH 32/34] =?UTF-8?q?fix-module/shoot-heatControl,,,=E7=83=AD?= =?UTF-8?q?=E9=87=8F=E5=8F=82=E6=95=B0=E5=BE=85=E8=AE=BEconfig.c-129-134?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- User/module/config.c | 1 + User/module/shoot.c | 5 +++++ User/module/shoot.h | 1 + 3 files changed, 7 insertions(+) diff --git a/User/module/config.c b/User/module/config.c index 2bc1baa..5a58db8 100644 --- a/User/module/config.c +++ b/User/module/config.c @@ -127,6 +127,7 @@ Config_RobotParam_t robot_config = { }, .heatControl={ .enable=true, + .safe_shots=5, // 安全出弹余量 .nmax=18.0f, // 最大射频 Hz .Hwarn=200.0f, // 热量预警值 .Hsatu=100.0f, // 热量饱和值 diff --git a/User/module/shoot.c b/User/module/shoot.c index 847efa6..c9bdee0 100644 --- a/User/module/shoot.c +++ b/User/module/shoot.c @@ -512,6 +512,11 @@ int8_t Shoot_CaluTargetAngle(Shoot_t *s, Shoot_CMD_t *cmd) /* 根据热量控制计算实际射频 */ float actual_freq = Shoot_CaluFreqByHeat(s); + /* 检查可发射弹丸数是否满足安全余量 */ + if (s->param->heatControl.enable && s->heatcontrol.shots_available <= s->param->heatControl.safe_shots) { + actual_freq = 0.0f; /* 禁止发弹 */ + } + float dt = s->timer.now - s->var_trig.time_lastShoot; float dpos; dpos = CircleError(s->target_variable.trig_angle, s->var_trig.trig_agl, M_2PI); diff --git a/User/module/shoot.h b/User/module/shoot.h index 0f14916..13972ed 100644 --- a/User/module/shoot.h +++ b/User/module/shoot.h @@ -164,6 +164,7 @@ typedef struct { }jamDetection;/* 卡弹检测参数 */ struct { bool enable; /* 是否启用热量控制 */ + uint16_t safe_shots;/* 安全余量，当shots_available小于等于该值时禁止发弹 */ float nmax;//最大射频 float Hwarn;//热量预警值 float Hsatu;//热量饱和值 From d5d9628cf20fe73cd44fd56fbbb38cdf734a3b5c Mon Sep 17 00:00:00 2001 From: xxxxm <2389287465@qq.com> Date: Mon, 16 Mar 2026 22:06:24 +0800 Subject: [PATCH 33/34] =?UTF-8?q?=E5=A2=9E=E5=8A=A0=E4=BA=86pc=E6=93=8D?= =?UTF-8?q?=E4=BD=9C=E5=92=8C=E6=8C=89=E9=94=AE=E8=A1=8C=E4=B8=BA?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- User/module/cmd/cmd.c | 26 ++++++++------------- User/module/cmd/cmd_adapter.c | 1 + User/module/cmd/cmd_behavior.c | 41 ++++++++++++++++++++++++++-------- User/module/cmd/cmd_types.h | 21 ++++++++--------- User/module/config.c | 4 ++-- User/module/shoot.c | 6 ++--- User/task/blink.c | 5 +++++ User/task/rc.c | 15 +++++++++++++ 8 files changed, 78 insertions(+), 41 deletions(-) diff --git a/User/module/cmd/cmd.c b/User/module/cmd/cmd.c index 1b96cb0..e60c015 100644 --- a/User/module/cmd/cmd.c +++ b/User/module/cmd/cmd.c @@ -233,14 +233,14 @@ static void CMD_NUC_BuildShootCmd(CMD_t *ctx) { ctx->output.shoot.cmd.firecmd = false; break; case 2: - if (ctx->input.rc.sw[1]==CMD_SW_DOWN) { - ctx->output.shoot.cmd.ready = true; - ctx->output.shoot.cmd.firecmd = true; - }else { - ctx->output.shoot.cmd.ready = true; - ctx->output.shoot.cmd.firecmd = false; - } - + if ((ctx->active_source == CMD_SRC_PC && ctx->input.pc.mouse.l_click) || + (ctx->active_source == CMD_SRC_RC && ctx->input.rc.sw[1] == CMD_SW_DOWN)) { + ctx->output.shoot.cmd.ready = true; + ctx->output.shoot.cmd.firecmd = true; + } else { + ctx->output.shoot.cmd.ready = true; + ctx->output.shoot.cmd.firecmd = false; + } break; } } @@ -388,19 +388,11 @@ static void CMD_PC_BuildBalanceChassisCmd(CMD_t *ctx) { ctx->output.balance_chassis.cmd.mode = CHASSIS_MODE_WHELL_LEG_BALANCE; float vx = 0.0f, wz = 0.0f; - uint32_t kb = ctx->input.pc.keyboard.bitmap; - - //案件的命令要放到behavior里， - // if (kb & CMD_KEY_W) vx += sens->move_sens; - // if (kb & CMD_KEY_S) vx -= sens->move_sens; - // if (kb & CMD_KEY_A) wz += sens->move_sens; - // if (kb & CMD_KEY_D) wz -= sens->move_sens; - // if (kb & CMD_KEY_SHIFT) { vx *= sens->move_fast_mult; wz *= sens->move_fast_mult; } - // if (kb & CMD_KEY_CTRL) { vx *= sens->move_slow_mult; wz *= sens->move_slow_mult; } ctx->output.balance_chassis.cmd.move_vec.vx = vx; ctx->output.balance_chassis.cmd.move_vec.wz = wz; ctx->output.balance_chassis.cmd.move_vec.vy = 0.0f; + } #endif /* CMD_ENABLE_SRC_PC && CMD_ENABLE_MODULE_BALANCE_CHASSIS */ #if CMD_ENABLE_SRC_NUC && CMD_ENABLE_MODULE_REFUI diff --git a/User/module/cmd/cmd_adapter.c b/User/module/cmd/cmd_adapter.c index ca92383..9638392 100644 --- a/User/module/cmd/cmd_adapter.c +++ b/User/module/cmd/cmd_adapter.c @@ -64,6 +64,7 @@ int8_t CMD_DR16_PC_GetInput(void *data, CMD_RawInput_t *output) { /* PC端鼠标映射 */ output->pc.mouse.x = dr16->data.mouse.x; output->pc.mouse.y = dr16->data.mouse.y; + output->pc.mouse.z = dr16->data.mouse.z; output->pc.mouse.l_click = dr16->data.mouse.l_click; output->pc.mouse.r_click = dr16->data.mouse.r_click; diff --git a/User/module/cmd/cmd_behavior.c b/User/module/cmd/cmd_behavior.c index 32e6d65..e47e96a 100644 --- a/User/module/cmd/cmd_behavior.c +++ b/User/module/cmd/cmd_behavior.c @@ -3,6 +3,7 @@ */ #include "cmd_behavior.h" #include "cmd.h" +#include "module/balance_chassis.h" #if CMD_ENABLE_MODULE_GIMBAL #include "module/gimbal.h" #endif @@ -16,6 +17,9 @@ int8_t CMD_Behavior_Handle_FORE(CMD_t *ctx) { #if CMD_ENABLE_MODULE_CHASSIS ctx->output.chassis.cmd.ctrl_vec.vy += ctx->config->sensitivity.move_sens; +#endif +#if CMD_ENABLE_MODULE_BALANCE_CHASSIS + ctx->output.balance_chassis.cmd.move_vec.vy += ctx->config->sensitivity.move_sens; #endif return CMD_OK; } @@ -23,6 +27,9 @@ int8_t CMD_Behavior_Handle_FORE(CMD_t *ctx) { int8_t CMD_Behavior_Handle_BACK(CMD_t *ctx) { #if CMD_ENABLE_MODULE_CHASSIS ctx->output.chassis.cmd.ctrl_vec.vy -= ctx->config->sensitivity.move_sens; +#endif +#if CMD_ENABLE_MODULE_BALANCE_CHASSIS + ctx->output.balance_chassis.cmd.move_vec.vy -= ctx->config->sensitivity.move_sens; #endif return CMD_OK; } @@ -30,6 +37,9 @@ int8_t CMD_Behavior_Handle_BACK(CMD_t *ctx) { int8_t CMD_Behavior_Handle_LEFT(CMD_t *ctx) { #if CMD_ENABLE_MODULE_CHASSIS ctx->output.chassis.cmd.ctrl_vec.vx -= ctx->config->sensitivity.move_sens; +#endif +#if CMD_ENABLE_MODULE_BALANCE_CHASSIS + ctx->output.balance_chassis.cmd.move_vec.vx -= ctx->config->sensitivity.move_sens; #endif return CMD_OK; } @@ -37,6 +47,9 @@ int8_t CMD_Behavior_Handle_LEFT(CMD_t *ctx) { int8_t CMD_Behavior_Handle_RIGHT(CMD_t *ctx) { #if CMD_ENABLE_MODULE_CHASSIS ctx->output.chassis.cmd.ctrl_vec.vx += ctx->config->sensitivity.move_sens; +#endif +#if CMD_ENABLE_MODULE_BALANCE_CHASSIS + ctx->output.balance_chassis.cmd.move_vec.vx += ctx->config->sensitivity.move_sens; #endif return CMD_OK; } @@ -45,14 +58,17 @@ int8_t CMD_Behavior_Handle_ACCELERATE(CMD_t *ctx) { #if CMD_ENABLE_MODULE_CHASSIS ctx->output.chassis.cmd.ctrl_vec.vx *= ctx->config->sensitivity.move_fast_mult; ctx->output.chassis.cmd.ctrl_vec.vy *= ctx->config->sensitivity.move_fast_mult; +#endif +#if CMD_ENABLE_MODULE_BALANCE_CHASSIS + ctx->output.balance_chassis.cmd.move_vec.vx *= ctx->config->sensitivity.move_fast_mult; + ctx->output.balance_chassis.cmd.move_vec.vy *= ctx->config->sensitivity.move_fast_mult; #endif return CMD_OK; } -int8_t CMD_Behavior_Handle_DECELERATE(CMD_t *ctx) { -#if CMD_ENABLE_MODULE_CHASSIS - ctx->output.chassis.cmd.ctrl_vec.vx *= ctx->config->sensitivity.move_slow_mult; - ctx->output.chassis.cmd.ctrl_vec.vy *= ctx->config->sensitivity.move_slow_mult; +int8_t CMD_Behavior_Handle_CLIMB(CMD_t *ctx) { +#if CMD_ENABLE_MODULE_BALANCE_CHASSIS + ctx->output.balance_chassis.cmd.mode = CHASSIS_MODE_CLIMB_STEP; #endif return CMD_OK; } @@ -72,12 +88,15 @@ int8_t CMD_Behavior_Handle_FIRE_MODE(CMD_t *ctx) { } int8_t CMD_Behavior_Handle_ROTOR(CMD_t *ctx) { -#if CMD_ENABLE_MODULE_CHASSIS - ctx->output.chassis.cmd.mode = CHASSIS_MODE_ROTOR; - ctx->output.chassis.cmd.mode_rotor = ROTOR_MODE_RAND; -#endif #if CMD_ENABLE_MODULE_GIMBAL ctx->output.gimbal.cmd.mode = GIMBAL_MODE_RELATIVE; + #if CMD_ENABLE_MODULE_CHASSIS + ctx->output.chassis.cmd.mode = CHASSIS_MODE_ROTOR; + ctx->output.chassis.cmd.mode_rotor = ROTOR_MODE_RAND; + #endif + #if CMD_ENABLE_MODULE_BALANCE_CHASSIS + ctx->output.balance_chassis.cmd.mode = CHASSIS_MODE_BALANCE_ROTOR; + #endif #endif return CMD_OK; } @@ -131,7 +150,11 @@ int8_t CMD_Behavior_Handle_CHECKSOURCERCPC(CMD_t *ctx) { } return CMD_OK; } - +extern bool reset; +int8_t CMD_Behavior_Handle_RESET(CMD_t *ctx) { + reset = !reset; + return CMD_OK; +} /* 行为配置表 - 由宏生成 */ static const CMD_BehaviorConfig_t g_behavior_configs[] = { CMD_BEHAVIOR_TABLE(BUILD_BEHAVIOR_CONFIG) diff --git a/User/module/cmd/cmd_types.h b/User/module/cmd/cmd_types.h index 818a593..24e6454 100644 --- a/User/module/cmd/cmd_types.h +++ b/User/module/cmd/cmd_types.h @@ -252,19 +252,20 @@ typedef enum { /* 行为定义 */ /* ========================================================================== */ /* 行为-按键映射宏表 */ -#define BEHAVIOR_CONFIG_COUNT (11) +#define BEHAVIOR_CONFIG_COUNT (12) #define CMD_BEHAVIOR_TABLE(X) \ - X(FORE, CMD_KEY_W, CMD_ACTIVE_PRESSED, CMD_MODULE_CHASSIS) \ - X(BACK, CMD_KEY_S, CMD_ACTIVE_PRESSED, CMD_MODULE_CHASSIS) \ - X(LEFT, CMD_KEY_A, CMD_ACTIVE_PRESSED, CMD_MODULE_CHASSIS) \ - X(RIGHT, CMD_KEY_D, CMD_ACTIVE_PRESSED, CMD_MODULE_CHASSIS) \ - X(ACCELERATE, CMD_KEY_SHIFT, CMD_ACTIVE_PRESSED, CMD_MODULE_CHASSIS) \ - X(DECELERATE, CMD_KEY_CTRL, CMD_ACTIVE_PRESSED, CMD_MODULE_CHASSIS) \ - X(FIRE, CMD_KEY_L_CLICK, CMD_ACTIVE_PRESSED, CMD_MODULE_SHOOT) \ + X(FORE, CMD_KEY_W, CMD_ACTIVE_PRESSED, CMD_MODULE_CHASSIS|CMD_MODULE_BALANCE_CHASSIS) \ + X(BACK, CMD_KEY_S, CMD_ACTIVE_PRESSED, CMD_MODULE_CHASSIS|CMD_MODULE_BALANCE_CHASSIS) \ + X(LEFT, CMD_KEY_A, CMD_ACTIVE_PRESSED, CMD_MODULE_CHASSIS|CMD_MODULE_BALANCE_CHASSIS) \ + X(RIGHT, CMD_KEY_D, CMD_ACTIVE_PRESSED, CMD_MODULE_CHASSIS|CMD_MODULE_BALANCE_CHASSIS) \ + X(ACCELERATE, CMD_KEY_SHIFT, CMD_ACTIVE_PRESSED, CMD_MODULE_CHASSIS|CMD_MODULE_BALANCE_CHASSIS) \ + X(ROTOR, CMD_KEY_CTRL, CMD_ACTIVE_PRESSED, CMD_MODULE_CHASSIS|CMD_MODULE_BALANCE_CHASSIS) \ + X(FIRE, CMD_KEY_L_CLICK, CMD_ACTIVE_PRESSED, CMD_MODULE_SHOOT) \ X(FIRE_MODE, CMD_KEY_B, CMD_ACTIVE_RISING_EDGE, CMD_MODULE_SHOOT) \ - X(ROTOR, CMD_KEY_E, CMD_ACTIVE_PRESSED, CMD_MODULE_CHASSIS) \ + X(CLIMB, CMD_KEY_R, CMD_ACTIVE_PRESSED, CMD_MODULE_BALANCE_CHASSIS) \ X(AUTOAIM, CMD_KEY_R_CLICK, CMD_ACTIVE_PRESSED, CMD_MODULE_NONE) \ - X(CHECKSOURCERCPC, CMD_KEY_CTRL|CMD_KEY_SHIFT|CMD_KEY_V, CMD_ACTIVE_RISING_EDGE, CMD_MODULE_NONE) + X(CHECKSOURCERCPC,CMD_KEY_CTRL|CMD_KEY_SHIFT|CMD_KEY_V, CMD_ACTIVE_RISING_EDGE, CMD_MODULE_NONE) \ + X(RESET, CMD_KEY_G, CMD_ACTIVE_RISING_EDGE, CMD_MODULE_NONE) /* 触发类型 */ typedef enum { CMD_ACTIVE_PRESSED, /* 按住时触发 */ diff --git a/User/module/config.c b/User/module/config.c index 5a58db8..65f3b20 100644 --- a/User/module/config.c +++ b/User/module/config.c @@ -113,8 +113,8 @@ Config_RobotParam_t robot_config = { .shoot_param = { .basic={ .projectileType=SHOOT_PROJECTILE_17MM, - .fric_num=2, - .extra_deceleration_ratio=1.0f, + .fric_num=2, + .extra_deceleration_ratio=1.0f, .num_trig_tooth=8, .shot_freq=1.0f, .shot_burst_num=3, diff --git a/User/module/shoot.c b/User/module/shoot.c index c9bdee0..4e76a51 100644 --- a/User/module/shoot.c +++ b/User/module/shoot.c @@ -516,7 +516,7 @@ int8_t Shoot_CaluTargetAngle(Shoot_t *s, Shoot_CMD_t *cmd) if (s->param->heatControl.enable && s->heatcontrol.shots_available <= s->param->heatControl.safe_shots) { actual_freq = 0.0f; /* 禁止发弹 */ } - + float dt = s->timer.now - s->var_trig.time_lastShoot; float dpos; dpos = CircleError(s->target_variable.trig_angle, s->var_trig.trig_agl, M_2PI); @@ -971,10 +971,10 @@ int8_t Shoot_Control(Shoot_t *s, Shoot_CMD_t *cmd) s->timer.now = BSP_TIME_Get_us() / 1000000.0f; s->timer.dt = (BSP_TIME_Get_us() - s->timer.lask_wakeup) / 1000000.0f; s->timer.lask_wakeup = BSP_TIME_Get_us(); - // Shoot_CaluTargetRPM(s,233); + Shoot_CaluTargetRPM(s,233); /* 运行热量检测状态机 */ - // Shoot_HeatDetectionFSM(s); + Shoot_HeatDetectionFSM(s); /* 运行卡弹检测状态机 */ Shoot_JamDetectionFSM(s, cmd); diff --git a/User/task/blink.c b/User/task/blink.c index 6d0fd0d..d31b6ca 100644 --- a/User/task/blink.c +++ b/User/task/blink.c @@ -17,6 +17,7 @@ /* USER STRUCT BEGIN */ BUZZER_t buzzer; static uint16_t count; +bool reset=0; /* USER STRUCT END */ /* Private function --------------------------------------------------------- */ @@ -54,6 +55,10 @@ void Task_blink(void *argument) { /* 播放100ms后停止 (50/500Hz = 0.1s) */ BUZZER_Stop(&buzzer); } + if (reset) { + __set_FAULTMASK(1); /* 关闭所有中断 */ + NVIC_SystemReset(); /* 系统复位 */ + } /* USER CODE END */ osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */ } diff --git a/User/task/rc.c b/User/task/rc.c index b46ddab..21ba275 100644 --- a/User/task/rc.c +++ b/User/task/rc.c @@ -8,6 +8,7 @@ /* USER INCLUDE BEGIN */ #include "device/dr16.h" #include "device/mrobot.h" +#include "stm32h7xx.h" /* USER INCLUDE END */ /* Private typedef ---------------------------------------------------------- */ @@ -16,6 +17,8 @@ /* Private variables -------------------------------------------------------- */ /* USER STRUCT BEGIN */ DR16_t dr16; +static DR16_SwitchPos_t last_sw_l = DR16_SW_ERR; /* 记录左拨杆上一次状态 */ +extern bool reset; /* USER STRUCT END */ /* Private function --------------------------------------------------------- */ @@ -75,6 +78,18 @@ void Task_rc(void *argument) { osMessageQueueReset(task_runtime.msgq.cmd.rc); osMessageQueuePut(task_runtime.msgq.cmd.rc, &dr16, 0, 0); + /* 检测左拨杆切换到UP位置时触发软件复位 */ + if (dr16.header.online) { + /* 拨杆从非UP状态切换到UP状态，且复位功能已使能，触发系统复位 */ + if ( + dr16.data.sw_l == DR16_SW_UP && + last_sw_l != DR16_SW_UP) { + reset=!reset; + } + + last_sw_l = dr16.data.sw_l; /* 更新拨杆状态 */ + } + /* USER CODE END */ osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */ } From 4caedf3329f4e473159c8dcfb9399732ce3af745 Mon Sep 17 00:00:00 2001 From: xxxxm <2389287465@qq.com> Date: Mon, 16 Mar 2026 22:11:12 +0800 Subject: [PATCH 34/34] 123 --- User/module/cmd/cmd_types.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/User/module/cmd/cmd_types.h b/User/module/cmd/cmd_types.h index 24e6454..7fa40da 100644 --- a/User/module/cmd/cmd_types.h +++ b/User/module/cmd/cmd_types.h @@ -265,7 +265,7 @@ typedef enum { X(CLIMB, CMD_KEY_R, CMD_ACTIVE_PRESSED, CMD_MODULE_BALANCE_CHASSIS) \ X(AUTOAIM, CMD_KEY_R_CLICK, CMD_ACTIVE_PRESSED, CMD_MODULE_NONE) \ X(CHECKSOURCERCPC,CMD_KEY_CTRL|CMD_KEY_SHIFT|CMD_KEY_V, CMD_ACTIVE_RISING_EDGE, CMD_MODULE_NONE) \ - X(RESET, CMD_KEY_G, CMD_ACTIVE_RISING_EDGE, CMD_MODULE_NONE) + X(RESET, CMD_KEY_CTRL|CMD_KEY_SHIFT|CMD_KEY_G, CMD_ACTIVE_RISING_EDGE, CMD_MODULE_NONE) /* 触发类型 */ typedef enum { CMD_ACTIVE_PRESSED, /* 按住时触发 */