From ee435d80013c0193d65644392c0079281d9ef834 Mon Sep 17 00:00:00 2001 From: Robofish <1683502971@qq.com> Date: Tue, 7 Oct 2025 21:19:04 +0800 Subject: [PATCH] =?UTF-8?q?=E6=B7=BB=E5=8A=A0ai?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- Core/Inc/FreeRTOSConfig.h | 2 +- User/device/ai.c | 66 +++++------------------------------ User/device/ai.h | 3 ++ User/module/balance_chassis.c | 17 +++++---- User/module/config.c | 8 ++--- 5 files changed, 27 insertions(+), 69 deletions(-) diff --git a/Core/Inc/FreeRTOSConfig.h b/Core/Inc/FreeRTOSConfig.h index 16c89ac..fee994f 100644 --- a/Core/Inc/FreeRTOSConfig.h +++ b/Core/Inc/FreeRTOSConfig.h @@ -187,7 +187,7 @@ standard names. */ /* USER CODE BEGIN Defines */ /* Section where parameter definitions can be added (for instance, to override default ones in FreeRTOS.h) */ -#define configAPPLICATION_ALLOCATED_HEAP 1 +// #define configAPPLICATION_ALLOCATED_HEAP 1 /* USER CODE END Defines */ #endif /* FREERTOS_CONFIG_H */ diff --git a/User/device/ai.c b/User/device/ai.c index 232f65c..70b634c 100644 --- a/User/device/ai.c +++ b/User/device/ai.c @@ -89,66 +89,18 @@ error: return DEVICE_ERR; } -// void AI_PackCmd(AI_t *ai, CMD_Host_t *cmd_host) { -// cmd_host->gimbal_delta.yaw = ai->from_host.data.gimbal.yaw; -// cmd_host->gimbal_delta.pit = ai->from_host.data.gimbal.pit; -// cmd_host->fire = (ai->from_host.data.notice & AI_NOTICE_FIRE); -// cmd_host->chassis_move_vec.vx = ai->from_host.data.chassis_move_vec.vx; -// cmd_host->chassis_move_vec.vy = ai->from_host.data.chassis_move_vec.vy; -// cmd_host->chassis_move_vec.wz = ai->from_host.data.chassis_move_vec.wz; -// } - -// int8_t AI_HandleOffline(AI_t *ai, CMD_Host_t *cmd_host) { -// if (ai == NULL) return DEVICE_ERR_NULL; -// if (cmd_host == NULL) return DEVICE_ERR_NULL; -// ai->ai_online = false; -// memset(&(ai->from_host), 0, sizeof(ai->from_host)); -// memset(cmd_host, 0, sizeof(*cmd_host)); -// return 0; -// } - -// int8_t AI_PackMCU(AI_t *ai, const AHRS_Quaternion_t *quat) { +// int8_t AI_PackMCU(AI_t *ai, const AI_Protucol_UpDataMCU_t *data){ +// if (ai == NULL || data == NULL) return DEVICE_ERR_NULL; // ai->to_host.mcu.id = AI_ID_MCU; -// memcpy((void *)&(ai->to_host.mcu.package.data.quat), (const void *)quat, -// sizeof(*quat)); -// ai->to_host.mcu.package.data.notice = 0; -// if (ai->status == AI_STATUS_AUTOAIM) -// ai->to_host.mcu.package.data.notice |= AI_NOTICE_AUTOAIM; -// else if (ai->status == AI_STATUS_HITSWITCH) -// ai->to_host.mcu.package.data.notice |= AI_NOTICE_HITBUFF; -// else if (ai->status == AI_STATUS_AUTOMATIC) -// ai->to_host.mcu.package.data.notice |= AI_NOTICE_AUTOMATIC; - -// ai->to_host.mcu.package.crc16 = CRC16_Calc( -// (const uint8_t *)&(ai->to_host.mcu.package), -// sizeof(ai->to_host.mcu.package) - sizeof(uint16_t), CRC16_INIT); +// ai->to_host.mcu.package = *data; +// ai->to_host.mcu.crc16 = +// CRC16_Calc((const uint8_t *)&(ai->to_host.mcu), sizeof(AI_UpPackageMCU_t) - 2); // return DEVICE_OK; // } -// int8_t AI_PackRef(AI_t *ai, const Referee_ForAI_t *ref) { -// (void)ref; -// ai->to_host.ref.id = AI_ID_REF; -// ai->to_host.ref.package.crc16 = CRC16_Calc( -// (const uint8_t *)&(ai->to_host.ref.package), -// sizeof(ai->to_host.ref.package) - sizeof(uint16_t), CRC16_INIT); -// return DEVICE_OK; -// } +int8_t AI_PackRef(AI_t *ai, const AI_UpPackageReferee_t *data); -// int8_t AI_StartSend(AI_t *ai, bool ref_update) { -// if (ref_update) { -// if (HAL_UART_Transmit_DMA( -// BSP_UART_GetHandle(BSP_UART_AI), (uint8_t *)&(ai->to_host), -// sizeof(ai->to_host.ref) + sizeof(ai->to_host.mcu)) == HAL_OK) -// return DEVICE_OK; -// else -// return DEVICE_ERR; -// } else { -// if (HAL_UART_Transmit_DMA(BSP_UART_GetHandle(BSP_UART_AI), -// (uint8_t *)&(ai->to_host.mcu), -// sizeof(ai->to_host.mcu)) == HAL_OK) -// return DEVICE_OK; -// else -// return DEVICE_ERR; -// } -// } +int8_t AI_HandleOffline(AI_t *ai); + +int8_t AI_StartSend(AI_t *ai, bool ref_online); diff --git a/User/device/ai.h b/User/device/ai.h index 567a279..101d780 100644 --- a/User/device/ai.h +++ b/User/device/ai.h @@ -20,6 +20,9 @@ extern "C" { /* Exported constants ------------------------------------------------------- */ /* Exported macro ----------------------------------------------------------- */ +#define AI_ID_MCU (0xC4) +#define AI_ID_REF (0xA8) +#define AI_ID_AI (0xA1) /* Exported types ----------------------------------------------------------- */ /* 电控 -> 视觉 MCU数据结构体*/ diff --git a/User/module/balance_chassis.c b/User/module/balance_chassis.c index 4436060..0e10ae1 100644 --- a/User/module/balance_chassis.c +++ b/User/module/balance_chassis.c @@ -284,6 +284,8 @@ void Chassis_Output(Chassis_t *c) { LowPassFilter2p_Apply(&c->filter.wheel_out[1], c->output.wheel[1]); Virtual_Chassis_SendWheelCommand(&virtual_chassis, c->output.wheel[0], c->output.wheel[1]); + // Virtual_Chassis_SendWheelCommand(&virtual_chassis, 0.0f, + // 0.0f); // 暂时不让轮子动 } int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) { @@ -385,7 +387,9 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) { PID_Calc(&c->pid.tp[0], c->vmc_[1].leg.theta, c->vmc_[0].leg.theta, c->vmc_[0].leg.d_theta, c->dt); // 右腿补偿力矩：使右腿theta向左腿theta靠拢（符号相反） - Delta_Tp[1] = -Delta_Tp[0]; + Delta_Tp[1] = c->vmc_[1].leg.L0 * 10.0f * + PID_Calc(&c->pid.tp[1], c->vmc_[0].leg.theta, + c->vmc_[1].leg.theta, c->vmc_[1].leg.d_theta, c->dt); // 左腿控制 { @@ -398,12 +402,12 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) { virtual_force[0] = (c->lqr[0].control_output.Tp) * sinf(c->vmc_[0].leg.theta) + pid_output + 60.0f + roll_compensation_force; - // virtual_force[0] = - // (c->lqr[0].control_output.Tp) * sinf(c->vmc_[0].leg.theta) + - // pid_output + 60.0f; + // VMC逆解算（包含摆角补偿） if (VMC_InverseSolve(&c->vmc_[0], virtual_force[0], c->lqr[0].control_output.Tp + Delta_Tp[0]) == 0) { + // if (VMC_InverseSolve(&c->vmc_[0], 0.0f, + // Delta_Tp[0]) == 0) { VMC_GetJointTorques(&c->vmc_[0], &c->output.joint[0], &c->output.joint[1]); } else { @@ -424,13 +428,12 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) { virtual_force[1] = (c->lqr[1].control_output.Tp) * sinf(c->vmc_[1].leg.theta) + pid_output + 65.0f - roll_compensation_force; - // virtual_force[1] = - // (c->lqr[1].control_output.Tp) * sinf(c->vmc_[1].leg.theta) + - // pid_output + 65.0f; // VMC逆解算（包含摆角补偿） if (VMC_InverseSolve(&c->vmc_[1], virtual_force[1], c->lqr[1].control_output.Tp + Delta_Tp[1]) == 0) { + // if (VMC_InverseSolve(&c->vmc_[1], 0.0f, + // Delta_Tp[1]) == 0) { VMC_GetJointTorques(&c->vmc_[1], &c->output.joint[3], &c->output.joint[2]); } else { diff --git a/User/module/config.c b/User/module/config.c index c88a709..ebdd892 100644 --- a/User/module/config.c +++ b/User/module/config.c @@ -100,7 +100,7 @@ Config_RobotParam_t robot_config = { .yaw_omega = { .k = 1.0f, .p = 1.0f, - .i = 0.5f, + .i = 0.3f, .d = 0.0f, .i_limit = 1.0f, .out_limit = 1.0f, @@ -108,7 +108,7 @@ Config_RobotParam_t robot_config = { .range = -1.0f, }, .yaw_angle = { - .k = 10.0f, + .k = 8.0f, .p = 3.0f, .i = 0.0f, .d = 0.0f, @@ -200,8 +200,8 @@ Config_RobotParam_t robot_config = { .range = -1.0f, }, .leg_theta={ - .k=1.0f, - .p=5.0f, /* 摆角比例系数 */ + .k=5.0f, + .p=2.0f, /* 摆角比例系数 */ .i=0.0f, /* 摆角积分系数 */ .d=0.0f, /* 摆角微分系数 */ .i_limit=0.0f, /* 积分限幅 */