249 lines
6.4 KiB
C
249 lines
6.4 KiB
C
/*
|
||
该任务用来处理来自不同控制方式下传回的期望的控制指令在此处统一为通用格式的控制指令发送给其他任务
|
||
|
||
*/
|
||
|
||
|
||
|
||
/* Includes ----------------------------------------------------------------- */
|
||
#include "cmd.h"
|
||
#include "gpio.h"
|
||
#include "user_math.h"
|
||
#include <string.h>
|
||
|
||
/* Private function -------------------------------------------------------- */
|
||
/*Export function --------------------------------------------------------------*/
|
||
|
||
int8_t CMD_Init(CMD_t *cmd){
|
||
/*若主结构体为空 自动返回错误 */
|
||
if(cmd == NULL) return-1;
|
||
/**/
|
||
cmd->CMD_CtrlType =RCcontrol;
|
||
cmd->CMD_mode =Normal;
|
||
|
||
return 0;
|
||
}
|
||
|
||
|
||
static void CMD_RcLogic(const CMD_RC_t *rc, CMD_t *cmd) {
|
||
|
||
|
||
if(rc->rc_type==RC_DR16){
|
||
cmd->Vx = rc->dr16.ch_r_x;
|
||
cmd->Vy = rc->dr16.ch_r_y;
|
||
cmd->Vw = rc->dr16.ch_l_x;
|
||
|
||
cmd->poscamear = rc->dr16.ch_l_y;
|
||
|
||
cmd->key_ctrl_l = rc->dr16.sw_l;
|
||
cmd->key_ctrl_r = rc->dr16.sw_r ;
|
||
}
|
||
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_x ;
|
||
|
||
cmd->poscamear = rc->LD.ch_l_y;
|
||
|
||
}
|
||
|
||
}
|
||
|
||
|
||
|
||
/**
|
||
* @brief rc失控时机器人恢复放松模式
|
||
*
|
||
* @param cmd 主结构体
|
||
*/
|
||
static void CMD_RcLostLogic(CMD_t *cmd){
|
||
/* 机器人底盘运行模式恢复至放松模式 */
|
||
cmd->CMD_CtrlType = RELAXED;
|
||
}
|
||
int8_t CMD_ParseRc(CMD_t *cmd,const CMD_RC_t *rc){
|
||
if (cmd == NULL) return -1;
|
||
if (rc == NULL) return -1;
|
||
|
||
switch (rc->rc_type){
|
||
|
||
case RC_DR16:
|
||
CMD_DR16(cmd,rc);
|
||
break ;
|
||
|
||
case RC_LD:
|
||
CMD_LD(cmd,rc);
|
||
break ;
|
||
|
||
case Control_loss:
|
||
CMD_RcLostLogic(cmd);
|
||
break ;
|
||
}
|
||
|
||
return 0;
|
||
}
|
||
|
||
|
||
|
||
int8_t CMD_ParseNuc(CMD_t *cmd,CMD_NUC_t *n){
|
||
if(cmd == NULL) return -1;
|
||
if(n == NULL) return -1;
|
||
|
||
cmd->cmd_status = n->status_fromnuc;
|
||
cmd->raw_status = n->ctrl_status;
|
||
|
||
for (int i = 0; i < 7; ++i)
|
||
{ // 从最低位到最高位遍历
|
||
cmd->status[i] = ((cmd->raw_status) & (1 << i)) ? 1 : 0;
|
||
}
|
||
switch(cmd->cmd_status){
|
||
|
||
case MID:
|
||
cmd->cmd_MID360.posx = n->MID360.vx;
|
||
cmd->cmd_MID360.posy = n->MID360.vy;
|
||
cmd->cmd_MID360.posw = n->MID360.wz;
|
||
|
||
cmd->pos =n->MID360 .pos ;
|
||
break;
|
||
|
||
case VISION :
|
||
cmd ->cmd_pick .posx =n->camera.data1 ;
|
||
cmd ->cmd_pick .posy =n->camera.data2 ;
|
||
cmd ->cmd_pick .posw =n->camera.data3 ;
|
||
|
||
break;
|
||
|
||
|
||
}
|
||
return 0;
|
||
}
|
||
|
||
|
||
|
||
|
||
/*DR16遥控器数据分配*/
|
||
int8_t CMD_DR16(CMD_t *cmd,const CMD_RC_t *rc) {
|
||
|
||
|
||
if(cmd == NULL) return -1;
|
||
|
||
cmd->Vx = rc->dr16.ch_r_x;
|
||
cmd->Vy = rc->dr16.ch_r_y;
|
||
cmd->Vw = rc->dr16.ch_l_x;
|
||
|
||
cmd->poscamear = rc->dr16.ch_l_y;
|
||
|
||
cmd->key_ctrl_l = rc->dr16.sw_l;
|
||
cmd->key_ctrl_r = rc->dr16.sw_r ;
|
||
|
||
// if ((rc->dr16.sw_l == CMD_SW_ERR) || (rc->dr16.sw_r == CMD_SW_ERR)) {
|
||
// cmd->CMD_CtrlType =RELAXED;
|
||
// }
|
||
// else if(rc->dr16.sw_l==CMD_SW_UP)
|
||
// {
|
||
// cmd ->CMD_CtrlType =ShootGame_Mode;
|
||
// if(rc->dr16.sw_r ==CMD_SW_UP) cmd ->CMD_mode =Shooting; //左上，右上，无模式
|
||
//
|
||
// if(rc->dr16.sw_r ==CMD_SW_MID) cmd ->CMD_mode =Normal; //左上，右中，无模式
|
||
//
|
||
// if(rc->dr16.sw_r ==CMD_SW_DOWN) cmd ->CMD_mode =ShootRst; //左上，右上，无模式
|
||
//
|
||
//// if(rc->dr16.res > 3000) cmd ->CMD_mode =ShootRst;
|
||
//// if(rc->dr16.res == 660) cmd ->CMD_mode =Shooting;
|
||
// }
|
||
//
|
||
// else if(rc->dr16.sw_l==CMD_SW_MID)
|
||
// {
|
||
//
|
||
// cmd ->CMD_CtrlType =RCcontrol;
|
||
// if(rc->dr16.sw_r ==CMD_SW_UP) cmd ->CMD_mode =Normal; //左中，右中，雷达
|
||
//
|
||
// if(rc->dr16.sw_r ==CMD_SW_MID) cmd ->CMD_mode =Normal; //左中，右中，无模式
|
||
//
|
||
// if(rc->dr16.sw_r ==CMD_SW_DOWN) cmd ->CMD_mode =Normal; //左中，右下，视觉
|
||
//
|
||
// }
|
||
// else if(rc->dr16.sw_l==CMD_SW_DOWN)
|
||
// {
|
||
// cmd ->CMD_CtrlType =RCcontrol;
|
||
// if(rc->dr16.sw_r ==CMD_SW_UP) cmd ->CMD_mode =Normal; //左下，右上，无模式
|
||
// if(rc->dr16.sw_r ==CMD_SW_MID) cmd ->CMD_mode = Normal; //左下，右中，无模式
|
||
// if(rc->dr16.sw_r ==CMD_SW_DOWN) cmd ->CMD_mode =Normal; //左下，右上，无模式
|
||
// }
|
||
|
||
if ((rc->dr16.sw_r == CMD_SW_ERR) || (rc->dr16.sw_l == CMD_SW_ERR)) {
|
||
cmd->CMD_CtrlType = RELAXED;
|
||
}
|
||
// 右拨杆控制 CtrlType，左拨杆控制 Mode
|
||
else if (rc->dr16.sw_r == CMD_SW_UP)
|
||
{
|
||
cmd->CMD_CtrlType = ShootGame_Mode;
|
||
|
||
if (rc->dr16.sw_l == CMD_SW_UP) cmd->CMD_mode = Normal; // 右上，左上，
|
||
if (rc->dr16.sw_l == CMD_SW_MID) cmd->CMD_mode = ShootRst; // 右上，左中，
|
||
if (rc->dr16.sw_l == CMD_SW_DOWN) cmd->CMD_mode = Normal; // 右上，左下，无模式
|
||
|
||
// 可选保留遥控旋钮控制
|
||
// if (rc->dr16.res > 3000) cmd->CMD_mode = ShootRst;
|
||
// if (rc->dr16.res == 660) cmd->CMD_mode = Shooting;
|
||
}
|
||
else if (rc->dr16.sw_r == CMD_SW_MID)
|
||
{
|
||
cmd->CMD_CtrlType = REPOSITION;
|
||
|
||
if (rc->dr16.sw_l == CMD_SW_UP) cmd->CMD_mode = Normal; // 右中，左上，
|
||
if (rc->dr16.sw_l == CMD_SW_MID) cmd->CMD_mode = Normal; // 右中，左中，无模式
|
||
if (rc->dr16.sw_l == CMD_SW_DOWN) cmd->CMD_mode = Normal; // 右中，左下，
|
||
}
|
||
else if (rc->dr16.sw_r == CMD_SW_DOWN)
|
||
{
|
||
cmd->CMD_CtrlType = RCcontrol;
|
||
|
||
if (rc->dr16.sw_l == CMD_SW_UP) cmd->CMD_mode = Normal; // 右下，左上，无模式
|
||
if (rc->dr16.sw_l == CMD_SW_MID) cmd->CMD_mode = Normal; // 右下，左中，无模式
|
||
if (rc->dr16.sw_l == CMD_SW_DOWN) cmd->CMD_mode = Normal; // 右下，左下，无模式
|
||
}
|
||
|
||
|
||
return 0;
|
||
}
|
||
|
||
int8_t CMD_LD(CMD_t *cmd,const CMD_RC_t *rc){
|
||
|
||
if(cmd == NULL) return -1;
|
||
|
||
|
||
cmd->Vx = rc->LD.ch_r_x;
|
||
cmd->Vy = rc->LD.ch_r_y;
|
||
cmd->Vw = rc->LD.ch_l_x;
|
||
cmd->poscamear = rc->LD.ch_l_y;
|
||
|
||
|
||
/*自动手动切换*/
|
||
if(rc->LD.key_A ==CMD_SW_DOWN ) cmd ->CMD_CtrlType =RCcontrol;
|
||
else if(rc->LD.key_A ==CMD_SW_UP) cmd ->CMD_CtrlType =AUTO;
|
||
|
||
|
||
if(cmd ->CMD_CtrlType ==AUTO){
|
||
|
||
/*自动下的*/
|
||
if (rc->LD.key_C ==CMD_SW_UP) cmd ->CMD_mode =AUTO_MID360_Adjust;
|
||
else {
|
||
|
||
if(rc->LD.key_E ==CMD_SW_DOWN ) cmd ->CMD_mode =Normal;
|
||
else if(rc->LD.key_E ==CMD_SW_UP) cmd ->CMD_mode =AUTO_MID360_Pitch;
|
||
else if(rc->LD .key_E ==CMD_SW_MID ) cmd ->CMD_mode =AUTO_MID360;
|
||
else cmd ->CMD_mode =Normal ;
|
||
}
|
||
}
|
||
else if(cmd ->CMD_CtrlType ==RCcontrol){
|
||
/*手动下的*/
|
||
if(rc->LD.key_G ==CMD_SW_DOWN ) cmd ->CMD_mode=Normal ;
|
||
else if(rc->LD.key_G ==CMD_SW_UP) cmd ->CMD_mode =Pitch;
|
||
else if(rc->LD .key_H==CMD_SW_UP) cmd ->CMD_mode =UP_Adjust;
|
||
else if(rc->LD.key_C == CMD_SW_UP) cmd->CMD_mode =Chassis_Adjust_Sick;
|
||
else cmd ->CMD_mode =Normal;
|
||
}
|
||
}
|
||
|