LM629运动控制器操作

//对于右移大于或等于位宽的操作，或者右移负数的操作，其结果将依赖于编译器的处理和硬件指令的处理，并不唯一。
#include<msp430x14x.h>
#define RESET 0x00 //复位指令
#define PORT8 0X05 //PWM输出指令
#define DFH   0X02 //原点定义指令
#define RSTI  0x1D //复位中断指令
#define LFIL  0x1E //装入PID参数指令
#define UDF   0x04 //PID参数有效指令
#define LTRJ  0x1F //装入运动参数指令
#define STT   0x01 //运动参数有效指令
#define RDDP  0x08 //读预定位置指令
#define RDRP  0x0A //读实际位置指令
#define RDDV  0x07 //读预定速度指令
#define RDRV  0x0B //读实际速度指令
#define MSKI  0x1c //屏蔽中断指令
//其中读状态指令RDSTAT无操作码
#define DataDIR P4DIR
#define DataOUT P4OUT
#define DataIN  P4IN
#define CtrlDIR P5DIR
#define CtrlOUT P5OUT
#define HI BIT0 //中断引脚
#define RST BIT4//复位引脚
#define PS BIT5 //指令，数据选择位
#define WR BIT6 
#define RD BIT1
#define CSR BIT3
#define CSL BIT2
#define MOT BIT7
#define pre_v0 0x03
#define pre_v1 0x00
#define pre_v2 0x00
void init_sys();
void init_port();
unsigned char rd_st();//读状态(主要用于检测629是否处于忙状态)
unsigned char rd_data();//读信息（包括读预定位置，实际位置，预定速度，实际速度等）
void wr_data(unsigned char val);//写数据
void wr_cmd(unsigned char cmd);//写指令
//在读，写数据之前必须先写入相应的指令
void check_busy();
void DELAY();
void delay4ms();
void filter_sel();//PID参数设置
void traj_selInit();//运动参数初始化，包括装加速度acc,装入后以后将无法再次装入，
                    //否则会使得程序运行不正常。这个函数用于初始化时调用，主要是装入加速度，
                    //由于加速度只能一次性装入，故这个函数也是一次性使用。
void GoInit();//将初始轨迹参数装入到629
void traj_sel(unsigned char ctr1,unsigned char ctr2,unsigned char v1,unsigned char v2,unsigned char v3,unsigned char p1,unsigned char p2,unsigned char p3,unsigned char p4);//轨迹参数设置
void run_stright(unsigned char v1,unsigned char v2,unsigned char v3,unsigned char p1,unsigned char p2,unsigned char p3,unsigned char p4);
void turn_left(unsigned char pl1,unsigned char pl2,unsigned char pl3,unsigned char pl4,unsigned char pr1,unsigned pr2,unsigned char pr3,unsigned char pr4);
void turn_right(unsigned char pl1,unsigned char pl2,unsigned char pl3,unsigned char pl4,unsigned char pr1,unsigned pr2,unsigned char pr3,unsigned char pr4);
void run_back(unsigned char v1,unsigned char v2,unsigned char v3,unsigned char p1,unsigned char p2,unsigned char p3,unsigned char p4);
void stop();
unsigned int rd_front();//读前排灯数据
unsigned int counter_table();//数格
void correct_left(unsigned char v1,unsigned char v2,unsigned char v3 );//左轮纠偏
void correct_right(unsigned char v4,unsigned char v5,unsigned char v6);//右轮纠偏
void init_TimerA();
unsigned int light_val;
unsigned int counter_tab=0,sign=0;//sign为经过十字交叉点标志位
unsigned char sign_turnleft=0,sign_turnright=0,sign_runback=0,flag_runback=0;
void main()
{
  WDTCTL=WDTPW+WDTHOLD;
  init_sys();
  init_port();
  GoInit();
  //turn_right(0x00,0x00,0x15,0x10,0x00,0x00,0x15,0x10);//后转
  run_stright(pre_v0,pre_v1,pre_v2,0x00,0xff,0xff,0xff);
  init_TimerA();
  while(1)
  {
    if((counter_tab==9)&(sign_turnright==0))      
   {
      _DINT();
      stop();
      //turn_right(0x00,0x00,0x0c,0x31,0x00,0x00,0x08,0x00); 3 7 10 12 15 25 28 31 / 3 8 12 15 19 30 34 38
      turn_right(0x00,0x00,0x08,0x00,0x00,0x00,0x0d,0x30);
      _EINT();
      run_stright(pre_v0,pre_v1,pre_v2,0x00,0xff,0xff,0xff);
   }
   /*if((counter_tab==3)&(sign_turnright==1))      
   {
      _DINT();
      stop();
      //turn_right(0x00,0x00,0x0c,0x31,0x00,0x00,0x08,0x00); 3 7 10 12 15 25 28 31 / 3 8 12 15 19 30 34 38
      turn_right(0x00,0x00,0x0a,0x00,0x00,0x00,0x0c,0x30);
      _EINT();
      run_stright(pre_v0,pre_v1,pre_v2,0x00,0xff,0xff,0xff);
   }
   if((counter_tab==7)&(sign_turnleft==0))      
   {
      _DINT();
      stop();
      //turn_left(0x00,0x00,0x0b,0x30,0x00,0x00,0x0b,0x00);
      turn_left(0x00,0x00,0x0a,0x30,0x00,0x00,0x0c,0x00);
      _EINT();
      run_stright(pre_v0,pre_v1,pre_v2,0x00,0xff,0xff,0xff);
   }
   if((counter_tab==10)&(sign_turnleft==1))      
   {
      _DINT();
      stop();
      turn_left(0x00,0x00,0x0a,0x00,0x00,0x00,0x0c,0x30);
      _EINT();
      run_stright(pre_v0,pre_v1,pre_v2,0x00,0xff,0xff,0xff);
   }
    if((counter_tab==12)&(sign_turnright==2))      
   {
      _DINT();
      stop();
      turn_right(0x00,0x00,0x0a,0x00,0x00,0x00,0x0c,0x30);
      _EINT();
      run_stright(pre_v0,pre_v1,pre_v2,0x00,0xff,0xff,0xff);
   }
   if((counter_tab==15)&(sign_turnright==3))      
   {
      _DINT();
      stop();
      turn_right(0x00,0x00,0x18,0x30,0x00,0x00,0x12,0x00);//后转
      _EINT();
      //run_stright(0x00,0xc5,0xff,0x00,0xff,0xff,0xff);
       run_stright(pre_v0,pre_v1,pre_v2,0x00,0xff,0xff,0xff);
   }
    if((counter_tab==25)&(sign_turnright==4))      
   {
      _DINT();
      stop();
      turn_right(0x00,0x00,0x0a,0x00,0x00,0x00,0x0c,0x31);
      _EINT();
      run_stright(pre_v0,pre_v1,pre_v2,0x00,0xff,0xff,0xff);
   }
    if((counter_tab==28)&(sign_turnleft==2))      
   {
      _DINT();
      stop();
      turn_left(0x00,0x00,0x0a,0x30,0x00,0x00,0x0c,0x00);
      _EINT();
      run_stright(pre_v0,pre_v1,pre_v2,0x00,0xff,0xff,0xff);
   }
   if(counter_tab==31) 
      stop();
   */
   //纠偏
    switch(light_val)
    {
    case 0xf3e0:correct_left(0x02,0x20,0x00); break;
    case 0xf7e0:correct_left(0x02,0x2f,0x00); break;
    case 0xe7e0:correct_left(0x02,0x30,0x00); break;
    case 0xefe0:correct_left(0x02,0x3f,0x00); break;
    case 0xcfe0:correct_left(0x02,0x40,0x00); break;
    case 0xdfe0:correct_left(0x02,0x4f,0x00); break;
    case 0x9fe0:correct_left(0x02,0x60,0x00); break;
    case 0xbfe0:correct_left(0x02,0x6f,0x00); break;
    case 0x3fe0:correct_left(0x02,0x70,0x00); break;
    case 0x7fe0:correct_left(0x02,0x7f,0x00); break;
   
    case 0xfbe0:run_stright(pre_v0,pre_v1,pre_v2,0x00,0xff,0xff,0xff);break;

    case 0xf9e0:correct_right(0x02,0x20,0x00); break;
    case 0xfde0:correct_right(0x02,0x2f,0x00); break;
    case 0xfce0:correct_right(0x02,0x30,0x00); break;
    case 0xfee0:correct_right(0x02,0x3f,0x00); break;
    case 0xfe60:correct_right(0x02,0x40,0x00); break;
    case 0xff60:correct_right(0x02,0x4f,0x00); break;
    case 0xff20:correct_right(0x02,0x60,0x00); break;
    case 0xffa0:correct_right(0x02,0x6f,0x00); break;
    case 0xff80:correct_right(0x02,0x70,0x00); break;
    case 0xffc0:correct_right(0x02,0x7f,0x00); break;
    case 0xffe0: break;//没有采样到白线 
    default:break;
    }
  }
   
}

void init_sys()
{
  unsigned int i;  
  BCSCTL1&=~XT2OFF;//打开XT2振荡器
  do
  { 
    IFG1&=~OFIFG;
    for(i=0;i<0xfe;i++);
  }while((IFG1&OFIFG)!=0);
  BCSCTL2|=(SELM_2+SELS);//MCLK,SMCLK的时钟源位XT2
}

void init_port()
{
  CtrlDIR=0Xff;
  DataDIR=0Xff;
  P6DIR=0Xff;
  P2DIR=0X00;
  P3DIR=0X00;
  P1DIR=0X00;
  P2IE|=BIT1;
  P2IES&=~BIT1;
  P2IE|=BIT0;
  P2IES&=~BIT0;
}

void DELAY()
{
  unsigned int i;
  for(i=0;i<200;i++);
}

void delay4ms()
{
  long int i;
  for(i=0;i<16000;i++);
}

void check_busy()
{ 
  unsigned char status;
  do
  {
    status=rd_st();
  }while(status&0x01);//不断读取状态信息，直至状态字节的最低位为0，表明629处于闲状态。
}

void wr_cmd(unsigned char cmd)
{
  CtrlDIR=0Xff;
  DataDIR=0Xff;
  CtrlOUT&=~HI;//中断允许关
  CtrlOUT&=~PS;//PS=0
  CtrlOUT|=RD;//RD=1
  CtrlOUT&=~WR;//WR=0 写允许（低电平有效）
  DataOUT=cmd;
  DELAY();
  CtrlOUT|=PS;//PS=1
  CtrlOUT|=WR;//WR=1
  CtrlOUT|=HI;
}

void wr_data(unsigned char val)
{
  //unsigned char i;
  CtrlDIR=0Xff;
  DataDIR=0Xff;
  CtrlOUT&=~HI;
  CtrlOUT|=PS;//PS=1
  CtrlOUT|=RD;//RD=1
  CtrlOUT&=~WR;//WR=0
  DataOUT=val;
  DELAY();
  CtrlOUT|=WR;//WR=1
  CtrlOUT|=HI;
}

unsigned char rd_st()//状态指令RDSTAT无操作码
{
  unsigned char temp1;
  CtrlDIR=0Xff;
  DataDIR=0X00;
  CtrlOUT|=HI;
  CtrlOUT&=~PS;//PS=0
  CtrlOUT&=~RD;//RD=0
  CtrlOUT|=WR;//WR=1
  DELAY();
  temp1=DataIN;
  CtrlOUT|=RD;//RD=1
  CtrlOUT|=PS;//PS=1
  return temp1;
}

unsigned char rd_data()
{
  unsigned char temp2;
  CtrlDIR=0Xff;
  DataDIR=0X00;
  CtrlOUT|=HI;
  CtrlOUT|=PS;//PS=1
  CtrlOUT&=~RD;//RD=0
  CtrlOUT|=WR;//WR=1
  DELAY();
  temp2=DataIN;
  CtrlOUT|=RD;//RD=1
  return temp2;
}


void filter_sel()
{ 
  check_busy();
  wr_cmd(LFIL);
  DELAY();
  check_busy();
  wr_data(0x08);//采样时间间隔1280μs
  DELAY();
  check_busy();
  wr_data(0x0e);//写PI//0x0c
  DELAY();
  check_busy();
  wr_data(0x00);//P 高位
  DELAY();
  check_busy();
  wr_data(0xa0);//P 低位
  DELAY();
  check_busy();
  wr_data(0x00);//i 高位
  DELAY();
  check_busy();
  wr_data(0x0a);//i 低位
  DELAY();
  check_busy();
  wr_data(0x00);//d
  DELAY();
  check_busy();
  wr_data(0x0a);
  DELAY();
  check_busy();
  wr_cmd(UDF);
}

void traj_sel(unsigned char ctr1,unsigned char ctr2,unsigned char v1,unsigned char v2,unsigned char v3,unsigned char p1,unsigned char p2,unsigned char p3,unsigned char p4)
{
  check_busy();
  wr_cmd(LTRJ);
  DELAY();
  check_busy();
  wr_data(ctr1);
  DELAY();
  check_busy();
  wr_data(ctr2);
  DELAY();
  //装速度
  check_busy();
  wr_data(0x00);
  DELAY();
  check_busy();
  wr_data(v1);
  DELAY();
  check_busy();
  wr_data(v2);
  DELAY();
  check_busy();
  wr_data(v3);
  DELAY();
  //装位置
  check_busy();
  wr_data(p1);
  DELAY();
  check_busy();
  wr_data(p2);
  DELAY();
  check_busy();
  wr_data(p3);
  DELAY();
  check_busy();
  wr_data(p4);
  DELAY();
}

//由于629选择的是位置模式，直线行走的条件是装入的位置值必须大于当前位置值
void run_stright(unsigned char v1,unsigned char v2,unsigned char v3,unsigned char p1,unsigned char p2,unsigned char p3,unsigned char p4)
{ 
  CtrlOUT&=~CSL;
  check_busy();
  wr_cmd(RSTI);
  DELAY();
  filter_sel();
  DELAY();
  traj_sel(0x00,0x0a,v1,v2,v3,p1,p2,p3,p4);
  DELAY();
  CtrlOUT|=CSL;
  
  CtrlOUT&=~CSR;//CS=0
  check_busy();
  wr_cmd(RSTI);
  DELAY();
  filter_sel();
  DELAY();
  traj_sel(0x00,0x0a,v1,v2,v3,p1,p2,p3,p4);
  DELAY();
  CtrlOUT|=CSR;//CS=1
  //让左右轮装载的数据同时有效，从而保证左右轮同步
  CtrlOUT&=~CSL;
  CtrlOUT&=~CSR;
  check_busy();
  wr_cmd(STT);
  DELAY();
  CtrlOUT|=CSL;
  CtrlOUT|=CSR;
}

void run_back(unsigned char v1,unsigned char v2,unsigned char v3,unsigned char p1,unsigned char p2,unsigned char p3,unsigned char p4)
{ 
  long temp,relpsl,relpsr,movps,loadl,loadr;
  unsigned char loadp1,loadp2,loadp3,loadp4,loadp5,loadp6,loadp7,loadp8,state_back;
  //读左轮实际位置
  CtrlOUT&=~CSL;
  check_busy();
  wr_cmd(RDRP);
  DELAY();
  check_busy();
  temp=rd_data();//读左轮实际位置(最高位)
  relpsl=temp<<24;
  DELAY();
  check_busy();
  temp=rd_data();//读实际位置
  relpsl|=temp<<16;
  DELAY();
  check_busy();
  temp=rd_data();//读实际位置
  relpsl|=temp<<8;
  check_busy();
  DELAY();
  temp=rd_data();//读实际位置(最低位)
  relpsl|=temp;//最终relps1存放的是30位左轮实际位置值(位置数据是30位有符号数,数据范围C0000000~3FFFFFFF)
  //处理
  movps=((unsigned long)p1<<24)+((unsigned long)p2<<16)+((unsigned long)p3<<8)+(unsigned long)p4;//p1,p2必须转换成无符号长整形，不然左移结果将不确定
  loadl=relpsl-movps;//后退数据必须小于当前数据
  loadp1=(loadl&0xff000000)>>24;
  loadp2=(loadl&0x00ff0000)>>16;
  loadp3=(loadl&0x0000ff00)>>8;
  loadp4=loadl&0x000000ff;
  //给左轮装入新的轨迹参数
  check_busy();
  wr_cmd(RSTI);
  DELAY();
  filter_sel();
  DELAY();
  traj_sel(0x00,0x0a,v1,v2,v3,loadp1,loadp2,loadp3,loadp4);
  DELAY();
  CtrlOUT|=CSL;
  //读右轮实际位置
  CtrlOUT&=~CSR;//CS=0
  check_busy();
  wr_cmd(RDRP);
  DELAY();
  check_busy();
  temp=rd_data();//读右轮实际位置(最高位)
  relpsr=temp<<24;
  DELAY();