245 lines
10 KiB
C
245 lines
10 KiB
C
/* =============================================================================
|
|
* 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
|