match.c

飞思卡尔智能车例程

#include <hidef.h>      /* common defines and macros */
#include <mc9s12dg128.h>     /* derivative information */
#include "lib.c"     
#include "funtion.h"

#define Vmax 1800    //设置最大占空比为1800
#define Vmin 300       //设置最小占空比为300

#define DV  1000        //最大误差限制值

#define P   320        //PID 的比例 
#define I   10         //PID 的积分
#define D   20         //PID 的微分

volatile uchar m_en;    //比赛使能,=1可以前进,=0则停车

volatile uint p0=0;       //PWM 输出值
volatile int SV=0;        //车速设定值
volatile int e[3]={0,0,0};   //PID偏差
volatile int ms[3]={0,0,0};  //最近三次的测量值
volatile int sp[15]={0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};  //保存前三的的车速
volatile uchar spd=0;        //当前车速实测值
volatile uint t0;            //记录上一次的时间
volatile uint p0;            //上一次PID结果
                             
                             
interrupt 14 void ch6(void)   //通道6输入捕捉中断
{
 uchar tmp;
 int t1;
 int tt;
 long ptt=0;
 long itt=0;
 long dtt=0;      //PID 临时值
 long pidt;
 uint p1=0,p2=0;
 TFLG1=0x40;    //清中断标志
// PORTB=0xff;
 tt=TC6;   //读取当前捕捉值
 if(tt>=t0)
    t1=tt-t0;
 else
    t1=50000-t0+tt;    //越界
 t0=tt;                //保存当前时间
 n++;                   //每个脉冲,计数器加1
 
 if(m_en)       //比赛进行中
   {
   //车速计算公式 V=255254/t1  cm/S
    
    tt=255254/t1;    //求瞬时速度,经过最近1CM的速度
    t1=tt;
    for(tmp=0;tmp<15;tmp++)
      t1+=*(sp+tmp);
    t1>>=4;    //求平均速度,最近走16CM的平均速度
    
    for(tmp=14;tmp;tmp--)   //  保存最近16次车速
       *(sp+tmp)=*(sp+tmp-1);
    *sp=tt;
    
    *(ms+2)=*(ms+1);    //保存最近三次测量值
    *(ms+1)=*ms;
    *ms=t1;
    
    //微分先行     
    tt=t1+D*((*ms)-(2*(*(ms+1)))+(*(ms+2)))/100;
    
    *(e+2)=*(e+1);
    *(e+1)=*e;     //保存最近三次误差
    *e=SV-tt;       //计算误差
    
    spd=t1>>1;   
       
     //当前车速   
         
    if(m_en>1)    //第一次进入,不进行控制
       m_en--;
     
    else     //以后
     {
                 
  /*   if((*e)<-30)    //车速过快, 刹车
       { p2=0; BR_s; } 
    else if((*e)<-25) 
       { BR_c; p2=0;} 
    else if((*e)>60)   //车速过慢,强制加速   
       { BR_c; p2=1800; }
    else if((*e)>45)   //车速过慢,强制加速   
       { BR_c; p2=p1+150; }
    else if((*e)>30) 
       { BR_c; p2=p1+100; } 
    else if((*e)>20) 
       { BR_c; p2=p1+50; }     
    else               //正常输出  
       { BR_c; p2=p1;}              */
       
     if(*e<-30)         //车速过快,刹车
         {
          BR_c;
          p1=Vmin;
         } 
     else if(*e<-15) 
        {
         p1=Vmin;
        }
         
     else if(*e>20)    //误差大于15时使用大的
        {
        ptt=20*((*e)-(*(e+1)));
        itt=24*(*e);
        //dtt=D*((*e)-(2*(*(e+1)))+(*(e+2))); //计算PID值
        pidt=(ptt+itt)>>4;
        p1=p0+pidt;
         
        } 
                    //*/
      
     else        //误差小使用PID调节
       {
        ptt=P*((*e)-(*(e+1)));
        itt=I*(*e);
        //dtt=D*((*e)-(2*(*(e+1)))+(*(e+2))); //计算PID值
        pidt=(ptt+itt)>>4;
        p1=p0+pidt;
       }
       
        
        if(p1>Vmax)         //限制最大速度
            p1=Vmax,p2=Vmax;  
        else if(p1<Vmin)    //限制最小速度
           p1=Vmin,p2=Vmax;             
     
      p0=p1;      //保存上一次值
      
    tmp=SCI0SR1;       //数据发到串口
    SCI0DRL=t1/2;   //((tt)&0x00ff);
     
    speed(p1);
     }
   }
 // PORTB=0x00; 
}


void match (void)  //比赛
{
 char tmp;
 char tmp1;
 int t1=0;
 uint i;
 uchar kv;
 
 uint dir_c0=0;
 uint dir_c1=0;      //flash 计数器
 
 wu(CN1[8],CN1[9],CN1[10],CN1[11]);

 delay(10);
 PIEH=0x83;     //enable interrupt
 
 SV=L_S;  //以最低设定速度作为初始速度
 
 p0=550;
 speed(550);
 
 delay(300);			//等待800mS
 m_en=20;      //使能
  
 while(m_en)   //loop
  { 
    if(vcrf)    //一场采集完成
      {
     //  PORTB|=0xff;
       vcrf=0;
       sg();    //取图象的重心
       gc();    //求场重心和曲率
       
     //  if((abs(*gx))<8)
          dir(15*(*gx));      
      /* else 
         {
          if(*us>4)      //超过3个不可见点
            dir(18*(*gx));
          else if(*us>2)
            dir(16*(*gx));
          else
            dir(14*(*gx));
         }              //*/
       for(i=0;i<16;i++) 
          buf[dir_c0++]=*(gg1+i);   //保存传感器读取的状态*/
        
       buf[dir_c0++]=*gx;         //保存当前场重心
              
       buf[dir_c0++]=spd;         //保存现时车速
              
       buf[dir_c0++]=*e/2;        //保存误差
              
       buf[dir_c0++]=p0>>8;
              
       buf[dir_c0++]=p0&0x00ff;    //保存PWM占空比,2个字节
             
     /*buf[dir_c0]=*us;         //保存不可见点数
       dir_c0++;
       
       buf[dir_c0]=cinc;
       dir_c0++;
       
       buf[dir_c0]=cdec;
       dir_c0++;              //*/
       
       for(i=0;i<11;i++)       //凑足128/N个字节(N为正整数)
         buf[dir_c0++]=0;         //其它地方填入0
                  
      /* for(i=0;i<16;i++) 
        {
         buf[dir_c0++]=*(gg1+i);
         //dir_c0++;
        }              //*/
      if(dir_c0==256)   //一个block 用完,保存
         {sent(1);dir_c0=264;}            //写入缓冲器1
      else if(dir_c0==392)  //将数据从缓冲器写入flash
         {load(1,dir_c1);dir_c1++;}      //写入dir_c1号 block
      else if(dir_c0==520)  //另一个block 用完,保存
         {sent(2);dir_c0=0;}
      else if(dir_c0==128)  //将数据从缓冲器写入flash
         {if(dir_c1) {load(2,dir_c1);dir_c1++;}} //第一次不写   */
      //   PORTB=0x00;   
    }
  }
 PIEH=0x10;     //关按键中断,仅开遥控中断
 dir(0);
 speed(0);        //停车
 wu(CN1[12],CN1[9],CN1[10],CN1[11]);

 //保存取后一个block
 delay(10);
 if(dir_c0<128)         //保存
  {
   load(2,dir_c1);
   dir_c1++;
   delay(10);
  } 
 for(i=dir_c0;i<520;i++)  //空的地方写70
    buf[i]=0x70;
    
 if(dir_c0<256) { sent(1); delay(10); }
 
 if(dir_c0<392) { load(1,dir_c1);dir_c1++;delay(10); }
 sent(2);           //BLOCK 2一定要写
 delay(10);
 load(2,dir_c1);
 dir_c1++;
 delay(10);
 PORTB=0xff;       //关LED
 
 while(kv!=7)   //
  {
   kv=key();
      
   if(kv==2||kv==9)     //传输数据
     {
      for(i=0;i<dir_c1;) 
        {
         d_out(i);      //将第i个block数据发到串口
         i+=2;
        }
    }
  }
}