unnormal

//---------------------------------------------------CCD

#defineodd_evenPTIT_PTIT3

#defineROW_START30//begintosamplefromlinestart

#defineROW_END240//endflagofsampling

unsignedcharfrm,open_frm,flag1,finish=0;

#defineROW_MAX12//最大取12行

#defineLINE_MAX52

unsignedcharimage_data[ROW_MAX][LINE_MAX];

//unsignedcharimage_simple[10];/////////装10个黑点，只处理10个

//charrow,line;

charrow;

unsignedintline;

unsignedchartemp;

unsignedintrowcount;

unsignedcharline_temp;

//-------------------------------------------CCD_PROCESS

#defineRMV_ROW2//抛弃前2行0-4行

#defineVALVE24//valvetodecideblacktrackorwhitetrack

unsignedcharblack_x[ROW_MAX];

#defineAbso_Center25//中心位置22

unsignedintcigma,curve;

unsignedintCurve_lev2=45;//大弯区分S直与曲线,直和S应该很小

unsignedintCurve_lev1=24;//小弯valvetodecidestraighttrackornot

unsignedcharaverage,Relative_Poz;

unsignedintsum;

charr_HEAD;

unsignedcharflag_fail=0,flag_BigCurv=0,flag_Straight=0,flag_Curv=0;//

charhead;

unsignedcharflag_black_x_normal;

#definenormal_black_x6//needforadjust,mayneedbelarger,udfortellcross/start_line/ring

frommiss_line

//--------------------------------------------PWMCONTROL

#definePWM_MID2813//2813//mid

//#definePWM_RIGHT3750//right

#definePWM_RIGHT3500//3600

//#definePWM_LEFT1875//left

#definePWM_LEFT2200

intspeed,lastspeed;

intsteer,laststeer;

intrev_speed,last_rev_speed;

intspeed_e,lastspeed_e;

intrev_speed_e,last_rev_speed_e;

//#definehigh_speed250//speedudonstraighttrack

intspeed_MAX;

intspeed_t_curve;

//#definelow_speed100//speedudontheturn

unsignedcharstand;

//unsignedintspeed_vari;

unsignedcharK_speed=1,K_steer=1;

//-------------------------------------------OTHERVAR

unsignedchara,b,c,x;

charm,n,i,j,s;

//unsignedcharStart_Flag=0,Circle=0,Circle_Count=2,BEGIN=0,STOP=0;//探测起跑线,一圈

停车

unsignedcharstart_count;

unsignedcharflag_start,flag_stop,flag_heap;

#definecircle2//run2circle

voidIRQ_ISR(void);

voidRTI_ISR(void);

intpa_count;

intspeed_t,rev_speed_t;

intKp=20,Kd=3;

#include

#include

#include"includes.h"

#pragmaLINK_INFODERIVATIVE"mc9s12xs128"

//--------------------------------------------------------------------------------------

//------initial----------initial---------------initial------------------initial------

//-----------------------------------------------------------------------------------

voidinit_PLL(void)//CPU总线时钟60M

{

REFDV=0x43;

SYNR=0xce;

POSTDIV=0x00;//busperiod=16Mhz*(SYNR+1)/(REFDV+1)=60M

while(0==CRGFLG_LOCK);//waitforVCOtostablize

CLKSEL=0x80;//openPLL

}

voidinit_PORT(void)//端口初始化

{

DDRT_DDRT3=0;//PT3作为奇偶场信号输入

DDRT_DDRT1=0;//PT1作为测速信号输入

//DDRJ_DDRJ6=1;//控制电机使能，输出

IRQCR=0xc0;//外部IRQ使能，行同步，下降沿

//IRQCR_IRQE=1;

//IRQCR_IRQEN=1;

DDRA=0x00;//A口作为速度调试输入

DDRB=0xff;

PORTB=0x01;

}

voidinit_PWM(void)

{

PWME=0x00;//pwmdisable

PWMPOL=0xff;//输出开场为高电平,当给定计数到来时变低电平

PWMCLK=0x00;//PWM0,1--A;PWM2,3--B;PWM4,5--A;

PWMPRCLK=0x55;//A=B=60M/32=

PWMCAE=0x00;//对齐方式：左对齐

PWMCTL_CON01=1;//PWM01合并16bit

PWMPER01=375;//PWM1Period=375/=0.2ms.f=5kHZ

PWMDTY01=258;//

PWME_PWME1=1;//电机enable

PWMCTL_CON23=1;//PWM23合并16bit

PWMPER23=375;//PWM3Period=37500/=20ms,f=50HZ

PWMDTY23=0;//thedutycycleis2813/37500=7.5%

PWME_PWME3=1;//舵机enable

PWMCTL_CON45=1;//PWM45合并16bit

PWMPER45=37500;//PWM5Period=1875/=1ms.f=5kHZ

PWMDTY45=PWM_MID;//

PWME_PWME5=1;//电机enable

}

voidinit_ECT(void)//定时器初始化??

{

TIOS=0;//通道0-7作为输入

TCNT=0x00;//计数器清零

TSCR1=0x80;//计数器使能

TCTL4=0x07;//设置通道0捕获上、下沿,通道1捕获上沿

PACTL=0X50;//PA使能，上升沿

}

voidinit_AD(void)

{

ATD0CTL1=0x00;//ExternaltriggesourceisAN0,8-bitdata,Nodischargebeforesampling

ATD0CTL2=0x60;//quickclearCCFx,continetotransformunderwaitmode,Disableexternal

trigger,ATDSequenceCompleteinterruptrequestsaredisabled,ATDCompareinterruptrequests

aredisabled

ATD0CTL3=0x88;//onetransforminonequence,NoFIFO,Rightjustied,contineto

transformunderfreezemode

ATD0CTL4=0x01;//fourclocks,ATDCLK=[BusClock*0.5]/[PRS+1]=15M

ATD0CTL5=0x20;//ContinuousConversionSequenceMode,AnalogInputChannelisAN0

ATD0DIEN=0x00;//inhibitdigitalinput

}

//-------------------------------------------------------------------------------------

//rtirealtimeinterrutinitial

//---120-7130-8.....160-11230-18300-23

//100-9

//------------------

staticvoidRTI_Init(void){

RTICTL=0xff;//8*2(16)scale32.75(16M)

CRGINT=0x80;//enable

}

//------

//------------------------------------------------------------------------------------------------

//----functions---------------functions-----------functions--------functions---------functions--------------

-----------------------------

//------------------------------------------------------------------------------------------------

unsignedintabs_sub(unsignedintnum1,unsignedintnum2)

{unsignedintdifference;

if(num1>=num2){

difference=num1-num2;

}el{

difference=num2-num1;

}

returndifference;

}

//--------

//delay

//--------

voiddelay(intDly_time)

{

for(i=0;i

}

//-----------------------------------------------------------------------------------

//-滤波

//---------------------------------------------------------------------------------------

unsignedcharget_mid(unsignedchara,unsignedcharb,unsignedcharc)

{

unsignedcharx=0;

if(a>b){x=b;b=a;a=x;}

if(b>c){x=c;c=b;b=x;}

if(a>c){x=c;c=a;a=x;}

returnb;

}

//-------------------------------------------------------

//jump_count

//-------------------------------------------------------

unsignedcharjump_6(unsignedcharRow){

unsignedinti1,i2,i3,i4,i5;

unsignedcharflag_6jump=0;

for(i1=LINE_MAX-1;i1>=10;i1--){//fori1

if(image_data[Row][i1-2]+VALVE

black

for(i2=i1-3;i2>=8;i2--){

if(image_data[Row][i2]+VALVE

for(i3=i2-2;i3>=10;i3--){

if(image_data[Row][i3-2]+VALVE

for(i4=i3-2;i4>=8;i4--){

if(image_data[Row][i4]+VALVE

for(i5=i4-2;i5>=10;i5--){

if(image_data[Row][i5-2]+VALVE

flag_6jump=1;//total5jumps

i1=8;i2=8;i3=8;i4=8;i5=8;//outlinescan

}//endjump5

}//endfor

}//endjump4

}//endfor

}//endjump3

}//endfor(j=i-2;j>10;j--)

}//endifjump2

}//endfor(i=k-3;i>8;i--)

}//endif(image_data[Row][k-2]+VALVE

}//endfori1

returnflag_6jump;

}//endjump_count

//-------------------------------------------------------------------

//--处理图像Process_ImgNEW

//--------------------------------------------------------------------

voidProcess_ImgNEW(void)//udtoprocessimage

{

unsignedcharflag_right_x,JumpCount;

unsignedchardistance=0,temp_flag=0;

unsignedchartemp_black;

head=1;

flag_BigCurv=0;

flag_Straight=0;

flag_fail=0;

flag_Curv=0;

flag_heap=0;

for(row=ROW_MAX-2;row

for(line=LINE_MAX-1;line>8;line--){

if(image_data[row][line]+VALVE

temp_black=line;

for(i=1;i<10;i++){

if(image_data[row][line-i]+VALVE

black_x[row]=temp_black;

distance=black_x[row]-distance;//第11行-第10行中点

i=10;

line=8;

temp_flag++;

}

}

}

}}

if(temp_flag!=2)flag_fail=1;//假如找不到边界

//endfor

if(flag_fail==0){//计算前面黑点

for(row=ROW_MAX-3;row>0;row--){//forrow

flag_right_x=0;

for(line=LINE_MAX-1;line>=10;line--){//forline

if(image_data[row][line-2]+VALVE

black_x[row]=line-2;//

flag_right_x=1;//findrightblack_x

line=8;//outlinescan

}

}//endforline

if(flag_right_x){//cafindrightblack_x

if(abs_sub(black_x[row],black_x[row+1])>normal_black_x)

flag_black_x_normal=0;//unnormal

el

flag_black_x_normal=1;//normal

}//endif(flag_right_x)

if(!flag_right_x){//canotfindrightblcak_x

if((black_x[row+1]>LINE_MAX-5)||(black_x[row+1]<12)){//thenearerrowgettingoutvisual

head=row+1;

row=0;//outrowscan

}el{//judgeasnoi,cross,orleftring

black_x[row]=black_x[row+1];

//row=row-1;//nextrow

}

}//endif(!flag_right_x)

if(!flag_black_x_normal){//judgeasrightring,startlineorcross

JumpCount=jump_6(row);

if(JumpCount){//judgeasstartline

start_count++;

black_x[row]=black_x[row+1];

if(!flag_start){//whennostartdothis

if(start_count>=1)

flag_start=1;//start

if(flag_start)start_count=0;//oncestart,start_countrestart

}//endif(!flag_start)

el{//whenhasstarteddothis

if(start_count==circle)

flag_stop=1;//stop

}

}//endif(JumpCount>=6)

el{//judgeascross

black_x[row]=black_x[row+1];

//row=row-1;//nextrow

}//endel

}//endif(!flag_black_x_normal)

}//endforrow

//滤波，得到滤波后每行黑线的中心位置

for(i=2;i

{

temp=get_mid(black_x[i-1],black_x[i],black_x[i+1]);

black_x[i]=temp;

}

sum=0;

for(row=ROW_MAX-1;row>=head;row--){//取黑线数组的第head行-最后一行计算中

心average

sum=sum+black_x[row];

}

average=sum/(ROW_MAX-head);

Relative_Poz=abs_sub(average,Abso_Center);

if((head>r_HEAD)&&(Relative_Poz<=5))

flag_heap=1;//上坡

if((head>r_HEAD)&&(Relative_Poz>3))

flag_BigCurv=1;//看不到头，并且相对位移大,是大弯

elif((Relative_Poz<=4)&&(black_x[head]>20)&&(black_x[head]<31))

flag_Straight=1;//相对位移小，可能是直线可能是小S

el{//

flag_Curv=1;//视野可见，但位移大,大S

sum=0;

for(row=ROW_MAX-1;row>=6;row--){//重计算中心average

sum=sum+black_x[row];

}

average=sum/(ROW_MAX-6);

}

}//endifnotfail

}

//---------------------------------------------------------------------

//re_tPID

//---------------------------------------------------------------------

voidre_tPID(void)

{

if(flag_BigCurv)

{

steer=PWM_MID+5*(average-Abso_Center)+80*(black_x[head]-Abso_Center);

speed_t=10;

speed_e=speed_t-pa_count;

if(pa_count-speed_t>=1){//bigrever

speed=0;

rev_speed=350;

}

elif((pa_count-speed_t>=0)&&(pa_count-speed_t<2)){//slow

speed=0;

rev_speed=last_rev_speed-2*Kp*speed_e-Kd*(speed_e-lastspeed_e);

if(rev_speed>speed_MAX)rev_speed=speed_MAX;

}

el{//rever

rev_speed=0;

speed=lastspeed+2*Kp*speed_e+Kd*(speed_e-lastspeed_e);

if(speed>speed_MAX)speed=speed_MAX;

}

if(pa_count<8)speed=speed_MAX;

lastspeed_e=speed_e;

//last_rev_speed_e=rev_speed_e;

}

if(flag_Straight){

speed=speed_MAX;//最大速度

rev_speed=0;

steer=PWM_MID;

lastspeed_e=0;

}

if(flag_fail){

//speed=lastspeed;steer=laststeer;//维持

speed=150;

if(laststeer-PWM_MID>100)

steer=laststeer+200;

elif(PWM_MID-laststeer>100)

steer=laststeer-200;

elsteer=laststeer;

}

if(flag_Curv){

steer=PWM_MID+10*(average-Abso_Center)+20*(black_x[head]-Abso_Center);//转舵

speed_t=speed_t_curve;

speed_e=speed_t-pa_count;

if(pa_count-speed_t>=0){//slow

speed=0;

rev_speed=last_rev_speed-Kp*speed_e-Kd*(speed_e-lastspeed_e);

if(rev_speed>speed_MAX)rev_speed=speed_MAX;

}

el{//rever

rev_speed=0;

speed=lastspeed+Kp*speed_e+Kd*(speed_e-lastspeed_e);

if(speed>speed_MAX)speed=speed_MAX;

}

lastspeed_e=speed_e;

//last_rev_speed_e=rev_speed_e;

}

if(flag_heap){

speed=speed_MAX;

steer=PWM_MID;

}

if(steer>PWM_RIGHT)steer=PWM_RIGHT;

if(steer

PWMDTY01=speed;

PWMDTY23=rev_speed;

PWMDTY45=steer;

lastspeed=speed;

last_rev_speed=rev_speed;

laststeer=steer;

}

voidkey_con(void){

switch(PORTA)

{

ca0b10000000:speed_MAX=160;speed_t_curve=14;r_HEAD=3;break;

ca0b11000000:speed_MAX=170;speed_t_curve=14;r_HEAD=3;break;

ca0b11100000:speed_MAX=180;speed_t_curve=14;r_HEAD=3;break;

ca0b11110000:speed_MAX=190;speed_t_curve=13;r_HEAD=3;break;

ca0b11111000:speed_MAX=200;speed_t_curve=13;r_HEAD=3;break;

ca0b11111100:speed_MAX=210;speed_t_curve=12;r_HEAD=2;break;

ca0b11111110:speed_MAX=220;speed_t_curve=12;r_HEAD=2;break;

ca0b11111111:speed_MAX=230;speed_t_curve=12;r_HEAD=2;break;

}

}

//----------------------------------------------------------------------------------------

//----main------------------main----------------------main----------------------main------

//-----------------------------------------------------------------------------------------

voidmain(void)

{

init_PLL();

init_PWM();

init_PORT();

init_ECT();

RTI_Init();

IRQCR=0x00;//diableIRQ

PWMDTY45=PWM_MID;

PWMDTY01=0;

EnableInterrupts;

PWMDTY01=100;

lastspeed=100;

pa_count=2;

flag_start=1;

start_count=0;

key_con();

while(1)

{

if(odd_even==1)

{

while(odd_even==1);

row=0;rowcount=0;

IRQCR=0xc0;

//EnableInterrupts;

while(rowcount<=ROW_END){

}

if(rowcount>ROW_END)

IRQCR=0x00;

//DisableInterrupts;

Process_ImgNEW();

re_tPID();

}

if(odd_even==0)

{

while(odd_even==0);

row=0;rowcount=0;

IRQCR=0xc0;

//EnableInterrupts;

while(rowcount<=ROW_END){

}

if(rowcount>ROW_END)

IRQCR=0x00;

//DisableInterrupts;

Process_ImgNEW();

re_tPID();

}

}

}

//----------------------------------------------------------------------------------------

//-----interrupt-------------interrupt------------------interrupt-----------------interrupt

//-----------------------------------------------------------------------------------------

#pragmaCODE_SEGNON_BANKED

voidinterrupt6IRQ_ISR()

{

rowcount++;

if((rowcount>ROW_START)&&(rowcount%17==0)&&(row

{

init_AD();

for(line=0;line

{

while(!ATD0STAT2_CCF0);

image_data[row][line]=ATD0DR0L;

}

row++;

ATD0CTL2=0x00;

}

}

//----------------------

//realtimeinterrupt

//-----------------------

voidinterrupt7RTI_ISR(void){

PORTB++;

CRGFLG|=0x80;

pa_count=PACNT;

PACNT=0x00;

}

//------------------------------end---------------end----------------------------------