epos.c

epos串口驱动

#include "EPOS.h"
#include "config.h"


void Epos_SDOWrite(Epos* epos,INT32U ID,INT16U Index,INT8U SubIndex,INT32S param)
{	
	epos->buf.ID = 0x600+ID;
	epos->buf.Buf[0]= 0x22;
	epos->buf.Buf[1]= Index&0xFF;
	epos->buf.Buf[2]= (Index&0xFF00)>>8;
	epos->buf.Buf[3]= SubIndex;
	epos->buf.Buf[4]= param&0xFF;
	epos->buf.Buf[5]= (param&0xFF00)>>8;
	epos->buf.Buf[6]= (param&0xFF0000)>>16;
	epos->buf.Buf[7]= (param&0xFF000000)>>24;
	epos->buf.DataLen = 8;
	epos->buf.IsExtend=0;
	epos->buf.IsRemote=0;
	Write(epos->CAN_Handles, &(epos->buf), CANTxCMD_NOR);	
}

INT32S Epos_SDORead(Epos* epos,INT32U ID,INT16U Index,INT8U SubIndex,INT8U type)
{   
    INT32S tmp;
    epos->buf.ID=0x600+ID;
    epos->buf.Buf[0]=0x40;
    epos->buf.Buf[1]= Index&0xFF;
	epos->buf.Buf[2]= (Index&0xFF00)>>8;
	epos->buf.Buf[3]= SubIndex;
	epos->buf.Buf[4]=0x00;
	epos->buf.Buf[5]=0x00;
	epos->buf.Buf[6]=0x00;
	epos->buf.Buf[7]=0x00;
	epos->buf.DataLen = 8;
	epos->buf.IsExtend=0;
	epos->buf.IsRemote=0;
	Write(epos->CAN_Handles, &(epos->buf), CANTxCMD_NOR);
	epos->buf.ID=0x580+ID;
    epos->buf.Buf[1]= Index&0xFF;
	epos->buf.Buf[2]= (Index&0xFF00)>>8;
	epos->buf.Buf[3]= SubIndex;
	epos->buf.DataLen = 8;
	epos->buf.IsExtend=0;
	epos->buf.IsRemote=0;
	if(type==1)     epos->buf.Buf[0]=0x4F;
	else if(type==2)epos->buf.Buf[0]=0x4B;
	else            epos->buf.Buf[0]=0x43;
	Read(epos->CAN_Handles, &(epos->buf));
	if(type==1)     tmp=epos->buf.Buf[4];
	else if(type==2)tmp=epos->buf.Buf[4]+epos->buf.Buf[5]<<8;
	else if(type==4)tmp=epos->buf.Buf[4]+epos->buf.Buf[5]<<8+epos->buf.Buf[6]<<16+epos->buf.Buf[7]<<24;
	return tmp;
}


BOOL Epos_SDOWriteT(Epos* epos,INT32U ID,INT8U cmd,INT16U Index,INT8U SubIndex,INT32U n,INT32S param)
{
	BOOL r = FALSE;
	epos->buf.ID = 0x600+ID;
	epos->buf.Buf[0]= cmd;
	epos->buf.Buf[1]= Index&0xFF;
	epos->buf.Buf[2]= (Index&0xFF00)>>8;
	epos->buf.Buf[3]= SubIndex;
	epos->buf.Buf[4]= param&0xFF;
	epos->buf.Buf[5]= (param&0xFF00)>>8;
	epos->buf.Buf[6]= (param&0xFF0000)>>16;
	epos->buf.Buf[7]= (param&0xFF000000)>>24;
	epos->buf.DataLen = 8;
	epos->buf.IsExtend=0;
	epos->buf.IsRemote=0;
	r = Write(epos->CAN_Handles, &(epos->buf), CANTxCMD_NOR);
	return r;
}

BOOL Epos_SDOReadT(Epos* epos,INT32U ID,INT8U cmd,INT16U Index,INT8U SubIndex)
{
	BOOL r = FALSE;
	epos->buf.ID = 0x580+ID;
	epos->buf.Buf[0]= cmd;
	epos->buf.Buf[1]= Index&0xFF;
	epos->buf.Buf[2]= (Index&0xFF00)>>8;
	epos->buf.Buf[3]= SubIndex;
	epos->buf.DataLen = 8;
	epos->buf.IsExtend=0;
	epos->buf.IsRemote=0;
	r = Read(epos->CAN_Handles, &(epos->buf));
	return r;
}

INT32S	Epos_GetPosition(Epos* epos)
{
	INT32S res = 0; 
	Epos_SDOWriteT(epos,epos->NODE_ID,0x40,0x6064,0x00,0,0x00);
	Epos_SDOReadT(epos,epos->NODE_ID,0x43,0x6064,0x00);
	res = (((INT32S)(epos->buf.Buf[7]))<<24)+(((INT32S)(epos->buf.Buf[6]))<<16)+(((INT32S)(epos->buf.Buf[5]))<<8) + (INT32S)(epos->buf.Buf[4]);
	return res;
}
	
	
    

void Epos_Init(Epos* epos1,Epos* epos2,INT8U CAN_ID,INT8U NODE_ID1,INT8U NODE_ID2)
{
	epos1->isInit = FALSE;
	epos1->CAN_Handles = CAN_Create(CAN_ID,10,BTR_1000K,NULL);
	if(epos1->CAN_Handles==NULL)
		return;
	epos1->NODE_ID = NODE_ID1;
	epos1->isInit = TRUE;
	
	epos2->NODE_ID = NODE_ID2;
	epos2->isInit = TRUE;
	epos2->CAN_Handles = epos1->CAN_Handles;
	
	SetDefIRQAddr((void*)IntEnty);
	epos1->CurMode=PPMODE;
	epos1->opt=0x001f;
	epos1->Acc=MAX_ACC;
	epos1->Dec=MAX_ACC;
	epos2->CurMode=PPMODE;
	epos2->opt=0x001f;
	epos2->Acc=MAX_ACC;
	epos2->Dec=MAX_ACC;
    Epos_ParamInit(epos1);
    Epos_ParamInit(epos2);
    Epos_ReadyPPM(epos1,0,0,1); 	
    Epos_ReadyPPM(epos2,0,0,1); 
}

void Epos_ParamInit(Epos* epos)
{
    Epos_SDOWrite(epos,epos->NODE_ID,0x6065,0x00,MAX_F_ERR);
	OSTimeDly(2);
	Epos_SDOWrite(epos,epos->NODE_ID,0x607F,0x00,MAX_P_V);
	OSTimeDly(2);
	Epos_SDOWrite(epos,epos->NODE_ID,0x6083,0x00,MAX_ACC);
	OSTimeDly(2);
	Epos_SDOWrite(epos,epos->NODE_ID,0x6084,0x00,MAX_ACC);
	OSTimeDly(2);
    Epos_SDOWrite(epos,epos->NODE_ID,0x6085,0x00,QDEC);
	OSTimeDly(2);
}
void Epos_ReadyP(Epos* epos,INT32S speed,INT32S position,INT8U mode,INT32S opt)
{   
    INT8U i=0;
    if(mode!=epos->CurMode)
    {
       i=1;
       epos->CurMode=mode;
    }
    if(mode==PPMODE)
    { 
      epos->opt=opt;
      Epos_ReadyPPM(epos,speed,position,i);
    }
    else if(mode==PVMODE)
    { 
      epos->opt=0x000f;
      Epos_ReadyPVM(epos,speed,i);
    }
}


void Epos_ReadyPPM(Epos* epos,INT32S speed,INT32S position,INT8U opt)
{	
    if(opt)
    {	
	    Epos_SDOWrite(epos,epos->NODE_ID,0x6060,0x00,0x01);
	    OSTimeDly(2);	
	    Epos_SDOWrite(epos,epos->NODE_ID,0x6040,0x00,0x06);
	    OSTimeDly(2);
	    Epos_SDOWrite(epos,epos->NODE_ID,0x6040,0x00,0x0f);
	    OSTimeDly(2);
    }	
    Epos_SDOWrite(epos,epos->NODE_ID,0x6081,0x00,speed);
	OSTimeDly(2);	
	Epos_SDOWrite(epos,epos->NODE_ID,0x607A,0x00,position);
	OSTimeDly(2);
}

void Epos_ReadyPVM(Epos* epos,INT32S speed,INT8U opt)
{	
    if(opt)
    {	
	    Epos_SDOWrite(epos,epos->NODE_ID,0x6060,0x00,0x03);
	    OSTimeDly(2);	
	    Epos_SDOWrite(epos,epos->NODE_ID,0x6040,0x00,0x06);
	    OSTimeDly(2);	
	    Epos_SDOWrite(epos,epos->NODE_ID,0x6040,0x00,0x0f);
	    OSTimeDly(2);
	}
	Epos_SDOWrite(epos,epos->NODE_ID,0x60FF,0x00,speed);
	OSTimeDly(2);	
}

void Epos_RunNP(Epos* epos,INT32S speed,INT32S position,INT8U mode)
{  
    INT8U i=0;
    if(mode!=epos->CurMode)
    {
       i=1;
       epos->CurMode=mode;
    }
    if(mode==PMODE)Epos_RunPM(epos,position,i);
	else if(mode==VMODE)Epos_RunVM(epos,speed,i);
}


void Epos_RunPM(Epos* epos,INT32S position,INT8U opt)
{	
    if(opt)	
	{
        Epos_SDOWrite(epos,epos->NODE_ID,0x6060,0x00,0xFF);
	    OSTimeDly(2);	
	    Epos_SDOWrite(epos,epos->NODE_ID,0x6065,0x00,2147483640);
	    OSTimeDly(2);	
	    Epos_SDOWrite(epos,epos->NODE_ID,0x6040,0x00,0x06);
	    OSTimeDly(2);
	    Epos_SDOWrite(epos,epos->NODE_ID,0x6040,0x00,0x0f);
	    OSTimeDly(2);
	}
	Epos_SDOWrite(epos,epos->NODE_ID,0x2062,0x00,position);
	OSTimeDly(2);
}

void Epos_RunVM(Epos* epos,INT32S speed,INT8U opt)
{	
    if(opt)
    {
	  Epos_SDOWrite(epos,epos->NODE_ID,0x6060,0x00,0xFE);
	  OSTimeDly(2);	
	  Epos_SDOWrite(epos,epos->NODE_ID,0x6040,0x00,0x06);
	  OSTimeDly(2);	
	  Epos_SDOWrite(epos,epos->NODE_ID,0x6040,0x00,0x0f);
	  OSTimeDly(2);
	}
	Epos_SDOWrite(epos,epos->NODE_ID,0x206B,0x00,speed);
	  OSTimeDly(2);		
}


void Epos_Stop(Epos* epos)
{
	Epos_SDOWrite(epos,epos->NODE_ID,0x6040,0x00,0x10f);
}

void Epos_SetAcc(Epos* epos,INT32S acc)
{   
    epos->Acc=acc;
	Epos_SDOWrite(epos,epos->NODE_ID,0x6083,0x00,acc);
}

void Epos_SetDec(Epos* epos,INT32S dec)
{
    epos->Dec=dec;
	Epos_SDOWrite(epos,epos->NODE_ID,0x6084,0x00,dec);
}