RMUL2025/User/component/cmd.c

440 lines
11 KiB
C
Raw Normal View History

2025-03-09 18:01:50 +08:00
/*
*/
#include "cmd.h"
#include <string.h>
/**
* @brief
*
* @param cmd
* @param behavior
* @return uint16_t
*/
static inline CMD_KeyValue_t CMD_BehaviorToKey(CMD_t *cmd,
CMD_Behavior_t behavior)
{
return cmd->param->map.key_map[behavior].key;
}
static inline CMD_ActiveType_t CMD_BehaviorToActive(CMD_t *cmd,
CMD_Behavior_t behavior)
{
return cmd->param->map.key_map[behavior].active;
}
/**
* @brief
*
* @param rc
* @param key
* @param stateful
* @return true
* @return false
*/
static bool CMD_KeyPressedRc(const CMD_RC_t *rc, CMD_KeyValue_t key)
{
/* 按下按键为鼠标左、右键 */
if (key == CMD_L_CLICK)
{
return rc->mouse.l_click;
}
if (key == CMD_R_CLICK)
{
return rc->mouse.r_click;
}
return rc->key & (1u << key);
}
static bool CMD_BehaviorOccurredRc(const CMD_RC_t *rc, CMD_t *cmd,
CMD_Behavior_t behavior)
{
CMD_KeyValue_t key = CMD_BehaviorToKey(cmd, behavior);
CMD_ActiveType_t active = CMD_BehaviorToActive(cmd, behavior);
bool now_key_pressed, last_key_pressed;
/* 按下按键为鼠标左、右键 */
if (key == CMD_L_CLICK)
{
now_key_pressed = rc->mouse.l_click;
last_key_pressed = cmd->mouse_last.l_click;
}
else if (key == CMD_R_CLICK)
{
now_key_pressed = rc->mouse.r_click;
last_key_pressed = cmd->mouse_last.r_click;
}
else
{
now_key_pressed = rc->key & (1u << key);
last_key_pressed = cmd->key_last & (1u << key);
}
switch (active)
{
case CMD_ACTIVE_PRESSING:
return now_key_pressed && !last_key_pressed;
case CMD_ACTIVE_RASING:
return !now_key_pressed && last_key_pressed;
case CMD_ACTIVE_PRESSED:
return now_key_pressed;
}
}
/**
* @brief pc行为逻辑
*
* @param rc
* @param cmd
* @param dt_sec
*/
static void CMD_PcLogic(const CMD_RC_t *rc, CMD_t *cmd, float dt_sec)
{
cmd->gimbal.mode = GIMBAL_MODE_ABSOLUTE;
/* 云台设置为鼠标控制欧拉角的变化,底盘的控制向量设置为零 */
cmd->gimbal.delta_eulr.yaw =
(float)rc->mouse.x * dt_sec * cmd->param->sens_mouse;
cmd->gimbal.delta_eulr.pit =
(float)(-rc->mouse.y) * dt_sec * cmd->param->sens_mouse;
cmd->chassis.ctrl_vec.vx = cmd->chassis.ctrl_vec.vy = 0.0f;
cmd->shoot.reverse_trig = false;
/* 按键行为映射相关逻辑 */
if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_FORE))
{
cmd->chassis.ctrl_vec.vy += cmd->param->move.move_sense;
}
if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_BACK))
{
cmd->chassis.ctrl_vec.vy -= cmd->param->move.move_sense;
}
if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_LEFT))
{
cmd->chassis.ctrl_vec.vx -= cmd->param->move.move_sense;
}
if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_RIGHT))
{
cmd->chassis.ctrl_vec.vx += cmd->param->move.move_sense;
}
if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_ACCELERATE))
{
cmd->chassis.ctrl_vec.vx *= cmd->param->move.move_fast_sense;
cmd->chassis.ctrl_vec.vy *= cmd->param->move.move_fast_sense;
}
if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_DECELEBRATE))
{
cmd->chassis.ctrl_vec.vx *= cmd->param->move.move_slow_sense;
cmd->chassis.ctrl_vec.vy *= cmd->param->move.move_slow_sense;
}
if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_FIRE))
{
/* 切换至开火模式,设置相应的射击频率和弹丸初速度 */
cmd->shoot.mode = SHOOT_MODE_LOADED;
cmd->shoot.fire = true;
}
else
{
/* 切换至准备模式,停止射击 */
cmd->shoot.mode = SHOOT_MODE_LOADED;
cmd->shoot.fire = false;
}
if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_FIRE_MODE))
{
/* 每按一次依次切换开火下一个模式 */
cmd->shoot.fire_mode++;
cmd->shoot.fire_mode %= FIRE_MODE_NUM;
}
if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_ROTOR))
{
/* 切换到小陀螺模式 */
cmd->chassis.mode = CHASSIS_MODE_ROTOR;
cmd->chassis.mode_rotor = ROTOR_MODE_RAND;
}
if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_OPENCOVER))
{
/* 每按一次开、关弹舱盖 */
cmd->shoot.cover_open = !cmd->shoot.cover_open;
}
if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_BUFF))
{
if (cmd->ai_status == AI_STATUS_HITSWITCH)
{
/* 停止ai的打符模式停用host控制 */
CMD_RefereeAdd(&(cmd->referee), CMD_UI_HIT_SWITCH_STOP);
cmd->host_overwrite = false;
cmd->ai_status = AI_STATUS_STOP;
}
else if (cmd->ai_status == AI_STATUS_AUTOAIM)
{
/* 自瞄模式中切换失败提醒 */
}
else
{
/* ai切换至打符模式启用host控制 */
CMD_RefereeAdd(&(cmd->referee), CMD_UI_HIT_SWITCH_START);
cmd->ai_status = AI_STATUS_HITSWITCH;
cmd->host_overwrite = true;
}
}
if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_AUTOAIM))
{
if (cmd->ai_status == AI_STATUS_AUTOAIM)
{
/* 停止ai的自瞄模式停用host控制 */
cmd->host_overwrite = false;
cmd->ai_status = AI_STATUS_STOP;
CMD_RefereeAdd(&(cmd->referee), CMD_UI_AUTO_AIM_STOP);
}
else
{
/* ai切换至自瞄模式启用host控制 */
cmd->ai_status = AI_STATUS_AUTOAIM;
cmd->host_overwrite = true;
CMD_RefereeAdd(&(cmd->referee), CMD_UI_AUTO_AIM_START);
}
}
else
{
cmd->host_overwrite = false;
// TODO: 修复逻辑
}
if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_REVTRIG))
{
/* 按下拨弹反转 */
cmd->shoot.reverse_trig = true;
}
if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_FOLLOWGIMBAL35))
{
cmd->chassis.mode = CHASSIS_MODE_FOLLOW_GIMBAL;
}
/* 保存当前按下的键位状态 */
cmd->key_last = rc->key;
memcpy(&(cmd->mouse_last), &(rc->mouse), sizeof(cmd->mouse_last));
}
/**
* @brief rc行为逻辑
*
* @param rc
* @param cmd
* @param dt_sec
*/
static void CMD_RcLogic(const CMD_RC_t *rc, CMD_t *cmd, float dt_sec)
{
switch (rc->sw_l)
{
/* 左拨杆相应行为选择和解析 */
case CMD_SW_UP:
cmd->chassis.mode = CHASSIS_MODE_BREAK;
cmd->ai_status = AI_STATUS_STOP;
cmd->host_overwrite = false;
break;
case CMD_SW_MID:
cmd->chassis.mode = CHASSIS_MODE_FOLLOW_GIMBAL;
cmd->ai_status = AI_STATUS_AUTOAIM;
cmd->host_overwrite = true;
break;
case CMD_SW_DOWN:
cmd->chassis.mode = CHASSIS_MODE_ROTOR;
cmd->chassis.mode_rotor = ROTOR_MODE_CW;
cmd->ai_status = AI_STATUS_AUTOAIM;
cmd->host_overwrite = true;
break;
case CMD_SW_ERR:
cmd->chassis.mode = CHASSIS_MODE_RELAX;
cmd->ai_status = AI_STATUS_STOP;
cmd->host_overwrite = false;
break;
}
switch (rc->sw_r)
{
/* 右拨杆相应行为选择和解析*/
case CMD_SW_UP:
cmd->gimbal.mode = GIMBAL_MODE_ABSOLUTE;
cmd->shoot.mode = SHOOT_MODE_SAFE;
cmd->fire = false;
2025-03-09 18:01:50 +08:00
break;
case CMD_SW_MID:
cmd->gimbal.mode = GIMBAL_MODE_ABSOLUTE;
cmd->shoot.fire = false;
cmd->shoot.mode = SHOOT_MODE_LOADED;
cmd->fire = true;
2025-03-09 18:01:50 +08:00
break;
case CMD_SW_DOWN:
cmd->gimbal.mode = GIMBAL_MODE_ABSOLUTE;
cmd->shoot.mode = SHOOT_MODE_LOADED;
cmd->shoot.fire_mode = FIRE_MODE_SINGLE;
cmd->shoot.fire = true;
cmd->fire = true;
2025-03-09 18:01:50 +08:00
break;
case CMD_SW_ERR:
cmd->gimbal.mode = GIMBAL_MODE_RELAX;
cmd->shoot.mode = SHOOT_MODE_RELAX;
cmd->fire = false;
2025-03-09 18:01:50 +08:00
}
/* 将操纵杆的对应值转换为底盘的控制向量和云台变化的欧拉角 */
if (cmd->ai_status == AI_STATUS_STOP || cmd->host_overwrite == false)
{
cmd->chassis.ctrl_vec.vx = rc->ch_l_x;
cmd->chassis.ctrl_vec.vy = rc->ch_l_y;
cmd->gimbal.delta_eulr.yaw = rc->ch_r_x * dt_sec * cmd->param->sens_rc;
cmd->gimbal.delta_eulr.pit = rc->ch_r_y * dt_sec * cmd->param->sens_rc;
}
}
/**
* @brief rc失控时机器人恢复放松模式
*
* @param cmd
*/
static void CMD_RcLostLogic(CMD_t *cmd)
{
/* 机器人底盘、云台、射击运行模式恢复至放松模式 */
cmd->chassis.mode = CHASSIS_MODE_RELAX;
cmd->gimbal.mode = GIMBAL_MODE_RELAX;
cmd->shoot.mode = SHOOT_MODE_RELAX;
}
/**
* @brief
*
* @param cmd
* @param param
* @return int8_t 0
*/
int8_t CMD_Init(CMD_t *cmd, const CMD_Params_t *param)
{
/* 指针检测 */
if (cmd == NULL)
return -1;
if (param == NULL)
return -1;
/* 设置机器人的命令参数初始化控制方式为rc控制 */
cmd->pc_ctrl = false;
cmd->param = param;
return 0;
}
/**
* @brief
*
* @param cmd
* @return true
* @return false
*/
inline bool CMD_CheckHostOverwrite(CMD_t *cmd) { return cmd->host_overwrite; }
/**
* @brief
*
* @param rc
* @param cmd
* @param dt_sec
* @return int8_t 0
*/
int8_t CMD_ParseRc(CMD_RC_t *rc, CMD_t *cmd, float dt_sec)
{
/* 指针检测 */
if (rc == NULL)
return -1;
if (cmd == NULL)
return -1;
/* 在pc控制和rc控制间切换 */
if (CMD_KeyPressedRc(rc, CMD_KEY_SHIFT) &&
CMD_KeyPressedRc(rc, CMD_KEY_CTRL) && CMD_KeyPressedRc(rc, CMD_KEY_Q))
cmd->pc_ctrl = true;
if (CMD_KeyPressedRc(rc, CMD_KEY_SHIFT) &&
CMD_KeyPressedRc(rc, CMD_KEY_CTRL) && CMD_KeyPressedRc(rc, CMD_KEY_E))
cmd->pc_ctrl = false;
/*c当rc丢控时恢复机器人至默认状态 */
if ((rc->sw_l == CMD_SW_ERR) || (rc->sw_r == CMD_SW_ERR))
{
CMD_RcLostLogic(cmd);
}
else
{
if (cmd->pc_ctrl)
{
CMD_PcLogic(rc, cmd, dt_sec);
}
else
{
CMD_RcLogic(rc, cmd, dt_sec);
}
}
return 0;
}
/**
* @brief
*
* @param host host数据
* @param cmd
* @param dt_sec
* @return int8_t 0
*/
int8_t CMD_ParseHost(const CMD_Host_t *host, CMD_t *cmd, float dt_sec)
{
(void)dt_sec; /* 未使用dt_sec消除警告 */
/* 指针检测 */
if (host == NULL)
return -1;
if (cmd == NULL)
return -1;
/* 云台欧拉角设置为host相应的变化的欧拉角 */
cmd->gimbal.delta_eulr.yaw = host->gimbal_delta.yaw;
cmd->gimbal.delta_eulr.pit = host->gimbal_delta.pit;
/* host射击命令设置不同的射击频率和弹丸初速度 */
if(cmd->fire){
if (host->fire)
{
cmd->shoot.mode = SHOOT_MODE_LOADED;
cmd->shoot.fire = true;
}
else
{
// cmd->shoot.mode = SHOOT_MODE_SAFE;
cmd->shoot.mode = SHOOT_MODE_LOADED;
}
2025-03-09 18:01:50 +08:00
}
return 0;
}
/**
* @brief Referee发送的命令
*
* @param ref
* @param cmd
* @return int8_t 0
*/
int8_t CMD_RefereeAdd(CMD_RefereeCmd_t *ref, CMD_UI_t cmd)
{
/* 指针检测 */
if (ref == NULL)
return -1;
/* 越界检测 */
if (ref->counter >= CMD_REFEREE_MAX_NUM || ref->counter < 0)
return -1;
/* 添加机器人当前行为状态到画图的命令队列中 */
ref->cmd[ref->counter] = cmd;
ref->counter++;
return 0;
}