📄 cmdanalyse.c
字号:
/*
*********************************************************************************************************
* uC/OS-II
* The Real-Time Kernel
*
* (c) Copyright 1998-2005, Micrium, Weston, FL
* All Rights Reserved
*
*
* Microchip Application Code
*
* File : CmdAnalyse.C
* By : tbh080604
*********************************************************************************************************
*/
#include <includes.h>
/*
*********************************************************************************************************
* CONSTANTS
*********************************************************************************************************
*/
/*
*********************************************************************************************************
* VARIABLES
*********************************************************************************************************
*/
INT8U *RxMsg;//=RecvBuf;
INT8U Err;
INT8U RecErr=0x00;
INT8U CmdCode=0x00;
MASS_TECH_STRUCT MassTech;
MASS_STAT_STRUCT MassStat={0,0,0,0,0,0,0,0,0,0,000,0,0000};
/*
*********************************************************************************************************
* FUNCTION PROTOTYPES
*********************************************************************************************************
*/
static INT8U JudgeRxLrc(INT8U *data);
static INT8U GetLrc(INT8U *asc_data,INT8U asc_length);
INT32U AsciiToINT32U(INT8U *asc_data,INT8U asc_length);
void IntToAscii(INT32U data,INT8U *asc_data,INT8U asc_length);
void SendAscii(void *param_value,INT8U param_asc_length);
#ifdef USER_DEBUG
#define GET_CUR(an,anval,ancur) \
(ancur)=(INT32S)((anval)*3300UL/0x1000-2500)*CNT_CURRENT_LIMIT/625;\
PrintHex((ancur))
#endif
#define GET_CMD_PARAM_DATA(addr,length) \
INT8S j;\
INT8U TMP_ARRAY[((length+1)>>1)]; \
j=((length&0x01)!=0x00)?-1:0; \
for(i=0;i<(length>>1);i++) \
TMP_ARRAY[((length+1)>>1)-1-i]=AsciiToINT32U(addr+j+(i<<1),2); \
if((length&0x01)!=0x00) \
TMP_ARRAY[0]=AsciiToINT32U(addr+j+(i<<1),2);
OS_STK MassTechTaskStk[MASS_TECH_TASK_STK_SIZE];
OS_STK YZTaskStk[YZ_TASK_STK_SIZE];//升降出退任务堆栈
OS_STK MomiTaskStk[MOMI_TASK_STK_SIZE];
OS_STK TatakiTaskStk[TATAKI_TASK_STK_SIZE];
#define SETUP_TASK(name,NAME)\
OSTaskDel(NAME##_TASK_PRIO);\
OSTaskCreateExt(name##Task,\
(void *)0,\
(OS_STK *)&name##TaskStk[0],\
NAME##_TASK_PRIO,\
NAME##_TASK_PRIO,\
(OS_STK *)&name##TaskStk[NAME##_TASK_STK_SIZE-1],\
NAME##_TASK_STK_SIZE,\
(void *)0,\
OS_TASK_OPT_STK_CHK | OS_TASK_OPT_STK_CLR);\
OSTaskNameSet(NAME##_TASK_PRIO, #name"Task", &Err);
/*
*********************************************************************************************************
* COMMAND EXECUTE
*
*********************************************************************************************************
*/
#define CHK_PARAM_VOID(param,lmt,outv) (((param)>(lmt))&&((param)!=(outv)))
//手技数据检查
INT8U CheckTechData(void)
{
if CHK_PARAM_VOID(MassTech.MassMode,0x0C,0x0F)
return 0;
if CHK_PARAM_VOID(MassTech.FollowMode,0x02,0x00)
return 0;
if CHK_PARAM_VOID(MassTech.RepeatMode,0x02,0x00)
return 0;
if CHK_PARAM_VOID(MassTech.YBegin,CNT_UD_LIMIT,0x00)
return 0;
if CHK_PARAM_VOID(MassTech.YEnd,CNT_UD_LIMIT,0x00)
return 0;
if CHK_PARAM_VOID(MassTech.ActWidth,0x03,0x00)
return 0;
if CHK_PARAM_VOID(MassTech.MomiSpeed,0x0A,0x0F)
return 0;
if CHK_PARAM_VOID(MassTech.TatakiMode,0x03,0x0F)
return 0;
if CHK_PARAM_VOID(MassTech.TatakiHSpeed,0x0A,0x0F)
return 0;
if CHK_PARAM_VOID(MassTech.TatakiLSpeed,0x0A,0x0F)
return 0;
if CHK_PARAM_VOID(MassTech.UpDownSpeed,0x0A,0x0F)
return 0;
if CHK_PARAM_VOID(MassTech.FrontBackSpeed,0x0A,0x0F)
return 0;
if CHK_PARAM_VOID(MassTech.KeepTime,0x0A,0x0F)
return 0;
if CHK_PARAM_VOID(MassTech.ActStrength,0x0D,0x00)
return 0;
if CHK_PARAM_VOID(MassTech.AreaDivNum,CNT_PART_DIVIS_NUM,0x00)
return 0;
if CHK_PARAM_VOID(MassTech.RepeatNum,0x0F,0x00)
return 0;
if (
(MassTech.FollowMode!=0)&&
(
(MassTech.YBegin<CNT_UD_NECK)||(MassTech.YEnd<CNT_UD_NECK)||
(MassTech.AreaDivNum>CNT_PART_DIVIS_NUM)||
(MassTech.RepeatNum==TECH_VOID)
)
)
return 0;
return 1;
}
//更新运行部位反馈
void GetMassPart(void)
{
INT8U i;
//部位反馈仅在初始化後为0，当到达限位后设为非0，以后实时检测运行部位状况
if(MassStat.MassPart!=0)
{
for(i=CNT_UD_BACE;(i<=CNT_UD_LIMIT)&&(BodyPointHeight[i-1]<UDNowPst);i++);
MassStat.MassPart=i;//更新运行部位状态
}
}
//更新马达运行状态反馈
void GetMotRun(void)
{
if((MomiCwPwmSet==0x0000)&&(MomiCcwPwmSet==0x0000))
MassStat.MomiRun=0;
else
MassStat.MomiRun=1;
if(TatakiPwmSet==0x0000)
MassStat.TatakiRun=0;
else
MassStat.TatakiRun=1;
if((FrontPwmSet==0x0000)&&(BackPwmSet==0x0000))
MassStat.ZRun=0;
else
MassStat.ZRun=1;
if((UpPwmSet==0x0000)&&(DownPwmSet==0x0000))
MassStat.YRun=0;
else
MassStat.YRun=1;
}
void CmdAnalyse (void )
{
INT8U i,j;
INT8U cmd_num=0x00;
while (TRUE)
{
RxMsg=OSMboxPend(MboxRx_Finish,CNT_RX_ERR_JUDGE_TIME,&Err);
if(RxMsg==(void*)2) //接收字符错误
{
CmdCode=0xFF;
RecErr=0x00;
SendAscii(&RecErr,1);
}
else if(RxMsg==(void*)0) //接收超时
{
CmdCode=0xFF;
RecErr=0x01;
SendAscii(&RecErr,1);
}
#ifndef USER_DEBUG
else if(!JudgeRxLrc(RxMsg))
{
CmdCode=0xFF;
RecErr=0x02;
SendAscii(&RecErr,1);
}
#endif
else
{
CmdCode=AsciiToINT32U(RxMsg+1,2);
cmd_num=AsciiToINT32U(RxMsg+3,1);
if((cmd_num!=0x00)&&(MassStat.MassPart!=0))//只有在初始化完成后才能建立按摩过程
{
//复位各马达控制任务
SETUP_TASK(MassTech,MASS_TECH);
SETUP_TASK(YZ,YZ);
SETUP_TASK(Momi,MOMI);
SETUP_TASK(Tataki,TATAKI);
MassStat.CmdNum=cmd_num;//更新指令序号
MassStat.MassRun=1;//设置指令运行状态为正在运行
StopMass();//停止各马达运行
SetupMbox();//清空各马达控制任务所用邮箱
}
switch(CmdCode)
{
case CMD_GET_STATUS:
GetMotRun();
GetMassPart();
SendAscii(&MassStat,CNT_STAT_PARAM_LENGTH);
break;
case CMD_SET_MASS_TECH:
{
GET_CMD_PARAM_DATA(RxMsg+4,CNT_TECH_PARAM_LENGTH);
MassTech=*(MASS_TECH_STRUCT*)(TMP_ARRAY);
}
if(CheckTechData())
{
SendAscii((void*)0,0);
OSMboxPost(MboxCmd_TechStart,MBOX_VALID);//通知手技处理任务开始执行手技
}
else
{
CmdCode=0xFF;
RecErr=0x03;
SendAscii(&RecErr,1);
}
break;
case CMD_SET_SPEED:
{
MASS_SPED_STRUCT mass_speed; //在内部定义可以节省堆栈空间，因为只有执行到这里时才为该变量分配堆栈
GET_CMD_PARAM_DATA(RxMsg+4,CNT_SPEED_PARAM_LENGTH);
mass_speed=*(MASS_SPED_STRUCT*)(TMP_ARRAY);
COPY_SPEED(MassTech,mass_speed);
}
SendAscii((void*)0,0);
break;
case CMD_SET_MASS_STOP:
{
//删除各按摩任务
OSTaskDel(MASS_TECH_TASK_PRIO);
OSTaskDel(YZ_TASK_PRIO);
OSTaskDel(MOMI_TASK_PRIO);
OSTaskDel(TATAKI_TASK_PRIO);
MassStat.MassRun=0;//设置指令运行状态为完成
StopMass();//停止各马达运行
SetupMbox();//清空各马达控制任务所用邮箱
}
SendAscii((void*)0,0);
break;
#ifdef USER_DEBUG
case 'a':
GET_CUR(0x00,An0Val,An0Cur);
break;
case 'b':
GET_CUR(0x01,An1Val,An1Cur);
break;
case 'c':
GET_CUR(0x02,An2Val,An2Cur);
break;
case 'd':
GET_CUR(0x03,An3Val,An3Cur);
break;
#endif
}
//MASS_TXSTART(MsgB);
}
//OSTimeDlyHMSM(0, 0, 0, 50); /* Delay 25ms */
}
}
INT8U GetLrc(INT8U *asc_data,INT8U asc_length)
{
INT8U i,lrc=0x00;
//计算从第2字符起到倒数第3字符止(将结束符和LRC字符除外)的LRC数值
for(i=0;i<asc_length;i++)
{
lrc+=*(asc_data+i);
}
return lrc;
}
INT8U JudgeRxLrc(INT8U *data)
{
INT8U i,lrc=0x00;
//计算从第2字符起到倒数第3字符止(将结束符和LRC字符除外)的LRC数值
for(i=1;(i<CNT_RECV_MAX_NUM)&&(*(RxMsg+i+2)!=CNT_RECV_END_BYTE);i++)
{
lrc+=*(RxMsg+i);
}
if((i<CNT_RECV_MAX_NUM)&&(lrc==AsciiToINT32U(RxMsg+i,2)))
return 1;
else
return 0;
}
INT32U AsciiToINT32U(INT8U *asc_data,INT8U asc_length)
{
INT8U i,tmp;
INT32U hex_data=0x0000;
for(i=0;i<asc_length;i++)
{
tmp=*(asc_data+i);
if((tmp>='0')&&(tmp<='9'))
tmp=tmp-'0';
else if((tmp>='a')&&(tmp<='f'))
tmp=tmp-'a'+0x0A;
else if((tmp>='A')&&(tmp<='F'))
tmp=tmp-'A'+0x0A;
hex_data=(hex_data<<4)+tmp;
}
return hex_data;
}
void IntToAscii(INT32U data,INT8U *asc_data,INT8U asc_length)
{
INT8U i,tmp;
for(i=0;i<asc_length;i++)
{
tmp=(data&0x000F);
if(tmp<=0x9)
tmp=tmp+'0';
else if((tmp>=0xA)&&(tmp<=0xF))
tmp=tmp-0x0A+'A';
*(asc_data+asc_length-1-i)=tmp;
data=(data>>4);
}
}
void SendAscii(void *param_value,INT8U param_asc_length)
{
INT8U send_asc[CNT_SEND_MAX_NUM],lrc;
send_asc[0]=CNT_SEND_DATA_START_BYTE;
IntToAscii(CmdCode,send_asc+1,2);
IntToAscii(*(INT32U*)param_value,send_asc+3,param_asc_length);
lrc=GetLrc(send_asc+1,param_asc_length+2);
IntToAscii(lrc,send_asc+param_asc_length+3,2);
send_asc[param_asc_length+5]='\r';
send_asc[param_asc_length+6]=CNT_SEND_END_BYTE;
MASS_TXSTART(send_asc);
}
void StopMass(void)
{
UDAimPst=UDNowPst;
FBAimPst=FBNowPst;
TatakiPwmSet = 0x0000;
TestPwmSet = 0x0000;
MomiCwPwmSet = 0x0000;
MomiCcwPwmSet = 0x0000;
}
//复位邮箱
void SetupMbox(void)
{
OSMboxAccept(MboxCN_Width );
OSMboxAccept(MboxCmd_TechStart );
OSMboxAccept(MboxTech_Finish );
OSMboxAccept(MboxTech_SurMove );
OSMboxAccept(MboxTech_Momi );
OSMboxAccept(MboxTech_Tataki );
OSMboxAccept(MboxSurMove_Nose );
OSMboxAccept(MboxMomi_Finish );
OSMboxAccept(MboxTataki_Finish );
OSMboxAccept(MboxUD_Sur );
OSMboxAccept(MboxFB_Sur );
}
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -