|
|
//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í)鐘選擇寄存器, ClockSAlockSB
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ù)***************************/
|
評(píng)分
-
查看全部評(píng)分
|
QQ好友和群
QQ空間
騰訊微博
騰訊朋友
收藏
淘帖
頂
踩
