当前位置:文档之家› 飞思卡尔智能车程序

飞思卡尔智能车程序

Main.c#include <hidef.h> /* common defines and macros */#include <mc9s12db128.h> /* derivative information */#pragma LINK_INFO DERIVATIVE "mc9s12db128b"#include "define.h"#include "init.h"// variable used in video processvolatile unsigned char image_data[ROW_MAX][LINE_MAX] ; // data array of picture unsigned char black_x[ROW_MAX] ; // 0ne-dimensional arrayunsigned char row ; // x-position of the arrayunsigned char line ; // y-position of the arrayunsigned int row_count ; // row counterunsigned char line_sample ; // used to counter in ADunsigned char row_image ;unsigned char line_temp ; // temperary variable used in data transferunsigned char sample_data[LINE_MAX] ; // used to save one-dimension array got in interruption// variables below are used in speed measureUnsigned char pulse[5] ; // used to save data in PA processUnsigned char counter; // temporary counter in Speed detectUnsigned char cur_speed; // current speedshort stand;short data;unsigned char curve ; // valve used to decide straight or turnshort 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;row<ROW_MAX;row++){for(line=LINE_MIN;line<LINE_MAX-3;line++){if(image_data[row][line]>image_data[row][line+3]+VALVE){for(i=3;i<10;i++){if(image_data[row][line+i]+VALVE<image_data[row][line+i+3]){ black_x[row]=line+i/2+2;i=10;}}line=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;row<FIRST_FIVE;row++){sum=sum+black_x[row];}average=sum/FIRST_FIVE;curve=0;for(row=0;row<FIRST_FIVE;row++){curve=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; //currentstatic 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 boundsshort Bounds(short data){if(data>right_limit){data = right_limit;}if(data<left_limit){data = left_limit;}return 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_speed<cur_speed){PWMDTY2=low_speed;}Else{PWMDTY2=high_speed;}}/***************************************************************************** *\Main\***************************************************************************** */void main(void) {// main functiioninit_PORT() ;// port initializationinit_PLL() ;// setting of the PLLinit_ECT();init_PWM() ;// PWM INITIALIZATIONinit_SPEED() ;init_SCI() ;for(;;) {if(field_signal==0){ // even->oddwhile(field_signal==0);row_count=0;row_image=0;EnableInterrupts;while(row_count<ROW_END){get_black_wire();speed_control();steer_control();}DisableInterrupts;}else{ // odd->evenwhile(field_signal==1);row_count=0;row_image=0;EnableInterrupts;while(row_count<ROW_END){get_black_wire();speed_control();steer_control();}DisableInterrupts;}/* transmit_sci('x');for(row=0;row<ROW_MAX;row++){transmit_sci(black_x[row]);}transmit_sci(curve);*/}}interrupt 6 void IRQ_ISR(){row_count++;if((row_count>ROW_START)&&(row_count%INTERVAL==0)&&(row_image<ROW_MAX )){init_AD();for(line_sample=0;line_sample<LINE_MAX;line_sample++){while(!ATD0STAT1_CCF0); // WAIT FOR TRANSFORM TO ENDsample_data[line_sample]=signal_in; // A/D transfer}ATD0CTL2=0x00;row_image++;}if((row_count>ROW_START)&&(row_count%INTERVAL==2)&&(row_image<ROW_MAX +1)){for(line_temp=0;line_temp<LINE_MAX;line_temp++){image_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 ResultRegisters#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 eachpicture#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 6400 //#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_BIT7Init.c // include initial function in this file#include <hidef.h> /* common defines and macros */#include <mc9s12db128.h> /* 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 captureTCTL4=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 signalinputDDRJ_DDRJ6=1;// Port J6 enable 33886 0 enable// Led portDDRA_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_PTT0DDRT_DDRT7=1;DDRT_DDRT6=1;DDRT_DDRT5=1;DDRT_DDRT4=1;DDRP_DDRP4=1; //PTP_PTP0DDRP_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 interruptATD0CTL3=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=8MHZATD0CTL5=0xA0; // right-algned unsigned,single channel,channel 0ATD0DIEN=0x00; // inhibit digital input}/*----------------------------------------------------------------------------*\init_PWM\*----------------------------------------------------------------------------*/void init_PWM(void)// PWM initialize{PTJ_PTJ6 = 0 ; // "0" enable 33886 motor, "1" disable itPWME = 0x00 ; // PWW is disabledPWMCTL_CON01 = 1 ; // combine PWM0,1PWMPRCLK = 0x33 ; // A=B=32M/8=4MPWMSCLA = 100 ; // SA=A/2/100=20kPWMSCLB = 1 ; // SB=B/2/1 =2000kPWMCLK = 0b00011100; // PWM0,1-A; PWM2,3-SB; PWM4-SAPWMPOL = 0xff ; // Duty=High TimePWMCAE = 0x00 ; // left-alignedPWMPER0 = 0x4e ;PWMPER1 = 0x20 ;// 20000 = 0x4e20; Frequency=A/20000=200HzPWMDTY0 = 0x18 ;PWMDTY1 = 0x6a ; // initialize PWMPWME_PWME1 = 1 ; // enable steerPWMDTY2 = 20 ; // Duty cyclePWMPER2 = 200 ; // Frequency=SB/200=10KPWME_PWME2 = 1 ; // enable motor/*----------------------------------------------------------------------------*\init_SPEED\*----------------------------------------------------------------------------*/void init_SPEED(void) {DDRM_DDRM0 =0 ; //code switch 1 on RP1DDRM_DDRM1 =0 ; //code switch 1 on RP1DDRM_DDRM2 =0 ; //code switch 1 on RP1DDRM_DDRM3 =0 ; //code switch 1 on RP1DDRM_DDRM4 =0 ; //code switch 1 on RP1DDRM_DDRM5 =0 ; //code switch 1 on RP1ATD0DIEN_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)=19200SCI0CR1=0x00 ; //SCI0CR2=0b00001100 ;}Init.hvoid 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 codeRAM: section; Fuzzy Membership sets; input membership variablesabsentry fuzvarfuzvar: ds.b 5 ; inputsZ: equ 0 ; indicate of straight lineVS: equ 1 ; turn slightlyS: equ 2 ; turn a littleBB: equ 3 ; a big turnVB: equ 4 ; a very big turn;output membership variablesabsentry fuzoutfuzout: ds.b 4 ; outputsremain: equ 5 ; no change on kplittle: equ 6 ; little change on kpbig: equ 7 ; big change on Kpvery_big: equ 8 ; very big change on kp EEPROM: section; fuzzifications_tab: dc.b 0,35,0,8 ; indicate of straight linedc.b 0,69,8,8 ; turn slightlydc.b 35,104,8,8 ; turn a littledc.b 69,138,8,8 ; a big turndc.b 104,255,8,0 ; a very big turnrules: ;constructing of ruledc.b Z, $FE,remain,$FEdc.b VS, $FE,little,$FEdc.b S, $FE,big,$FEdc.b BB, $FE,big,$FEdc.b VB, $FE,very_big,$FEdc.b $FF ;end of the ruleaddsingleton:dc.b 0,1,2,3 ; setting of the weightabsentry FuzzyLogicFuzzyLogic:pshxldx #s_tabldy #fuzvarmem ; number of mem indicates the number of input memmemmemmemldab #4 ; number of output fuzzy membership setscloop:clr 1,y+ ; clear output fuzzy variablesdbne b,cloopldx #rulesldy #fuzvarldaa #$FFrevldy #fuzoutldx #addsingletonldab #4wavediv ;tfr y,d ; return dpowerpulxrtsmemmemldab #4 ; number of output fuzzy membership setscloop:clr 1,y+ ; clear output fuzzy variablesdbne b,cloopldx #rulesldy #fuzvarldaa #$FFrevldy #fuzoutldx #addsingletonldab #4wavediv ;tfr y,d ; return dpowerpulxrts6。

相关主题