freescale.txt

飞思卡尔智能车程序光电组,对参加智能车光电组比赛的同学有帮助

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 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;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; //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(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 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_count<ROW_END){ 
get_black_wire(); 
speed_control(); 
steer_control(); 
} 
DisableInterrupts; 
} 
else{ // odd->even 
while(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 END 
sample_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 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 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_BIT7 
Init.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 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