25_R1_chassis/User/device/cmd.c
2025-07-11 21:39:07 +08:00

249 lines
6.0 KiB
C
Raw Blame History

This file contains ambiguous Unicode characters

This file contains Unicode characters that might be confused with other characters. If you think that this is intentional, you can safely ignore this warning. Use the Escape button to reveal them.

/*
该任务用来处理来自不同控制方式下传回的期望的控制指令在此处统一为通用格式的控制指令发送给其他任务
*/
/* Includes ----------------------------------------------------------------- */
#include "cmd.h"
#include "gpio.h"
#include <string.h>
#include "define.h"
/* Private function -------------------------------------------------------- */
/*Export function --------------------------------------------------------------*/
void CMD_Init(CMD_t *cmd){
cmd->C_cmd.mode = STOP;
cmd->cmd_power_on_safe = 0; // 初始为不安全状态
// cmd->C_cmd.pos = POS_1;
}
static void CMD_remote(const CMD_RC_t *rc, CMD_t *cmd) {
if(rc->rc_type==RC_DR16){
cmd->Vx = rc->DJ.ch_r_x;
cmd->Vy = rc->DJ.ch_r_y;
cmd->Vw = rc->DJ.ch_l_y;
cmd->throttle = rc->DJ.ch_l_x;
}
if(rc->rc_type==RC_LD){
cmd->Vx = rc->LD.ch_r_x;
cmd->Vy = rc->LD.ch_r_y;
cmd->Vw = rc->LD .ch_l_y;
cmd->throttle = rc->LD.ch_l_x;
}
}
/**
* @brief rc失控时机器人恢复放松模式
*
* @param cmd 主结构体
*/
static void CMD_RcLostLogic(CMD_t *cmd){
/* 机器人底盘运行模式恢复至放松模式 */
cmd->C_cmd.mode = STOP;
}
void CMD_ParseRc(CMD_t *cmd,CMD_RC_t *rc){
switch (rc->rc_type){
case RC_DR16:
/*c当rc丢控时恢复机器人至默认状态 */
if ((rc->DJ.sw_l == CMD_SW_ERR) || (rc->DJ.sw_r == CMD_SW_ERR)) {
CMD_RcLostLogic(cmd);
cmd->cmd_power_on_safe = 0;
}
else{
/* 上电保护检查 */
// if(!cmd->cmd_power_on_safe && !pos.Action_ready){
// if (rc->sw_l == CMD_SW_MID && pos.Action_ready){
// cmd->cmd_power_on_safe = 1;
// }
// else{
// // 否则保持停止状态
// cmd->C_cmd.mode = STOP;
// }
// }
// else{
CMD_remote(rc, cmd);
// }
}
break;
case RC_LD:
if (rc->LD.key_D == 0) {
CMD_RcLostLogic(cmd);
}
else{
/* 上电保护检查 */
// if(!cmd->cmd_power_on_safe){
// if (rc->LD.key_A == CMD_SW_DOWN && rc->LD.key_D == CMD_SW_MID && rc->LD.key_E == CMD_SW_MID){
// cmd->cmd_power_on_safe = 1;
// }
// else{
// // 否则保持停止状态
// cmd->C_cmd.mode = STOP;
// }
// }
// else{
CMD_remote(rc, cmd);
// }
}
break;
case Control_loss:
CMD_RcLostLogic(cmd);
break;
}
}
int8_t CMD_CtrlSet(CMD_t *cmd,const CMD_RC_t *rc){
//大疆
if(rc->rc_type==RC_DR16){
switch(rc->DJ.sw_l){
case CMD_SW_UP:
cmd->C_cmd.mode = RC;
break;
case CMD_SW_DOWN:
cmd->C_cmd.mode = NAVI;
break;
case CMD_SW_MID:
cmd->C_cmd.mode = STOP;
}
}
//乐迪
else if(rc->rc_type==RC_LD){
//模式切换
switch(rc->LD.key_D){
case CMD_SW_UP:
cmd->C_cmd.mode = NAVI;
break;
case CMD_SW_DOWN:
cmd->C_cmd.mode = RC;
break;
case CMD_SW_MID:
cmd->C_cmd.mode = STOP;
}
//导航切换
switch(rc->LD.key_E){
case CMD_SW_UP:
cmd->C_cmd.nuc_radar = ANGLE;
break;
case CMD_SW_MID:
cmd->C_cmd.nuc_radar = WORLD;
break;
case CMD_SW_DOWN:
cmd->C_cmd.nuc_radar = RADAR_RESET;
}
//锁定切换
switch(rc->LD.key_A){
case CMD_SW_UP:
cmd->C_cmd.communicate = YES;
break;
case CMD_SW_DOWN:
cmd->C_cmd.communicate = NO;
}
//点位切换
static int previous_pos = -1;
if(rc->LD.knob_left > 199 && rc->LD.knob_left < 265){ //200
if(cmd->C_cmd.pos != POS_1) {
if(previous_pos != POS_1) cmd->C_cmd.dribble = RUNNING;
cmd->C_cmd.pos = POS_1;
previous_pos = POS_1;
}
}
else if(rc->LD.knob_left > 265 && rc->LD.knob_left < 405){//330
if(cmd->C_cmd.pos != POS_2) {
if(previous_pos != POS_2) cmd->C_cmd.dribble = RUNNING;
cmd->C_cmd.pos = POS_2;
previous_pos = POS_2;
}
}
else if(rc->LD.knob_left > 405 && rc->LD.knob_left < 554){ //479
if(cmd->C_cmd.pos != POS_3) {
if(previous_pos != POS_3) cmd->C_cmd.dribble = RUNNING;
cmd->C_cmd.pos = POS_3;
previous_pos = POS_3;
}
}
else if(rc->LD.knob_left > 554 && rc->LD.knob_left < 704){ //629
if(cmd->C_cmd.pos != POS_4) {
if(previous_pos != POS_4) cmd->C_cmd.dribble = RUNNING;
cmd->C_cmd.pos = POS_4;
previous_pos = POS_4;
}
}
else if(rc->LD.knob_left > 704 && rc->LD.knob_left < 853){//778
if(cmd->C_cmd.pos != POS_5) {
if(previous_pos != POS_5) cmd->C_cmd.dribble = RUNNING;
cmd->C_cmd.pos = POS_5;
previous_pos = POS_5;
}
}
else if(rc->LD.knob_left > 853 && rc->LD.knob_left < 1002){//928
if(cmd->C_cmd.pos != POS_6) {
if(previous_pos != POS_6) cmd->C_cmd.dribble = RUNNING;
cmd->C_cmd.pos = POS_6;
previous_pos = POS_6;
}
}
else if(rc->LD.knob_left > 1002 && rc->LD.knob_left < 1150){ //1075
if(cmd->C_cmd.pos != POS_7) {
if(previous_pos != POS_7) cmd->C_cmd.dribble = RUNNING;
cmd->C_cmd.pos = POS_7;
previous_pos = POS_7;
}
}
else if(rc->LD.knob_left > 1150 && rc->LD.knob_left < 1300){ //1224
if(cmd->C_cmd.pos != POS_8) {
if(previous_pos != POS_8) cmd->C_cmd.dribble = RUNNING;
cmd->C_cmd.pos = POS_8;
previous_pos = POS_8;
}
}
else if(rc->LD.knob_left > 1300 && rc->LD.knob_left < 1448){ //1373
if(cmd->C_cmd.pos != POS_9) {
if(previous_pos != POS_9) cmd->C_cmd.dribble = RUNNING;
cmd->C_cmd.pos = POS_9;
previous_pos = POS_9;
}
}
//运球时锁死底盘
// if(rc->LD.key_G == CMD_SW_DOWN&&cmd->C_cmd.pos != POS_1){
// cmd->C_cmd.dribble = Pause;
// }
}
return 0;
}
//接收码盘导航的返回数据传入cmd_t结构体
int8_t CMD_ParseAction(CMD_t *cmd,CMD_ACTION_t *act)
{
cmd ->C_navi .vx =act ->out .Vx ;
cmd ->C_navi .vy =act ->out .Vy ;
cmd ->C_navi .wz =act ->out .Vw ;
return 0;
}
int8_t CMD_ParseNuc(CMD_t *cmd,CMD_NUC_t *n){
#ifdef nuc_1
cmd->nuc.flag = n->nuc.flag;
cmd->nuc.angle = n->nuc.angle;
cmd->nuc.yaw = n->nuc.yaw;
#elif defined(nuc_2)
cmd->nuc.angle = n->nuc.angle;
cmd->nuc.yaw = n->nuc.yaw;
#endif
return 0;
}