飞思卡尔智能车主程序

#include /* common defines and macros */ #include /* derivative information */ #pragma LINK_INFO DERIVATIVE \"mc9s12db128b\" #include \"define.h\" #include \"init.h\"

// variable used in video process

volatile unsigned char image_data[ROW_MAX][LINE_MAX] ; // data array of picture unsigned char black_x[ROW_MAX] ; // 0ne-dimensional array unsigned char row ; // x-position of the array unsigned char line ; // y-position of the array unsigned int row_count ; // row counter

unsigned char line_sample ; // used to counter in AD unsigned char row_image ;

unsigned char line_temp ; // temperary variable used in data transfer

unsigned char sample_data[LINE_MAX] ; // used to save one-dimension array got in interruption

// variables below are used in speed measure

Unsigned char pulse[5] ; // used to save data in PA process Unsigned char counter; // temporary counter in Speed detect Unsigned char cur_speed; // current speed short stand; short data;

unsigned char curve ; // valve used to decide straight or turn short Bounds(short data); short FuzzyLogic(short stand);

/*----------------------------------------------------------------------------*\\ receive_sci

\\*----------------------------------------------------------------------------*/ unsigned char receive_sci(void) // receive data through sci { unsigned char sci_data; while(SCI0SR1_RDRF!=1); sci_data=SCI0DRL; return sci_data; }

/*----------------------------------------------------------------------------*\\ transmit_sci

\\*----------------------------------------------------------------------------*/

void transmit_sci(unsigned char transmit_data) // send data through sci {

while(SCI0SR1_TC!=1);

while(SCI0SR1_TDRE!=1); SCI0DRL=transmit_data; }

/***************************************************************************** ***/

/*----------------------------------------------------------------------------*\\ abs_sub

\\*----------------------------------------------------------------------------*/ unsigned char abs_sub(unsigned char num1, unsigned char num2) { unsigned char difference; if(num1>=num2){ difference=num1-num2; }else{

difference=num2-num1; }

return difference; }

void pwm_set(unsigned int dutycycle) {

PWMDTY1=dutycycle&0x00FF; PWMDTY0=dutycycle>>8; }

void get_black_wire(void) // used to extract black wire { unsigned char i;

for(row=0;rowfor(line=LINE_MIN;lineif(image_data[row][line]>image_data[row][line+3]+VALVE){ for(i=3;i<10;i++){

if(image_data[row][line+i]+VALVEline=LINE_MAX; } else{

//black_x[row]=(black_x[row]/45)*78; } } } }

/*----------------------------------------------------------------------------*\\ speed_control

\\*----------------------------------------------------------------------------*/ void speed_control(void) {

unsigned int sum,average; sum=0;

for(row=0;rowaverage=sum/FIRST_FIVE; curve=0;

for(row=0;rowcurve=curve+abs_sub(black_x[row],average); if(curve>CURVE_MAX){ curve_flag=0; speed=low_speed;} else{ curve_flag=1; speed=hign_speed; } } }

/*----------------------------------------------------------------------------*\\ steer_control

\\*----------------------------------------------------------------------------*/ void steer_control(void) { unsigned int dutycycle; unsigned char video_center; unsigned int coefficient; int E,U; //current static int e=0;

video_center=(LINE_MIN+LINE_MAX)/2;

stand=abs_sub(black_x[1]+ black_x[9],2*black_x[5]); E=video_center-black_x[8];

coefficient=30+1*FuzzyLogic(stand); U=coefficient*E;

dutycycle=Bounds(center+U); pwm_set(dutycycle); }

// make sure it is within bounds short Bounds(short data){ if(data>right_limit){

data = right_limit; }

if(datareturn data; }

Void speed_get(void) {

Unsigned char temp; Counter++; Temp=PACN1;

cur_speed=temp-pulse[counter-1]; pulse[counter-1]=temp; if(counter==5) {

counter=0; } }

Void set_speed(unsigned char desired_speed) {

If(desired_speedPWMDTY2=low_speed; } Else {

PWMDTY2=high_speed; } }

/***************************************************************************** *\\ Main

\\***************************************************************************** */

void main(void) { // main functiion init_PORT() ; // port initialization init_PLL() ; // setting of the PLL init_ECT();

init_PWM() ;

// PWM INITIALIZATION init_SPEED() ; init_SCI() ; for(;;) {

if(field_signal==0){ // even->odd while(field_signal==0); row_count=0; row_image=0; EnableInterrupts;

while(row_countDisableInterrupts; }

else{ // odd->even while(field_signal==1); row_count=0; row_image=0; EnableInterrupts;

while(row_countDisableInterrupts; }

/* transmit_sci('x');

for(row=0;rowtransmit_sci(curve); */ } }

interrupt 6 void IRQ_ISR() {

row_count++;

if((row_count>ROW_START)&&(row_count%INTERVAL==0)&&(row_image{ init_AD();

for(line_sample=0;line_sampleATD0CTL2=0x00; row_image++; }

if((row_count>ROW_START)&&(row_count%INTERVAL==2)&&(row_imagefor(line_temp=0;line_tempimage_data[row_image-1][line_temp]=sample_data[line_temp]; } } }

/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// // THE END //

/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// Define.h // all macros are define in this header file

/*----------------------------------------------------------------------------*\\ macro need to be used in video sample

\\*----------------------------------------------------------------------------*/

////////////////////////////

#define signal_in ATD0DR0L // signal from video: right aligned mode， // only use low 8-bit in ATD Conversion Result Registers

#define field_signal PTT_PTT2 // field signal is sent into PortT_bit2 #define LINE_MIN 12 // first effective pint in each row

#define LINE_MAX 78 // number of points sampled in each row #define ROW_MAX 10 // number of rows needed to be sampled in each picture

#define ROW_START 50 // begin to sample from line start #define ROW_END 300 // end flag of sampling #define INTERVAL 20 // interval between effective rows #define VALVE 24 // valve to decide black track or white track #define FIRST_FIVE 5

/*----------------------------------------------------------------------------*\\ 舵机控制变量

\\*----------------------------------------------------------------------------*/

#define left_limit 7400 // #define center 00 // #define right_limit 5400 //

//#define coefficient 30 // (LEFT-RIGHT)/(LINE_MAX-LINE_MIN) /*----------------------------------------------------------------------------*\\ 速度控制变量

\\*----------------------------------------------------------------------------*/ #define curve_flag PORTE_BIT2 // indicate straight line or not #define speed PWMDTY2 // speed of the car

#define CURVE_MAX 24 // valve to decide straight track or not #define hign_speed 120 // speed used on straight track #define low_speed 100 // speed used on the turn

/*----------------------------------------------------------------------------*\\ define jump wire; code switch; Led

\\*----------------------------------------------------------------------------*/ #define JP4_1 PTT_PTT7 // JP4 #define JP4_2 PTT_PTT6 #define JP4_3 PTT_PTT5 #define JP4_4 PTT_PTT4 #define JP4_5 PTP_PTP4 #define JP4_6 PTP_PTP5 #define JP4_7 PTP_PTP6 // define code switch #define RP1_1 PTM_PTM0 #define RP1_2 PTM_PTM1 #define RP1_3 PTM_PTM2 #define RP1_4 PTM_PTM3 #define RP1_5 PTM_PTM4 #define RP1_6 PTM_PTM5 #define RP1_7 PORTAD0_PTAD4 #define RP1_8 PORTAD0_PTAD3 // define Led

#define Led1 PORTA_BIT4 #define Led2 PORTA_BIT5 #define Led3 PORTA_BIT6 #define Led4 PORTA_BIT7

Init.c // include initial function in this file

#include /* common defines and macros */ #include /* derivative information */ #include \"define.h\" /* all macro included */ #include \"init.h\" /* all init function included */

#pragma LINK_INFO DERIVATIVE \"mc9s12db128b\"

/*----------------------------------------------------------------------------*\\ init_PLL

\\*----------------------------------------------------------------------------*/ void init_PLL(void) // setting of the PLL {

REFDV=3;

SYNR=7; // bus period=16Mhz*(SYNR+1)/(REFDV+1) while(0==CRGFLG_LOCK); // wait for VCO to stablize CLKSEL=0x80; // open PLL }

Void init_ECT(void); {

TIOS_IOS3=0; // set PT3 as input capture

TCTL4=0b11000000; // set pt3 as any edge triggered ICPAR_PA1EN=1; // PA1 enabled }

/*----------------------------------------------------------------------------*\\ init_PORT

\\*----------------------------------------------------------------------------*/ void init_PORT(void) // port initialization {

DDRT_DDRT2=0;

// Port M1 function as even-odd field signal input

DDRJ_DDRJ6=1;

// Port J6 enable 33886 0 enable // Led port DDRA_BIT4 =1; DDRA_BIT5 =1; DDRA_BIT6 =1; DDRA_BIT7 =1;

INTCR_IRQE =1; // IRQ Select Edge Sensitive Only INTCR_IRQEN=1; // External IRQ Enable //输出指示 JP4_1 PTT_PTT0 DDRT_DDRT7=1; DDRT_DDRT6=1; DDRT_DDRT5=1; DDRT_DDRT4=1;

DDRP_DDRP4=1; //PTP_PTP0 DDRP_DDRP5=1;

DDRP_DDRP7=1; }

/*----------------------------------------------------------------------------*\\ init_AD

\\*----------------------------------------------------------------------------*/ void init_AD(void) // initialize AD {

ATD0CTL2=0xC0;

// open AD, quick clear, no wait mode, inhibit outer awake, inhibit interrupt ATD0CTL3=0x08;

// one transform in one sequence, No FIFO, contine to transform under freeze mode ATD0CTL4=0x81;

// 8-bit precision, two clocks, ATDClock=[BusClock*0.5]/[PRS+1] ; PRS=1, divider=4 ; BusClock=8MHZ

ATD0CTL5=0xA0; // right-algned unsigned，single channel， channel 0

ATD0DIEN=0x00; // inhibit digital input }

/*----------------------------------------------------------------------------*\\ init_PWM

\\*----------------------------------------------------------------------------*/ void init_PWM(void) // PWM initialize {

PTJ_PTJ6 = 0 ; // \"0\" enable 33886 motor, \"1\" disable it PWME = 0x00 ; // PWW is disabled PWMCTL_CON01 = 1 ; // combine PWM0,1 PWMPRCLK = 0x33 ; // A=B=32M/8=4M PWMSCLA = 100 ; // SA=A/2/100=20k PWMSCLB = 1 ; // SB=B/2/1 =2000k

PWMCLK = 0b00011100; // PWM0,1-A; PWM2,3-SB; PWM4-SA PWMPOL = 0xff ; // Duty＝High Time PWMCAE = 0x00 ; // left-aligned PWMPER0 = 0x4e ; PWMPER1 = 0x20 ;

// 20000 = 0x4e20; Frequency=A/20000=200Hz PWMDTY0 = 0x18 ;

PWMDTY1 = 0x6a ; // initialize PWM PWME_PWME1 = 1 ; // enable steer PWMDTY2 = 20 ; // Duty cycle

PWMPER2 = 200 ; // Frequency=SB/200=10K

PWME_PWME2 = 1 ; // enable motor }

/*----------------------------------------------------------------------------*\\ init_SPEED

\\*----------------------------------------------------------------------------*/ void init_SPEED(void) {

DDRM_DDRM0 =0 ; //code switch 1 on RP1 DDRM_DDRM1 =0 ; //code switch 1 on RP1 DDRM_DDRM2 =0 ; //code switch 1 on RP1 DDRM_DDRM3 =0 ; //code switch 1 on RP1 DDRM_DDRM4 =0 ; //code switch 1 on RP1 DDRM_DDRM5 =0 ; //code switch 1 on RP1

ATD0DIEN_IEN4 = 1; //code switch 1 on RP1,Enable Digital Input PAD4 ATD0DIEN_IEN3 = 1; //code switch 1 on RP1,Enable Digital Input PAD3 if(RP1_1==1) { speed= hign_speed

+2*(RP1_2*10+RP1_3*5+RP1_4*2+RP1_5*2+RP1_6+RP1_7+RP1_8); } else{

speed= hign_speed

-2*(RP1_2*10+RP1_3*5+RP1_4*2+RP1_5*2+RP1_6+RP1_7+RP1_8); } }

/***************************************************************************** ***/

/*----------------------------------------------------------------------------*\\ init_SCI

\\*----------------------------------------------------------------------------*/ void init_SCI(void) // initialize SCI {

SCI0BD = 104 ; // bode rate=32M/(16*SCI0BD)=19200 SCI0CR1=0x00 ; // SCI0CR2=0b00001100 ; } Init.h

void init_PLL(void); void init_AD(void); void init_PWM(void); void init_SPEED(void); void init_SCI(void); void init_PORT(void); Void init_ECT(void);

Fuzzy.asm // S12 fuzzy logic code RAM: section

; Fuzzy Membership sets ; input membership variables absentry fuzvar fuzvar: ds.b 5 ; inputs

Z: equ 0 ; indicate of straight line VS: equ 1 ; turn slightly S: equ 2 ; turn a little BB: equ 3 ; a big turn VB: equ 4 ; a very big turn ;output membership variables absentry fuzout fuzout: ds.b 4 ; outputs

remain: equ 5 ; no change on kp little: equ 6 ; little change on kp big: equ 7 ; big change on Kp

very_big: equ 8 ; very big change on kp EEPROM: section ; fuzzification

s_tab: dc.b 0,35,0,8 ; indicate of straight line dc.b 0,69,8,8 ; turn slightly dc.b 35,104,8,8 ; turn a little dc.b 69,138,8,8 ; a big turn dc.b 104,255,8,0 ; a very big turn rules: ;

constructing of rule dc.b Z, $FE,remain,$FE dc.b VS, $FE,little,$FE dc.b S, $FE,big,$FE dc.b BB, $FE,big,$FE dc.b VB, $FE,very_big,$FE dc.b $FF ;end of the rule addsingleton:

dc.b 0,1,2,3 ; setting of the weight absentry FuzzyLogic FuzzyLogic: pshx ldx #s_tab ldy #fuzvar

mem ; number of mem indicates the number of input mem

mem mem mem

ldab #4 ; number of output fuzzy membership sets cloop:

clr 1,y+ ; clear output fuzzy variables dbne b,cloop ldx #rules ldy #fuzvar ldaa #$FF rev ldy #fuzout ldx #addsingleton ldab #4 wav ediv ;

tfr y,d ; return dpower pulx rts

————完————

6