飞思卡尔光电源代码

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中断服务***************************/

帐号		自动登录	找回密码
密码			立即注册

飞思卡尔光电源代码

评分

相关帖子