main.c

mc9s12dg128基于传感器的模糊算法实现

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

#pragma LINK_INFO DERIVATIVE "mc9s12dg128b"
//全局变量定义
unsigned int sensor_h=0;
unsigned int sensor_k=0;
unsigned int sensor_kh;
unsigned int head_buffer=0;
unsigned int body_buffer; 
unsigned int get_speed=0;
    int Hhead;
    int Bbody;
    int Center;
    int angle = 0;
   #pragma CODE_SEG NON_BANKED   
   interrupt void myISR(void)
     
     {
           TSCR2_TOI=0;
           TFLG2_TOF=1; 
           get_speed=PACN3;       //速度，20ms采一次
           PACN3=0;
           TSCR2_TOI=1;
     }
//子函数声名 
  void Make_Turn(void);
  void Init_PORTHK(void);
  void sensor_route(void);
  void Init_PLL(void);
  void Pwm_Init(void) ;
  void delay1s(long a); 
  void ECT_init(void) ;
//子函数定义
  void ECT_init(void)
  
  {
  TIOS=0x00;      //all input capture
  TSCR1_TFFCA=0;  //FAST FLAG CLEAR 
  TCTL4_EDG0A=0;  //rising edges only
  TCTL4_EDG0B=1; 
  TCTL4_EDG1A=0;
  TCTL4_EDG1B=1; 
  TCTL4_EDG2A=0;
  TCTL4_EDG2B=1; 
  TIE_C1I=0;      //场允许中断 
  TIE_C2I=0;      //行中断关
  TIE_C0I=0;      //黑线处理中断关
  //speed PT3初始化
  TCTL4_EDG3A=1;
  TCTL4_EDG3B=0;      //PT3上升沿捕捉 
  //PACN3=0;            //脉冲累加寄存器  
  PACTL_PAEN=0;       //关闭十六位计数器  
  ICPAR=0x08;         //使能PT3口8位计数功能  
  DLYCT=0x01;           //Delay 256 bus clock cycles,消除抖动    
  //ICSYS_PACMX=0;        //允许循环计数:溢出后自动装0重新运行  
  //ICPAR_PA3EN=0x08 ;     // 使能PT3脉冲累加器的保持寄存器   
  //TIE_C3I=1;           //
  
  //TSCR2_TOI=1;         //计时器中断使能
  TSCR2_PR=1;            //计时器预分频
  //TCNT=0;              //计数器初始化，从0开始计数
  TSCR1_TEN=1;         //timer enable
  }
   
  void Init_PORTHK(void)
  {
   DDRK = 0x00;     //八个端口设置为输入
   PUCR_PUPKE = 1;  //设置上拉电阻
   DDRH = 0x00;     //八个端口设置为输入
   RDRH = 0xff;     //降功率启动
   PERH = 0xff;     //设置上拉下拉功能
  }                                                      

//*初始化时钟*/   
 void Init_PLL(void){
   REFDV=0x01;//initiate the PLL clock
   SYNR=0x02; //Osc 16M,bus clock 24MHz
   while(!(CRGFLG&0x08)) {
          }//wait until steady
   CLKSEL=0x80;//select the PLLCLK
 }
 //软件延时
  void delay1s(long a) 
{
    long i;
    for (i=0;i<a;i++)
     asm("nop");
   }
//舵机与电机相应端口初始化   
 void Pwm_Init(void) 
 {
      
    PWMCTL_CON45=1;         //四五两个八位的通道结合成十六位的通道从五通道输出
    PWMCLK=0x0A;             //通道1选择时钟SA,通道3选择时钟SB,通道5选择时钟B
    PWMPRCLK=0x33;          //十六位输出选用1/8时钟频率,八位输出选用1/128时钟频率
    PWMSCLA=0x08;            // Clock A / (2 * PWMSCLA)
    PWMSCLB=0x08;            // Clock B / (2 * PWMSCLB)
     PWMPOL=0xf7;
    PWMPER45=60000;         //周期为20ms
    PWMDTY45=4500;          // 为了设定占空比占空比为1.5ms?初始1200
    PWMPER1=100;
    PWMDTY1=39;              //占空比20%
    PWMPER3=0;
    PWMDTY3=0;              //占空比80%
    PWME=0x2A;              //电机1,3通道,舵机5通道使能
   
    }
 //传感器搜寻算法
  void sensor_route(void) 
  { sensor_h = PTH;
    sensor_k = PORTK&0x3f;
       sensor_kh=   PORTK&0x01;
        head_buffer=0;
    body_buffer = (sensor_k>>1)%32;
    head_buffer =  0 + sensor_h ;
      if(sensor_kh)
         head_buffer+=256;
    //将传感器得到的二进制信号用于小车转向依据
    switch(head_buffer) 
    {
    case 0b011010111: Hhead = 0;  break;//处于中线上
    case 0b010110111: Hhead =-1;  break;//处于左1传感器
    case 0b001110111: Hhead =-2;  break;//处于左2传感器
    case 0b011110110: Hhead =-3;  break;//处于左3传感器
    case 0b111110111: Hhead =-4;  break;//处于左4传感器
    case 0b011100111: Hhead = 1;  break;//处于右1传感器
    case 0b011111111: Hhead = 2;  break;//处于右2传感器
    case 0b011110011: Hhead = 3;  break;//处于右3传感器
    case 0b011110101: Hhead = 4;  break;//处于右4传感器
    default:          break;
    }
   switch(body_buffer)
    {
      case  0b10010: Bbody = 0;  break;//处于中线上
      case  0b11110: Bbody =-1;  break;//处于左一位置
      case  0b00110: Bbody =-2;  break;//处于左二位置
      case  0b10100: Bbody = 1;  break;//处于右一位置
      case  0b10111: Bbody = 2;  break;//处于右二位置
    } 
  }

void Make_Turn(void)
 {

    if (Hhead ==0)
      {
          if (Bbody == -2)   angle=17;
          if (Bbody == -1)   angle=9;
          if (Bbody == 0)    angle=0;
          if (Bbody == 1)    angle=-9;
          if (Bbody == 2)    angle=-17;
          else angle=10*Hhead;
      }    
      
     if ((Hhead >= -2) && (Hhead < 0))
      {
          if (Bbody == -2)   angle=8;
          if (Bbody == -1)   angle=0;
          if (Bbody == 0)    angle=-9;
          if (Bbody == 1)    angle=-17;
          if (Bbody == 2)    angle=-24;
          else angle=10*Hhead;
       } 

    if ((Hhead >0) &&(Hhead <= 2) )
      {
          if (Bbody == -2)   angle=24;
          if (Bbody == -1)   angle=17;
          if (Bbody == 0)    angle=9;
          if (Bbody == 1)    angle=0;
          if (Bbody == 2)    angle=-8;
          else angle=10*Hhead;
      }
 
    if ((Hhead >=-4) &&(Hhead <= -3))
     {
        if( Hhead ==-3 )
        {
  
          if (Bbody == -2)   angle =-2;
          if (Bbody == -1)   angle=-11;
          if (Bbody == 0)    angle=-19;
          if (Bbody == 1)    angle=-26;
          if (Bbody == 2)    angle=-32;
          else angle=10*Hhead;
         } 
          
         if( Hhead ==-4 )
        {
  
          if (Bbody == -2)   angle=-9;
          if (Bbody == -1)   angle=-17;
          if (Bbody == 0)    angle=-25;
          if (Bbody == 1)    angle=-32;
          if (Bbody == 2)    angle=-37;
          else angle=10*Hhead;
         } 
      }
      
    if ((Hhead >=3) &&(Hhead <= 4))
     {
        if( Hhead ==3 )
        {
  
          if (Bbody == -2)   angle=32;
          if (Bbody == -1)   angle=26;
          if (Bbody == 0)    angle=19;
          if (Bbody == 1)    angle=11;
          if (Bbody == 2)    angle=2;
          else angle=10*Hhead;
         } 
          
         if( Hhead ==4 )
        {
  
          if (Bbody == -2)   angle=37;
          if (Bbody == -1)   angle=32;
          if (Bbody == 0)    angle=25;
          if (Bbody == 1)    angle=17;
          if (Bbody == 2)    angle=9;
          else angle = 10*Hhead;
         } 
      }  
      if( (angle>=25)|| (angle<=-25) )
       PWMDTY1 = 30;              //占空比20%
      else  
      PWMDTY1 = 45;              //占空比40%
     PWMDTY45 = 4600 + 25*angle;
     delay1s(1000);
 }
 
void main(void)
 {  
    Init_PLL();
    ECT_init();
    Init_PORTHK();
     Pwm_Init();
    sensor_route();
  //   TSCR2_TOI=1; 
  EnableInterrupts;

  for(;;) 
  {
     sensor_route();
     Make_Turn() ;
  } /* wait forever */
  /* please make sure that you never leave this function */
}