跳舞小说作品:我写的2812PID+无线+串口源程序 - liaoyunlong888的日志 - 网易博...

#define INA1 GpioDataRegs.GPADAT.bit.GPIOA11
#define INA2 GpioDataRegs.GPADAT.bit.GPIOA12
#define INB1 GpioDataRegs.GPADAT.bit.GPIOA13
#define INB2 GpioDataRegs.GPADAT.bit.GPIOA14
#define CS GpioDataRegs.GPBDAT.bit.GPIOB0
#define CE GpioDataRegs.GPBDAT.bit.GPIOB1
//#define CLK GpioDataRegs.GPADAT.bit.GPIOA2
#define CLK GpioDataRegs.GPFDAT.bit.GPIOF2
//#define DATA GpioDataRegs.GPFDAT.bit.GPIOF4
#define DATA GpioDataRegs.GPFDAT.bit.GPIOF1
//#define DR GpioDataRegs.GPFDAT.bit.GPIOF5
#define DR1 GpioDataRegs.GPEDAT.bit.GPIOE1
#define NOP asm(" nop")
//PID

int   Len,Len_1,Ren,Ren_1;                         //  左右轮误差量
int   Lyn,Lyn_1,Lyn_2,Ryn,Ryn_1,Ryn_2;             //  左右轮反馈值
int   Lun,Lun_1,Run,Run_1;                         //  左右轮输出值
int   D_Lunp,D_Luni,D_Lund,D_Runp,D_Runi,D_Rund;  
int  Lkp,Rkp;                                     //  左右轮比例系数
int  Lki,Rki;                                     //  左右轮积分系数
int  Lkd,Rkd;
int  ski,hki;
int vl=0;
int vr=0;
unsigned int speed_measure;  //采样完成标志,通讯完成标
unsigned int comm_old[2];
int maxv=255;
int speedrun[3000];
//发射模块部分
unsigned int InitWord[15]={
       0xB8,0xB8,      //通道2、通道1 数据包长度 11V11,23字节
       0x00,0x00,0x55,0x55,0xff,  //通道2地址
       0x00,0x00,0x55,0x55,0xff,  //通道1地址
       0x63,       //24位地址（位7-2），16位CRC校验（位1）,使能CRC（位0）
       0x6f,       //one recieve(Bit7,0),Shock Mode(Bit6,1),Data rate(Bit 5)1M,Crystall(Bit4-2,011),RFPower(Bit1-0)
       0x65       //channal select(Bit7-1)2450M,RX/TX mode(Bit0,1 reciever mode)
       };
int CMRtemp,bflag;
int new_comm=0;
int err=0;
int Rxword[23]={0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};  //11辆小车左右轮速数组
int address=0;   //初始车号
int channel=50; //初始频道号
int comvr,comvl;
int number=0;
int counter=0;
int newset=0;

void Sys_int();
void Sysclk_int();
void IO_int();
void Eva_int();
void wat_int();
void PID();
void PWM_out();
void Init_recieve();
void Delay(unsigned int timer);
void Clock();
void SCI_int();
interrupt void T3pint_isr(void);
interrupt void XINT2_isr(void);
interrupt void SCIRXINTA_ISR(void);
//PWM_out
void main()
{
  Sys_int();  
//PID参0数值
  Lkp =80;//左轮速初使给定P
  Rkp =60;//右轮速初使给定P
  Lkd =30;//右轮速初使给定D
  Rkd =100;//右轮速初使给定D
  hki=10;
  ski=2;
  speed_measure=0;
 
   Len=0;
   Len_1=0;
   Ren=0;
   Ren_1=0;
 
   Lun=0;
   Lun_1=0;
   Run=0;
   Run_1=0;
 
   Lyn=0;
   Lyn_1=0;
   Lyn_2=0;
   Ryn=0;
   Ryn_1=0;
   Ryn_2=0; 
  comm_old[0]=0;
  comm_old[1]=0;

  EvaRegs.T1CON.bit.TENABLE=1;//使能定时器1~4
  EvaRegs.T2CON.bit.TENABLE=1;
  EvbRegs.T3CON.bit.TENABLE=1;
  EvbRegs.T4CON.bit.TENABLE=1;
  DINT;
  IER = 0x0000;
  IFR = 0x0000;
  /*设置中断服务程序入口地址*/ 
  EALLOW; 
  PieVectTable.T3PINT = &T3pint_isr;
  PieVectTable.XINT2 = &XINT2_isr;
  PieVectTable.RXAINT =&SCIRXINTA_ISR;
  EDIS;
  XIntruptRegs.XINT2CR.all=0x0005; //DR1外部中断2使能上升沿  
/*开中断*/
  IER |= M_INT4;
  IER |= M_INT1; //？？？？？
  IER |= M_INT9;
  PieCtrl.PIEIER4.bit.INTx4 = 1;//使能中断T3PINT
  PieCtrl.PIEIER1.bit.INTx5 = 1;//使能中断XINT1
  PieCtrl.PIEIER9.bit.INTx1 = 1;//使能中断RX
  EINT;  
  ERTM;
  while(1)
  {
     EALLOW;
       SysCtrlRegs.WDCNTR=0x0055;//看门狗计数器清零
     SysCtrlRegs.WDCNTR=0x00AA;
     EDIS;
  if(SciaRegs.SCIRXST.bit.RXRDY == 1)
       {
          counter++;
         }
  if(speed_measure==1)
  {
    /* if (k<1000)//测试用，记录左右轮速
    {
      speedrun[k]=vr;k++;
    }*/
      if (new_comm == 1)
  {   new_comm =0;
    if (Rxword[2*address + 2] > 128)   comvr = Rxword[2*address + 2] - 256 ;
    else                     comvr = Rxword[2*address + 2];

    if (Rxword[2*address + 1] > 128)   comvl = Rxword[2*address + 1] - 256 ;
    else                     comvl = Rxword[2*address + 1];

    if(comvl == 81)   comvl = 0;
    if(comvr == 81)   comvr = 0;

       comm_old[0]=comvr;
    comm_old[1]=comvl;
  }     
    speed_measure=0;
    PID();
    DINT;//关中断     
       PWM_out();
       EINT;//开中断
  } //运行110/75（us）=1.467us，一个采样周期大约能采样到120个脉冲。
 
  }
}  //END MAIN

void Sys_int()
{
  DINT;
  SysCtrlRegs.WDCR= 0x0068;  //屏蔽看门狗,使用晶振时钟OSCCLK/512(WDCR.2~0)
  Sysclk_int();
  InitPieCtrl();
  InitPieVectTable();
  IO_int();
  Eva_int();
  SCI_int();
  Init_recieve();
  InitXintf();
  wat_int();
  SysCtrlRegs.WDCR=0x0028;//使能看门狗(WDCR.6)
  EINT;//开放全局中断
}
//晶振初始化；外接30MHz晶体,初始化锁相环：外部晶体5倍频150MHz。
void Sysclk_int()
{
  unsigned int i;
  EALLOW;
  SysCtrlRegs.WDCR= 0x0068; 
  SysCtrlRegs.PLLCR=0xA;  //SYSCLKOUT=5*30MHz=150MHz
  //等待锁相环稳定
  asm ("NOP");
  for(i= 0; i< 5000; i++){}

  SysCtrlRegs.HISPCP.all=0x0001;//HSPCLK=SYSCLKOUT=75M
  SysCtrlRegs.LOSPCP.all=0x0002;//LSPCLK=SYSCLKOUT/4=37.5M
  SysCtrlRegs.PCLKCR.bit.EVAENCLK=1;//使能EVA外设中的高速时钟HSPCLK
  SysCtrlRegs.PCLKCR.bit.EVBENCLK=1;
  SysCtrlRegs.PCLKCR.bit.SCIENCLKA=1;//使能SCI模块的LSPCLK时钟
  EDIS;
}
//看门狗初始化：
void wat_int()
{
  EALLOW;
  SysCtrlRegs.WDCNTR=0x0055;//看门狗计数器清零
  SysCtrlRegs.WDCNTR=0x00AA;
  EDIS; 
} 

//IO口初始化：
void IO_int()
{
     EALLOW;
//pwm1,3
 GpioMuxRegs.GPAMUX.bit.PWM1_GPIOA0 =1;
 GpioMuxRegs.GPAMUX.bit.PWM3_GPIOA2 =1;

//QEP1,2,3,4
    GpioMuxRegs.GPAMUX.bit.CAP1Q1_GPIOA8 =1;
 GpioMuxRegs.GPAMUX.bit.CAP2Q2_GPIOA9 =1;
 GpioMuxRegs.GPBMUX.bit.CAP4Q1_GPIOB8 =1;
 GpioMuxRegs.GPBMUX.bit.CAP5Q2_GPIOB9 =1;

//INA1
 GpioMuxRegs.GPAMUX.bit.TDIRA_GPIOA11 =0;//配置成I/O口
 GpioMuxRegs.GPADIR.bit.GPIOA11 =1;//配置成输出 
//INA2
 GpioMuxRegs.GPAMUX.bit.TCLKINA_GPIOA12 =0;
 GpioMuxRegs.GPADIR.bit.GPIOA12 =1;
//INB1
 GpioMuxRegs.GPAMUX.bit.C1TRIP_GPIOA13 = 0;
 GpioMuxRegs.GPADIR.bit.GPIOA13 =1;
//INB2
 GpioMuxRegs.GPAMUX.bit.C2TRIP_GPIOA14 =0;
 GpioMuxRegs.GPADIR.bit.GPIOA14 =1;

//CE
 GpioMuxRegs.GPBMUX.bit.PWM8_GPIOB1 =0;//配置成I/O口
 GpioMuxRegs.GPBDIR.bit.GPIOB1 =1;//配置成输出
//CS
 GpioMuxRegs.GPBMUX.bit.PWM7_GPIOB0 =0;//配置成I/O口
 GpioMuxRegs.GPBDIR.bit.GPIOB0 =1;//配置成输出
//DATA
 GpioMuxRegs.GPFMUX.bit.SPISOMIA_GPIOF1 =0;//配置成I/O口
// GpioMuxRegs.GPFDIR.bit.GPIOF1=1;//配置成输出
//CLK
 GpioMuxRegs.GPFMUX.bit.SPICLKA_GPIOF2 =0;//配置成I/O口
 GpioMuxRegs.GPFDIR.bit.GPIOF2=1;//配置成输出
//DR
    GpioMuxRegs.GPEMUX.bit.XINT2_ADCSOC_GPIOE1 = 1;//使能I/O口第二功能
 GpioMuxRegs.GPEDIR.bit.GPIOE1 =0;//配置成输入
//串口初始化
    GpioMuxRegs.GPFMUX.bit.SCITXDA_GPIOF4 = 1;
 GpioMuxRegs.GPFMUX.bit.SCIRXDA_GPIOF5 = 1;
     EDIS;

}
//EVA初始化：（1）通用定时器1初始化：连续增模式，2分频。通用定时器2，4初始化：定向增减模式，不分频，QEP电路作为时钟
           //     通用定时器3用作采样周期，周期中断，连续增模式，内部高频时钟时钟0.4233*75Mhz*1000=0x7c03;
           //（2）比较单元1,2初始化：比较器输出方式是高有效。
           //（3）QEP单元初始化：检测上升沿。
void Eva_int()
{
    //T1 PWM
  EvaRegs.GPTCONA.all = 0x0000;
  EvaRegs.T1CNT=0x0000;
  EvaRegs.T1PR=0x00FF;//PWM周期值
  EvaRegs.T1CON.all=0x1100;//连续增模式T1CON（12，11），2分频T1CON（10~8)
    //T3采样周期
  EvbRegs.GPTCONB.all= 0x0000;
  EvbRegs.T3PR=0x7C03;//采样周期值0.4233ms
  EvbRegs.T3CNT=0x0000;//计数初值
  EvbRegs.T3CON.all=0x1000;//连增模式，内部高频时钟时钟，关定时器，
     
  //QEP通用定时器2,4初始化
  EvaRegs.T2CNT=0x0BB8;
  EvaRegs.T2PR=0x0FF0;
  EvaRegs.T2CON.all=0x1870;//内部高频时钟2分频，定向增减模式。

  EvbRegs.T4CNT=0x0BB8;
  EvbRegs.T4PR=0x0FF0;
  EvbRegs.T4CON.all=0x1830;
  //EVA完全比较单元1，2初始化
  EvaRegs.COMCONA.all=0x8260;//使能比较器COMCONA（15），使能全比较器COMCONA（9），使能全比较器1，2COMCONA（7~5）.
  EvaRegs.CMPR1 = 0x0000;
  EvaRegs.CMPR2 = 0x0000;
  EvaRegs.ACTR.all = 0x00AA;//比较器输出引脚4~1（ACTRA7~0）输出方式为高电平有效。 
 
  //QEP1,2,3,4初始化
  EvaRegs.CAPCON.all=0x8000; //禁止捕获单元
  EvbRegs.CAPCONB.all=0x8000;

  //定器3周期中断；
  EvbRegs.EVBIFRA.bit.T3PINT = 1;//中断标志置零
  EvbRegs.EVBIMRA.bit.T3PINT = 1;//使能通用定时器3周期中断

  //PIE定时器3周期中断INT4.4使能
  PieCtrl.PIEIER4.bit.INTx4=1;
  //外部中断1使能，用于连接DRI判断是否有通信信息?
 // PieCtrl.PIEIER1.bit.INTx5=0;
}

void SCI_int(void)
{
  SciaRegs.SCICCR.all = 0x0007;//00000111,0 stop bit,EVEN Parity,0,IDLE Mode,8bit
  SciaRegs.SCICTL1.all = 0x0003;//TX,RX
  SciaRegs.SCICTL2.all = 0x0002;//允许RX中断
  SciaRegs.SCIHBAUD = 0x0001;//=9600
  SciaRegs.SCILBAUD = 0x00E7;//BRR=150M/4(9600*8)-1=1E7h
  SciaRegs.SCICTL1.all = 0x0023;//软件复位
  return;
}

interrupt void T3pint_isr(void)
{
  DINT;
 // EvaRegs.T2CON.bit.TENABLE=0;//关闭定时器2，4
  //EvbRegs.T4CON.bit.TENABLE=0;
 
  speed_measure=1;
 
  vl=EvbRegs.T4CNT-3000;
  vr=EvaRegs.T2CNT-3000;

  EvaRegs.T2CNT=3000;//计数值清零
  EvbRegs.T4CNT=3000;

 // EvaRegs.T2CON.bit.TENABLE=1;//打开定时器2，4
  //EvbRegs.T4CON.bit.TENABLE=1;
 
  EvbRegs.EVBIFRA.bit.T3PINT = 1;//中断标志置零
  PieCtrl.PIEACK.bit.ACK4 = 1;
  EINT;
  return;
}
void PWM_out()
{ 
//    Run=255;
//  Lun=0; 
  if (Run < 0)          // 左轮
  {
    EvaRegs.CMPR2=-Run;
    INA1 = 0;
    INA2 = 1;
  }
  else
  {
    EvaRegs.CMPR2=Run;
    INA1 = 1;
    INA2 = 0;
  }

  if (Lun < 0)          // 右轮
  {
    EvaRegs.CMPR1=-Lun;
    INB1 = 1;
    INB2 = 0;
  }
  else
  {
    EvaRegs.CMPR1=Lun;
    INB1 = 0;
    INB2 = 1;
  }
}

void PID()//PID算法
{  
  Lyn = vl;
 Ryn = vr;

 Len = comm_old[0] - Lyn;
 Ren = comm_old[1] - Ryn;

 if ( (Len >= 5) || (Len <= -5) )  Lki = ski;       //积分分离
 else Lki = hki;

 if ( (Ren >= 5) || (Ren <= -5) )  Rki = ski;
 else Rki = hki;

        D_Lunp = Lkp * (Len - Len_1);       //左轮PID计算
 D_Luni = (Lki * Len)/10;
 D_Lund = Lkd * (Lyn_2-2 * Lyn_1 + Lyn);
 Lun = Lun_1 + (D_Lunp + D_Luni + D_Lund)/2;

 D_Runp = Rkp * (Ren - Ren_1);       //右轮PID计算
 D_Runi = (Rki * Ren)/10;
 D_Rund = Rkd * (Ryn_2-2 * Ryn_1 + Ryn);
 Run = Run_1 + (D_Runp + D_Runi + D_Rund)/2;
////////PID算法结束/////////////////////////////////////////////////
   

 Len_1 = Len;
 Lun_1 = Lun;
 Lyn_2 = Lyn_1;
 Lyn_1 = Lyn;

 Ren_1 = Ren;
 Run_1 = Run;
 Ryn_2 = Ryn_1;
 Ryn_1 = Ryn;
 if (Lun > maxv)           Lun = maxv;
 if (Lun < -maxv)          Lun = -maxv;

 if (Run > maxv)         
 Run = maxv;
 if (Run < -maxv)         Run = -maxv;    
}
//延时子程序
void Delay(unsigned int timer) //延时timer=t（us）(t为延时的时间)*75Mhz/7-2
{
  unsigned int j;
  for(j=0;j  ;
}
void Init_recieve()
{
 int Bytesend,Bitsend;
    EALLOW;
 GpioMuxRegs.GPFDIR.bit.GPIOF1=1;
 EDIS;
 /**********配置接收模块*************/
 CE = 0;
 Delay(5);     //延时580ns,Td min 50ns
 CS = 1;      //配置模式，CS=1，CE=0
 CLK = 0;
 Delay(55);      //延时5320ns,Tcs2data min 5us
 for(Bytesend=0;Bytesend<15;Bytesend++)
 {
  CMRtemp = InitWord[Bytesend];
  for(Bitsend=0;Bitsend<8;Bitsend++)
  {
   //DATA = bflag;
   bflag=0x0080&CMRtemp;
   if(bflag)
   DATA=1;
   else
   DATA=0;
   Delay(5);   //653ns,Ts min 500ns,
   Clock();
   CMRtemp = CMRtemp<<1;

  } 
 }

 CS = 0;

 CE = 1;      //配置为工作模式

 Delay(2100);      //Tsby2rx,196us，max 202us
    EALLOW;
 GpioMuxRegs.GPFDIR.bit.GPIOF1=0;
 EDIS;
}
void Clock()
{
 CLK = 1;
 Delay(5);        //延时580ns，Th min 500ns
 CLK = 0;
  
}
interrupt void XINT2_isr(void)
{
   int ByteRx,BitRx,mj,m;
 DINT;
 Delay(550);      //51us,Td（min 50 us）+Tdr2clk（min 50 us）
 for(ByteRx=0;ByteRx<23;ByteRx++)
 {
  CMRtemp = 0;
  for(BitRx=0;BitRx<8;BitRx++)
  {
   CMRtemp = CMRtemp<<1;
   CLK = 1;
   Delay(5);
   m=DATA;
   CMRtemp |= m;
   CLK = 0;
   Delay(5);
  }
  Rxword[ByteRx] = CMRtemp;
 }
  if(Rxword[0]==86)
     new_comm = 1;    //左右轮速通讯完成标志

  if(Rxword[0]-address==228)
  {     mj=Rxword[1];
     if(mj==Rxword[2]&&mj<7)
          new_comm=2;
      else
        err=1;
  }
//  number=0;
    PieCtrl.PIEACK.bit.ACK1 =1;
 EINT; 
}
//串口中断
interrupt void SCIRXINTA_ISR(void)
{
  int UARTtemp;
 PieCtrl.PIEACK.bit.ACK9 =1;
 number++;
   UARTtemp = SciaRegs.SCIRXBUF.bit.RXDT;//把buffer的值放到一个变量中去，这个地方接的应当是一个命令字
  // SciaRegs.SCICTL1.bit.SLEEP=0; 
 switch (UARTtemp)//判断命令字
 {
  case 87:
   DINT;
   while(!SciaRegs.SCIRXST.bit.RXRDY)
    ;
   if(SciaRegs.SCIRXBUF.bit.RXDT==0xa7)
              {
               while(!SciaRegs.SCIRXST.bit.RXRDY)
    ;
       if(SciaRegs.SCIRXST.bit.RXRDY > 10)
         {
       SciaRegs.SCITXBUF = 0x55;  //发送配置错误信息:55H,地址配置错误
       while(!SciaRegs.SCICTL2.bit.TXRDY)
     ;
          }
        else
       address =SciaRegs.SCIRXBUF.bit.RXDT;
    }
    else
    { SciaRegs.SCITXBUF = 0x55;  //发送配置错误信息:55H,地址配置错误
        while(!SciaRegs.SCICTL2.bit.TXRDY)
           ;
    }
  
   while(!SciaRegs.SCIRXST.bit.RXRDY)
    ;
   if(SciaRegs.SCIRXST.bit.RXRDY > 124)
   {
    SciaRegs.SCITXBUF = 0x56;  //发送配置错误信息:56H,频道配置错误
    while(!SciaRegs.SCICTL2.bit.TXRDY)
     ;
   }
   else
   {
    SciaRegs.SCITXBUF = 0xff;  //发送配置成功写入信息:ffH
    while(!SciaRegs.SCICTL2.bit.TXRDY)
     ;
    channel = SciaRegs.SCIRXBUF.bit.RXDT; //送入频道字节
    InitWord[14] = (channel << 1) + 1;
    newset = 1;
   }
    EINT;
   break;
  case 88:    //返回当前地址、频道和电压限
   DINT;
   SciaRegs.SCITXBUF = address;
   while(!SciaRegs.SCICTL2.bit.TXRDY)
    ;
   SciaRegs.SCITXBUF = channel;
   while(!SciaRegs.SCICTL2.bit.TXRDY)
    ;
 
    EINT;
   break;
  case 86:    //速度闭环测试，接收左右轮速
   DINT;
   while(!SciaRegs.SCIRXST.bit.RXRDY)
    ;
   comm_old[0] = SciaRegs.SCITXBUF;//0是左轮

   while(!SciaRegs.SCIRXST.bit.RXRDY)
    ;
   comm_old[1] = SciaRegs.SCITXBUF;//1是左轮

    EINT;
 }
 return;
}