lm629.h

LM629运动控制器操作

#define RESET  0x00  //复位
#define PORT8  0x05  //pwm输出指令
#define DFH    0x02  //定义原点
//#define SIP    0x03  //设定index位置
#define LPEI   0x1B  //误差中断
#define LPES   0x1A  //误差停指令
#define SBPA   0x20  //设定绝对断点w
#define SBPR   0x21  //设定相对断点
#define MSKI   0x1C  //屏蔽中断指令
#define RSTI   0x1D  //复位中断
#define LFIL   0x1E  //装入PID参数
#define UDF    0x04  //参数有效指令
#define LTRJ   0x1F  //装入运动参数
#define STT    0x01  //数据有效指令
#define RDSIGS 0x0C  //读信号寄存器指令
#define RDIP   0x09  //读index位置
#define RDDP   0x08  //读预定位置
#define RDRP   0x0A  //读实际位置
#define RDDV   0x07  //读预定速度指令
#define RDRV   0x0B  //读实际速度指令
#define RDSUM  0x0D  //读积分和指令
#define  DATAOUT   P4OUT
#define  DATAIN    P4IN
#define  XDATA     P4DIR
#define  COMMAND   P5OUT
#define  XCOM      P5DIR
#define  RST       BIT4
#define  PS        BIT5
#define  WR        BIT6
#define  RD        BIT1
#define  CSR       BIT3
#define  CSL       BIT2
#define  MOT       BIT7
//#define  HIL        BIT6      //p2.6
//#define  HIL        BIT6     //p2.5
void WDELAY()
{
  char i;
  for(i=0;i<50;i++);
}
void DELAY1()
{int j;
for(j=20000;j>1;j--);
}
//lm629.h
//lm629驱动程序
unsigned char WYK_RDSTAT(unsigned char lr)                         //ReaD STATus Byte
{
 unsigned char stat;
  XDATA=0X00;
  XCOM=0XFF;
  COMMAND=0XFF;
  if(lr=='l'){COMMAND&=~CSL;}
  else COMMAND&=~CSR;
  COMMAND&=~PS;  ;
  COMMAND&=~RD;
  WDELAY();
  stat=DATAIN;
  COMMAND|=RD; ;
  COMMAND|=PS;
 if(lr=='l'){COMMAND|=CSL;}
  else COMMAND|=CSR;
 return stat;
}
void WYK_WRCOM(unsigned char command,unsigned char lr)
{XCOM=0XFF;
 XDATA=0XFF;
 COMMAND=0XFF;
 P5OUT&=~BIT0;
if(lr=='l')
{ COMMAND&=~CSL;}  //LEFT
else COMMAND&=~CSR; //RIGHT
COMMAND&=~PS;
DATAOUT=command; ;
COMMAND&=~WR;
WDELAY();
COMMAND|=WR; ;
COMMAND|=PS;
if(lr=='l'){COMMAND|=CSL;}
  else COMMAND|=CSR;
}
void WYK_WRDATA(unsigned char data, unsigned char lr)
{XCOM=0XFF;
XDATA=0XFF;
COMMAND=0XFF;
P5OUT&=~BIT0;
if(lr=='l')
{ COMMAND&=~CSL;}
else COMMAND&=~CSR;
DATAOUT=data; ; ;
COMMAND&=~WR;
WDELAY();
COMMAND|=WR; ;
if(lr=='l'){COMMAND|=CSL;}
  else COMMAND|=CSR;

}
unsigned char WYK_RDDATA(unsigned char lr)
{
  unsigned char data;
  XDATA=0X00;
  XCOM=0XFF;
  COMMAND=0XFF;
  if(lr=='l'){COMMAND&=~CSL;}
  else COMMAND&=~CSR;
   ; ;
  COMMAND&=~RD;
  WDELAY();
  data=DATAIN;
  COMMAND|=RD; ;
 if(lr=='l'){COMMAND|=CSL;}
  else COMMAND|=CSR;
 return data;
}
void WYK_BUSY(unsigned char lr)
{
 while(WYK_RDSTAT(lr)&0X01);                            // if busy then loop
}
void WYK_MSKI(unsigned int mask,unsigned char lr)                           //MaSK Interrupts
{ unsigned char c;
 WYK_WRCOM(MSKI,lr);
 WYK_BUSY(lr);
c=(unsigned char)((mask&0xff00)>>8);
  WYK_WRDATA(c,lr);
c=(unsigned char)(mask&0x00ff);
WDELAY();
  WYK_WRDATA(c,lr);
  WYK_BUSY(lr);
}
void WYK_RSTI(unsigned int mask,unsigned char lr)                           //ReSeT Interrupts
{unsigned char c;
  WYK_WRCOM(RSTI,lr);
 WYK_BUSY(lr);
 c=(unsigned char)((mask&0xff00)>>8);
  WYK_WRDATA(c,lr);
c=(unsigned char)(mask&0x00ff);
WDELAY();
  WYK_WRDATA(c,lr);
  WYK_BUSY(lr);
}
// LM629  共22条指令
// 1.初始化指令（3条）
void WYK_RESET(void)                              //RESET the LM629
{
XCOM=0XFF;
COMMAND=0XFF;
COMMAND&=~RST;
DELAY1();
COMMAND|=RST;
DELAY1();
}
void WYK_RESET_CHK(void)
{
 unsigned char chk_char1,chk_char2;
XCOM=0XFF;
COMMAND=0X7F;
 //BREAK_ON();                                         //复位前先停止电机
RE:
  WDELAY();
chk_char1=WYK_RDSTAT('l');
if(!(chk_char1==0xc4||chk_char1==0x84))	
{WYK_RESET();
goto RE;
}	
WYK_RSTI(0x0000,'l');
WDELAY();chk_char2=WYK_RDSTAT('l');
if(!(chk_char2==0x80||chk_char2==0xc0))
{WYK_RESET();
goto RE;
}
chk_char1=WYK_RDSTAT('r');
if(!(chk_char1==0xc4||chk_char1==0x84))	
{  WYK_RESET();
goto RE;
}
WYK_RSTI(0x0000,'r');
WDELAY();
chk_char2=WYK_RDSTAT('r');
if(!(chk_char2==0x80||chk_char2==0xc0))
{WYK_RESET();
goto RE;
 }
COMMAND=0XFF;
  //BREAK_OFF();                                      //重新打开电机

}

void WYK_PORT8(unsigned char lr)                           //Set Output PORT Size to 8 Bits
{
  WYK_WRCOM(PORT8,lr);
  WYK_BUSY(lr);

 }

void WYK_DFH(unsigned char lr)                                //DeFine Home
{
  WYK_WRCOM(DFH,lr);
  WYK_BUSY(lr);

}

// 2.中断指令  （6条）
/*void WYK_SIP(unsigned char lr)                               //Set Index Position
{
  WYK_WRCOM(SIP,lr);
  WYK_BUSY(lr);
}

void WYK_LPEI(unsigned int PositionError,unsigned char lr)          //Load Position Error for Interrupt
{unsigned char c;
 WYK_WRCOM(LPEI,lr);
 WYK_BUSY(lr);
 c=(unsigned char)((PositionError&0xff00)>>8);
 WYK_WRDATA(c,lr);
 c=(unsigned char)(PositionError&0x00ff);
 WDELAY();
 WYK_WRDATA(c,lr);
 WYK_BUSY(lr);
}

void WYK_LPES(unsigned int PositionError,unsigned char lr)           //Load Position Error for Stopping
{unsigned char c;
 WYK_WRCOM(LPES,lr);
 WYK_BUSY(lr);
 c=(unsigned char)((PositionError&0xff00)>>8);
 WYK_WRDATA(c,lr);
 c=(unsigned char)(PositionError&0x00ff);
 WDELAY();
 WYK_WRDATA(c,lr);
 WYK_BUSY(lr);
}

void WYK_SBPA(unsigned long BreakPoint,unsigned char lr)                   //设置绝对位置断点
{unsigned char c;
 WYK_WRCOM(SBPA,lr);
 WYK_BUSY(lr);
 c=(unsigned char)((BreakPoint&0xff000000)>>24);
 WYK_WRDATA(c,lr);
 c=(unsigned char)((BreakPoint&0x00ff0000)>>16);
 WDELAY();
 WYK_WRDATA(c,lr);
 WYK_BUSY(lr);
 c=(unsigned char)((BreakPoint&0x0000ff00)>>8);
 WYK_WRDATA(c,lr);
 c=(unsigned char)(BreakPoint&0x000000ff);
 WDELAY();
 WYK_WRDATA(c,lr);
 WYK_BUSY(lr);
}
void WYK_SBPR(unsigned long BreakPoint,unsigned char lr)              // 设置相对断点
{unsigned char c;
 WYK_WRCOM(SBPR,lr);
 WYK_BUSY(lr);
 c=(unsigned char)((BreakPoint&0xff000000)>>24);
 WYK_WRDATA(c,lr);
 c=(unsigned char)((BreakPoint&0x00ff0000)>>16);
 WDELAY();
 WYK_WRDATA(c,lr);
 WYK_BUSY(lr);
 c=(unsigned char)((BreakPoint&0x0000ff00)>>8);
 WYK_WRDATA(c,lr);
 c=(unsigned char)(BreakPoint&0x000000ff);
 WDELAY();
 WYK_WRDATA(c,lr);
 WYK_BUSY(lr);
 }*/


//3.PI控制器指令(2条)
void WYK_LFIL(unsigned int command_word,unsigned int kp_data,unsigned int ki_data,unsigned int kd_data,unsigned int il_data,unsigned char lr)//Load FILter Parameters
{ //写LFIL命令
  unsigned char c;
 WYK_WRCOM(LFIL,lr);
 WYK_BUSY(lr);
  //写控制字
 c=(unsigned char)((command_word & 0xFF00)>>8);
 WYK_WRDATA(c,lr);
 c=(unsigned char)(command_word & 0x00FF);
 WDELAY();
 WYK_WRDATA(c,lr);
 WYK_BUSY(lr);
  if(command_word&0x0008) //写kp
  {c=(unsigned char)((kp_data & 0xFF00)>>8);
 WYK_WRDATA(c,lr);
 c=(unsigned char)(kp_data & 0x00FF);
 WDELAY();
 WYK_WRDATA(c,lr);
 WYK_BUSY(lr);}
 if(command_word&0x0004) //写ki
 {c=(unsigned char)((ki_data & 0xFF00)>>8);
 WYK_WRDATA(c,lr);
 c=(unsigned char)(ki_data & 0x00FF);
 WDELAY();
 WYK_WRDATA(c,lr);
 WYK_BUSY(lr);}
  if(command_word&0x0002)//写kd
  {c=(unsigned char)((kd_data & 0xFF00)>>8);
 WYK_WRDATA(c,lr);
 c=(unsigned char)(kd_data & 0x00FF);
 WDELAY();
 WYK_WRDATA(c,lr);
 WYK_BUSY(lr);}
  if(command_word&0x0001)//写il
  { c=(unsigned char)((il_data & 0xFF00)>>8);
 WYK_WRDATA(c,lr);
 c=(unsigned char)(il_data & 0x00FF);
 WDELAY();
 WYK_WRDATA(c,lr);
 WYK_BUSY(lr);}
}

void WYK_UDF(unsigned char lr)                                       //UpDate Filter
{WYK_WRCOM(UDF,lr);
 WYK_BUSY(lr);
}

// 4.运动指令 (2条)	
void WYK_LTRJ(unsigned int command_word,unsigned long acceleration_data,unsigned long velocity_data,signed long position_data,unsigned char lr)                  //Load TRaJectory Parameters
{unsigned char c;
  WYK_WRCOM(LTRJ,lr);
 WYK_BUSY(lr);
  //写控制字
 c=(unsigned char)((command_word & 0xFF00)>>8);
 WYK_WRDATA(c,lr);
 c=(unsigned char)(command_word & 0x00FF);
 WDELAY();
 WYK_WRDATA(c,lr);
 WYK_BUSY(lr);
  	  //写加速度
  if(command_word&0x0020)
   {    c=(unsigned char)((acceleration_data & 0xFF000000)>>24);
  WYK_WRDATA(c,lr);
 c=(unsigned char)((acceleration_data & 0x00FF0000)>>16);
 WDELAY();
 WYK_WRDATA(c,lr);
 WYK_BUSY(lr);
 c=(unsigned char)((acceleration_data & 0x0000FF00)>>8);
 WYK_WRDATA(c,lr);
 c=(unsigned char)(acceleration_data&0x000000FF);
 WDELAY();
 WYK_WRDATA(c,lr);
 WYK_BUSY(lr);}
 if(command_word&0x0008)
 {   //写速度
  c=(unsigned char)((velocity_data & 0xFF000000)>>24);
  WYK_WRDATA(c,lr);
 c=(unsigned char)((velocity_data & 0x00FF0000)>>16);
 WDELAY();
 WYK_WRDATA(c,lr);
 WYK_BUSY(lr);
 c=(unsigned char)((velocity_data & 0x0000FF00)>>8);
 WYK_WRDATA(c,lr);
 c=(unsigned char)(velocity_data&0x000000FF);
 WDELAY();
 WYK_WRDATA(c,lr);
 WYK_BUSY(lr);
   }
  if(command_word&0x0002)
        {//写位置
    c=(unsigned char)((position_data & 0xFF000000)>>24);
  WYK_WRDATA(c,lr);
 c=(unsigned char)((position_data & 0x00FF0000)>>16);
 WDELAY();
 WYK_WRDATA(c,lr);
 WYK_BUSY(lr);
 c=(unsigned char)((position_data & 0x0000FF00)>>8);
 WYK_WRDATA(c,lr);
 c=(unsigned char)(position_data&0x000000FF);
 WDELAY();
 WYK_WRDATA(c,lr);
 WYK_BUSY(lr);
}

}

void WYK_STT(unsigned char lr)                                    //STarT Motion Control
{
  WYK_WRCOM(STT,lr);
 WYK_BUSY(lr);
}
// 5.状态和信息指令(8条)
/*unsigned int WYK_RDSIGS(unsigned char lr)//ReaD SIGnalS Register
{ unsigned int rt=0x0000;
  unsigned char c;
  WYK_WRCOM(RDSIGS,lr);
 WYK_BUSY(lr);
 c=WYK_RDDATA(lr);
  rt+=(((unsigned int)c)<<8);
  WDELAY();
 c=WYK_RDDATA(lr);
  rt+=((unsigned int)c);
  WYK_BUSY(lr);
  return rt;
}*/

/*unsigned long WYK_RDIP(unsigned char lr)                           //ReaD Index Position
{
  unsigned long rt=0x00000000;
  unsigned char c;
  WYK_WRCOM(RDIP,lr);
  WYK_BUSY(lr);
   c=WYK_RDDATA(lr);
    rt+=(((unsigned long)c)<<24);
  WDELAY();
  c=WYK_RDDATA(lr);
  rt+=(((unsigned long)c)<<16);
  WYK_BUSY(lr);
  c=WYK_RDDATA(lr);
   rt+=(((unsigned long)c)<<8);
  WDELAY();
 c=WYK_RDDATA(lr);
  rt+=((unsigned long)c);
  WYK_BUSY(lr);
   return rt;
}

unsigned long WYK_RDDP(unsigned char lr)                             //ReaD Desired Position
{ unsigned long rt=0x00000000;
  unsigned char c;
  WYK_WRCOM(RDDP,lr);
  WYK_BUSY(lr);
   c=WYK_RDDATA(lr);
    rt+=(((unsigned long)c)<<24);
  WDELAY();
  c=WYK_RDDATA(lr);
  rt+=(((unsigned long)c)<<16);
  WYK_BUSY(lr);
  c=WYK_RDDATA(lr);
   rt+=(((unsigned long)c)<<8);
  WDELAY();
 c=WYK_RDDATA(lr);
  rt+=((unsigned long)c);
  WYK_BUSY(lr);
  return rt;
 }*/

unsigned long WYK_RDRP(unsigned char lr)                             //ReaD Real Position
{ unsigned long rt=0x00000000;
  unsigned char c;
  WYK_WRCOM(RDRP,lr);
  WYK_BUSY(lr);
   c=WYK_RDDATA(lr);
    rt+=(((unsigned long)c)<<24);
  WDELAY();
  c=WYK_RDDATA(lr);
  rt+=(((unsigned long)c)<<16);
  WYK_BUSY(lr);
  c=WYK_RDDATA(lr);
   rt+=(((unsigned long)c)<<8);
  WDELAY();
 c=WYK_RDDATA(lr);
  rt+=((unsigned long)c);
  WYK_BUSY(lr);
  return rt;
}

/*unsigned long WYK_RDDV(unsigned char lr)                             //ReaD Desired Velocity
{ unsigned long rt=0x00000000;
  unsigned char c;
  WYK_WRCOM(RDDV,lr);
  WYK_BUSY(lr);
  c=WYK_RDDATA(lr);
    rt+=(((unsigned long)c)<<24);
  WDELAY();
  c=WYK_RDDATA(lr);
  rt+=(((unsigned long)c)<<16);
  WYK_BUSY(lr);
  c=WYK_RDDATA(lr);
   rt+=(((unsigned long)c)<<8);
  WDELAY();
 c=WYK_RDDATA(lr);
  rt+=((unsigned long)c);
  WYK_BUSY(lr);
  return rt;
}

unsigned int WYK_RDRV(unsigned char lr)                             //ReaD Real Velocity
{ unsigned int rt=0x0000;
  unsigned char c;
  WYK_WRCOM(RDRV,lr);
  WYK_BUSY(lr);
  c=WYK_RDDATA(lr);
   rt+=(((unsigned int)c)<<8);
  WDELAY();
 c=WYK_RDDATA(lr);
  rt+=((unsigned int)c);
  WYK_BUSY(lr);
  return rt;
}

unsigned int WYK_RDSUM(unsigned char lr)          //ReaD Integration-Term SUMmation value
{ unsigned int rt=0x0000;
  unsigned char c;
  WYK_WRCOM(RDSUM,lr);
  WYK_BUSY(lr);
  c=WYK_RDDATA(lr);
  rt+=(((unsigned int)c)<<8);
  WDELAY();
  c=WYK_RDDATA(lr);
  rt+=((unsigned int)c);
  WYK_BUSY(lr);
  return rt;
}*/