Merge branch 'MC_02' of ssh://gitea.qutrobot.top:222/robofish/rm_balance into MC_02
This commit is contained in:
commit
eae02e4305
6
.github/copilot-instructions.md
vendored
Normal file
6
.github/copilot-instructions.md
vendored
Normal file
@ -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.
|
||||
@ -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
|
||||
|
||||
@ -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 DMA2_Stream0_IRQHandler(void);
|
||||
void DMA2_Stream1_IRQHandler(void);
|
||||
|
||||
@ -264,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.
|
||||
*/
|
||||
@ -392,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.
|
||||
*/
|
||||
|
||||
@ -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;
|
||||
|
||||
@ -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;
|
||||
}
|
||||
|
||||
|
||||
@ -6,13 +6,13 @@
|
||||
#include "device.h"
|
||||
|
||||
#include <string.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"
|
||||
//#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)
|
||||
|
||||
@ -11,14 +11,14 @@ extern "C" {
|
||||
/* Includes ----------------------------------------------------------------- */
|
||||
#include <cmsis_os2.h>
|
||||
#include <stdbool.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"
|
||||
#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 ----------------------------------------------------------- */
|
||||
@ -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];
|
||||
|
||||
266
User/module/aimbot.c
Normal file
266
User/module/aimbot.c
Normal file
@ -0,0 +1,266 @@
|
||||
#include "module/aimbot.h"
|
||||
#include "device/device.h"
|
||||
#include "bsp/uart.h"
|
||||
#include "component/crc16.h"
|
||||
#include <string.h>
|
||||
|
||||
/* =====================================================================
|
||||
* FDCAN 帧索引（反馈方向：下层板?上层板）
|
||||
* 注：上层板使用自己的IMU，下层板只发送射击数据与模式
|
||||
* ===================================================================== */
|
||||
#define FB_FRAME_DATA 0u /* 弹速(4B)+弹数(2B)+模式(1B)+保留(1B) */
|
||||
|
||||
/* =====================================================================
|
||||
* UART 通信接口（上层板 <EFBFBD>?上位<EFBFBD>?PC<EFBFBD>?
|
||||
* ===================================================================== */
|
||||
|
||||
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 发给上位机格式），修正了原始实现中的字段错误<EFBFBD>?
|
||||
* 四元数来<EFBFBD>?quat 参数；欧拉角、角速度来自 gimbal_fb->imu<EFBFBD>?
|
||||
*/
|
||||
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 通信接口（下层板 <EFBFBD>?上层板）
|
||||
* ===================================================================== */
|
||||
|
||||
/**
|
||||
* @brief 初始<EFBFBD>?Aimbot CAN 通信：注册指令接收队列和反馈收发队列<EFBFBD>?
|
||||
* 下层板只需注册 cmd_id；上层板只需注册 fb_base_id <EFBFBD>?6 <EFBFBD>?ID<EFBFBD>?
|
||||
* 本函数同时注册两侧所需 ID，上/下层板共用同一初始化流程即可<EFBFBD>?
|
||||
*/
|
||||
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 <EFBFBD>?Gimbal/IMU/Shoot 数据打包 CAN 反馈结构体<EFBFBD>?
|
||||
/**
|
||||
* @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 队列中非阻塞地取出上层板发来<EFBFBD>?AI 指令并解析<EFBFBD>?
|
||||
*
|
||||
* 指令帧格式（8 字节，与 vision_bridge 一致）<EFBFBD>?
|
||||
* data[0] : mode (1B)
|
||||
* data[1..3.5] : yaw (28bit 有符号定点数<EFBFBD>?.1µrad/LSB)
|
||||
* data[4.5..7] : pit (28bit 有符号定点数<EFBFBD>?.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 字节）<EFBFBD>?
|
||||
*
|
||||
* <EFBFBD>?vision_bridge.c <EFBFBD>?AI_SendCmdOnFDCAN 编码格式完全一致，
|
||||
* 上层板可直接调用本函数替<EFBFBD>?vision_bridge 中的同名函数<EFBFBD>?
|
||||
*/
|
||||
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;
|
||||
}
|
||||
|
||||
174
User/module/aimbot.h
Normal file
174
User/module/aimbot.h
Normal file
@ -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 <stdint.h>
|
||||
|
||||
// 数据包协议定义（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
|
||||
@ -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);
|
||||
static void Chassis_RecoverControl(Chassis_t *c);
|
||||
|
||||
/* Private functions -------------------------------------------------------- */
|
||||
@ -78,6 +79,7 @@ static void Chassis_ResetControllers(Chassis_t *c) {
|
||||
PID_Reset(&c->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]);
|
||||
@ -136,32 +138,45 @@ 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请求，需先切别的模式再回来 */
|
||||
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) {
|
||||
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);
|
||||
|
||||
/* RELAX -> BALANCE：根据 pitch 角度判断是否需要自起 */
|
||||
/* RELAX -> BALANCE：pitch过大时自动进入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;
|
||||
return CHASSIS_OK;
|
||||
if (fabsf(pitch) > 0.8f) {
|
||||
/* pitch过大，机体倒地，进入自起模式 */
|
||||
mode = CHASSIS_MODE_RECOVER;
|
||||
}
|
||||
/* 否则直接进入平衡模式 */
|
||||
}
|
||||
|
||||
/* 直接进入 RECOVER 时也重置状态机 */
|
||||
@ -170,6 +185,27 @@ 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);
|
||||
}
|
||||
|
||||
/* 进入小陀螺时重置旋转状态 */
|
||||
if (mode == CHASSIS_MODE_BALANCE_ROTOR) {
|
||||
c->rotor_state.current_spin_speed = 0.0f;
|
||||
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->climb.state_start_time = 0;
|
||||
}
|
||||
|
||||
c->mode = mode;
|
||||
@ -412,70 +448,222 @@ static void Chassis_JumpControl(Chassis_t *c, const Chassis_CMD_t *c_cmd, float
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 倒地自起控制
|
||||
* @note 自起流程（左右腿独立状态机）：
|
||||
* STRETCH(伸腿) -> BACKLEG(后甩腿) -> COMPLETE(完成)
|
||||
* 两腿均 COMPLETE 后自动切换到 WHELL_LEG_BALANCE 模式
|
||||
*
|
||||
* @brief 上台阶控制
|
||||
* @param c 底盘结构体指针
|
||||
* @param c_cmd 控制指令
|
||||
*
|
||||
* @note 流程：
|
||||
* EXTEND(伸腿前进，削弱前摆力) -> RETRACT(腿后摆超阈值，快速收腿) -> SETTLE(稳定) -> 自动退出到BALANCE
|
||||
* 单次执行：完成后即使CMD继续发CLIMB_STEP也不重复，需切换到其他模式后再回来
|
||||
*/
|
||||
static void Chassis_RecoverControl(Chassis_t *c) {
|
||||
float theta_l = c->vmc_[0].leg.theta;
|
||||
float theta_r = c->vmc_[1].leg.theta;
|
||||
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;
|
||||
|
||||
/* ===== 左腿状态机 ===== */
|
||||
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;
|
||||
/* 已完成一次上台阶，自动切回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: {
|
||||
/* 伸腿前进采用斜坡，避免切换模式时瞬间猛站 */
|
||||
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);
|
||||
|
||||
/* 摆力矩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 tp_initial = 0.5f; /* 初始Tp缩放，刚进入就削弱50% */
|
||||
float dynamic_scale;
|
||||
if (theta_abs < contact_angle) {
|
||||
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 = 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;
|
||||
|
||||
Chassis_VMCControl(c, &climb_cmd);
|
||||
Chassis_Output(c);
|
||||
|
||||
/* 先伸腿到位，再根据摆角触发收腿，避免刚进入模式就立刻缩回 */
|
||||
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);
|
||||
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;
|
||||
}
|
||||
|
||||
/* 超时保护：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;
|
||||
|
||||
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:
|
||||
default:
|
||||
c->climb.completed = true;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* @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 底盘结构体指针
|
||||
@ -519,6 +707,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);
|
||||
@ -607,6 +796,23 @@ 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;
|
||||
|
||||
/* 初始化小陀螺状态 */
|
||||
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;
|
||||
|
||||
/* 初始化自起状态 */
|
||||
c->recover.state[0] = RECOVER_STRETCH;
|
||||
c->recover.state[1] = RECOVER_STRETCH;
|
||||
c->recover.state_start_time = 0;
|
||||
|
||||
return CHASSIS_OK;
|
||||
}
|
||||
|
||||
@ -623,10 +829,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;
|
||||
}
|
||||
|
||||
/* 机械零点调整 */
|
||||
@ -644,9 +859,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);
|
||||
|
||||
@ -679,11 +902,29 @@ 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;
|
||||
}
|
||||
|
||||
/* 倒地自动检测：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) &&
|
||||
@ -716,84 +957,28 @@ 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: {
|
||||
/* 运行自起状态机 */
|
||||
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;
|
||||
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;
|
||||
|
||||
case CHASSIS_MODE_CLIMB_STEP:
|
||||
Chassis_ClimbControl(c, c_cmd);
|
||||
break;
|
||||
|
||||
default:
|
||||
return CHASSIS_ERR_MODE;
|
||||
}
|
||||
@ -848,21 +1033,19 @@ 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;
|
||||
if (c_cmd->mode == CHASSIS_MODE_BALANCE_ROTOR) {
|
||||
/* 小陀螺平移：将云台坐标系（用户视角）速度投影到机体坐标系 */
|
||||
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.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;
|
||||
}
|
||||
|
||||
|
||||
/* ==================== 状态更新 ==================== */
|
||||
@ -886,17 +1069,72 @@ 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,
|
||||
.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 + leg_x_comp,
|
||||
.d_x = c->chassis_state.target_velocity_x,
|
||||
};
|
||||
|
||||
/* ==================== 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) {
|
||||
/* 小陀螺模式：旋转速度在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，用减速斜坡 */
|
||||
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; /* 取反匹配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);
|
||||
}
|
||||
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;
|
||||
@ -948,13 +1186,26 @@ 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;
|
||||
|
||||
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; /* 髋宽的一半 */
|
||||
|
||||
@ -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 {
|
||||
@ -103,6 +113,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 +157,32 @@ 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;
|
||||
|
||||
/* 上台阶参数 */
|
||||
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;
|
||||
|
||||
/* 倒地自起参数 */
|
||||
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;
|
||||
@ -201,6 +238,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; /* 跳跃状态 */
|
||||
@ -214,8 +257,21 @@ typedef struct {
|
||||
/* 自起控制相关 */
|
||||
struct {
|
||||
Recover_State_t state[2]; /* 左右腿自起状态 */
|
||||
uint32_t state_start_time; /* 当前阶段开始时间 (ms) */
|
||||
} recover;
|
||||
|
||||
/* 上台阶控制相关 */
|
||||
struct {
|
||||
Climb_State_t state; /* 上台阶状态 */
|
||||
bool completed; /* 本次已完成标志，防止重复触发 */
|
||||
uint32_t state_start_time; /* 当前状态开始时间 (ms) */
|
||||
} climb;
|
||||
|
||||
/* 电机离线检测 */
|
||||
struct {
|
||||
bool any_offline; /* 是否有电机离线 */
|
||||
} motor_status;
|
||||
|
||||
/* PID计算的目标值 */
|
||||
struct {
|
||||
AHRS_Eulr_t chassis;
|
||||
@ -224,6 +280,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];
|
||||
@ -292,6 +349,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 底盘输出值
|
||||
*
|
||||
|
||||
@ -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;
|
||||
}
|
||||
@ -228,22 +228,19 @@ 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:
|
||||
ctx->output.shoot.cmd.ready = true;
|
||||
ctx->output.shoot.cmd.firecmd = false;
|
||||
break;
|
||||
case 2:
|
||||
if (ctx->input.rc.sw[1]==CMD_SW_DOWN) {
|
||||
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 {
|
||||
} else {
|
||||
ctx->output.shoot.cmd.ready = true;
|
||||
ctx->output.shoot.cmd.firecmd = false;
|
||||
}
|
||||
|
||||
break;
|
||||
}
|
||||
}
|
||||
@ -391,18 +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;
|
||||
|
||||
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
|
||||
|
||||
@ -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;
|
||||
|
||||
|
||||
@ -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)
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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(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_CTRL|CMD_KEY_SHIFT|CMD_KEY_G, CMD_ACTIVE_RISING_EDGE, CMD_MODULE_NONE)
|
||||
/* 触发类型 */
|
||||
typedef enum {
|
||||
CMD_ACTIVE_PRESSED, /* 按住时触发 */
|
||||
|
||||
@ -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 = {
|
||||
@ -110,19 +115,20 @@ 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
|
||||
.safe_shots=5, // 安全出弹余量
|
||||
.nmax=18.0f, // 最大射频 Hz
|
||||
.Hwarn=200.0f, // 热量预警值
|
||||
.Hsatu=100.0f, // 热量饱和值
|
||||
.Hthres=50.0f, // 热量阈值
|
||||
@ -151,10 +157,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 +186,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 +201,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,
|
||||
},
|
||||
@ -243,6 +249,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 +400,17 @@ Config_RobotParam_t robot_config = {
|
||||
.non_contact_theta = -0.01f, /* 离地摆角目标 (rad) */
|
||||
},
|
||||
|
||||
.rotor_ctrl = {
|
||||
.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° */
|
||||
},
|
||||
|
||||
.vmc_param = {
|
||||
{ // 左腿
|
||||
.leg_1 = 0.215f, // 后髋连杆长度 (L1) (m)
|
||||
@ -426,10 +454,23 @@ Config_RobotParam_t robot_config = {
|
||||
.retract_force = -20.0f, /* 收腿前馈力减小 */
|
||||
},
|
||||
|
||||
.climb = {
|
||||
.forward_speed = 1.5f, /* 上台阶前进速度 (m/s) */
|
||||
.theta_retract_threshold = 0.9f, /* 腿后摆收腿阈值 (rad)，约30° */
|
||||
.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,
|
||||
.phi = 0.0f,
|
||||
.phi = 0.01f,
|
||||
},
|
||||
|
||||
},
|
||||
@ -462,8 +503,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_CLIMB_STEP,
|
||||
#endif
|
||||
},
|
||||
},
|
||||
|
||||
@ -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;
|
||||
|
||||
|
||||
@ -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);
|
||||
@ -194,32 +208,17 @@ int8_t Gimbal_Control(Gimbal_t *g, Gimbal_CMD_t *g_cmd, Gimbal_AI_t *g_ai) {
|
||||
}
|
||||
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;
|
||||
}
|
||||
|
||||
|
||||
@ -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 {
|
||||
|
||||
@ -41,7 +41,7 @@ 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 */
|
||||
@ -51,6 +51,8 @@ void Task(void *argument) {
|
||||
/* Private macro ------------------------------------------------------------ */
|
||||
/* Private variables -------------------------------------------------------- */
|
||||
static bool last_firecmd;
|
||||
|
||||
float maxTrigrpm=4000.0f;
|
||||
/* Private function -------------------------------------------------------- */
|
||||
|
||||
/**
|
||||
@ -181,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;
|
||||
@ -510,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);
|
||||
@ -520,7 +527,9 @@ 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++;
|
||||
}
|
||||
|
||||
return SHOOT_OK;
|
||||
}
|
||||
|
||||
@ -601,7 +610,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;
|
||||
|
||||
@ -984,3 +993,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;
|
||||
}
|
||||
|
||||
@ -51,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;
|
||||
@ -159,6 +164,7 @@ typedef struct {
|
||||
}jamDetection;/* 卡弹检测参数 */
|
||||
struct {
|
||||
bool enable; /* 是否启用热量控制 */
|
||||
uint16_t safe_shots;/* 安全余量，当shots_available小于等于该值时禁止发弹 */
|
||||
float nmax;//最大射频
|
||||
float Hwarn;//热量预警值
|
||||
float Hsatu;//热量饱和值
|
||||
@ -216,6 +222,7 @@ typedef struct {
|
||||
struct {
|
||||
float fric_rpm; /* 目标摩擦轮转速 */
|
||||
float trig_angle;/* 目标拨弹位置 */
|
||||
float bullet_speed;/* 目标弹速 */
|
||||
}target_variable;
|
||||
|
||||
/* 反馈控制用的PID */
|
||||
@ -292,7 +299,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
|
||||
|
||||
@ -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 <string.h>
|
||||
/* 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); /* 运行结束，等待下一次唤醒 */
|
||||
}
|
||||
|
||||
}
|
||||
@ -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); /* 运行结束，等待下一次唤醒 */
|
||||
}
|
||||
|
||||
@ -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
|
||||
/* 从裁判系统读取最新数据（非阻塞） */
|
||||
|
||||
@ -61,6 +61,11 @@ 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);
|
||||
|
||||
@ -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); /* 运行结束，等待下一次唤醒 */
|
||||
}
|
||||
|
||||
@ -89,14 +89,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转发 */
|
||||
|
||||
@ -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);
|
||||
|
||||
|
||||
Loading…
Reference in New Issue
Block a user