📄 yztask0.c

📁 uCOS在PIC芯片中的移植：以PIC24HJ64GP210为例进行uCOS在Microchip芯片中的移植
💻 C
字号:
#define YZ_TASK
#include <includes.h>

INT8U UDNowPst,UDAimPst,UDAimSpd;
INT8U FBNowPst,FBAimPst,FBAimSpd;
static INT8U UDDivPtr,UDDivEndPtr=0,UDRpdNum;
void YZMove(void *hook());
void UDUnitHook(INT8U ud_en);
void PrsUnitHook(INT8U ud_en);

#define UD_FB(ud_pst,fb_pst) \
	PositionMove(ud_pst,fb_pst,PARAM_VOID,PARAM_VOID,PARAM_VOID,PARAM_VOID,PARAM_VOID)

void  YZTask(void *p_arg)
{
    void *msg;
    (void)p_arg;
    
    while (TRUE) 
    {  
        //等待移动开始信息
        msg=OSMboxPend(MboxTech_SurMove,0,&Err);  
        switch(((MSG_TECH_SUR_STRUCT*)msg)->MsgSurTech)
        {
        	case TECH_SUR_MASSMODE_PRS:
        	case TECH_SUR_MASSMODE_MOMI_PRS:
        	case TECH_SUR_MASSMODE_FB_PRS:
        	case TECH_SUR_MASSMODE_BODY_DTCT:
				UD_FB(PARAM_VOID,0x00);        
				UD_FB(BodyPointHeight[MassTech.YBegin-1],PARAM_VOID); 
				if(MassTech.FollowMode==TECH_VOID) 
				{
					//发布初始位置到信息
    				MsgSurMove_Nose.MsgClass=SUR_TECH_POSIT;
    				MsgSurMove_Nose.MsgParam=SUR_TECH_POSIT_INIT;
    				OSMboxPost(MboxSurMove_Nose,&MsgSurMove_Nose);
					
					//等待手技处理任务信息
    				OSMboxPend(MboxTech_SurMove,0,&Err);
    				
    				OneFBAct();
    				
					if(MassTech.RepeatNum==TECH_RPT_FOREVER) 
					{
					}
					else
					{
					}
				}
				else
				{ 
					YZMove(PrsUnitHook); 
					 
				}     
        		break;
        	case TECH_SUR_MASSMODE_SHDR_PRS:
        		break;
        	case TECH_SUR_MASSMODE_SHDR_DTCT:
        		break;
        	default:
				UD_FB(PARAM_VOID,0x00);        
				UD_FB(BodyPointHeight[MassTech.YBegin-1],PARAM_VOID);        
				UD_FB(PARAM_VOID,GetStrngDist(MassTech.ActStrength));      
				if(MassTech.FollowMode==TECH_VOID) 
				{
				}
				else
				{ 
					YZMove(UDUnitHook); 
					 
				}     
        		break;
        }
		OSTimeDlyHMSM(0, 0, 0, 50); 
    }
}

void UDUnitHook(INT8U ud_en)
{
	if(ud_en)
		UD_FB(DivisPoint[UDDivPtr],GetStrngDist(MassTech.ActStrength)); 
}

void PrsUnitHook(INT8U ud_en)
{
	OneFBAct();//上一分割点作动
	if(ud_en)
		UD_FB(DivisPoint[UDDivPtr],0x00); //下一分割点移动
}

void YZMove(void *hook())
{
	//初始化分割点
	DivisPointSet();
	UDDivPtr=0x00;
	UDRpdNum=0x00;
	
	//执行无升降的分割点动作
    *hook(0);
    
	//发布初始位置到信息
    MsgSurMove_Nose.MsgClass=SUR_TECH_POSIT;
    MsgSurMove_Nose.MsgParam=SUR_TECH_POSIT_INIT;
    OSMboxPost(MboxSurMove_Nose,&MsgSurMove_Nose);
	
	//等待手技处理任务信息
    OSMboxPend(MboxTech_SurMove,0,&Err);
	
	UDDivPtr++;
	
	//执行分割点动作
    *hook(1);
	
	if(UDDivPtr>=UDDivEndPtr)
	{
    	
		UDRpdNum++;
		
		if((UDRpdNum<MassTech.RepeatNum)||(MassTech.RepeatNum==TECH_RPT_FOREVER)) //反复次数未到或无限反复
		{
    	
			//发布终点位置到信息
    		MsgSurMove_Nose.MsgClass=SUR_TECH_POSIT;
    		MsgSurMove_Nose.MsgParam=SUR_TECH_POSIT_RANGE_END;
    		OSMboxPost(MboxSurMove_Nose,&MsgSurMove_Nose);
			
			//等待手技处理任务信息
    		OSMboxPend(MboxTech_SurMove,0,&Err);
			
			if(MassTech.RepeatMode==TECH_RPT_BIDIRECT) //双向
			{
				UDDivPtr--;
	
				//执行分割点动作
    			*hook(1);
    			
    			
				if(UDDivPtr==0x00)//分割点指针将为起始点
				{
    	
					UDRpdNum++;
					
					if((UDRpdNum<MassTech.RepeatNum)||(MassTech.RepeatNum==TECH_RPT_FOREVER)) //反复次数未到或无限反复
					{
    				
						//发布起点位置到信息
    					MsgSurMove_Nose.MsgClass=SUR_TECH_POSIT;
    					MsgSurMove_Nose.MsgParam=SUR_TECH_POSIT_RANGE_BEGIN;
    					OSMboxPost(MboxSurMove_Nose,&MsgSurMove_Nose);
						
						//等待手技处理任务信息
    					OSMboxPend(MboxTech_SurMove,0,&Err);
    				}
    				else
    				{
						//发布终止点位置到信息
    					MsgSurMove_Nose.MsgClass=SUR_TECH_POSIT;
    					MsgSurMove_Nose.MsgParam=SUR_TECH_POSIT_FINISH;
    					OSMboxPost(MboxSurMove_Nose,&MsgSurMove_Nose);
						
						//等待手技处理任务信息
    					OSMboxPend(MboxTech_SurMove,0,&Err);
    					
						//发布升降结束信息
    					MsgSurMove_Nose.MsgClass=SUR_TECH_UD_FINISH;
    					OSMboxPost(MboxSurMove_Nose,&MsgSurMove_Nose);
    				}
				}
    			
				
			}
			else
			{
			}
			
		}
		else
		{
			//发布终止点位置到信息
    		MsgSurMove_Nose.MsgClass=SUR_TECH_POSIT;
    		MsgSurMove_Nose.MsgParam=SUR_TECH_POSIT_FINISH;
    		OSMboxPost(MboxSurMove_Nose,&MsgSurMove_Nose);
			
			//等待手技处理任务信息
    		OSMboxPend(MboxTech_SurMove,0,&Err);
    		
			//发布升降结束信息
    		MsgSurMove_Nose.MsgClass=SUR_TECH_UD_FINISH;
    		OSMboxPost(MboxSurMove_Nose,&MsgSurMove_Nose);
		}
		
	}
    
}

void DivisPointSet(void)
{
	INT8U i,large,small,tmp_part,div_base;
	if((MassTech.YBegin<CNT_UD_NECK)||(MassTech.YBegin>CNT_UD_LIMIT)||(MassTech.YEnd<CNT_UD_NECK)||(MassTech.YEnd>CNT_UD_LIMIT)||(MassTech.AreaDivNum>CNT_PART_DIVIS_NUM))
		return;
	for(i=0;i<(CNT_PART_POINT_NUM-2)*CNT_PART_DIVIS_NUM+1;i++)
	{
		DivisPoint[i]=0x00;
	}
	if(MassTech.YBegin==MassTech.YEnd)
	{
		DivisPoint[0]=BodyPointHeight[MassTech.YBegin-1];
		UDDivEndPtr=0x00;
		return;
	}
	else if(MassTech.YBegin<MassTech.YEnd)
	{
		large=MassTech.YEnd;
		small=MassTech.YBegin;
	}
	else
	{
		large=MassTech.YBegin;
		small=MassTech.YEnd;
	}
	
	DivisPoint[0]=BodyPointHeight[small-1];
	div_base=0x00;
	
	for(tmp_part=small;tmp_part<large;tmp_part++)
	{
		for(i=1;i<MassTech.AreaDivNum;i++)
		{
			DivisPoint[div_base+i]=DivisPoint[div_base]+(INT16U)i*(BodyPointHeight[tmp_part]-BodyPointHeight[tmp_part-1])/MassTech.AreaDivNum;
		}
		div_base=div_base+i;
		DivisPoint[div_base]=BodyPointHeight[tmp_part];
	}
	UDDivEndPtr=div_base;
	
	if(MassTech.YBegin>MassTech.YEnd)
	{
		INT8U tmp_sml;
		for(i=0;i<((UDDivEndPtr+1)>>1);i++)
		{
			tmp_sml=DivisPoint[i];
			DivisPoint[i]=DivisPoint[UDDivEndPtr-i];
			DivisPoint[UDDivEndPtr-i]=tmp_sml;
		}
	}
	
}

void PositionMove(INT8U ud_pst,INT8U fb_pst,INT8U ud_spd,INT8U fb_spd,INT8U ud_dly,INT8U fb_dly,INT8U bd_chk)
{
	if(ud_pst!=PARAM_VOID)
	{
		if(ud_pst==UDNowPst)
            OSMboxPost(MboxUD_Sur,YZ_MBOX_PST_ARRV);
        else if(ud_spd==PARAM_VOID)
        {
        	UDAimSpd=DEFAULT_SPEED_UD;
        }
        else
        {
        	UDAimSpd=ud_spd;
        }
		UDAimPst=ud_pst;	
	}
	if(fb_pst!=PARAM_VOID)
	{
		if(fb_pst==FBNowPst)
            OSMboxPost(MboxFB_Sur,YZ_MBOX_PST_ARRV);
        else if(fb_spd==PARAM_VOID)
        {
        	FBAimSpd=DEFAULT_SPEED_FB;
        }
        else
        {
        	FBAimSpd=fb_spd;
        }
		FBAimPst=fb_pst;	
	}
	if(bd_chk!=PARAM_VOID)
	{
		if(bd_chk==TECH_SUR_MASSMODE_SHDR_DTCT)
		{
        	OSMboxPend(MboxUD_Sur,0,&Err);
			UDAimPst=UDNowPst;
        	OSMboxPost(MboxUD_Sur,YZ_MBOX_PST_ARRV);
        }
		else if(bd_chk==TECH_SUR_MASSMODE_BODY_DTCT)
		{
        	OSMboxPend(MboxFB_Sur,0,&Err);
			FBAimPst=FBNowPst;
        	OSMboxPost(MboxFB_Sur,YZ_MBOX_PST_ARRV);
        }
	}
	if(ud_dly!=PARAM_VOID)
	{
		OSTimeDlyHMSM(0, 0, 0,ud_dly); 
		UDAimPst=UDNowPst;
        OSMboxPost(MboxUD_Sur,YZ_MBOX_PST_ARRV);
	}
	if(fb_dly!=PARAM_VOID)
	{
		OSTimeDlyHMSM(0, 0, 0,fb_dly); 
		FBAimPst=FBNowPst;
        OSMboxPost(MboxFB_Sur,YZ_MBOX_PST_ARRV);
	}
	if(ud_pst!=PARAM_VOID)
	{
        OSMboxPend(MboxUD_Sur,0,&Err);
	}
	if(fb_pst!=PARAM_VOID)
	{
        OSMboxPend(MboxFB_Sur,0,&Err);
	}
	
}

void OneShdrAct(void)
{
	UD_FB(PB(CNT_SHOULDER_HEIGHT),PARAM_VOID);        
	OSTimeDlyHMSM(0, 0, 0,SHDR_KEP_TIME);
    if (VALID_LEVEL(MassTech.ActStrength) && VALID_LEVEL(MassTech.KeepTime))
    {
		UD_FB(PB(CNT_SHOULDER_HEIGHT)+ShdrPrsStrDistTab[MassTech.ActStrength-1],PARAM_VOID);        
		OSTimeDlyHMSM(0, 0, 0,PrsKeepTimeTab[MassTech.KeepTime-1]);
	}
	UD_FB(PB(CNT_SHOULDER_HEIGHT-CNT_SHDR_SUP),PARAM_VOID);        
}

void OneFBAct(void)
{
	if(MsgTech_SurMove.MsgSurTech==TECH_SUR_MASSMODE_BODY_DTCT)
	{
		UD_FB(PARAM_VOID,PF(CNT_FB_LIMIT_DIST));        
        MsgSurMove_Nose.MsgClass=SUR_TECH_POINT_CHK_FINISH;
        OSMboxPost(MboxSurMove_Nose,&MsgSurMove_Nose);
        
        UDAimPst=FBNowPst;
	}
	else
	{
		UD_FB(PARAM_VOID,GetSurPstDist(UDAimPst));        
        
        MsgSurMove_Nose.MsgClass=SUR_TECH_SURFACE_ARRIVE;
        OSMboxPost(MboxSurMove_Nose,&MsgSurMove_Nose);
        
        OSMboxPend(MboxTech_SurMove,0,&Err);
        
		UD_FB(PARAM_VOID,GetStrngDist(MassTech.ActStrength));        
        
        MsgSurMove_Nose.MsgClass=SUR_TECH_STRENG_ARRIVE;
        OSMboxPost(MboxSurMove_Nose,&MsgSurMove_Nose);
        
        if (VALID_LEVEL(MassTech.KeepTime)&& VALID_LEVEL(MassTech.FrontBackSpeed))
        {
        	if(MsgTech_SurMove.MsgSurTech==TECH_SUR_MASSMODE_PRS)
				OSTimeDlyHMSM(0, 0, 0,PrsKeepTimeTab[MassTech.KeepTime-1]);
			else 
				PositionMove(PARAM_VOID,GetStrngDist(TECH_FB_LIMIT),PARAM_VOID,MassTech.FrontBackSpeed,PARAM_VOID,PrsKeepTimeTab[MassTech.KeepTime-1],PARAM_VOID);
		}
        
        OSMboxPend(MboxTech_SurMove,0,&Err);
		UD_FB(PARAM_VOID,TECH_FB_BASE);        
	}
}

INT8U GetSurPstDist(INT8U ud_pst)
{
	INT8U i;
	if(AllSurPstDist[0][0]>=ud_pst)
		return AllSurPstDist[0][1];
	for(i=1;i<(CNT_PART_POINT_NUM-2)*CNT_PART_DIVIS_NUM+1;i++)
	{
		if(AllSurPstDist[i][0]>=ud_pst)
			return AllSurPstDist[i-1][1]+(AllSurPstDist[i][1]-AllSurPstDist[i-1][1])*(ud_pst-AllSurPstDist[i-1][0])/(AllSurPstDist[i][0]-AllSurPstDist[i-1][0]);
	}
}

INT8U GetStrngDist(INT8U strng)
{
	if(strng==TECH_FB_BASE)
		return 0x00;
	if(strng==TECH_FB_SUR)
		return GetSurPstDist(UDAimPst);
	if VALID_LEVEL(strng)
		return GetSurPstDist(UDAimPst)+StrDistTab[strng-1];
	if(strng==TECH_FB_LIMIT)
		return PF(CNT_FB_LIMIT_DIST);
}
⌨️ 快捷键说明

复制代码 Ctrl + C
搜索代码 Ctrl + F
全屏模式 F11
切换主题 Ctrl + Shift + D
显示快捷键 ?
增大字号 Ctrl + =
减小字号 Ctrl + -