lyf
267a659242
键鼠控制(此前 WASD 无反应): - 云台 robot_cmd_task:0x0304 与 USART3 帧内键鼠合并为共享视图 keyboard_merged(0x0304 优先); 键鼠模式由帧内键鼠动作进入,动云台左摇杆退出 - c_comm_task/c_comm:键鼠在控时底盘摇杆由 WASD+鼠标合成,改读共享视图(此前只认 0x0304) - dr16/调试输出:新增帧内键鼠原始字节诊断(key_lo/key_hi/mouse_*/s1/frames) 自定义UI(底盘板经裁判系统 USART6 绘制到选手端): - 新增 底盘/modules/comms/referee_ui.c|h:15 字节图形组包(带编译期断言), v5 卡片式 HUD = 俯仰梯(pitch) + 罗盘刻度带(底盘朝向) + 预估底盘功率 + 超级电容 + 摩擦轮 L/R 转速 + 小陀螺状态,yaw 在刻度带右侧;5Hz 刷新,约 50 图元/8 包 - referee.c 新增 referee_send_frame()(cmd 0x0301 机器人交互数据,CRC8+CRC16,USART6 TX DMA) - 新增 CToC 0x1B9:云台把 pitch 与摩擦轮转速带给底盘(底盘本地没有这两个量) - 版式避开官方保留区:顶部比分条/左下小地图/右下战车模块/中部两块官方保留 验证:云台 Code=99540 RO=1764 RW=420 ZI=62572;底盘 Code=99452 RO=1928 RW=416 ZI=62584;均 0 错误 0 警告。 未做实机验证(当前无板/无图传);UI 协议常量(UI_RECEIVER_ID/UI_SEND_HZ)与版式坐标集中在 referee_ui.h 宏区,便于按协议 PDF 一处校正。
328 lines
14 KiB
C
328 lines
14 KiB
C
/* =============================================================================
|
|
* 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
|