/* ============================================================================= * robot_cmd_task.c - 控制源汇总(2ms),发布 cmd_robot_t(robot_typedef.h) * 云台板: DR16 遥控器 + 图传键鼠(0x0304,referee 键盘钩子填充 keyboard_data) * 键鼠/遥控互斥(与 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; 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(); } void robot_cmd_task(void const * argument) { referee_set_keyboard_hook(referee_keyboard_hook); while (!app_ready) osDelay(2); for (;;) { keyboard_online = (keyboard_data.last_tick != 0 && HAL_GetTick() - keyboard_data.last_tick < DEVICE_OFFLINE_TIME_MS * 10); cmd_robot.online = (dr16_status == 1 && !dr16_offline()); cmd_robot.source = keyboard_online ? 1 : 0; if (cmd_robot.online) { /* ---- 云台轴增量 ---- */ float yaw_delta = 0, pitch_delta = 0; uint16_t pos = motor_pitch_gimbal.fdb.raw_angle; if (!keyboard_online) { /* 遥控器:左摇杆向上(>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 (keyboard_data.mouse_du > 8 && pos > PITCH_GM6020_POSITION_UPPER_LIMIT) pitch_delta -= GIMBAL_PITCH_LEVER_RATE / 8192.0f * 360.0f * ((float)keyboard_data.mouse_du * PC_MOUSE_DU_SENSITIVITY) / 660.0f; else if (keyboard_data.mouse_du < -8 && pos < PITCH_GM6020_POSITION_LOWER_LIMIT) pitch_delta += GIMBAL_PITCH_LEVER_RATE / 8192.0f * 360.0f * (-(float)keyboard_data.mouse_du * PC_MOUSE_DU_SENSITIVITY) / 660.0f; if (keyboard_data.mouse_rl != 0) yaw_delta -= GIMBAL_LEVER_SPEED_MAP_RATE * GIMBAL_YAW_PITCH_SPEED_RATE * GIMBAL_YAW_PITCH_SPEED_RATE * 0.0439453125f * ((float)keyboard_data.mouse_rl * PC_MOUSE_RL_SENSITIVITY / 660.0f * 2.0f); } cmd_robot.gimbal_yaw_delta = yaw_delta; cmd_robot.gimbal_pitch_delta = pitch_delta; /* ---- 发射机构 ---- */ if (!keyboard_online) { 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 = keyboard_data.key_push_q; cmd_robot.fire = keyboard_data.mouse_key_l; cmd_robot.eject = keyboard_data.key_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 沿翻转(仅在遥控源检测,键鼠 TODO); * 云台侧是数据源头,底盘只收状态位——重连瞬间保持态跳变不会在 * 底盘误触发沿(修复"重连误开小陀螺疯转") */ { static uint8_t spin_flag = 0, last_ctrl = 0; uint8_t ctrl_now = (!keyboard_online) ? dr16.key_push_ctrl : 0; 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