飛思卡爾光電源代碼

ID:122733 · 發表于 2016-5-23 10:29

//2米比較穩

#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;//電機轉速與給定轉速誤差
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數值臨時存放數組
word AD_QIUHE[1]=0;       //求和臨時存放數組
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  //復位,0復位
#define key_ok    PORTA_PA6  //1寫數據，0寫指令
#define key_out    PORTB_PB5  //數據
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;
/****延時**********************/
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延時函數
{
unsigned int i;
for (i=0;i<600;i++);

}

/***************自我保護函數**********************/

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) //定時中斷初始化函數 0.1MS定時中斷設置
{

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

//---------------------------------------------------------------------
// 函數功能采集CCD
// 形式參數：  無
// 函數返回值：無
//---------------------------------------------------------------------

/***************自適應曝光**********************/

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]);
}
}

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

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個點
  //AD配置成為8位精度，所以這里獲得的是一個字節數據
  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個點
  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);
  }

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

}

//---------------------------------------------------------------------
// 函數功能：配置單片機鎖相環，使其工作在40Mhz
// 形式參數：  無
// 函數返回值：無
//---------------------------------------------------------------------
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;
}

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

/////////////////////脈沖累加器初始化//////////////////////////////
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;//每一位對應相應通道中斷允許,0表示禁止中斷
  TIOS =0x00;//每一位對應通道的: 0輸入捕捉,1輸出比較
  TCTL3_EDG7x=1;//c-輸入捕捉7任何沿有效,
}

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

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

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

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

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

PWME=0xff;       //啟動PWM輸出

}
/////////////////////////速度控制函數///////////////////////////
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; //黑線
         }
      }
////////////賽道檢測/////////////

      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;

}

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

  BusCLK_40M();  //初始化IO

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

EnableInterrupts;

  for(;;)
  {

_FEED_COP(); /* feeds the dog */

}
}

//******************1ms中斷服務***************************/

帳號		自動登錄	找回密碼
密碼			立即注冊

久久久久久久999_99精品久久精品一区二区爱城_成人欧美一区二区三区在线播放_国产精品日本一区二区不卡视频_国产午夜视频_欧美精品在线观看免费

飛思卡爾光電源代碼

評分

相關帖子