From 596d3d4c651d9fe2c60aead6bc960c1b611fc27e Mon Sep 17 00:00:00 2001 From: TuxMonkey <8196772+tuxmonkey@user.noreply.gitee.com> Date: Tue, 3 Mar 2026 21:44:39 +0800 Subject: [PATCH] halfsteering --- .../half_steering/halfsteering_cmd.c | 169 +++++++++++++++++- .../half_steering/halfsteering_cmd.h | 18 +- .../half_steering/halfsteering_def.h | 34 +++- 3 files changed, 205 insertions(+), 16 deletions(-) diff --git a/User_Code/user_task/half_steering/halfsteering_cmd.c b/User_Code/user_task/half_steering/halfsteering_cmd.c index 46cfef7..9061843 100644 --- a/User_Code/user_task/half_steering/halfsteering_cmd.c +++ b/User_Code/user_task/half_steering/halfsteering_cmd.c @@ -1,5 +1,166 @@ -// -// Created by esqwt on 2026/3/2. -// - #include "halfsteering_cmd.h" +#include "halfsteering_def.h" +#include "chassis_half_steer.h" // 包含底盘应用层结构体 +#include "pid.h" +#include "rc.h" +#include "user_lib.h" +#include +#include + +// --- 外部依赖 --- +extern Chassis_HalfSteer_t chassis_half_steer; // 假设在app层定义好的底盘实例 + +// --- 本地任务数据 --- +static HalfSteer_Task_Data_t task_data; +static const RC_ctrl_t *local_rc_ctrl; + +// ================= PID 参数定义与实例化 ================= +static PIDInstance chassis_follow_pid; +// 按你的格式直接在这里定义并初始化配置结构体 +static PID_Init_Config_s chassis_follow_pid_conf = { + .Kp = 6.0f, + .Ki = 0.0f, + .Kd = 0.495f, + // .DeadBand = 0.5f, + //.CoefA = 0.2f, + //.CoefB = 0.3f, + //.Improve = PID_Trapezoid_Intergral | PID_DerivativeFilter | PID_Derivative_On_Measurement | PID_Integral_Limit, + //.IntegralLimit = 50.0f, + .MaxOut = 45.0f, + //.Derivative_LPF_RC = 0.01f, +}; + +// --- 私有函数声明 --- +static void ModeSelection(void); + +static void RemoteControlSet(Chassis_Ctrl_Cmd_s *cmd); + +/** + * @brief 任务初始化 + */ +void RobotCMDInit(void) +{ + // 1. 获取遥控器指针 + local_rc_ctrl = RC_Get_RC_Pointer(); // 请替换为你实际获取遥控器数据的接口 + + // 2. 初始化底盘跟随 PID + PIDInit(&chassis_follow_pid, &chassis_follow_pid_conf); + + // 3. 任务数据清零 + memset(&task_data, 0, sizeof(HalfSteer_Task_Data_t)); + task_data.current_mode = MODE_RELAX; + task_data.init_done = 1; +} + +/** + * @brief 任务核心循环 (放置于 RTOS Task 中) + */ +void RobotCMDTask(void) +{ + if (!task_data.init_done) return; + + Chassis_Ctrl_Cmd_s chassis_cmd_send; + memset(&chassis_cmd_send, 0, sizeof(Chassis_Ctrl_Cmd_s)); + + // 1. 状态机选择 + ModeSelection(); + + // 2. 根据状态设定底层指令 + RemoteControlSet(&chassis_cmd_send); + + // 3. 将计算完成的指令发送给底盘应用层 + Chassis_HalfSteer_Update(&chassis_half_steer, &chassis_cmd_send); +} + +/** + * @brief 状态机模式选择 (复刻你的多拨杆判断逻辑) + */ +static void ModeSelection(void) +{ + // 左侧[中], 右侧[下] -> 视觉模式 + if (switch_is_mid(local_rc_ctrl->rc.s[1]) && switch_is_down(local_rc_ctrl->rc.s[0])) + { + task_data.current_mode = MODE_AUTO_VISION; + } + // 左侧[中], 右侧[中] -> 遥控器不跟随 + else if (switch_is_mid(local_rc_ctrl->rc.switch[1]) + && + switch_is_mid(local_rc_ctrl->rc.switch[0]) + ) { + task_data.current_mode = MODE_NO_FOLLOW; + } + // 左侧[下], 右侧[下] -> 底盘小陀螺 + else + if (switch_is_down(local_rc_ctrl->rc.s[1]) && switch_is_down(local_rc_ctrl->rc.s[0])) + { + task_data.current_mode = MODE_REMOTE_SPIN; + } + // 左侧[下], 右侧[中] -> 遥控器不跟随 (你原代码中的独立判断) + else if (switch_is_down(local_rc_ctrl->rc.s[1]) && switch_is_mid(local_rc_ctrl->rc.s[0])) + { + task_data.current_mode = MODE_NO_FOLLOW; + } + // 左侧[下] (单边条件兜底) -> 默认跟随模式 + else if (switch_is_down(local_rc_ctrl->rc.s[1])) + { + task_data.current_mode = MODE_REMOTE_FOLLOW; + } + else + { + task_data.current_mode = MODE_RELAX; + } +} + +/** + * @brief 遥控器映射与 PID 计算 + */ +static void RemoteControlSet(Chassis_Ctrl_Cmd_s *cmd) +{ + if (task_data.current_mode == MODE_RELAX) + { + cmd->chassis_mode = CHASSIS_ZERO_FORCE; + return; + } + + // --- 1. 底盘基础平移 (水平和竖直方向) --- + cmd->vx = RC_CHASSIS_SPEED_SCALE * ((float) local_rc_ctrl->rc.ch[1] / 660.0f); // 摇杆前进 + cmd->vy = RC_CHASSIS_SPEED_SCALE * ((float) local_rc_ctrl->rc.ch[0] / 660.0f); // 摇杆平移 + + // 摇杆死区处理 + if (fabsf(local_rc_ctrl->rc.ch[1]) < RC_DEADBAND) cmd->vx = 0; + if (fabsf(local_rc_ctrl->rc.ch[0]) < RC_DEADBAND) cmd->vy = 0; + + // --- 2. 旋转量与模式映射 --- + switch (task_data.current_mode) + { + case MODE_REMOTE_FOLLOW: + cmd->chassis_mode = CHASSIS_FOLLOW_GIMBAL_YAW; + // 注意：此处需要你获取到底盘和云台的实际偏差角 (例如从 Gimbal 结构体或电机 feedback 中拿) + // 假设获取到的夹角叫 angle_error，并已转换到 [-180, 180] 之间 + float angle_error = 0.0f; /* 替换为获取真实误差的代码 */ + + // 使用在文件头部实例化的 chassis_follow_pid 计算 wz 输出 + cmd->wz = PIDCalculate(&chassis_follow_pid, angle_error, 0.0f) / 100.0f; + break; + + case MODE_REMOTE_SPIN: + cmd->chassis_mode = CHASSIS_ROTATE; + cmd->wz = RC_ROTATE_SPEED_SCALE; + break; + + case MODE_AUTO_VISION: + // 在你给的代码中，24赛季检录视觉模式时让底盘转小陀螺 + cmd->chassis_mode = CHASSIS_ROTATE; // CHASSIS_RE_ROTATE 对应底层小陀螺逻辑 + cmd->wz = RC_ROTATE_SPEED_SCALE; + break; + + case MODE_NO_FOLLOW: + cmd->chassis_mode = CHASSIS_NO_FOLLOW; + cmd->wz = 0.0f; // 如果需要拨杆控制旋转，可以在这里加入摇杆映射 + break; + + default: + cmd->chassis_mode = CHASSIS_ZERO_FORCE; + break; + } +} diff --git a/User_Code/user_task/half_steering/halfsteering_cmd.h b/User_Code/user_task/half_steering/halfsteering_cmd.h index 4985f28..f1000d5 100644 --- a/User_Code/user_task/half_steering/halfsteering_cmd.h +++ b/User_Code/user_task/half_steering/halfsteering_cmd.h @@ -1,8 +1,14 @@ -// -// Created by esqwt on 2026/3/2. -// +#ifndef HALFSTEERING_CMD_H +#define HALFSTEERING_CMD_H -#ifndef TRONONEH7_SCAFFOLD_HALFSTEERING_CMD_H -#define TRONONEH7_SCAFFOLD_HALFSTEERING_CMD_H +/** + * @brief 机器人核心控制任务初始化,会被RobotInit()调用 + */ +void RobotCMDInit(void); -#endif // TRONONEH7_SCAFFOLD_HALFSTEERING_CMD_H +/** + * @brief 机器人核心控制任务,200Hz频率运行(必须高于视觉发送频率) + */ +void RobotCMDTask(void); + +#endif // HALFSTEERING_CMD_H diff --git a/User_Code/user_task/half_steering/halfsteering_def.h b/User_Code/user_task/half_steering/halfsteering_def.h index ca9cc79..2fdd828 100644 --- a/User_Code/user_task/half_steering/halfsteering_def.h +++ b/User_Code/user_task/half_steering/halfsteering_def.h @@ -1,8 +1,30 @@ -// -// Created by esqwt on 2026/3/3. -// +#ifndef HALF_STEER_DEF_H +#define HALF_STEER_DEF_H -#ifndef TRONONEH7_SCAFFOLD_HALFSTEERING_DEF_H -#define TRONONEH7_SCAFFOLD_HALFSTEERING_DEF_H +#include "stdint.h" -#endif // TRONONEH7_SCAFFOLD_HALFSTEERING_DEF_H +// ================= 遥控器映射系数 ================= +#define RC_CHASSIS_SPEED_SCALE 80.0f // 摇杆转平移速度比例 +#define RC_ROTATE_SPEED_SCALE 1.0f // 小陀螺模式固定自旋速度 +#define RC_DEADBAND 10 // 摇杆死区 + +// ================= 机器人状态枚举 ================= +typedef enum +{ + MODE_RELAX = 0, // 急停/掉线模式 + MODE_REMOTE_FOLLOW, // 纯遥控器：底盘跟随云台 + MODE_REMOTE_SPIN, // 纯遥控器：底盘小陀螺 + MODE_AUTO_VISION, // 视觉辅助模式 + MODE_NO_FOLLOW, // 云台底盘分离 (不跟随) +} Robot_State_e; + +// ================= 内部控制对象结构体 ================= +typedef struct +{ + Robot_State_e current_mode; // 当前状态机模式 + uint8_t init_done; // 初始化完成标志 + + // 如果有视觉数据或云台数据需要跨函数传递，也可以加在这里 +} HalfSteer_Task_Data_t; + +#endif // HALF_STEER_DEF_H