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

#define EPOS_DELAY1  6
#define EPOS_DELAY2  60
#define EPOS_DELAY3  100

void Epos_Delay(INT8U time)
{
  INT16S i;
  for(;time>0;time--)for(i=0;i<50;i++);
}

void Epos_Write(Epos* epos,INT16U Index,INT8U SubIndex,INT32S param)
{	
	epos->buf.ID = 0x600+epos->NODE_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);
}

BOOL Epos_Read(Epos* epos,INT16U Index,INT8U SubIndex,INT32S* param)
{   
    BOOL r=FALSE;
    
    epos->buf.ID=0x600+epos->NODE_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;
	r=Write(epos->CAN_Handles, &(epos->buf), CANTxCMD_NOR);
	if(r==FALSE)return r;
	r=Read(epos->CAN_Handles,&(epos->buf));
	*param=(((INT32S)(epos->buf.Buf[7]))<<24)+(((INT32S)(epos->buf.Buf[6]))<<16)+(((INT32S)(epos->buf.Buf[5]))<<8) + (INT32S)(epos->buf.Buf[4]);
	return r;
}

void Epos_SInit(Epos* epos1,INT8U CAN_ID,INT8U NODE_ID1)
{
	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;
	
		
	SetDefIRQAddr((void*)IntEnty);
	epos1->CurMode=0x00;
	epos1->opt=0x001f;
	epos1->Acc=MAX_ACC;
	epos1->Dec=MAX_ACC;
	Epos_ParamInit(epos1);
    Epos_Run(epos1,0,0,PPMODE,0x007f);
}	
    

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=0x00;
	epos1->opt=0x001f;
	epos1->Acc=MAX_ACC;
	epos1->Dec=MAX_ACC;
	epos2->CurMode=0x00;
	epos2->opt=0x001f;
	epos2->Acc=MAX_ACC;
	epos2->Dec=MAX_ACC;
    Epos_ParamInit(epos1);
    Epos_ParamInit(epos2);
    Epos_Run(epos1,0,0,PPMODE,0x007f);
    Epos_Run(epos2,0,0,PPMODE,0x007f);
}

void Epos_ParamInit(Epos* epos)
{   
    Epos_Write(epos,0x6040,0x00,0x0080);
    Epos_Delay(EPOS_DELAY2);
    Epos_Write(epos,0x6065,0x00,MAX_F_ERR);
	Epos_Delay(EPOS_DELAY1);	
	Epos_Write(epos,0x607F,0x00,MAX_P_V);
	Epos_Delay(EPOS_DELAY1);	
	Epos_Write(epos,0x6083,0x00,MAX_ACC);
	Epos_Delay(EPOS_DELAY1);	
	Epos_Write(epos,0x6084,0x00,MAX_ACC);
	Epos_Delay(EPOS_DELAY1);	
    Epos_Write(epos,0x6085,0x00,QDEC);
	Epos_Delay(EPOS_DELAY1);	
}

void Epos_Run(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_RunPPM(epos,speed,position,i);
    }
    else if(mode==PVMODE)
    { 
      epos->opt=0x000f;
      Epos_RunPVM(epos,speed,i);
    }
    else if(mode==PMODE)Epos_RunPM(epos,position,i);
    else if(mode==VMODE)Epos_RunVM(epos,speed,i);
}


void Epos_RunPPM(Epos* epos,INT32S speed,INT32S position,INT8U opt)
{	
    if(opt)
    {	
	    Epos_Write(epos,0x6060,0x00,0x01);
	    Epos_Delay(EPOS_DELAY1);
	    Epos_Write(epos,0x6040,0x00,0x06);
	    Epos_Delay(EPOS_DELAY2);
	    Epos_Write(epos,0x6040,0x00,0x0f);
	    Epos_Delay(EPOS_DELAY2);	    
	}	
    Epos_Write(epos,0x6081,0x00,speed);
	Epos_Delay(EPOS_DELAY1);	
	Epos_Write(epos,0x607A,0x00,position);
	Epos_Delay(EPOS_DELAY1);
	Epos_Write(epos,0x6040,0x00,epos->opt);
	Epos_Delay(EPOS_DELAY2);
}

void Epos_RunPVM(Epos* epos,INT32S speed,INT8U opt)
{	
    if(opt)
    {	
	    Epos_Write(epos,0x6060,0x00,0x03);
	    Epos_Delay(EPOS_DELAY1);
	    Epos_Write(epos,0x6040,0x00,0x06);
	    Epos_Delay(EPOS_DELAY2);	
	    Epos_Write(epos,0x6040,0x00,0x0f);
	    Epos_Delay(EPOS_DELAY2);
	}
	Epos_Write(epos,0x60FF,0x00,speed);
	Epos_Delay(EPOS_DELAY1);
	Epos_Write(epos,0x6040,0x00,epos->opt);
	Epos_Delay(EPOS_DELAY2);	
}

void Epos_RunPM(Epos* epos,INT32S position,INT8U opt)
{	
    if(opt)	
	{
        Epos_Write(epos,0x6060,0x00,0xFF);
	    Epos_Delay(EPOS_DELAY1);	
	    Epos_Write(epos,0x6065,0x00,2147483640);
	    Epos_Delay(EPOS_DELAY1);
	    Epos_Write(epos,0x6040,0x00,0x06);
	    Epos_Delay(EPOS_DELAY2);
	    Epos_Write(epos,0x6040,0x00,0x0f);
	    Epos_Delay(EPOS_DELAY2);
	}
	    Epos_Write(epos,0x2062,0x00,position);
	    Epos_Delay(EPOS_DELAY2);
} 

void Epos_RunVM(Epos* epos,INT32S speed,INT8U opt)
{	
    if(opt)
    {
	  Epos_Write(epos,0x6060,0x00,0xFE);
	  Epos_Delay(EPOS_DELAY1);	
	  Epos_Write(epos,0x6040,0x00,0x06);
	  Epos_Delay(EPOS_DELAY2);
	  Epos_Write(epos,0x6040,0x00,0x0f);
	  Epos_Delay(EPOS_DELAY2);
	}
	  Epos_Write(epos,0x206B,0x00,speed);
	  Epos_Delay(EPOS_DELAY2);	
}


void Epos_RunCM(Epos* epos,INT32S Current,INT32S MAX_Speed)
{
    if(epos->CurMode!=CMODE)
    {
      epos->CurMode=CMODE;
      Epos_Write(epos,0x6060,0x00,0xFD);
	  Epos_Delay(EPOS_DELAY1);
	  Epos_Write(epos,0x6410,0x04,MAX_Speed);
	  Epos_Delay(EPOS_DELAY1);	
	  Epos_Write(epos,0x6040,0x00,0x06);
	  Epos_Delay(EPOS_DELAY3);
	  Epos_Write(epos,0x6040,0x00,0x0f);
	  Epos_Delay(EPOS_DELAY3);
    }
    Epos_Write(epos,0x6410,0x04,MAX_Speed);
	Epos_Delay(EPOS_DELAY1);
    Epos_Write(epos,0x2030,0x00,Current);
    Epos_Delay(EPOS_DELAY3);	
}


void Epos_Stop(Epos* epos)
{
	Epos_Write(epos,0x6040,0x00,0x10f);
	Epos_Delay(EPOS_DELAY2);
}


void Epos_ShutDown(Epos* epos)
{
    Epos_Write(epos,0x6040,0x00,0x0006);
    epos->CurMode=0x00;
    Epos_Delay(EPOS_DELAY2);
}

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

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