標(biāo)題: 飛思卡爾光電源代碼 [打印本頁(yè)]
作者: chosenone    時(shí)間: 2016-5-23 10:29
標(biāo)題: 飛思卡爾光電源代碼
//2米比較穩(wěn)

#include <hidef.h>           
#include "derivative.h"      
#include <MC9S12XS128.h>
#include  <math.h>

uchar  PIT_cnt=0;      
int  AD_number=20,left=0,right=128,zhongxian=64,zhongxianold=0,zuobian=19,youbian=110;            

char  dty3,dty4,dty6;
uchar dty11=0,dty21=0,yz=3,zuo=0,you=0,tb=0,m=0,n=0,shizi=0,qidian=0;
float dty5=0,dty7=0,D=0,D_last=0;
int   t1=0,t2=0,t3=0;

float PID1_P=7;
float PID1_D=80;



float P_speed=1.9;
float I_speed=2.3;
int E1,E2,E3;
float Up,Ud;
float carspeed_error=0;//電機(jī)轉(zhuǎn)速與給定轉(zhuǎn)速誤差
float carspeed_now=0;                          
float speed_other=140;
float CarSpeedUse=170;
int carspeed_flag;
int CarSpeed_give=0;//速度PWM輸入
int dtysu;
int carspeed_time0=0;
int carspeed_time1=0;
int carspeed_time2=0;

int stop_flage=0;


float P=5;
float I=3;               


float dty=0;        //   變量



word AD_CAIJI[1]=0;         //采集的AD數(shù)值臨時(shí)存放數(shù)組
word AD_QIUHE[1]=0;         //求和臨時(shí)存放數(shù)組
word AD_JUNZHI[1]=0;        //平均值

#define DTYMAX   245
#define DTYMIN   -245
#define TSL1401_SI(x)   (PORTA_PA0=(x))
#define TSL1401_CLK(x)  (PORTA_PA1=(x))

#define    key_up      PORTA_PA5  //片選
#define    key_down    PORTA_PA7  //復(fù)位,0復(fù)位
#define    key_ok      PORTA_PA6  //1寫數(shù)據(jù)，0寫指令
#define    key_out     PORTB_PB5  //數(shù)據(jù)                             
void TSL1401_GetLine(uchar *pixel);

int PIT_cnt2;
float Factor_LR;
ulong Pixels=0;
int mid =0;
char flage=0;
int qian,bai,shi,ge;
uchar integration_piont;
uchar keydty1;

int PixelAverageVoltageError = 0;
int sumci=0,xy=0;
ulong sum;

unsigned char IntegrationTime = 4;

uchar gPixel[128] = {0};
uchar zPixel[128] = {0};
uchar y[128] = {0};
char lujing[40]={0},lu=0;
int  ljsum=0;
int lj=0;
/****延時(shí)**********************/
void delay(int us)
{
  unsigned int i;
  for(i=0;i<us;i++)
  {
    asm(nop);
  }
}

void DelayMs(int ms)
{
  short x,y;
  for(x=0;x<4106;x++)
  {
    for(y=ms;y>0;y--)
    {
    }
  }
}
void delay_1ms(void)//1ms延時(shí)函數(shù)
{
        unsigned int i;
        for (i=0;i<600;i++);

}

/***************自我保護(hù)函數(shù)**********************/

void CarSelfProtection(void)
{
    char i,k,s=0;
    k=0;
    for(i=19;i<110;i++)
        {
             if(zPixel[i]==0)
             {
              k++;
             }            
        }
        if(k>=125)
        s++;
        else s=0;
        if(s>=10)
        {  
            PWME=0x00;
            s=0;
        }
}


/********************PIT初始化******************/

void PIT_Init(void)   //定時(shí)中斷初始化函數(shù) 0.1MS定時(shí)中斷設(shè)置
{

    PITCFLMT_PITE=0; //定時(shí)中斷通道關(guān)     
    PITCE_PCE0=1;    //定時(shí)器通道 0使能  定時(shí)1ms用
    PITMUX_PMUX0=1;  //定時(shí)器通道 0選擇8位計(jì)數(shù)器1
    PITMTLD1=40-1;   //8位計(jì)數(shù)器0初值設(shè)定。40分頻，在 40MHzBusClock下，為 1MHz。即 1us.
    PITLD0=1000-1;   //16位計(jì)數(shù)器初值設(shè)定。1000*1us=1ms
    PITINTE_PINTE0=1;//開中斷，定時(shí)器中斷通道 0中斷使能   
    PITCFLMT_PITE=1; //定時(shí)器通道使能
}


//---------------------------------------------------------------------
// 函數(shù)功能采集CCD
// 形式參數(shù)：  無(wú)
// 函數(shù)返回值：無(wú)   
//---------------------------------------------------------------------


/***************自適應(yīng)曝光**********************/

void CalculateIntegrationTime(void)
{
    uint TargetPixelAverageVoltage = 40,max=0;
    unsigned char i=0;
    for(i=29;i<100;i++)
        {

             if(gPixel[i]>max)
             {
              max=gPixel[i];
             }
        }

    PixelAverageVoltageError = TargetPixelAverageVoltage - max;
    if(PixelAverageVoltageError < -6)
        IntegrationTime-=1;
    if(PixelAverageVoltageError > 6)
        IntegrationTime+=1;
    if(IntegrationTime <= 1)
        IntegrationTime = 1;
    if(IntegrationTime >= 4)
        IntegrationTime = 4;
}
/***************中值濾波**********************/
void Image_Filte(void)
{
   unsigned char *a_point,*b_point,*c_point;
   unsigned char a1,b1,c1,d1,j;     

      for(j=1;j<128;j++)
    {
      a_point=&gPixel[j-1];
      b_point=&gPixel[j];
      c_point=&gPixel[j+1];  
       a1=*a_point;
       b1=*b_point;
       c1=*c_point;   
    if(a1>=b1) {d1=b1;b1=a1;a1=d1;};   
    if(a1>=c1) {d1=c1;c1=a1;a1=d1;};
    if(b1>=c1) {d1=c1;c1=b1;b1=d1;};
     *(b_point)=b1;
    }

}
void DCT(void)
{
    uchar j;
    for(j=1;j<127;j++)
    {
      y[j]=(byte)abs(gPixel[j-1]-gPixel[j+1]);
    }
}

//----------------------------------------------------------------------
//函 數(shù) 名:TSL1401_GetLine                                                      
//功    能:獲得AD采樣像素                                       
//參    數(shù):*pixel  獲得的像素值                          
//返    回:無(wú)                                                                                       
//----------------------------------------------------------------------

void StartIntegration(void)
{
    uchar i;

    TSL1401_SI(1);         /* SI  = 1 */
    delay(8);
    TSL1401_CLK(1);        /* CLK = 1 */
    delay(8);
    TSL1401_SI(0);         /* SI  = 0 */
    delay(8);
    TSL1401_CLK(0);        /* CLK = 0 */

    for(i=0; i<127; i++)
    {
       delay(16);      
       TSL1401_CLK(1);    /* CLK = 1 */
       delay(16);
       TSL1401_CLK(0);    /* CLK = 0 */
    }
}




void TSL1401_GetLine(uchar *pixel)
{
  uchar i;

  //開始SI
  TSL1401_SI(0);
  TSL1401_CLK(0);
  TSL1401_SI(1);
  delay(8);
  TSL1401_CLK(1);
  delay(8);
  TSL1401_SI(0);
  delay(8);
  //采集第1個(gè)點(diǎn)
  //AD配置成為8位精度，所以這里獲得的是一個(gè)字節(jié)數(shù)據(jù)
  delay(40);
  while(!ATD0STAT0_SCF);
  //ad1=(byte)ATD0DR2L;
  //ad2=(byte)ATD0DR2H;
  //pixel[0] =ad1*2+ad2 ;
  pixel[0] =(byte)ATD0DR2;
  TSL1401_CLK(0);

  //采集第2~128個(gè)點(diǎn)
  for(i=1; i<128; i++)
  {
    delay(16);
    TSL1401_CLK(1);
    delay(16);
    while(!ATD0STAT2_CCF2);
    //ad1=(byte)ATD0DR2L;
    //ad2=(byte)ATD0DR2H;
    //pixel[i] =ad1*2+ad2 ;
    pixel[i] =(byte)ATD0DR2;
    TSL1401_CLK(0);
  }

  //發(fā)送第129個(gè)clk
   delay(8);
   TSL1401_CLK(1);
   delay(8);
   TSL1401_CLK(0);
   delay(8);
   Image_Filte() ;

}

//---------------------------------------------------------------------
// 函數(shù)功能：配置單片機(jī)鎖相環(huán)，使其工作在40Mhz
// 形式參數(shù)：  無(wú)
// 函數(shù)返回值：無(wú)   
//---------------------------------------------------------------------
void BusCLK_40M(void)
{   
    CLKSEL=0X00;                                //disengage PLL to system
    PLLCTL_PLLON=1;                        //turn on PLL
    SYNR =0xc0 | 0x04;                        
    REFDV=0x80 | 0x01;
    POSTDIV=0x00;       //pllclock=2*osc*(1+SYNR)/(1+REFDV)=80MHz;
    _asm(nop);          //BUS CLOCK=40M
    _asm(nop);
    while(!(CRGFLG_LOCK==1));          //when pll is steady ,then use it;
    CLKSEL_PLLSEL =1;                        //engage PLL to system;
}


//---------------------------------------------------------------------
//函 數(shù) 名:ADCInit                                                      
//功    能:A/D轉(zhuǎn)換初始化,設(shè)置A/D轉(zhuǎn)換時(shí)鐘頻率為1MHz                     
//參    數(shù):無(wú)                                                           
//返    回:無(wú)                                                           
//---------------------------------------------------------------------
/********************ATD初始化程序******************/
void AD_Init(void)
{
    ATD0CTL1=0b00000000;//   8 位精度
    ATD0CTL2=0b01000000;//   禁止外部觸發(fā),標(biāo)志位快速清零,中斷禁止
    ATD0CTL3=0b10011000;/*   7;右對(duì)齊無(wú)符號(hào);
                             6~3:轉(zhuǎn)換序列長(zhǎng)度為3;
                             No FIFO模式,Freeze模式下繼續(xù)轉(zhuǎn)換?  */
    ATD0CTL4=0b00000111;//   4AD采樣周期,ATDClock=[BusClock*0.5]/[PRS+1]  ; PRS=15, divider=32 ?
    ATD0CTL5=0b00110000;//   特殊通道禁止,多通道采樣,掃描模式連續(xù)采樣,開始為 AN0
    ATD0DIEN=0b00000000;//   禁止數(shù)字輸入   
}
//----------------------------------------------------------------------
//函 數(shù) 名:延時(shí)函數(shù) delay(), DelayMs()                                                     
//功    能:延時(shí)函數(shù)                                                              
//返    回:無(wú)                                                                                       
//----------------------------------------------------------------------


/////////////////////脈沖累加器初始化//////////////////////////////
void init_MC(void)
{
  PACTL=0X50;//PT7 PIN,PACN32 16BIT,NOT INTERRUPT
  TCTL3=0xc0;//c-輸入捕捉7任何沿有效,            
  TCTL4=0xc0;//0表示ICx禁止, 1表示上升沿, 2表示下降沿, 3表示任何沿      
  TIE  =0x00;//每一位對(duì)應(yīng)相應(yīng)通道中斷允許,0表示禁止中斷
  TIOS =0x00;//每一位對(duì)應(yīng)通道的: 0輸入捕捉,1輸出比較
  TCTL3_EDG7x=1;//c-輸入捕捉7任何沿有效,
}

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


    PWME=0xff;   //啟動(dòng)PWM輸出 */
    PWME=0x00;        // 禁止PWM輸出
    PWMCTL=0xf0;      //通道級(jí)聯(lián)
    PWMCAE=0X00;      //0左對(duì)齊 1中心對(duì)齊
    PWMPRCLK=0X00;    //2分頻
    PWMCLK =0XFF;      //時(shí)鐘選擇寄存器,  ClockSAlockSB   
    PWMSCLA=2;         //左右電機(jī)ClockSA=ClockA/(2*PWMSCLA)=20000k/(2*50)=20kHZ
    PWMSCLB=25;        //舵機(jī)用ClockSB=ClockA/(2*PWMSCLB)=20000k/(2*50)=200HZ
    PWMPOL =0XFF;       //輸出極性選擇 0起始為低電平 1起始為高電平


    PWMPER01=1000;     // 正傳    20khz                        
    PWMDTY01=200;        /*左對(duì)齊，起始輸出為高電平時(shí)，占空比=(PWMDTY3+1)/(PWMPER0+1) */   


    PWMPER45=1000;      //反轉(zhuǎn)    20khz                  
    PWMDTY45=0;        /*左對(duì)齊，起始輸出為高電平時(shí)，占空比=(PWMDTY3+1)/(PWMPER0+1) */

    PWMPER23=4000;      //  舵機(jī)   200hz                  
    PWMDTY23=1240;        /*左對(duì)齊，起始輸出為高電平時(shí)，占空比=(PWMDTY3+1)/(PWMPER0+1) */     

    PWME=0xff;          //啟動(dòng)PWM輸出


}
/////////////////////////速度控制函數(shù)///////////////////////////
void CarSpeedjust(void)
{




  E1=CarSpeedUse-carspeed_now;

  Up=E2-E1;

  E2=E1;

  CarSpeed_give=P_speed*Up+I_speed*E2;


  if(CarSpeed_give>=0)

  {

  if(CarSpeed_give>= 600) CarSpeed_give= 600;
  PWMDTY45=0;
  PWMDTY01=CarSpeed_give;
  if(CarSpeed_give<=10)
  CarSpeed_give=0;
  }
  else
  {

   if(CarSpeed_give<=-600) CarSpeed_give=-600;
   PWMDTY01=0 ;
   PWMDTY45=-CarSpeed_give;
   if(CarSpeed_give>=-10)
   CarSpeed_give=0;
  }




}
/////////////////////////方向控制?///////////////////////////

void BlackLine_Acquire(void)
{
    uchar i,j,l,p,temp;
    char kao=0;
    p=40;
    zuo=0;
    you=0;
    xy=0;
    DCT();
        for(l=19;l<110;l++)  
        {
           if(y[l]>yz)
           {
            zPixel[l]=1;//白線
            l+=7;
            xy++;
           }
           else
           {   
           zPixel[l]=0;   //黑線
           }
        }  
    ////////////賽道檢測(cè)/////////////

        for(i=zhongxian+1;i<youbian;i++)
        {
             if(y[i]>yz)
             {

                if(gPixel[i-3]-gPixel[i+3]>3)
                {

                  right=i;
                  you=1;
                  break;

                }
                else you=0;
             }
        }



        for(j=zhongxian;j>zuobian;j--)
        {                  
             if(y[j]>yz)
             {

              if(gPixel[j+3]-gPixel[j-3]>3)
               {

                  left=j;
                  zuo=1;
                  break;
               } else zuo=0;
             }
        }                  

        if(zuo==1&&you==1)
        {
            m=0;
            n=0;
            carspeed_flag=2;
            shizi=0;
            zhongxian=(left+right)/2;
            dty3=(right+left)/2-64;

        }

        if(zuo==0&&you==0)
        {

           carspeed_flag=0;

           if(gPixel[66]>21)
           {
              if(shizi>=10)
              {  
                  if(m==1)
                  {
                  n=1;  
                  zhongxian=78;
                  }
                  if(m==2)
                  {
                  n=2;  
                  zhongxian=50;
                  }
                  if(m==0)
                  zhongxian=64;
              } else zhongxian=64;
              dty3=0;
           }
           else
           {  
               if(dty6>23)
               {
                  zhongxian=109;
                  dty3=50;                 
               }
               else if(dty6<-23)
               {
                  zhongxian=19;
                  dty3=-50;  
               }
           }
        }
            temp=dty3-dty6;
        dty6=dty3;
        D=D/2+temp/2;


        dty4=abs(dty3);
        dty5=PID1_P*dty3+D*PID1_D;

        if(dty5>=210)dty5=210;      
        if(dty5<=-210)dty5=-210;  
        PWMDTY23=(int)dty5+1240;     




}

/////////////////////////主函數(shù)////////////////////////////////
void main(void)
{

  BusCLK_40M();  //初始化IO

  AD_Init(); //初始化ADC
  PIT_Init();
  init_MC();   
  PWM_Init();

        EnableInterrupts;

  for(;;)
  {




    _FEED_COP(); /* feeds the dog */



   }
}


//******************1ms中斷服務(wù)***************************/









歡迎光臨 (http://www.torrancerestoration.com/bbs/) Powered by Discuz! X3.1