183 lines
4.4 KiB
C
183 lines
4.4 KiB
C
/*
|
||
控制命令
|
||
*/
|
||
|
||
#include "cmd.h"
|
||
|
||
#include <string.h>
|
||
|
||
|
||
|
||
/**
|
||
* @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_RELAX;
|
||
switch (rc->sw_r) {
|
||
/* 右拨杆相应行为选择和解析*/
|
||
case CMD_SW_UP:
|
||
cmd->chassis.action = CHASSIS_ACTION_NONE;
|
||
break;
|
||
case CMD_SW_MID:
|
||
cmd->chassis.action = CHASSIS_ACTION_NONE;
|
||
break;
|
||
case CMD_SW_DOWN:
|
||
cmd->chassis.action = CHASSIS_ACTION_NONE;
|
||
break;
|
||
case CMD_SW_ERR:
|
||
cmd->chassis.action = CHASSIS_ACTION_NONE;
|
||
break;
|
||
}
|
||
break;
|
||
|
||
case CMD_SW_MID:
|
||
cmd->chassis.mode = CHASSIS_MODE_DAMP;
|
||
switch (rc->sw_r) {
|
||
/* 右拨杆相应行为选择和解析*/
|
||
case CMD_SW_UP:
|
||
cmd->chassis.action = CHASSIS_ACTION_NONE;
|
||
break;
|
||
case CMD_SW_MID:
|
||
cmd->chassis.action = CHASSIS_ACTION_NONE;
|
||
break;
|
||
case CMD_SW_DOWN:
|
||
cmd->chassis.action = CHASSIS_ACTION_NONE;
|
||
break;
|
||
case CMD_SW_ERR:
|
||
cmd->chassis.action = CHASSIS_ACTION_NONE;
|
||
break;
|
||
}
|
||
break;
|
||
|
||
case CMD_SW_DOWN:
|
||
cmd->chassis.mode = CHASSIS_MODE_POSITION;
|
||
switch (rc->sw_r) {
|
||
/* 右拨杆相应行为选择和解析*/
|
||
case CMD_SW_UP:
|
||
cmd->chassis.action = CHASSIS_ACTION_NONE;
|
||
break;
|
||
case CMD_SW_MID:
|
||
cmd->chassis.action = CHASSIS_ACTION_STAND;
|
||
break;
|
||
case CMD_SW_DOWN:
|
||
cmd->chassis.action = CHASSIS_ACTION_TROT;
|
||
break;
|
||
case CMD_SW_ERR:
|
||
cmd->chassis.action = CHASSIS_ACTION_NONE;
|
||
break;
|
||
}
|
||
break;
|
||
|
||
case CMD_SW_ERR:
|
||
cmd->chassis.mode = CHASSIS_MODE_RELAX;
|
||
switch (rc->sw_r) {
|
||
/* 右拨杆相应行为选择和解析*/
|
||
case CMD_SW_UP:
|
||
cmd->chassis.action = CHASSIS_ACTION_NONE;
|
||
break;
|
||
case CMD_SW_MID:
|
||
cmd->chassis.action = CHASSIS_ACTION_NONE;
|
||
break;
|
||
case CMD_SW_DOWN:
|
||
cmd->chassis.action = CHASSIS_ACTION_NONE;
|
||
break;
|
||
case CMD_SW_ERR:
|
||
cmd->chassis.action = CHASSIS_ACTION_NONE;
|
||
break;
|
||
}
|
||
break;
|
||
}
|
||
/* 将操纵杆的对应值转换为底盘的控制向量和云台变化的欧拉角 */
|
||
cmd->chassis.ctrl_vec.vx = rc->ch_l_y;
|
||
cmd->chassis.ctrl_vec.vy = rc->ch_l_x;
|
||
cmd->chassis.ctrl_vec.wz = rc->ch_r_x;
|
||
cmd->chassis.delta_hight = rc->ch_res;
|
||
|
||
}
|
||
|
||
// /**
|
||
// * @brief rc失控时机器人恢复放松模式
|
||
// *
|
||
// * @param cmd 主结构体
|
||
// */
|
||
static void CMD_RcLostLogic(CMD_t *cmd) {
|
||
/* 机器人底盘、云台、射击运行模式恢复至放松模式 */
|
||
cmd->chassis.mode = CHASSIS_MODE_RELAX;
|
||
|
||
}
|
||
|
||
// /**
|
||
// * @brief 初始化命令解析
|
||
// *
|
||
// * @param cmd 主结构体
|
||
// * @param param 参数
|
||
// * @return int8_t 0对应没有错误
|
||
// */
|
||
int8_t CMD_Init(CMD_t *cmd) {
|
||
/* 指针检测 */
|
||
if (cmd == NULL) return -1;
|
||
|
||
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;
|
||
|
||
/*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);
|
||
// }
|
||
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;
|
||
|
||
|
||
// return 0;
|
||
// }
|