求红外线传感器的程序

sjb3345514

如题

821316166

#include <hidef.h>      /* common defines and macros */
#include <MC9S12XS128.h>     /* derivative information */
#pragma LINK_INFO DERIVATIVE "mc9s12xs128"
      
//static unsigned char direction_turn[12]={888,1098,1208,1328,1481,1612,1730,1856,1988,2100,2222,2368};
//Chapter 12
//Periodic Interrupt Timer (S12PIT24B4CV1)   Page349
//The PIT module has no external pins.
//PIT 模式没有外部引脚
//========================================================================================

unsigned char light=0;  //激光管检测标志
unsigned short turn_value=0;  //转向的PWM数值
unsigned short direction_turn[7]={333,430,560,647,705,780,888}; //转向给定值初始化
short speed_set[7]={250,300,350,400,350,300,250};   //速度给定值
short speed_flag=0;   //速度档位标志位  
short speed[3]={0,0,0};  //速度检测函数
short pulse_count=0;     //编码器脉冲计数值
short speed_expect=0;   //理想速度
short kp=2;    //比例环节
short ki=0;    //积分环节
short kd=1;    //微分环节
short ek1=0;    //误差1
short ek2=0;    //误差2
short ek3=0;    //误差3
short speed_add=0;   //速度增量

//=======================================================================================


void PLL_Init() //时钟初始化
{
   
   REFDV=0x81;                     /* PLLCLK=2*OSCCLK*(SYNR+1)/(REFDV+1)*/
   SYNR=2;                      /* 锁相环时钟=2*16*(2+1)/(1+1)=48MHZ */
   while(!(CRGFLG&0x08));       /* 总线时钟=48/2=24MHZ */
   CLKSEL=0x80;                    

}
void PWM_Init() //PWM初始化
{        

   
   PWME=0x00;         //关闭PWM使能
   PWMPRCLK=0x66;     //A,B时钟均为总线的64分频,375KHZ
   //PWMSCLA=0x01;      //clockSA=clockA/(2*PWMSCLA) = 1500KHZ
   //PWMSCLB=0X01;      //clockSB=clockB/(2*PWMSCLB) =1500KHZ
   PWMCLK=0x00;      
   PWMPOL=0xFF;       //PWM输出起始电平为高电平
   PWMCAE=0x00;       //输出左对齐
   PWMCTL=0xf0;       //通道01，23，45，67级联
   
   PWMPER01=5999;    //舵机频率为62.5Hz
   PWMDTY01=647;     //占空比a=（PWMDTY01+1）/(PWMPER01+1)   
   
   PWMPER23=1000;     //PWM通道3周期为375HZ  
   PWMDTY23=0;        //占空比a=（PWMDTY01+1）/(PWMPER01+1)占空比50% ~~ 150
   
   PWMPER45=1000;      //PWM通道5周期为0.10ms 10KZH 300=0.00010/(1/3000000)
   PWMDTY45=300;      //占空比a=（PWMDTY01+1）/(PWMPER01+1)
   
   PWMPER67=375;    //频率为1000Hz
   PWMDTY67=200;     //
   

   PWME=0xff;         //使能pwm
}
void  Pit0_Init()    //PIT初始化
{
  PITCFLMT_PITE=0;          //关PIT使能  
  PITCE_PCE0=1;             //通道0使能
  PITMUX_PMUX0=0;           //通道0接微时钟0
  PITMTLD0=99;            //微时钟0值设置为7f
  PITLD0=3839;            //time-out period = (PITMTLD + 1) * (PITLD + 1) / fBUS.
                            //时间计算 100*3840/24000000=0.016s
  PITINTE_PINTE0=1;         //通道0中断时能
  PITCFLMT_PITE=1;         //PIT使能
}


void ECT_Init()
{
  
   TIOS=0x00;                  /* OC0路为输出比较，OC1路为输入捕捉 */
   TSCR2=0x06;                 /* 定时器溢出中断禁止，计数器自由运行禁止复位，64分频 */
   TSCR1=0x80;                 /* 定时器使能 */
   TIE=0x01;                   /* 输出比较相应中断使能 */
   TCTL4=0x01;
   
}

void dly_1ms()
{
int i,j;
for(i=0;i<200;i++)
{
  for(j=0;j<1000;j++){;}
}
}

  void sam_position() //车位检测函数
{
     int i=0,j=0;
     unsigned char m=0,n=0;
     n=PORTA;
     for(i;i<10;i++)
     {
        m=PORTA;
        if(n==m)
           j++;
     }
     if(j>6)
       light=n;
            
}

  void check_start()  //检测起始线
{  
   if((light&&4)||(light&&16))
       start_flag++;
}
  
  void check_start()  
{
   

  }
  
  void turning()    //舵机转向函数
{
   switch(light)
   {
    case 1:if(turn_value==direction_turn[1])  //出界判断算法
           {
              turn_value=direction_turn[0];
              speed_expect=speed_set[0];
           }
           else if(turn_value==direction_turn[0])
           {
                 turn_value=direction_turn[0];
                 speed_expect=speed_set[0];
           }
           break;
    case 2:if(turn_value==direction_turn[0])   
           {
            turn_value=direction_turn[1];
            speed_expect=speed_set[1];
           }
           else if(turn_value==direction_turn[1])   
           {
            turn_value=direction_turn[1];
            speed_expect=speed_set[1];
           }
           else if(turn_value==direction_turn[2])   
           {
            turn_value=direction_turn[1];
            speed_expect=speed_set[1];
           }
           break;
    case 4:if(turn_value==direction_turn[1])   
           {
            turn_value=direction_turn[2];
            speed_expect=speed_set[2];
           }
           else if(turn_value==direction_turn[2])   
           {
            turn_value=direction_turn[2];
            speed_expect=speed_set[2];
           }
           else if(turn_value==direction_turn[3])   
           {
            turn_value=direction_turn[2];
            speed_expect=speed_set[2];
           }
           break;
    case 8:if(turn_value==direction_turn[2])   
           {
            turn_value=direction_turn[3];
            speed_expect=speed_set[3];
           }
           else if(turn_value==direction_turn[3])   
           {
            turn_value=direction_turn[3];
            speed_expect=speed_set[3];
           }
           else if(turn_value==direction_turn[4])   
           {
            turn_value=direction_turn[3];
            speed_expect=speed_set[3];
           }
           break;
    case 16:if(turn_value==direction_turn[3])   
           {
            turn_value=direction_turn[4];
            speed_expect=speed_set[4];
           }
           else if(turn_value==direction_turn[4])   
           {
            turn_value=direction_turn[4];
            speed_expect=speed_set[4];
           }
           else if(turn_value==direction_turn[5])   
           {
            turn_value=direction_turn[4];
            speed_expect=speed_set[4];
           }
           break;
    case 32:if(turn_value==direction_turn[4])   
           {
            turn_value=direction_turn[5];
            speed_expect=speed_set[5];
           }
           else if(turn_value==direction_turn[5])   
           {
            turn_value=direction_turn[5];
            speed_expect=speed_set[5];
           }
           else if(turn_value==direction_turn[6])   
           {
            turn_value=direction_turn[5];
            speed_expect=speed_set[5];
           }
           break;
    case 64:if(turn_value==direction_turn[5])  //出界判断算法
           {
              turn_value=direction_turn[6];
              speed_expect=speed_set[6];
           }
           else if(turn_value==direction_turn[6])
           {
                 turn_value=direction_turn[6];
                 speed_expect=speed_set[6];
           }
           break;
    default:break;
   }
   PWMDTY01=turn_value;   

}     


void check_speed()     //速度检测函数
{
     ek3=ek2;   //计算速度差值
     ek2=ek1;
     ek1=speed_expect-pulse_count;
     speed[2]=speed[1];  //当前速度放在[0],之前放在[1],[2]
     speed[1]=speed[0];
     speed[0]=pulse_count;
     
     pulse_count=0;
}

void speed_down()    //制动函数
{
     PWMDTY23=300;   //电机反向供电
     PWMDTY45=0;
     
}
  
  void speed_pid()  //PID算法
{
    speed_add=kp*(ek1-ek2)+ki*ek1+kd*(ek1-2*ek2+ek3);  //PID增量式
  
}

  void driver()     //驱动电机控制函数
{   
      
      //if(((turn_value>705)||(turn_value<560))&&(speed[0]>200)) //当前速度若远超给定速度
      //  {
          //speed_down();
      //  }
      //else
      // {
         PWMDTY23=0;
         speed_pid();
         PWMDTY45=PWMDTY45+speed_add;
      // }
      if(PWMDTY45>600)
         PWMDTY45=600;
     


  
}


  void main()
{
   
   DisableInterrupts;           /* 关中断 */
   PLL_Init();
   PWM_Init();
   Pit0_Init();
   ECT_Init();
   turn_value=direction_turn[3];
   DDRA=0x00;
   DDRB=0xFF;
   PORTB=0X00;
   EnableInterrupts;     
   for(;;)
   {     
      //sam_position();
      //turning();
  
   }
}

#pragma CODE_SEG NON_BANKED

void interrupt 8 Timer0_ISR(void)
{
   
    pulse_count++;
    TFLG1_C0F=1;                 /* TC0端有中断产生 */
   
}

void interrupt 66 PIT0_ISR(void)                        
{                                   
   sam_position();     //车位检测函数
   turning();          //舵机转向函数
   check_speed();     //速度检测函数
   driver();         // 驱动电机控制函数
   PITTF_PTF0=1;        /* PIT0端有中断产生,清除标志位 */
}