/* ============================================================================= * robot_cmd_task.c - 控制源汇总(2ms),发布 cmd_robot_t(robot_typedef.h) * 云台板: DR16 遥控器 + 键鼠(0x0304 优先;USART3 的 18 字节帧里同样带键鼠,两路已合并,见 KB_FROM_DBUS_FRAME) * 键鼠/遥控互斥(与 legacy 一致);摇杆->云台增量与限位门控; * Q 摩擦轮/左键发射/E 退弹/拨轮;首帧触发 gimbal_reset * 底盘板: 从 ctoc_remote(0x149/0x189 解析结果)生成: * S2 上=慢陀螺 下=快陀螺 中=跟随;Ctrl 切换保持型小陀螺; * vx/vy 来自右摇杆(键鼠由云台合成);左摇杆水平=手动旋转(死区外 * 覆盖跟随输出);Shift=加速 * ========================================================================== */ #include "app_tasks.h" #include "cmsis_os.h" #include "dr16.h" #include "c_comm.h" #include "robot_typedef.h" #include "robot_param.h" #if defined(ROBOT_TYPE_GIMBAL) #include "robot_cmd_task.h" #include "referee.h" #include "gimbal.h" #elif defined(ROBOT_TYPE_CHASSIS) #include "referee.h" #endif /* robot_cmd_task(2ms):汇总控制源,发布 cmd_robot_t * 云台板:DR16 遥控器 + 图传键鼠(0x0304) * 底盘板:CToC 转发的"虚拟遥控器"(0x149/0x189) */ volatile cmd_robot_t cmd_robot; #if defined(ROBOT_TYPE_GIMBAL) keyboard_data_t keyboard_data; /* 键鼠来源合并视图(声明见 robot_cmd_task.h):云台控制与 c_comm_task 底盘摇杆合成共用 */ keyboard_data_t keyboard_merged; uint8_t keyboard_merged_online = 0; static uint8_t keyboard_online = 0; void dr16_frame_callback(const uint8_t * frame, uint16_t len) { if (len == 18) dr16_parse(frame); } /* 0x0304 自定义控制器数据 -> keyboard_data(中断上下文) */ static void referee_keyboard_hook(const uint8_t data[12]) { keyboard_data.mouse_rl = (int16_t)(((uint16_t)data[1] << 8) | data[0]); keyboard_data.mouse_du = (int16_t)(((uint16_t)data[3] << 8) | data[2]); keyboard_data.mouse_wheel = (int16_t)(((uint16_t)data[5] << 8) | data[4]); keyboard_data.mouse_key_l = data[6]; keyboard_data.mouse_key_r = data[7]; keyboard_data.key_w = data[8] & 0x01; keyboard_data.key_s = (data[8] >> 1) & 0x01; keyboard_data.key_a = (data[8] >> 2) & 0x01; keyboard_data.key_d = (data[8] >> 3) & 0x01; keyboard_data.key_shift = (data[8] >> 4) & 0x01; keyboard_data.key_ctrl = (data[8] >> 5) & 0x01; keyboard_data.key_q = (data[8] >> 6) & 0x01; keyboard_data.key_e = (data[8] >> 7) & 0x01; /* Q/E/Shift/Ctrl 按下翻转(legacy 键盘边沿语义) */ if (keyboard_data.key_last_q == 0 && keyboard_data.key_q == 1) keyboard_data.key_push_q = !keyboard_data.key_push_q; if (keyboard_data.key_last_e == 0 && keyboard_data.key_e == 1) keyboard_data.key_push_e = !keyboard_data.key_push_e; if (keyboard_data.key_last_shift == 0 && keyboard_data.key_shift == 1) keyboard_data.key_push_shift = !keyboard_data.key_push_shift; if (keyboard_data.key_last_ctrl == 0 && keyboard_data.key_ctrl == 1) keyboard_data.key_push_ctrl = !keyboard_data.key_push_ctrl; keyboard_data.key_last_q = keyboard_data.key_q; keyboard_data.key_last_e = keyboard_data.key_e; keyboard_data.key_last_shift = keyboard_data.key_shift; keyboard_data.key_last_ctrl = keyboard_data.key_ctrl; keyboard_data.last_tick = HAL_GetTick(); } /* ---- 键鼠输入来源(2026-10-16 修) ---- * 背景:客户端键鼠是从 USART3 那 18 字节帧进来的(data[6..15]),dr16_parse 早就 * 解析进 dr16.mouse_rl / dr16.key_w 等字段,但控制逻辑只读 0x0304 填的 * keyboard_data,于是"键鼠控制完全没用"。此开关让两路合并。 * 1 = 除 0x0304 外同时接受帧内键鼠:见到键鼠动作即进入键鼠模式并保持, * 直到操作手去动云台左摇杆才交还遥控器(右摇杆在底盘,键鼠模式下仍要能用)。 * 0 = 旧行为:只有 0x0304 在线才走键鼠。 */ #define KB_FROM_DBUS_FRAME 1 void robot_cmd_task(void const * argument) { referee_set_keyboard_hook(referee_keyboard_hook); while (!app_ready) osDelay(2); /* 键鼠模式状态:帧内键鼠动作置 1,左摇杆动作清 0 */ static uint8_t kb_mode = 0; for (;;) { keyboard_online = (keyboard_data.last_tick != 0 && HAL_GetTick() - keyboard_data.last_tick < DEVICE_OFFLINE_TIME_MS * 10); #if KB_FROM_DBUS_FRAME { uint8_t kb_act = 0, rc_act = 0; /* 键鼠动作:鼠标移动 / 滚轮 / 左右键 / 任意按键(data[14]或data[15]非 0) */ if (dr16.mouse_rl > 1 || dr16.mouse_rl < -1) kb_act = 1; if (dr16.mouse_du > 1 || dr16.mouse_du < -1) kb_act = 1; if (dr16.mouse_wheel != 0) kb_act = 1; if (dr16.mouse_key_l || dr16.mouse_key_r) kb_act = 1; if (dr16.key_code_lo || dr16.key_code_hi) kb_act = 1; /* 遥控器动作:只认云台左摇杆 */ if (dr16.rc_l_UD > 1050 || dr16.rc_l_UD < 1000) rc_act = 1; if (dr16.rc_l_RL > 1050 || dr16.rc_l_RL < 1000) rc_act = 1; if (kb_act) kb_mode = 1; if (rc_act) kb_mode = 0; } #endif /* 是否键鼠在控:0x0304 在线时始终算键鼠(保持原有优先级) */ uint8_t kb_active = (keyboard_online || kb_mode) ? 1 : 0; /* ---- 合并成共享视图 keyboard_merged ---- * 0x0304 在线 → 整份用 keyboard_data;否则用 USART3 帧里那一套。 * 云台控制、发射机构、以及 c_comm_task 里 WASD→底盘摇杆的合成都读这个 * 视图,避免两处各判一次导致"云台跟鼠标了、底盘却还在收遥控器原始摇杆"。 */ if (keyboard_online) { keyboard_merged = keyboard_data; } else { keyboard_merged.mouse_rl = dr16.mouse_rl; keyboard_merged.mouse_du = dr16.mouse_du; keyboard_merged.mouse_wheel = dr16.mouse_wheel; keyboard_merged.mouse_key_l = dr16.mouse_key_l; keyboard_merged.mouse_key_r = dr16.mouse_key_r; keyboard_merged.key_w = dr16.key_w; keyboard_merged.key_s = dr16.key_s; keyboard_merged.key_a = dr16.key_a; keyboard_merged.key_d = dr16.key_d; keyboard_merged.key_shift = dr16.key_shift; keyboard_merged.key_ctrl = dr16.key_ctrl; keyboard_merged.key_q = dr16.key_q; keyboard_merged.key_e = dr16.key_e; /* alpha 键:dr16 侧那两位恒为 0(见 dr16.c 的 >>9/>>10),照搬保持语义 */ keyboard_merged.key_alpha1 = dr16.key_alpha1; keyboard_merged.key_alpha2 = dr16.key_alpha2; keyboard_merged.key_push_q = dr16.key_push_q; keyboard_merged.key_push_e = dr16.key_push_e; keyboard_merged.key_push_shift = dr16.key_push_shift; keyboard_merged.key_push_ctrl = dr16.key_push_ctrl; keyboard_merged.last_tick = dr16_last_tick; } keyboard_merged_online = kb_active; /* 控制逻辑用的简写(取自共享视图) */ int16_t kb_mouse_rl = keyboard_merged.mouse_rl; int16_t kb_mouse_du = keyboard_merged.mouse_du; uint8_t kb_mouse_l = keyboard_merged.mouse_key_l; uint8_t kb_push_q = keyboard_merged.key_push_q; uint8_t kb_push_e = keyboard_merged.key_push_e; cmd_robot.online = (dr16_status == 1 && !dr16_offline()); cmd_robot.source = kb_active ? 1 : 0; if (cmd_robot.online) { /* ---- 云台轴增量 ---- */ float yaw_delta = 0, pitch_delta = 0; uint16_t pos = motor_pitch_gimbal.fdb.raw_angle; if (!kb_active) { /* 遥控器:左摇杆向上(>1050)为抬头(pitch_delta减小,pos向2260减小),门控须 pos > UPPER_LIMIT(2260); * 左摇杆向下(<1000)为低头(pitch_delta增大,pos向3950增大),门控须 pos < LOWER_LIMIT(3950) */ if (dr16.rc_l_UD > 1050 && pos > PITCH_GM6020_POSITION_UPPER_LIMIT) pitch_delta -= GIMBAL_PITCH_LEVER_RATE / 8192.0f * 360.0f * ((float)dr16.rc_l_UD - 1024.0f) / 660.0f; else if (dr16.rc_l_UD < 1000 && pos < PITCH_GM6020_POSITION_LOWER_LIMIT) pitch_delta += GIMBAL_PITCH_LEVER_RATE / 8192.0f * 360.0f * (1024.0f - (float)dr16.rc_l_UD) / 660.0f; if (dr16.rc_l_RL > 1050 || dr16.rc_l_RL < 1000) yaw_delta -= GIMBAL_LEVER_SPEED_MAP_RATE * GIMBAL_YAW_PITCH_SPEED_RATE * GIMBAL_YAW_PITCH_SPEED_RATE * 0.0439453125f * ((float)dr16.rc_l_RL - 1024.0f) / 660.0f; } else { /* 键鼠:鼠标前推(>8)为抬头(pitch_delta减小),门控须 pos > UPPER_LIMIT;鼠标后拉(<-8)为低头(pitch_delta增大),门控须 pos < LOWER_LIMIT */ if (kb_mouse_du > 8 && pos > PITCH_GM6020_POSITION_UPPER_LIMIT) pitch_delta -= GIMBAL_PITCH_LEVER_RATE / 8192.0f * 360.0f * ((float)kb_mouse_du * PC_MOUSE_DU_SENSITIVITY) / 660.0f; else if (kb_mouse_du < -8 && pos < PITCH_GM6020_POSITION_LOWER_LIMIT) pitch_delta += GIMBAL_PITCH_LEVER_RATE / 8192.0f * 360.0f * (-(float)kb_mouse_du * PC_MOUSE_DU_SENSITIVITY) / 660.0f; if (kb_mouse_rl != 0) yaw_delta -= GIMBAL_LEVER_SPEED_MAP_RATE * GIMBAL_YAW_PITCH_SPEED_RATE * GIMBAL_YAW_PITCH_SPEED_RATE * 0.0439453125f * ((float)kb_mouse_rl * PC_MOUSE_RL_SENSITIVITY / 660.0f * 2.0f); } cmd_robot.gimbal_yaw_delta = yaw_delta; cmd_robot.gimbal_pitch_delta = pitch_delta; /* ---- 发射机构 ---- * 键鼠模式用 Q / 鼠标左键 / E(两路来源已合并进 kb_*,0x0304 优先); * 遥控模式用 S1 与拨轮。kb_push_* 是"按下翻转"的常驻状态, * 且键鼠模式只在动左摇杆时才退出,所以摩擦轮/E 的状态不会自己掉。 */ if (!kb_active) { cmd_robot.friction_on = (dr16.s1 == 1); cmd_robot.fire = (dr16.thumb_wheel < 1000); cmd_robot.eject = (dr16.thumb_wheel > 1050); } else { cmd_robot.friction_on = kb_push_q; cmd_robot.fire = kb_mouse_l; cmd_robot.eject = kb_push_e; } cmd_robot.thumb_wheel = dr16.thumb_wheel; } else { cmd_robot.gimbal_yaw_delta = 0; cmd_robot.gimbal_pitch_delta = 0; cmd_robot.friction_on = 0; cmd_robot.fire = 0; cmd_robot.eject = 0; } /* 小陀螺使能:Ctrl 沿翻转。键鼠模式下取键鼠那一路的 Ctrl(0x0304 的 * keyboard_data 或帧内 dr16.key_push_ctrl),遥控模式用遥控器 Ctrl。 * 云台侧是数据源头,底盘只收状态位——重连瞬间保持态跳变不会在 * 底盘误触发沿(修复"重连误开小陀螺疯转") */ { static uint8_t spin_flag = 0, last_ctrl = 0; uint8_t ctrl_now = keyboard_online ? keyboard_data.key_push_ctrl : dr16.key_push_ctrl; if (last_ctrl == 0 && ctrl_now == 1) spin_flag = !spin_flag; last_ctrl = ctrl_now; cmd_robot.spin_flag = spin_flag; } /* 遥控器重新连接瞬间(start_flag==2)请求云台复位,与 legacy 一致; * 同时清小陀螺使能(重连不应保持按 Ctrl 的历史状态) */ cmd_robot.gimbal_reset = 0; if (dr16_start_flag == 2) { dr16_start_flag = 0; cmd_robot.gimbal_reset = 1; cmd_robot.spin_flag = 0; } monitor_heartbeat(HB_CMD); osDelay(2); } } #elif defined(ROBOT_TYPE_CHASSIS) void robot_cmd_task(void const * argument) { while (!app_ready) osDelay(2); for (;;) { /* 板间通讯在线 = 云台侧遥控器在线 且 收帧新鲜 */ /* 云台电机故障位(0x189 Data[6] bit7)置位时同样视为离线,底盘停车 */ uint8_t ctoc_alive = (ctoc_remote.status == 1 && ctoc_remote.last_tick != 0 && HAL_GetTick() - ctoc_remote.last_tick < CTOC_OFFLINE_TIME_MS && ctoc_remote.gimbal_fault == 0); cmd_robot.online = ctoc_alive; cmd_robot.source = 0; if (cmd_robot.online) { /* 底盘模式:小陀螺使能位(0x189 bit2,云台侧做 Ctrl 沿检测)优先 * SPIN_FAST;否则右拨杆 上=慢陀螺 下=快陀螺 中=跟随。 * 底盘不做沿检测——遥控重连瞬间转发保持态跳变会被误判为按键, * 导致误开小陀螺疯转(已踩坑) */ if (ctoc_remote.spin_flag) cmd_robot.chassis_mode = CHASSIS_MODE_SPIN_FAST; else if (ctoc_remote.s2 == 1) cmd_robot.chassis_mode = CHASSIS_MODE_SPIN_SLOW; else if (ctoc_remote.s2 == 2) cmd_robot.chassis_mode = CHASSIS_MODE_SPIN_FAST; else cmd_robot.chassis_mode = CHASSIS_MODE_FOLLOW; /* 平移:右摇杆(键盘模式由云台合成 WASD 进摇杆量) */ cmd_robot.vx = (1024.0f - (float)ctoc_remote.rc_r_ud) * MECANUM_CHASSIS_SPEED_SCALE; cmd_robot.vy = (1024.0f - (float)ctoc_remote.rc_r_rl) * MECANUM_CHASSIS_SPEED_SCALE; /* 无手动旋转:左摇杆水平是云台 Yaw 控制量,底盘再取用即一杆 * 两用冲突(拨 yaw 摇杆底盘同转);旋转由跟随/小陀螺接管 */ cmd_robot.wz_manual = 0; cmd_robot.gimbal_reset = 0; if (ctoc_remote.start_flag == 2) { ctoc_remote.start_flag = 0; cmd_robot.gimbal_reset = 1; // 复用字段:跟随朝向重新锁定 } } else { cmd_robot.chassis_mode = CHASSIS_MODE_OFF; cmd_robot.vx = 0; cmd_robot.vy = 0; cmd_robot.wz_manual = 0; } monitor_heartbeat(HB_CMD); osDelay(2); } } #endif