飞思卡尔智能车程序
第五界飞思卡尔智能车大赛程序(光电组)
}
else if(turn_value==direction_turn[0])
{
turn_value=direction_turn[0];
speed_expect=speed_set[0];
}
break;
case 2:if(turn_value==direction_turn[0])
//The PIT module has no external pins.
//PIT 模式没有外部引脚
unsigned char light=0; //激光管检测标志
unsigned short turn_value=0; //转向的PWM数值
unsigned short direction_turn[7]={333,430,560,647,705,780,888}; //转向给定值初始化
{
turn_value=direction_turn[6];
speed_expect=speed_set[6];
}
else if(turn_value==direction_turn[6])
{
turn_value=direction_turn[6];
speed_expect=speed_set[6];
}
else if(turn_value==direction_turn[2])
{
turn_value=direction_turn[1];
speed_expect=speed_set[1];
}
break;
case 4:if(turn_value==direction_turn[1])
飞思卡尔智能车dg128单片机控制程序代码
void AD_Init(void)
{
ATD0CTL2=0xC0; //AD模块上电, 快速清零, 无等待模式, 禁止外部触发, 中断禁止
ATD0CTL3=0x44; //每次转换8个序列, FIFO, Freeze模式下继续转
ATD0CTL4=0x02; //10位精度, 采样时间为2个AD时钟周期,ATDClock=4MHz
//设置舵机
PWMCTL_CON01=1; //使得通道0，1成为16位pwm
PWMPER0 =0x75;
PWMPER1 =0x30; //舵机的频率是： 24M/8/30000=100Hz,T=10ms
PWMDTY01=4500; // 对应为4500/30000的占空比,待调整
Infrared_detect();
data_handle();
motor_ctl();
steer_ctl();
}
}
void interrupt 26 MDC_ISR(void)
{
static unsigned int number_count=0; static unsigned int start=0; static
go=2;
if(begin>=150)
go=3;
}
}
}
//-----系统初始化-----------------------
void system_init(void) //system initiat
void speed_ctl(void); //速度控制
void motor_ctl(void); //电机控制
void PACBInit(void);
飞思卡尔智能车最终完整程序 具有很高的参考价值
#include<hidef.h>/* common defines andmacros */#include<mc9s12dg128.h>/* derivativeinformation *///#include "PWM.h"//#include "AD.h"#include"control.h"#pragma LINK_INFO DERIVATIVE "mc9s12dg128b"word AD_wData[9]; //全局变量存放AD0,AD1，AD2的结果word sum[9]; //初始化时为求平均值，全白中，各个灯的FF次的电压和word avrg0[9]; //全白时各个灯的平均电压word summ[9];//初始化时为求平均值，全黑中，各个灯的FF次的电压和word avrg1[9]; //全黑时各个灯的平均电压word ss[9]; //实际采集来的各个灯的电压word s[9]; // 实际采集来的各个灯的电压word sum2[8];//用于存放两两灯电压之和word k; //用于存放比较出的最大值uint h=1500;//转角大小int flag = 0;//标志中间灯是否第一次在黑道附近int flagg=0;//标志灯从哪边感应到黑道int flagg1=0;//标志灯从哪边感应到黑道int flagg0=0;//标志是左边还是右边出道int j=0;dword i;dword m;dword s0;dword s1;dword p11=0;//以下四个变量用于记录黑道处于同一侧的时间dword p12=0;dword p21=0;dword p22=0;word max0[9]=0;//初始化时采集来的黑道的值int g=0;//为过滤算法使用word cha[9];//用来存放黑白电压差int ffgg0=0;//标志是否用中间板采的数据int ffgg1=0;//标志是否用中间板采的数据word sum0=0; //初始化时采集来个灯的全白电压和//word sum1=0; //实际采集来的左个灯的电压和//word sum22=0; //实际采集来的右个灯的电压和int fla=0;//标志是出道还是入道void AD_Init();void PWM_Init();void PWM_Init1();//void PID();void AD_Init(void) //AD初始化{//控制寄存器：上电，标志位快速清零，开中断ATD0CTL2 =(ATD0CTL2_ADPU_MASK|ATD0CTL2_AFFC_MASK|ATD0CTL2_ASCIE_MASK) ;ATD1CTL2 =(ATD1CTL2_ADPU_MASK|ATD1CTL2_AFFC_MASK|ATD1CTL2_ASCIE_MASK) ;//控制寄存器：转换序列长度为ATD0CTL3 =0x78;//(ATD0CTL3_S2C_MASK|ATD0CTL3_S1C_MASK);ATD1CTL3 =0x78;//(ATD1CTL3_S2C_MASK|ATD1CTL3_S1C_MASK);//控制寄存器：ATD0CTL4 =(ATD0CTL4_SRES8_MASK|ATD0CTL4_PRS1_MASK|ATD0CTL4_PRS0_MASK) ;ATD1CTL4 =(ATD1CTL4_SRES8_MASK|ATD1CTL4_PRS1_MASK|ATD1CTL4_PRS0_MASK) ;//控制寄存器：ATD0CTL5 =(ATD0CTL5_DJM_MASK|ATD0CTL5_SCAN_MASK|ATD0CTL5_MULT_MASK);ATD1CTL5 =(ATD1CTL5_DJM_MASK|ATD1CTL5_SCAN_MASK|ATD1CTL5_MULT_MASK); ATD0DIEN=0x00; // 禁止数字输入缓冲ATD1DIEN=0x00; // 禁止数字输入缓冲}#pragma CODE_SEG NON_BANKED //中断服务程序#pragma TRAP_PROCvoid interrupt 22 Int_AD0(void){AD_wData[0] = ATD0DR0; //将结果寄存器中的值存放到数组中AD_wData[1] = ATD0DR1; //将结果寄存器中的值存放到数组中AD_wData[2] = ATD0DR2; //将结果寄存器中的值存放到数组中AD_wData[3] = ATD0DR3;AD_wData[4] = ATD0DR4;AD_wData[5] = ATD0DR5;AD_wData[6] = ATD0DR6;AD_wData[7] = ATD0DR7;AD_wData[8] = ATD1DR0;}#pragma CODE_SEG DEFAULTword max(word a,word b,word c,word d,word e,wordf,word r,word w) {word maxx=0;if(a>maxx)maxx=a;if(b>maxx)maxx=b;if(c>maxx)maxx=c;if(d>maxx)maxx=d;if(e>maxx)maxx=e;if(f>maxx)maxx=f;if(r>maxx)maxx=r;if(w>maxx)maxx=w;return maxx;}void delay0(){for(i=0;i<0xFFFF;i++)for(m=0;m<0x05;m++);}void delay1(){for(i=0;i<0xFFFF;i++);// for(i=0;i<0xFFFF;i++);}void main(void){AD_Init(); //AD 初始化DDRB = 0xFF;DDRA_BIT6=0; //A_BIT6口作为第二块板左边传感器的输入口DDRA_BIT7=0; //A_BIT7口作为第二块板右边传感器的输入口 PORTB = 0xFF;p=0;for(j=0;j<9;j++){AD_wData[j] = 0; //全局变量初始化sum[j]=0;avrg0[j]=0;avrg1[j]=0;summ[j]=0;}for(j=0;j<9;j++) {max0[j]=0;ss[j]=0;}for(j=0;j<8;j++)sum2[j]=0;EnableInterrupts; //开AD中断for(i=0;i<0xFFFF;i++);for(i=0;i<0xFF;i++) //只能是FF，防止下面sum溢出 {for(j=0;j<9;j++)//采集白道路信息{sum[j]=sum[j]+AD_wData[j];}}for(i=0;i<9;i++) {sum0=sum0+sum[i]/0xFF;avrg0[i]=sum[i]/0xFF;}PORTB=sum[0]/0xFF; //显示通道采集到的值 delay0();PORTB=0x00;//显示马上得进行黑道信息采集了 delay1();for(j=0;j<9;j++){for(m=0;m<0xFF;m++){summ[j]=summ[j]+AD_wData[j];}avrg1[j]=summ[j]/0xFF;PORTB=avrg1[j]; //显示采来的黑道信息cha[j]=avrg1[j]-avrg0[j];delay0();PORTB=0x00; //显示马上得进行下一次黑道信息采集了 delay1();}PORTB=0x00;//灯全亮，提示车马上就可以跑了delay1();PWM_Init() ;PWM_Init1(1500,1,200);for(i=0;i<0xFFF;i++);// delay1();for(;;){int f=0;u3=100;if(flagk1==1){p21=0;flagk2=0;p11++;if(p11==0xFFF)flagkk1=1;}else if(flagk2==1){p11=0;flagk1=0;p21++;if(p21==0xFFF)flagkk2=1;}for(f=0;f<9;f++){s[f]=AD_wData[f];ss[f]=s[f]-(avrg0[f]-0x50); //当前值减去初始白道值，以便比较}for(f=0;f<8;f++)sum2[f]=ss[f]+ss[f+1]; //两两灯电压之和//减去.6V防止溢出*******************if(AD_wData[0]<0xC0&& AD_wData[1]<0xC0&&AD_wData[2]<0xC0&&AD_wData[3]<0xC0&&AD_wData[4]<0xC0&&AD_wData[5]<0xC0&&AD_wData[6]<0xC0&&AD_wD ata[7]<0xC0&&AD_wData[8]<0xC0){if(sum2[0]<0xC0&&sum2[1]<0xC0&&sum2[2]<0xC0&&sum2[3]<0xC0&&sum2[4]<0xC0&&sum2[5]<0xC0&&sum2[6]<0xC0&&sum2[7]<0xD0){fla=1;if(flagg0==1){for(i=0;i<0xFF;i++);PWM_Init1(1140,u1,200);flagk1=1;flagkk2=0;for(;;){if(AD_wData[4]>0xB0||AD_wData[5]>0xB0||AD_wData[6]>0xB0|| AD_wData[7]>0xB0||AD_wData[8]>0xB0){flagg0=0;break;}}}else if(flagg0==2){for(i=0;i<0xFF;i++);PWM_Init1(1860,u1,200);flagk1=0;flagkk2=1;for(;;)if(AD_wData[0]>0xB0||AD_wData[1]>0xB0||AD_wData[2]>0xB0||AD_wData[3]>0xB0||AD_wData[4]>0xB0){flagg0=0;break;}}}else{}}else{if(s[0]-(avrg0[0]-0x13)<0x40 &&s[1]-(avrg0[1]-0x13)<0x40 &&s[2]-(avrg0[2]-0x13)<0x40 && s[3]-(avrg0[3]-0x13)<0x40 &&s[4]-(avrg0[4]-0x13)<0x40 && s[5]-(avrg0[5]-0x13)<0x40 &&s[6]-(avrg0[6]-0x13)<0x40 && s[7]-(avrg0[7]-0x13)<0x40&&s[8]-(avrg0[8]-0x13)<0x40)///////////注意调整该值***************{/* if(PORTA_BIT6!=0||PORTA_BIT7!=0){if(PORTA_BIT6!=0&&PORTA_BIT7==0)PWM_Init1(1900,200,1);else if(PORTA_BIT7!=0&&PORTA_BIT6==0)PWM_Init1(1100,200,1);}*/}else{k=max(sum2[0],sum2[1],sum2[2],sum2[3],sum2[4],sum2[5],sum2[ 6],sum2[7]);//谁两和最大，黑道就在谁两之间if(k==sum2[0]){p=0;flagg0=2;if(fla==1)control_11();else if(fla==0) control_1();}else{if(k==sum2[1]){p=0;p1=0;fla=0;control_2(s[1],s[2],ss[1],ss[2],cha[1],cha[2],avrg0[1],avrg 0[2]);}else{if(k==sum2[2]){p=0;p1=0;fla=0;control_3(s[2],s[3],ss[2],ss[3],cha[2],cha[3],avrg0[2],avrg 0[3]);}else{if(k==sum2[3]){p=0;p1=0;fla=0;control_4(s[3],s[4],ss[3],ss[4],cha[3],cha[4],avrg0[3],avrg 0[4]);}else{if(k==sum2[4]){fla=0;p1=0;control_5(s[4],s[5],ss[4],ss[5],cha[4],cha[5],avrg0[4],avrg 0[5]);}else{if(k==sum2[5]){fla=0;p1=0;control_6(s[5],s[6],ss[5],ss[6],cha[5],cha[6],avrg0[5],avrg 0[6]);}else{if(k==sum2[6]){p=0;p1=0;fla=0;control_7(s[6],s[7],ss[6],ss[7],cha[6],cha[7],avrg0[6],avrg 0[7]);}else{if(k==sum2[7]){p=0;flagg0=1;if(fla==0) control_8();elseif(fla==1)control_88(); } else{}}}}}}}}}}}}。
飞思卡尔摄像头组智能车程序代码
//---------------------------------------------------CCD#define odd_even PTIT_PTIT3#define ROW_START 30 // begin to sample from line start#define ROW_END 240 // end flag of samplingunsigned char frm,open_frm,flag1,finish=0;#define ROW_MAX 12 //最大取12行#define LINE_MAX 52unsigned char image_data[ROW_MAX][LINE_MAX];//unsigned char image_simple[10]; /////////装10个黑点，只处理10个//char row,line;char row;unsigned int line;unsigned char temp;unsigned int rowcount;unsigned char line_temp;//-------------------------------------------CCD_PROCESS#define RMV_ROW 2 //抛弃前2行0-4行#define V ALVE 24 // valve to decide black track or white trackunsigned char black_x[ROW_MAX];#define Abso_Center 25 //中心位置22unsigned int cigma,curve;unsigned int Curve_lev2=45;//大弯区分S直与曲线,直和S应该很小unsigned int Curve_lev1=24;//小弯valve to decide straight track or notunsigned char average,Relative_Poz;unsigned int sum;char r_HEAD;unsigned char flag_fail=0,flag_BigCurv=0,flag_Straight=0,flag_Curv=0; //char head;unsigned char flag_black_x_normal;#define normal_black_x 6 //need for adjust ,may need be larger,used for tell cross/start_line/ring from miss_line//--------------------------------------------PWM CONTROL#define PWM_MID 2813//2813 //mid//#define PWM_RIGHT 3750 //right#define PWM_RIGHT 3500//3600//#define PWM_LEFT 1875 //left#define PWM_LEFT 2200int speed,lastspeed;int steer,laststeer;int rev_speed,last_rev_speed;int speed_e,lastspeed_e;int rev_speed_e,last_rev_speed_e;//#define high_speed 250 // speed used on straight trackint speed_MAX;int speed_set_curve;//#define low_speed 100 // speed used on the turnunsigned char stand;//unsigned int speed_vari;unsigned char K_speed=1,K_steer=1;//-------------------------------------------OTHER V ARunsigned char a,b,c,x;char m,n,i,j,s;//unsigned char Start_Flag=0,Circle=0,Circle_Count=2,BEGIN=0,STOP=0;//探测起跑线,一圈停车unsigned char start_count;unsigned char flag_start,flag_stop,flag_heap;#define circle 2 //run 2 circlevoid IRQ_ISR(void);void RTI_ISR(void);int pa_count;int speed_set,rev_speed_set;int Kp=20,Kd=3;#include <hidef.h>#include <MC9S12XS128.h>#include "includes.h"#pragma LINK_INFO DERIV ATIVE "mc9s12xs128"//--------------------------------------------------------------------------------------//------initial----------initial---------------initial------------------initial------//-----------------------------------------------------------------------------------void init_PLL(void) //CPU总线时钟60M{REFDV=0x43;SYNR=0xce;POSTDIV=0x00; //bus period=16Mhz*(SYNR+1)/(REFDV+1)=60Mwhile(0==CRGFLG_LOCK); //wait for VCO to stablizeCLKSEL=0x80; //open PLL}void init_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;}void init_PWM(void){PWME=0x00; //pwm disablePWMPOL=0xff; //输出开场为高电平, 当给定计数到来时变低电平PWMCLK=0x00; //PWM0,1--A; PWM2,3--B; PWM4,5--A;PWMPRCLK=0x55; //A=B=60M/32=PWMCAE=0x00; //对齐方式：左对齐PWMCTL_CON01=1; //PWM01 合并16 bitPWMPER01=375; //PWM1 Period=375/=0.2ms.f=5kHZPWMDTY01=258; //PWME_PWME1 = 1; //电机enablePWMCTL_CON23=1; //PWM23 合并16 bitPWMPER23=375; //PWM3 Period=37500/=20ms,f=50HZPWMDTY23=0; // the duty cycle is 2813/37500=7.5%PWME_PWME3=1; // 舵机enablePWMCTL_CON45=1; //PWM45 合并16 bitPWMPER45=37500; //PWM5 Period=1875/=1ms.f=5kHZPWMDTY45=PWM_MID; //PWME_PWME5=1; //电机enable}void init_ECT(void) //定时器初始化??{TIOS=0; //通道0-7作为输入TCNT=0x00; //计数器清零TSCR1=0x80; //计数器使能TCTL4=0x07; //设置通道0捕获上、下沿,通道1捕获上沿PACTL=0X50; //PA使能，上升沿}void init_AD(void){ATD0CTL1=0x00; //External trigge source is AN0,8-bit data,No discharge before sampling ATD0CTL2=0x60; //quick clear CCFx,contine to transform under wait mode,Disable external trigger,ATD Sequence Complete interrupt requests are disabled,A TD Compare interrupt requests are disabledATD0CTL3=0x88; //one transform in one sequence, No FIFO,Right justied,contine to transform under freeze modeATD0CTL4=0x01; //four clocks,ATDCLK=[BusClock*0.5]/[PRS+1]=15MATD0CTL5=0x20; //Continuous Conversion Sequence Mode,Analog Input Channel is AN0 ATD0DIEN=0x00; //inhibit digital input//-------------------------------------------------------------------------------------//rti realtime interrut initial//--- 120-7 130-8 ..... 160-11 230-18 300-23// 100-9//------------------static void RTI_Init(void){RTICTL=0xff; // 8* 2(16) scale 32.75(16M)CRGINT=0x80; //enable}//------//------------------------------------------------------------------------------------------------//----functions---------------functions-----------functions--------functions---------functions-------------------------------------------//------------------------------------------------------------------------------------------------unsigned int abs_sub(unsigned int num1, unsigned int num2){ unsigned int difference;if(num1>=num2){difference=num1-num2;}else{difference=num2-num1;}return difference;}//--------//delay//--------void delay(int Dly_time){for(i=0;i<Dly_time;i++);}//-----------------------------------------------------------------------------------//- 滤波//---------------------------------------------------------------------------------------unsigned char get_mid(unsigned char a,unsigned char b,unsigned char c){unsigned char x=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;}return b;}//-------------------------------------------------------// jump_count//-------------------------------------------------------unsigned char jump_6(unsigned char Row){unsigned int i1,i2,i3,i4,i5;unsigned char flag_6jump=0;for(i1=LINE_MAX-1;i1>=10;i1--){ //for i1if(image_data[Row][i1-2]+V ALVE<image_data[Row][i1]){//if find right blcak_x ,jump1 ,i2 no1 blackfor(i2=i1-3;i2>=8;i2--){if(image_data[Row][i2]+V ALVE<image_data[Row][i2-2]){// jump2 i3 no2 whitefor(i3=i2-2;i3>=10;i3--){if(image_data[Row][i3-2]+V ALVE<image_data[Row][i3]){// jump3 i4 no3 blackfor(i4=i3-2;i4>=8;i4--){if(image_data[Row][i4]+V ALVE<image_data[Row][i4-2]){// jump4 i5 no4 whitefor(i5=i4-2;i5>=10;i5--){if(image_data[Row][i5-2]+V ALVE<image_data[Row][i5]){// jump5 i6 no5 blackflag_6jump=1; //total 5 jumpsi1=8;i2=8;i3=8;i4=8;i5=8;//out line scan} //end jump5} //end for} //end jump4} //end for} //end jump3}//end for(j=i-2;j>10;j--)}//end if jump2}//end for(i=k-3;i>8;i--)}//end if(image_data[Row][k-2]+V ALVE<image_data[Row][k])}//end for i1return flag_6jump;}//end jump_count//-------------------------------------------------------------------// --处理图像Process_ImgNEW//--------------------------------------------------------------------void Process_ImgNEW(void) // used to process image{unsigned char flag_right_x,JumpCount;unsigned char distance=0,temp_flag=0;unsigned char temp_black;head=1;flag_BigCurv=0;flag_Straight=0;flag_fail=0;flag_Curv=0;flag_heap=0;for(row=ROW_MAX-2;row<ROW_MAX;row++){//找近2行黑点for(line=LINE_MAX-1;line>8;line--){if(image_data[row][line]+V ALVE<image_data[row][line+3]){ //发现右点temp_black=line;for(i=1;i<10;i++){if(image_data[row][line-i]+V ALVE<image_data[row][line-i-3]){ //发现左点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;//假如找不到边界//end forif(flag_fail==0){ //计算前面黑点for(row=ROW_MAX-3;row>0;row--){ //for rowflag_right_x=0;for(line=LINE_MAX-1;line>=10;line--){ //for lineif(image_data[row][line-2]+V ALVE<image_data[row][line]){//if find right blcak_xblack_x[row]=line-2; //flag_right_x=1; //find right black_xline=8;//out line scan}}//end for lineif(flag_right_x){ //case find right black_xif(abs_sub(black_x[row],black_x[row+1])>normal_black_x)flag_black_x_normal=0;//unnormalelseflag_black_x_normal=1;//normal} //end if(flag_right_x)if(!flag_right_x){ //case not find right blcak_xif((black_x[row+1]>LINE_MAX-5)||(black_x[row+1]<12)){// the nearer row getting out visual head=row+1;row=0;// out row scan} else{ // judge as noise, cross ,or left ringblack_x[row]=black_x[row+1];//row=row-1;//next row}}//end if(!flag_right_x)if(!flag_black_x_normal){ //judge as right ring,start line or crossJumpCount=jump_6(row);if(JumpCount){//judge as start linestart_count++;black_x[row]=black_x[row+1];if(!flag_start){//when no start do thisif(start_count>=1)flag_start=1;// startif(flag_start)start_count=0;//once start,start_count restart} //end if(!flag_start)else{ //when has started do thisif(start_count==circle)flag_stop=1;//stop}}//end if(JumpCount>=6)else{ //judge as crossblack_x[row]=black_x[row+1];//row=row-1;//next row}//end else}//end if(!flag_black_x_normal)}//end for row//滤波，得到滤波后每行黑线的中心位置for(i=2;i<ROW_MAX-1;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行-最后一行计算中心averagesum=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; //看不到头，并且相对位移大,是大弯else if((Relative_Poz<=4) &&(black_x[head]>20)&&(black_x[head]<31))flag_Straight=1; //相对位移小，可能是直线可能是小Selse{ //flag_Curv=1;//视野可见，但位移大,大Ssum=0;for(row=ROW_MAX-1;row>=6;row--){ //重计算中心averagesum=sum+black_x[row];}average=sum/(ROW_MAX-6);}}//end if not fail}//---------------------------------------------------------------------// re_setPID//---------------------------------------------------------------------void re_setPID(void){if(flag_BigCurv){steer=PWM_MID+5*(average-Abso_Center)+80*(black_x[head]-Abso_Center); speed_set=10;speed_e=speed_set-pa_count;if(pa_count-speed_set>=1){ //big reversespeed=0;rev_speed=350;}else if((pa_count-speed_set>=0)&&(pa_count-speed_set<2)){ //slowspeed=0;rev_speed=last_rev_speed-2*Kp*speed_e-Kd*(speed_e-lastspeed_e);if(rev_speed>speed_MAX) rev_speed=speed_MAX;}else{ //reverserev_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;else if(PWM_MID-laststeer>100)steer=laststeer-200;else steer=laststeer;}if(flag_Curv) {steer=PWM_MID+10*(average-Abso_Center)+20*(black_x[head]-Abso_Center); //转舵speed_set=speed_set_curve;speed_e=speed_set-pa_count;if(pa_count-speed_set>=0){ //slowspeed=0;rev_speed=last_rev_speed-Kp*speed_e-Kd*(speed_e-lastspeed_e);if(rev_speed>speed_MAX) rev_speed=speed_MAX;}else{ //reverserev_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<PWM_LEFT) steer=PWM_LEFT;PWMDTY01=speed;PWMDTY23=rev_speed;PWMDTY45=steer;lastspeed=speed;last_rev_speed=rev_speed;laststeer=steer;}void key_con(void){switch(PORTA){case 0b10000000: speed_MAX=160;speed_set_curve=14;r_HEAD=3; break;case 0b11000000: speed_MAX=170;speed_set_curve=14;r_HEAD=3; break;case 0b11100000: speed_MAX=180;speed_set_curve=14;r_HEAD=3; break;case 0b11110000: speed_MAX=190;speed_set_curve=13;r_HEAD=3; break;case 0b11111000: speed_MAX=200;speed_set_curve=13;r_HEAD=3; break;case 0b11111100: speed_MAX=210;speed_set_curve=12;r_HEAD=2; break;case 0b11111110: speed_MAX=220;speed_set_curve=12;r_HEAD=2; break;case 0b11111111: speed_MAX=230;speed_set_curve=12;r_HEAD=2; break; }}//----------------------------------------------------------------------------------------//----main ------------------main----------------------main----------------------main------ //-----------------------------------------------------------------------------------------void main(void){init_PLL();init_PWM();init_PORT();init_ECT();RTI_Init();IRQCR=0x00; //diable IRQPWMDTY45=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_setPID();}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_setPID();}}}//----------------------------------------------------------------------------------------// -----interrupt-------------interrupt------------------interrupt-----------------interrupt //-----------------------------------------------------------------------------------------#pragma CODE_SEG NON_BANKEDvoid interrupt 6 IRQ_ISR(){rowcount++;if((rowcount>ROW_START)&&(rowcount%17==0)&&(row<ROW_MAX)) {init_AD();for(line=0;line<LINE_MAX;line++){while(!ATD0STA T2_CCF0);image_data[row][line]= A TD0DR0L;}row++;A TD0CTL2=0x00;}}//----------------------// realtime interrupt//-----------------------void interrupt 7 RTI_ISR(void){PORTB++;CRGFLG|=0x80;pa_count=PACNT;PACNT=0x00;}//------------------------------end---------------end----------------------------------。
飞思卡尔智能车程序代码
#define flag 1;
extern uchar cflag;
extern int i,j,m,n;
extern byte cs[40][60];
/*-----------------------*/
/*-------初始化----------*/
void InputInit(){
#pragma TRAP_PROC
void IRQ_ISR()
{
TIE_C1I=1; ;
//DDRB=0XFF;
//PORTB=0XF0;
//while(1);
i=0;
j=0;
}
#pragma CODE_SEG DEFAULT
#pragma CODE_SEG NON_BANKED
#pragma TRAP_PROC
TIE_C1I=0 ; //channel 0 interrupt DISable
TIOS_IOS1=0 ; //channel 0 input capture
TCTL4_EDG1A=1;
TCTL4_EDG1B=0;
}
/*-----------------------*/
/*--中断初始化--------------------*/
void init_IRQ() {
INTCR_IRQE=1; // IRQ select edge sensitive only
INTCR_IRQEN=1; // external IRQ enable
}
/*------ADT初始化--------------*/
void ADCInit(void)
void linenihe(void);
第五届飞思卡尔智能车电磁组程序
第五届飞思卡尔智能车电磁组获奖程序MC9S12XS128单片机、用前置线圈检测磁感线、用无线蓝牙采集数据、干簧管检测起跑线磁铁。
#include <hidef.h> /* common defines and macros */#include "derivative.h" /* derivative-specific definitions */#include <stdio.h>/****************************************************************************** ******一·全局变量声明模块******************************************************************************* ******/typedef unsigned char INT8U;typedef unsigned int INT16U;typedef int INT32;typedef struct {INT8U d; //存放这一次AD转换的值}DATA;/****************************************************全局变量声明区*****************************************************/DA TA data[6]={0}; //全局变量数组，存放赛道AD转换最终结果INT8U a[6][8]={0}; //全局变量用来存放赛道AD转换中间结果INT8U cross0,cross1; //记录十字叉线#define LED PORTA_PA7#define LED_CS PORTA_PA0byte START ;INT16U dianji0;//用来存放上次电机转速PWM，来判断是否减速#define duojmax 9200 //向左转向最大值#define duojmid 8400 //打在中间#define duojmin 7600 //向右转向最小值#define duojcs 8000;#define dianjmax 1200#define dianjmin 10#define dianjmid 600static INT8U look=0,look1=0;int road_change[100]={0}; //判断赛道情况数组int roat_change0;int *r_change0; //指向数组最后一位int *r_change1; //指向数组倒数第二位int sum_front=0,sum_back=0; //分别存储数组前后两部分的和INT16U waittime=0;INT8U choise; //读拨码开关数值/******************************速度测量参数定时********************************/#define PIT0TIME 800 //定时0初值：设定为4MS 测一次速度，采一次AD值#define PIT1TIME 1390 //定时1初值：设定为7ms定时基值/*******************************脉冲记数变量*******************************/ static INT16U PulseCnt;//最终的脉冲数/******************************电机PID变量*********************************/float speed_return_m ;struct {int error0;int error1;int error2;int speed;int chage;float q0,q1,q2,Kp,Kd,Ki;}static SpeedPid;/********************************速度变量设定*******************************/ INT8U speedmax ; //直道加速INT8U speedmin ; //急转弯刹车INT8U speedmid ; //弯道内部限速INT8U speedaveg ; //INT8U breaktime ; //刹车时间////////////////////////////////////////////////////////////////////////////#define speederror_min 2 //允许的最小误差static int NowSpeed;static int speed_control; //存储pid输出值static int speed_return;/*******************************舵机PID参数******************************/struct{int error0;int error1;int error2;int chage;float Kp,Kd,Ki;}PositionPid;int change;static INT16U angle_left [52]={8550,8562,8574,8586,8598,8610,8622,8634,8646,8658,8670,8682,8694,8706,8718,8730,8 742,8754,8766,8778,8790,8802,8814,8826,8838,8850,8862,8874,8886,8898,8910,8922,8934,894 6,8958,8970,8982,8994,9006,9018,9030,9042,9054,9066,9078,9090,9102,9114,9126,9138,9150,9 150};static INT16U angle_right[52]={8250,8238,8226,8214,8202,8190,8178,8166,8154,8142,8130,8118,8106,8094,8 082,8070,8058,8046,8034,8022,8010,7998,7986,7974,7962,7950,7938,7926,7914,7902,7890,787 8,7866,7854,7842,7830,7818,7806,7794,7782,7770,7758,7746,7734,7722,7710,7698,7686,7674,7 662,7650,7650};static INT16U *angle_l=angle_left ,*angle_r=angle_right;static INT16U angle_control=duojmid; //舵机PWM最终控制量static INT16U angle_control0=duojmid;static INT16U angle_control1=duojmid;static INT16U break_pwm=0;INT16U angle_return;/****************************lcd液晶显示变量定义**************************/#define LCD_DATA PORTB#define LCD_RS PORTA_PA4 //PA6#define LCD_RW PORTA_PA5 //PA7#define LCD_E PORTA_PA6 //PA7INT8U start[]={"WELCOME TO LZJTU"};INT8U date[]={"2011-3-15 TUS"};INT8U time[]={"00:00:00"};INT16U Counter=0;INT8U Counter0=0,select=0,min=0;INT8U Counter1=0;INT8U LCD_choice;/**************************标志变量区*************************************/INT8U stop_flag=0;INT8U start_flag=0;INT8U backflag=0;INT8U AD_start ;INT8U zhijwan=0 ;INT8U shizi=0;/****************************************************************************** ******二·初始化函数模块******************************************************************************* ******//**************************************************************1. 芯片初始化--------MCUInit（）**************************************************************/void MCUInit(void){//////////////////////////////////////////////////////////////////////////////////////////// ********总线周期计算方法******** //// fBUS=fPLL/2 //// fvoc=2*foscclk*(synr+1)/(refdv+1) //// PLL=2*16M*(219+1)/(69+1)=96Mhz /////////////////////////////////////////////////////////////////////////////////////////////////CLKSEL=0X00;PLLCTL_PLLON=1; //锁相环控制SYNR = 0X40|0X05;REFDV =0X80|0X01;POSTDIV=0X00;while( CRGFLG_LOCK != 1); //等待锁相环时钟稳定，稳定后系统总线频率为24MHz CLKSEL_PLLSEL = 0x01; //选定锁相环时钟PLLCTL=0xf1; //锁相环控制//时钟合成fpllclk=2*foscclk*(synr+1)/(refdv+1)//synr=2;refdv=1;外部时钟foscclk=16mb//fpllclk=48mb 总线时钟24mb// CRGFLG=0x40; //时钟复位控制// CRGINT=0x00 ; //时钟复位中断使能// CLKSEL =0xc0; //时钟选择//COPCTL =0x00;// ARMCOP =0x00; //看门狗复位// RTICTL =0x00; //实时中断}/**************************************************************2. AD转换初始化--------ADCInit（）**************************************************************/void ADCInit(void){A TD0CTL1=0x00;A TD0CTL2=0x40; //0100,0000，自动清除使能控制位，忽略外部触发//转换结束允许中断，中断禁止A TD0CTL3=0xA4; //0100,0100，转换序列长度为4；FIFO模式,冻结模式下继续转换A TD0CTL4=0x05; //00001000,8位精度,PRS=5，ATDCLOCK=BusClock(24mb)/(5+1)*2,约为2MHz,采样周期位4倍AD周期A TD0DIEN=0x00; //输入使能禁止}/**************************************************************3. PWM初始化--------PWMInit（）**************************************************************/void PWMInit(void) //PWM初始化{//总线频率24mb//1. 选择时钟：PWMPRCLK,PWMSCLA,PWMSCLB,PWMCLKPWME=0x00; //PWM通道关闭PWMPRCLK=0x01; //00010011时钟源A=BusClockA/2=48M/2=24MB;//低位clockA:01,45;高位clockB:23,67 时钟源B=48/1=48MBPWMSCLA =2; //ClockSA=ClockA/2/2=24MB/4=6MBPWMSCLB =2; //ClockSB=ClockB/2/2=12MBHzPWMCLK =0xFF; //通道均级联，均用SA,SB ,且都为6MB//2. 选择极性：PWMPOLPWMPOL =0xff; //电机正反转寄存器（PWMPOL）起始输出为高电平//3. 选择对齐方式：PWMCAEPWMCAE=0x00; //输出左对齐//4.PWMCTL PWM控制寄存器PWMCTL=0xF0; //01,23,45,67通道都级连，输出风别由1，3，5，7口控制//5. 使能PWM通道; PWME//6. 对占空比和周期编程//周期计算公式：输出周期=通道时钟周期*（PWMPERX+1）//占空比：=(PWMPERYX+1)/（PWMPERX+1）//开始时刻应使舵机打直，电机不转//1.通道45用来控制舵机PWMPWMPER45=60000-1; //PWM01=6MB/(60000)=100HzPWME_PWME5 =0; //舵机PWM通道开//2.通道23用来控制电机PWM1,通道01用来作为电机PWM2PWMPER23=1200-1;//电机正转PWM周期初始化。
飞思卡尔智能车完整程序
#include <hidef.h> /* common defines and macros */#include <mc9s12dg128.h> /* derivative information */ #include "lib.c"#include "funtion.h"#pragma LINK_INFO DERIV ATIVE "mc9s12dg128b"void s_ini(void) //系统初始化{uchar tmp;SYNR=3;REFDV=1;PLLCTL=0x50;delay(100);CLKSEL|=0x80; //超频到48M, 总线24M// INITRG = 0x00; /* lock registers block to 0x0000 */// INITRM = 0x39; /* lock Ram to end at 0x3FFF */// INITEE = 0x09; /* lock EEPROM block to end at 0x0fff */ delay(100);PORTB=0xff;DDRB=0xff;DDRA=0x00; //A口设置为输入PTH=0x00;DDRH=0x68; //PH1=inputPERH=0x97; //enable pullPPSH=0x80; //PH7,PH6,PH5 pull up, PH4 pull down PIEH=0x00; //使能遥控中断PH7PTP =0x00;DDRP=0x30; //PP4，PP5 outputDDRA=0x00;PUCR=0x01; //Port A enable pull upPTJ=0x00;DDRJ=0x00;PERJ=0x00; //diable pullPPSJ=0x80;PIEJ=0xc0; //enable PJ interruptPTT=0x00;DDRT=0x00; //PT7-PT4 =inputPERT=0x40; //enable pull for PT6// PACN3=0x00;// PACN2=0x00;// PACTL=0x40; //enable pulse couter B// DL YCT=0x00; //TIOS=0x80; //ch 7 compare modeOC7M=0x00;OC7D=0x00;// PACTL=0x74;TSCR2=0x0f; //when ch7 compare rst pre div =128 TC7=50000; //timer for 200mS //*/TSCR1=0x80; //enable timerTCTL1=0x00;TCTL2=0x00;TCTL3=0x20;TCTL4=0x00; //通道6下降沿输入捕捉TIE =0xC0; //中断使能ini_at45(); //初始化SPI0PWME=0xc3; //enable ch1, ch7PWMPOL=0xc3; //high level effectPWMDTY6=(uchar)(3000>>8);PWMDTY7=(uchar)(3000&0x00ff); //占空比PWMPER6=(uchar)(40000>>8); // 舵机PWMPER7=(uchar)(40000&0x00ff); //周期PWMDTY0=(uchar)(0>>8);PWMDTY1=(uchar)(0&0x00ff); //占空比// PWMPER0=(uchar)(2000>>8); //电机PWMPER1=(uchar)(2000&0x00ff); //周期PWMCLK=0x00;PWMPRCLK=0x40; //SB=Fck/1 SA=Fck/8PWMCTL=0x90; //4_16 bit PWM //*/SCI0BDH=0; //串口0初始化SCI0BDL=35; //set bandrate=57600bpsSCI0CR1=0x00;SCI0CR2=0x2c; //enable to sent and receivetmp=SCI0SR1;A TD0CTL2=0xc0; //enable ATD, interruptA TD0CTL3=0x08;A TD0CTL4=0x80; //8bit,A TD1CTL2=0xc0; //enable ATD, interruptA TD1CTL3=0x08;A TD1CTL4=0x80; //8bit,A TD0DIEN=0x00; //AD口数字输入使能,使能CCD输入A TD1DIEN=0x80; //CCD二元输入ECLKDIV=0x5C; //EEPROM时钟分频器INTCR=0xc0; //行同步中断, 下降沿触发,delay(10);SE_s; //舵机供电}interrupt 20 void sci0(void) //SCI0 interrupt{uchar sta,das;sta=SCI0SR1; //read the statedas=SCI0DRL;if(sta&0x40) //sent finish{}if(sta&0x20) //receve a data{speed(das*8);SCI0DRL=das;}}interrupt 15 void ch7(void) //ch7 interrupt 266mS {TFLG1=0x80; //clr flagif(m_en) //LED falshPORTB^=0xf0;}void choice(void) //choice a funtion{uchar kv;uchar pp=0;uchar tmp=0;V1: kv=0;wu(CN1[0],CN1[1],CN1[2],CN1[3]); //displayset(2);go(0,pp); //flashfor(;;) //loop{kv=key();if(kv==5&&pp) //lastpp--,go(0,pp);else if(kv==6&&pp<3) //nextpp++,go(0,pp);if(kv==8||kv==10) //enter{if(pp==0) //进入比赛{match();goto V1;}else if(pp==1) //传感受器测方式{text();goto V1;}else if(pp==2) //CCD 测试{t_ccd();goto V1;}else if(pp==3) //参数设定{setting();goto V1;}} //*/}}void main(void) {/* put your own code here */volatile uint t1=0;s_ini(); //系统初始化EnableInterrupts;get_s(); //获取设定参数choice(); //shoicefor(;;) {} /* wait forever *//* please make sure that you never leave this function */ }/****************************************************************************** FILE : datapage.cPURPOSE : paged data access runtime routinesMACHINE : Freescale 68HC12 (Target)LANGUAGE : ANSI-CHISTORY : 21.7.96 first version created******************************************************************************/#include "hidef.h"#include "non_bank.sgm"#include "runtime.sgm"#ifndef __HCS12X__ /* it's different for the HCS12X. See the text below at the #else // __HCS12X__ *//*According to the -Cp option of the compiler the__DPAGE__, __PPAGE__ and __EPAGE__ macros are defined.If none of them is given as argument, then no page accesses should occur andthis runtime routine should not be used !To be on the save side, the runtime routines are created anyway.If some of the -Cp options are given an adapted versions which only covers theneeded cases is produced.*//* if no compiler option -Cp is given, it is assumed that all possible are given : *//* Compile with option -DHCS12 to activate this code */#if defined(HCS12) || defined(_HCS12) || defined(__HCS12__) /* HCS12 family has PPAGE register only at 0x30 */#define PPAGE_ADDR (0x30+REGISTER_BASE)#ifndef __PPAGE__ /* may be set already by option -CPPPAGE */#define __PPAGE__#endif/* Compile with option -DDG128 to activate this code */#elif defined DG128 /* HC912DG128 derivative has PPAGE register only at 0xFF */#define PPAGE_ADDR (0xFF+REGISTER_BASE)#ifndef __PPAGE__ /* may be set already by option -CPPPAGE */#define __PPAGE__#endif#elif defined(HC812A4)/* all setting default to A4 already */#endif#if !defined(__EPAGE__) && !defined(__PPAGE__) && !defined(__DPAGE__)/* as default use all page registers */#define __DPAGE__#define __EPAGE__#define __PPAGE__#endif/* modify the following defines to your memory configuration */#define EPAGE_LOW_BOUND 0x400u#define EPAGE_HIGH_BOUND 0x7ffu#define DPAGE_LOW_BOUND 0x7000u#define DPAGE_HIGH_BOUND 0x7fffu#define PPAGE_LOW_BOUND (DPAGE_HIGH_BOUND+1)#define PPAGE_HIGH_BOUND 0xBFFFu#define REGISTER_BASE 0x0u#ifndef DPAGE_ADDR#define DPAGE_ADDR (0x34u+REGISTER_BASE)#endif#ifndef EPAGE_ADDR#define EPAGE_ADDR (0x36u+REGISTER_BASE)#endif#ifndef PPAGE_ADDR#define PPAGE_ADDR (0x35u+REGISTER_BASE)#endif/*The following parts about the defines are assumed in the code of _GET_PAGE_REG :- the memory region controlled by DPAGE is above the area controlled by the EPAGE andbelow the area controlled by the PPAGE.- the lower bound of the PPAGE area is equal to be the higher bound of the DPAGE area + 1*/#if EPAGE_LOW_BOUND >= EPAGE_HIGH_BOUND || EPAGE_HIGH_BOUND >= DPAGE_LOW_BOUND || DPAGE_LOW_BOUND >= DPAGE_HIGH_BOUND || DPAGE_HIGH_BOUND >= PPAGE_LOW_BOUND || PPAGE_LOW_BOUND >= PPAGE_HIGH_BOUND#error /* please adapt _GET_PAGE_REG for this non default page configuration */#endif#if DPAGE_HIGH_BOUND+1 != PPAGE_LOW_BOUND#error /* please adapt _GET_PAGE_REG for this non default page configuration */#endif/* this module does either control if any access is in the bounds of the specified page or *//* ,if only one page is specified, just use this page. *//* This behavior is controlled by the define USE_SEVERAL_PAGES. *//* If !USE_SEVERAL_PAGES does increase the performance significantly *//* NOTE : When !USE_SEVERAL_PAGES, the page is also set for accesses outside of the area controlled *//* by this single page. But this is should not cause problems because the page is restored to the old value before any other access could occur */#if !defined(__DPAGE__) && !defined(__EPAGE__) && !defined(__PPAGE__)/* no page at all is specified *//* only specifying the right pages will speed up these functions a lot */#define USE_SEVERAL_PAGES 1#elif defined(__DPAGE__) && defined(__EPAGE__) || defined(__DPAGE__) && defined(__PPAGE__) || defined(__EPAGE__) && defined(__PPAGE__)/* more than one page register is used */#define USE_SEVERAL_PAGES 1#else#define USE_SEVERAL_PAGES 0#if defined(__DPAGE__) /* check which pages are used */#define PAGE_ADDR PPAGE_ADDR#elif defined(__EPAGE__)#define PAGE_ADDR EPAGE_ADDR#elif defined(__PPAGE__)#define PAGE_ADDR PPAGE_ADDR#else /* we do not know which page, decide it at runtime */#error /* must not happen */#endif#endif#if USE_SEVERAL_PAGES /* only needed for several pages support *//*--------------------------- _GET_PAGE_REG --------------------------------Runtime routine to detect the right register depending on the 16 bit offset partof an address.This function is only used by the functions below.Depending on the compiler options -Cp different versions of _GET_PAGE_REG are produced.Arguments :- Y : offset part of an addressResult :if address Y is controlled by a page register :- X : address of page register if Y is controlled by an page register- Zero flag cleared- all other registers remain unchangedif address Y is not controlled by a page register :- Zero flag is set- all registers remain unchanged--------------------------- _GET_PAGE_REG ----------------------------------*/#if defined(__DPAGE__)#ifdef __cplusplusextern "C"#endif#pragma NO_ENTRY#pragma NO_EXIT#pragma NO_FRAMEstatic void NEAR _GET_PAGE_REG(void) { /*lint -esym(528, _GET_PAGE_REG) used in asm code */__asm {L_DPAGE:CPY #DPAGE_LOW_BOUND ;// test of lower bound of DPAGE#if defined(__EPAGE__)BLO L_EPAGE ;// EPAGE accesses are possible#elseBLO L_NOPAGE ;// no paged memory below accesses#endifCPY #DPAGE_HIGH_BOUND ;// test of higher bound DPAGE/lower bound PPAGE#if defined(__PPAGE__)BHI L_PPAGE ;// EPAGE accesses are possible#elseBHI L_NOPAGE ;// no paged memory above accesses#endifFOUND_DPAGE:LDX #DPAGE_ADDR ;// load page register address and clear zero flagRTS#if defined(__PPAGE__)L_PPAGE:CPY #PPAGE_HIGH_BOUND ;// test of higher bound of PPAGEBHI L_NOPAGEFOUND_PPAGE:LDX #PPAGE_ADDR ;// load page register address and clear zero flagRTS#endif#if defined(__EPAGE__)L_EPAGE:CPY #EPAGE_LOW_BOUND ;// test of lower bound of EPAGEBLO L_NOPAGECPY #EPAGE_HIGH_BOUND ;// test of higher bound of EPAGEBHI L_NOPAGEFOUND_EPAGE:LDX #EPAGE_ADDR ;// load page register address and clear zero flagRTS#endifL_NOPAGE:ORCC #0x04 ;// sets zero flagRTS}}#else /* !defined(__DPAGE__) */#if defined( __PPAGE__ )#ifdef __cplusplusextern "C"#endif#pragma NO_ENTRY#pragma NO_EXIT#pragma NO_FRAMEstatic void NEAR _GET_PAGE_REG(void) { /*lint -esym(528, _GET_PAGE_REG) used in asm code */__asm {L_PPAGE:CPY #PPAGE_LOW_BOUND ;// test of lower bound of PPAGE#if defined( __EPAGE__ )BLO L_EPAGE#elseBLO L_NOPAGE ;// no paged memory below#endifCPY #PPAGE_HIGH_BOUND ;// test of higher bound PPAGEBHI L_NOPAGEFOUND_PPAGE:LDX #PPAGE_ADDR ;// load page register address and clear zero flagRTS#if defined( __EPAGE__ )L_EPAGE:CPY #EPAGE_LOW_BOUND ;// test of lower bound of EPAGEBLO L_NOPAGECPY #EPAGE_HIGH_BOUND ;// test of higher bound of EPAGEBHI L_NOPAGEFOUND_EPAGE:LDX #EPAGE_ADDR ;// load page register address and clear zero flagRTS#endifL_NOPAGE: ;// not in any allowed page area;// its a far access to a non paged variableORCC #0x04 ;// sets zero flagRTS}}#else /* !defined(__DPAGE__ ) && !defined( __PPAGE__) */#if defined(__EPAGE__)#ifdef __cplusplusextern "C"#endif#pragma NO_ENTRY#pragma NO_EXIT#pragma NO_FRAMEstatic void NEAR _GET_PAGE_REG(void) { /*lint -esym(528, _GET_PAGE_REG) used in asm code */__asm {L_EPAGE:CPY #EPAGE_LOW_BOUND ;// test of lower bound of EPAGEBLO L_NOPAGECPY #EPAGE_HIGH_BOUND ;// test of higher bound of EPAGEBHI L_NOPAGEFOUND_EPAGE:LDX #EPAGE_ADDR ;// load page register address and clear zero flagRTSL_NOPAGE: ;// not in any allowed page area;// its a far access to a non paged variableORCC #0x04 ;// sets zero flagRTS}}#endif /* defined(__EPAGE__) */#endif /* defined(__PPAGE__) */#endif /* defined(__DPAGE__) */#endif /* USE_SEVERAL_PAGES *//*--------------------------- _SET_PAGE --------------------------------Runtime routine to set the right page register. This routine is used if the compilerdoes not know the right page register, i.e. if the option -Cp is used for more thanone pageregister or if the runtime option is used for one of the -Cp options.Arguments :- offset part of an address in the Y register- page part of an address in the B registerResult :- page part written into the correct page register.- the old page register content is destroyed- all processor registers remains unchanged--------------------------- _SET_PAGE ----------------------------------*/#ifdef __cplusplusextern "C"#endif#pragma NO_ENTRY#pragma NO_EXIT#pragma NO_FRAMEvoid NEAR _SET_PAGE(void) {#if USE_SEVERAL_PAGES__asm {PSHX ;// save X register__PIC_JSR(_GET_PAGE_REG)BEQ L_NOPAGESTAB 0,X ;// set page registerL_NOPAGE:PULX ;// restore X registerRTS}#else /* USE_SEVERAL_PAGES */__asm {STAB PAGE_ADDR ;// set page registerRTS}#endif /* USE_SEVERAL_PAGES */}/*--------------------------- _LOAD_FAR_8 --------------------------------This runtime routine is used to access paged memory via a runtime function.It may also be used if the compiler option -Cp is not used with the runtime argument.Arguments :- offset part of an address in the Y register- page part of an address in the B registerResult :- value to be read in the B register- all other registers remains unchanged- all page register still contain the same value--------------------------- _LOAD_FAR_8 ----------------------------------*/#ifdef __cplusplusextern "C"#endif#pragma NO_ENTRY#pragma NO_EXIT#pragma NO_FRAMEvoid NEAR _LOAD_FAR_8(void) {#if USE_SEVERAL_PAGES__asm {PSHX ;// save X register__PIC_JSR(_GET_PAGE_REG)BEQ L_NOPAGEPSHA ;// save A registerLDAA 0,X ;// save page registerSTAB 0,X ;// set page registerLDAB 0,Y ;// actual load, overwrites pageSTAA 0,X ;// restore page registerPULA ;// restore A registerPULX ;// restore X registerRTSL_NOPAGE:LDAB 0,Y ;// actual load, overwrites pagePULX ;// restore X registerRTS}#else /* USE_SEVERAL_PAGES */__asm {PSHA ;// save A registerLDAA PAGE_ADDR ;// save page registerSTAB PAGE_ADDR ;// set page registerLDAB 0,Y ;// actual load, overwrites pageSTAA PAGE_ADDR ;// restore page registerPULA ;// restore A registerRTS}#endif /* USE_SEVERAL_PAGES */}/*--------------------------- _LOAD_FAR_16 --------------------------------This runtime routine is used to access paged memory via a runtime function.It may also be used if the compiler option -Cp is not used with the runtime argument.Arguments :- offset part of an address in the Y register- page part of an address in the B registerResult :- value to be read in the Y register- all other registers remains unchanged- all page register still contain the same value--------------------------- _LOAD_FAR_16 ----------------------------------*/#ifdef __cplusplusextern "C"#endif#pragma NO_ENTRY#pragma NO_EXIT#pragma NO_FRAMEvoid NEAR _LOAD_FAR_16(void) {#if USE_SEVERAL_PAGES__asm {PSHX ;// save X register__PIC_JSR(_GET_PAGE_REG)BEQ L_NOPAGEPSHA ;// save A registerLDAA 0,X ;// save page registerSTAB 0,X ;// set page registerLDY 0,Y ;// actual load, overwrites addressSTAA 0,X ;// restore page registerPULA ;// restore A registerPULX ;// restore X registerRTSL_NOPAGE:LDY 0,Y ;// actual load, overwrites addressPULX ;// restore X registerRTS}#else /* USE_SEVERAL_PAGES */__asm {PSHA ;// save A registerLDAA PAGE_ADDR ;// save page registerSTAB PAGE_ADDR ;// set page registerLDY 0,Y ;// actual load, overwrites addressSTAA PAGE_ADDR ;// restore page registerPULA ;// restore A registerRTS}#endif /* USE_SEVERAL_PAGES */}/*--------------------------- _LOAD_FAR_24 --------------------------------This runtime routine is used to access paged memory via a runtime function.It may also be used if the compiler option -Cp is not used with the runtime argument. Arguments :- offset part of an address in the Y register- page part of an address in the B registerResult :- value to be read in the Y:B registers- all other registers remains unchanged- all page register still contain the same value--------------------------- _LOAD_FAR_24 ----------------------------------*/#ifdef __cplusplusextern "C"#endif#pragma NO_ENTRY#pragma NO_EXIT#pragma NO_FRAMEvoid NEAR _LOAD_FAR_24(void) {#if USE_SEVERAL_PAGES__asm {PSHX ;// save X register__PIC_JSR(_GET_PAGE_REG)BEQ L_NOPAGEPSHA ;// save A registerLDAA 0,X ;// save page registerSTAB 0,X ;// set page registerLDAB 0,Y ;// actual load, overwrites page of addressLDY 1,Y ;// actual load, overwrites offset of addressSTAA 0,X ;// restore page registerPULA ;// restore A registerPULX ;// restore X registerRTSL_NOPAGE:LDAB 0,Y ;// actual load, overwrites page of addressLDY 1,Y ;// actual load, overwrites offset of addressPULX ;// restore X registerRTS}#else /* USE_SEVERAL_PAGES */__asm {PSHA ;// save A registerLDAA PAGE_ADDR ;// save page registerSTAB PAGE_ADDR ;// set page registerLDAB 0,Y ;// actual load, overwrites page of addressLDY 1,Y ;// actual load, overwrites offset of addressSTAA PAGE_ADDR ;// restore page registerPULA ;// restore A registerRTS}#endif /* USE_SEVERAL_PAGES */}/*--------------------------- _LOAD_FAR_32 --------------------------------This runtime routine is used to access paged memory via a runtime function.It may also be used if the compiler option -Cp is not used with the runtime argument.Arguments :- offset part of an address in the Y register- page part of an address in the B registerResult :- low 16 bit of value to be read in the D registers- high 16 bit of value to be read in the Y registers- all other registers remains unchanged- all page register still contain the same value--------------------------- _LOAD_FAR_32 ----------------------------------*/#ifdef __cplusplusextern "C"#endif#pragma NO_ENTRY#pragma NO_EXIT#pragma NO_FRAMEvoid NEAR _LOAD_FAR_32(void) {#if USE_SEVERAL_PAGES__asm {PSHX ;// save X register__PIC_JSR(_GET_PAGE_REG)BEQ L_NOPAGELDAA 0,X ;// save page registerPSHA ;// put it onto the stackSTAB 0,X ;// set page registerLDD 2,Y ;// actual load, low wordLDY 0,Y ;// actual load, high wordMOVB 1,SP+,0,X ;// restore page registerPULX ;// restore X registerRTSL_NOPAGE:LDD 2,Y ;// actual load, low wordLDY 0,Y ;// actual load, high wordPULX ;// restore X registerRTS}#else /* USE_SEVERAL_PAGES */__asm {LDAA PAGE_ADDR ;// save page registerPSHA ;// put it onto the stackSTAB PAGE_ADDR ;// set page registerLDD 2,Y ;// actual load, low wordLDY 0,Y ;// actual load, high wordMOVB 1,SP+,PAGE_ADDR ;// restore page registerRTS}#endif /* USE_SEVERAL_PAGES */}/*--------------------------- _STORE_FAR_8 --------------------------------This runtime routine is used to access paged memory via a runtime function.It may also be used if the compiler option -Cp is not used with the runtime argument.Arguments :- offset part of an address in the Y register- page part of an address in the B register- value to be stored in the B registerResult :- value stored at the address- all registers remains unchanged- all page register still contain the same value--------------------------- _STORE_FAR_8 ----------------------------------*/#ifdef __cplusplusextern "C"#endif#pragma NO_ENTRY#pragma NO_EXIT#pragma NO_FRAMEvoid NEAR _STORE_FAR_8(void) {#if USE_SEVERAL_PAGES__asm {PSHX ;// save X register__PIC_JSR(_GET_PAGE_REG)BEQ L_NOPAGEPSHB ;// save B registerLDAB 0,X ;// save page registerMOVB 0,SP, 0,X ;// set page registerSTAA 0,Y ;// store the value passed in ASTAB 0,X ;// restore page registerPULB ;// restore B registerPULX ;// restore X registerRTSL_NOPAGE:STAA 0,Y ;// store the value passed in APULX ;// restore X registerRTS}#else /* USE_SEVERAL_PAGES */__asm {PSHB ;// save A registerLDAB PAGE_ADDR ;// save page registerMOVB 0,SP,PAGE_ADDR ;// set page registerSTAA 0,Y ;// store the value passed in ASTAB PAGE_ADDR ;// restore page registerPULB ;// restore B registerRTS}#endif /* USE_SEVERAL_PAGES */}/*--------------------------- _STORE_FAR_16 --------------------------------This runtime routine is used to access paged memory via a runtime function.It may also be used if the compiler option -Cp is not used with the runtime argument.Arguments :- offset part of an address in the Y register- page part of an address in the B register- value to be stored in the X registerResult :- value stored at the address- all registers remains unchanged- all page register still contain the same value--------------------------- _STORE_FAR_16 ----------------------------------*/#ifdef __cplusplusextern "C"#endif#pragma NO_ENTRY#pragma NO_EXIT#pragma NO_FRAMEvoid NEAR _STORE_FAR_16(void) {#if USE_SEVERAL_PAGES__asm {PSHX ;// save X register__PIC_JSR(_GET_PAGE_REG)BEQ L_NOPAGEPSHALDAA 0,X ;// save page registerSTAB 0,X ;// set page registerMOVW 1,SP,0,Y ;// store the value passed in XSTAA 0,X ;// restore page registerPULA ;// restore A registerPULX ;// restore X registerRTSL_NOPAGE:STX 0,Y ;// store the value passed in XPULX ;// restore X registerRTS}#else /* USE_SEVERAL_PAGES */__asm {PSHA ;// save A registerLDAA PAGE_ADDR ;// save page registerSTAB PAGE_ADDR ;// set page registerSTX 0,Y ;// store the value passed in XSTAA PAGE_ADDR ;// restore page registerPULA ;// restore A registerRTS}#endif /* USE_SEVERAL_PAGES */}/*--------------------------- _STORE_FAR_24 --------------------------------This runtime routine is used to access paged memory via a runtime function.It may also be used if the compiler option -Cp is not used with the runtime argument.Arguments :- offset part of an address in the Y register- page part of an address in the B register- value to be stored in the X:A registers (X : low 16 bit, A : high 8 bit)Result :- value stored at the address- all registers remains unchanged- all page register still contain the same value--------------------------- _STORE_FAR_24 ----------------------------------*/#ifdef __cplusplusextern "C"#endif#pragma NO_ENTRY#pragma NO_EXIT#pragma NO_FRAMEvoid NEAR _STORE_FAR_24(void) {#if USE_SEVERAL_PAGES__asm {PSHX ;// save X register__PIC_JSR(_GET_PAGE_REG)BEQ L_NOPAGEPSHALDAA 0,X ;// save page registerSTAB 0,X ;// set page registerMOVW 1,SP, 1,Y ;// store the value passed in XMOVB 0,SP, 0,Y ;// store the value passed in ASTAA 0,X ;// restore page registerPULA ;// restore A registerPULX ;// restore X registerRTSL_NOPAGE:STX 1,Y ;// store the value passed in XSTAA 0,Y ;// store the value passed in XPULX ;// restore X registerRTS}#else /* USE_SEVERAL_PAGES */__asm {PSHA ;// save A registerLDAA PAGE_ADDR ;// save page registerSTAB PAGE_ADDR ;// set page registerMOVB 0,SP, 0,Y ;// store the value passed in ASTX 1,Y ;// store the value passed in XSTAA PAGE_ADDR ;// restore page registerPULA ;// restore A registerRTS}#endif /* USE_SEVERAL_PAGES */}/*--------------------------- _STORE_FAR_32 --------------------------------This runtime routine is used to access paged memory via a runtime function.It may also be used if the compiler option -Cp is not used with the runtime argument.Arguments :- offset part of an address in the Y register- page part of an address is on the stack at 3,SP (just below the return address)- value to be stored in the X:D registers (D : low 16 bit, X : high 16 bit)Result :- value stored at the address- all registers remains unchanged- the page part is removed from the stack- all page register still contain the same value--------------------------- _STORE_FAR_32 ----------------------------------*/#ifdef __cplusplusextern "C"#endif#pragma NO_ENTRY#pragma NO_EXIT#pragma NO_FRAMEvoid NEAR _STORE_FAR_32(void) {#if USE_SEVERAL_PAGES__asm {PSHX ;// save X register__PIC_JSR(_GET_PAGE_REG)BEQ L_NOPAGEPSHDLDAA 0,X ;// save page registerMOVB 6,SP, 0,X ;// set page registerMOVW 2,SP, 0,Y ;// store the value passed in X (high word)MOVW 0,SP, 2,Y ;// store the value passed in D (low word)STAA 0,X ;// restore page registerPULD ;// restore A registerBRA doneL_NOPAGE:MOVW 0,SP, 0,Y ;// store the value passed in X (high word)STD 2,Y ;// store the value passed in D (low word) done:PULX ;// restore X registerMOVW 0,SP, 1,+SP ;// move return addressRTS}#else /* USE_SEVERAL_PAGES */__asm {PSHD ;// save D registerLDAA PAGE_ADDR ;// save page registerLDAB 4,SP ;// load page part of addressSTAB PAGE_ADDR ;// set page registerSTX 0,Y ;// store the value passed in XMOVW 0,SP, 2,Y ;// store the value passed in D (low word)STAA PAGE_ADDR ;// restore page registerPULD ;// restore D registerMOVW 0,SP, 1,+SP ;// move return addressRTS}#endif /* USE_SEVERAL_PAGES */}/*--------------------------- _FAR_COPY_RC --------------------------------This runtime routine is used to access paged memory via a runtime function.It may also be used if the compiler option -Cp is not used with the runtime argument.Arguments :- offset part of the source int the X register- page part of the source in the A register- offset part of the dest int the Y register- page part of the dest in the B register- number of bytes to be copied is defined by the next 2 bytes after the return address.Result :- memory area copied- no registers are saved, i.e. all registers may be destroyed- all page register still contain the same value as before the call- the function returns after the constant defining the number of bytes to be copiedstack-structure at the loop-label:0,SP : destination offset2,SP : source page3,SP : destination page4,SP : source offset6,SP : points to length to be copied. This function returns after the sizeA usual call to this function looks like:struct Huge src, dest;; ...LDX #srcLDAA #PAGE(src)LDY #destLDAB #PAGE(dest)JSR _FAR_COPY_RCDC.W sizeof(struct Huge); ...--------------------------- _FAR_COPY_RC ----------------------------------*/#ifdef __cplusplusextern "C"#endif#pragma NO_ENTRY#pragma NO_EXIT#pragma NO_FRAMEvoid NEAR _FAR_COPY_RC(void) {#if USE_SEVERAL_PAGES__asm {DEX ;// source addr-=1, because loop counter ends at 1PSHX ;// save source offsetPSHD ;// save both pagesDEY ;// destination addr-=1, because loop counter ends at 1PSHY ;// save destination offsetLDY 6,SP ;// Load Return addressLDX 2,Y+ ;// Load Size to copySTY 6,SP ;// Store adjusted return addressloop:LDD 4,SP ;// load source offset。
飞思卡尔智能车程序
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。
飞思卡尔程序
I
全国大学生智能汽车竞赛
5420,5180,4920,4650,4390,4130,3870,3620,3370,3110,2840}; unsigned char Rev_High[15] = {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0}; unsigned int timer_count = 0; int now_position = 0; int last_position = 0; const char Mid_PointWeight[15] = {2,4,68,10,12,14,16,18,20,22,24,26,28,30}; const unsigned char Laser_Ctrl[16] = {0X44,0X41,0X40,0X15,0X14,0X10,0X05,0X04, 0X01,0X00,0X45,0X50,0X51,0X54,0X55,0X11}; unsigned char Out_flag = 0; unsigned char Scan_flag = 0; unsigned char Delay_flag = 0; unsigned char Rev_HighTemp[15][10] = { 0 }; uchar HighP = 0; uchar HighD = 0; uchar MotorP = 0; uchar MotorI = 0; uchar MotorD = 0; int Save_Position[20]; char Load_Position[20]; uint Now_Speed = 0; uint Last_motor_ctrl = 0; uchar Lcd_tableP[] = "P765432101234567"; uchar Lcd_tableD[] = "D000000000000000"; void INIT_REGISTER(void) { /****************锁相环升频 SetBusCLK_40M**********************/ CLKSEL=0X00; //disengage PLL to system PLLCTL_PLLON=1; //turn on PLL SYNR=0X80|0X09; //pllclock=2*osc*(1+SYNR)/(1+REFDV)=160MHz; REFDV=0X80|0X01; //BUS CLOCK=40M POSTDIV=0X01; _asm(nop); _asm(nop); while(!(CRGFLG_LOCK==1)); //when pll is steady ,then use it; CLKSEL_PLLSEL =1; /*********************INIT_I/O********************************/ DDRB = 0XFF; //输出
飞思卡尔智能车程序汇总
#include "24c02.h"
#include "button.h"
#define servo_period 20 //ms
#define motor_pwm_frequ 1 //khz
void delay(unsigned int ms)
{
unsigned int i;
PWMPER23 = 80/4*servo_period*125;
PWMPER01 = 80/4*servo_period*125;
PWMDTY67 =0;
PWMDTY45 =0;
PWMDTY23 =0;
PWMDTY01 =0;
}
void main(void) {
/* put your own code here */
disply_listchar(0,3," TURN POWER OFF ",1);
disply_listchar(0,4,"=================",1);
disply_listchar(0,5," ",1);
delay(setnum[1]*100);
PORTB_PB0=0;
}
}
3.电机程序
#include <hidef.h> /* common defines and macros */
#include "derivative.h" /* derivative-specific definitions */
unsigned char j;
for(i=0;i<ms;i++)
第六届飞思卡尔杯智能小车大赛源程序
*******************/// 结构体定义在头文件中void delay(int delaytime); void longdelay(int delaytime); _n )o 哈哈~// 延时大约 3us//长延时大约3ms ，没有细算，本应该用定时器的，0( nunsigned char Max(int bijiao[],unsigned char n); // 最大值，返回数组下标 unsigned char Min(int bijiao[],unsigned char n); // 最小值，返回数组下标 int Midnum(int bijiao[],unsigned char n);// 中值，返回中间数下标void MaxtoMin(int bijiao[]，flag[]，int n); int Average(int bijiao[],unsigned char n);// 求中间值 int abs(int i);int PIDcalc1(int Pcoe,int Icoe,int Dcoe,int Error,int LastError,int PrevError,int SumError,int iError_max);// 位置 pid 用于驱动舵机 ;// 位置式 PID 用于驱动舵机int PIDcalc2(int PcoeS,int IcoeS,int DcoeS,int SetPoint,int CurrentPoint,int LastErrorS,intPrevErrorS,int PrevPrevErrorS,int Iterm_max);// 增量式 PID 用于驱动马达 currentpoint 当前 测量值 ******************************************************************************************************************************************************** *******************/ /* --------------------------- 以下内部文件调用 --------------------------- */ #ifndef _H_SURELOCATION_#define _H_SURELOCATION_ int BackLocationtest(void); /* ------------------------------- 以下外部文件调用 --------------------------- */ int SureLocationTest(void); int Wandaotest(void); int Crosstest(void);#endif **************************************************************************** ****************************************************************************#ifndef _H_HARDWARE_*******************//* ----------------------------------文件名 ： Bace.h ----------------------------------------- */头文件**************************************************************************** /* ----------------------------- 功能 ： Base.c 对应头文件 ------------------------- */ /****************************************************************************** *****************/* -------------------------------文件名 ： SureLocation.h---------------------------------- *//* ----------------------------- 功能 SureLocation.c 对应头文件 ------------------------------ */ ***************** /* -------------------------------文件名 ： HardWare.h---------------------------------- *//* ----------------------------- 功能 HardWare.c 对应头文件------------------------------ */ *****************#define _H_HARDWARE_ /* ------------------------------- 以下内部函数调用 -------------------------------- */ void CLK_init(void); void ECT_init(void); void IO_init(void); void PWM_init(void); void AD_init(void); /* ------------------------------- 以下外部函数调用 -------------------------------- */ void init(void);unsigned char GetATD0(unsigned char ch); #endif/****************************************************************************** *******************//****************************************************************************** *******************/ /* ------------------------------- 以下内部文件调用 --------------------------- */ #ifndef _H_CONTROL_#define _H_CONTROL_ void duojicontrol(void); void upv(int vj); void downv(int vj);int SpeedWanted(void); void ControlMotor(int uin); void MotorControl(void);/* ------------------------------- 以下外部文件调用 --------------------------- */ void Control(void); #endif/****************************************************************************** *****************/* /* 文件名 ： HardWareDriver.h --- 功能 HardWareDriver.c 对应头文件*/ *//*************************************************************************************************/#ifndef _H_HARDWAREDRIVER#define _H_HARDWAREDRIVER/* 以下外部函数调用*/void MOD_init(void); void LEDtest(void);void Xianshi(unsigned char sta); #endif/****************************************************************************** *******************/ /* /* 文件名功能 ： Control.c 对应头文件 Control.h*/*//* ----------------------------- 文件名：Sensor.h ------------------------------------------------- *//* ------------------------- 功能：Sensor.c对应头文件------------------------------ */ **************************************************************************** *******************//* ----------------------------- 以下内部文件调用--------------------------------- */void GetADdata(int testevetime);void Getordinarydata(void);void ADdataOk(void);/* ----------------------------- 以下外部文件调用--------------------------------- */ voidProcess(void);/* ----------------------------- 功能包括其它所有头文件的文件 ------------------------ */ **************************************************************************** *******************/#ifndef _H_INCLUDE_#define _H_INCLUDE_#include "Sensor.h"// #include "LCD.h"#include "Surelocation.h"#include "Control.h"#include "base.h"#include "HardWare.h"#include "Hardwaredriver.h"#include "MC9S12XS128.h"/* --------------------- 以下传感器整定参数-------------------- */#define FRONTNUM 8//前排传感器数4#define TESTTIME 4//每次采集每个传感器读取的数值数目#define HISTORYNUM 10//历史数据保存量#define MIDYUZHI 3//中间两传感器迷失后的和#define ZHONGJIANYUZHI 35//线在两个传感器中间和某个传感器之下时的阈值#define ZHONGJIAN 210//用来检测车子是否在跑到中心(赛车中心距离赛道中心八厘米)中间两传感器的两单峰函数叠加为单峰函数峰值250#define ADYUZHI 10 //AD 数据校验阈值#define LOCYUZHI 68 //跨区阈值#define COE 1/* --------------------- 以下电机整定参数-------------------- */#define PWMMOTOT 800 //电机PWM 周期;//电机PWM 周期1.6ms ,电机周期随便只关心占空比。
飞思卡尔智能车程序汇总
1.流水灯程序：#include <>/* common defines and macros */ #include ""/* derivative-specific definitions */ void delay(unsigned int ms){unsigned int i;unsigned char j;for(i=0;i<ms;i++){for(j=0;j<100;j++);}}void main(void) {/* put your own code here */EnableInterrupts;DDRA=0x00;DDRB_DDRB0=1;for(;;){delay(25000);PORTB_PB0=1;delay(25000);PORTB_PB0=0;}}2.液晶屏按键程序#include <>/* common defines and macros */ #include ""/* derivative-specific definitions */ #include ""#include ""#include ""void delay(unsigned int ms){unsigned int i;unsigned char j;for(i=0;i<ms;i++){for(j=0;j<100;j++);}}/******************************倍频*****************************************/void vPLLInit(void){unsigned char refdv;refdv=3;CLKSEL=0X00;机程序#include <>/* common defines and macros */ #include ""/* derivative-specific definitions */ #include ""#include ""#include ""#define servo_period 20盘控速#include <>/* common defines and macros */#include ""/* derivative-specific definitions */#include ""#include ""#include ""#define servo_period 20 PACTL_PEDGE=1;PACTL_PAEN=1; }磁车程序#include <>/* common defines and macros */#include <>#include ""/* derivative-specific definitions */#include ""#include ""#include ""PACTL_PEDGE=1;PACTL_PAEN=1;}5.);}5.;for(i=0;i<8000;i++){for(j=0;j<4;j++){AD_wData[j]=ReadATD(i)/255.*5.;if(AD_wData[j]>max_v[j]) max_v[j]= AD_wData[j];}delayms(1);}5.;if(AD_wData[i]>=min_v[i]){AD_wData[i]=(AD_wData[i]-min_v[i])*5./(max_v[i]-min_v[i]); //if(AD_wData[i]>100)AD_wData[i]=100;}elseAD_wData[i]=0;if(AD_wData[i]>miny) ////{miny=AD_wData[i];minx=i;}//printp("%03d,",AD_wData[i]);sumxy1+=dif_x[i]*AD_wData[i];sumy1+=AD_wData[i];}servo_change=(sumxy1/sumy1)*K1 ; set_servo(servo_change);//////增添电机 //////////增添电机 ////}}。
飞思卡尔智能车S12XS128PWM控制程序编写
飞思卡尔智能车S12XS128PWM控制程序编写SeptStringS原创，转载请注明。
对于飞思卡尔智能车，电机和舵机的控制通常使⽤的都是以PWM脉冲宽度调制的⽅法实现的，其可⾏性基于电机可以由占空⽐控制转速，⽽舵机也由脉宽控制摆动。
PWM 调制波有 8 个输出通道，每⼀个输出通道都可以独⽴的进⾏输出。
每⼀个输出通道都有⼀个精确的计数器（计算脉冲的个数），⼀个周期控制寄存器和两个可供选择的时钟源。
每⼀个 PWM 输出通道都能调制出占空⽐从 0—100% 变化的波形。
PWM控制程序的编写⼀般按照以下的⼀种流程：1，禁⽌PWM模块；//这是由于改变周期和脉宽等操作需要在PWM禁⽌的情况下才能被设置2，PWM级联选择，是否级联通道67,45,23,01；//最多单独使⽤8个8位和级联使⽤4个16位3，给通道选择时钟源控制位；//0,1,4,5通道可选择ClockA和ClockSA；2,3,6,7通道可选择ClockB和ClockSB4，给时钟源A\B预分频；//可对总线时钟进⾏预分频，确定ClockA和ClockB，满⾜1,2,4,8,16,32,64,128这8个分频量5，根据时钟源A\B确定时钟源SA\SB；//由ClockA和ClockB、分频设值来确定ClockA和ClockB，满⾜1-255的分频量6，输出极性的选择；//也就是选择输出极性先低后⾼还是先⾼后低7，对齐⽅式的选择；//可设置为左对齐或者中间对齐⽅式8，实际通道频率的计算；//也就是周期的设定9，占空⽐寄存器的设置；//占空⽐常数的设定，可以以此决定占空⽐10，使能PWM模块。
//你已经⾸尾呼应了，有⽊有接下来通过寄存器的介绍，以上⾯流程为⼤纲，详细地说明⼀下该如何操作~~【PWME】寄存器PWME = (PWME~7 | PWME~6 | PWME~5 | PWME~4 | PWME~3 | PWME~2 | PWME~1 | PWME~0)将每⼀位设置为1即可使能该位，0对应的既是禁⽌。
飞思卡尔智能车电磁组第七届可用程序
int Pcrtl(int ch1,int ch2 )
{
E0=ch1-0; //增量计算
Error0=E0-E1;
Error1=E1-E2;
iIncpid=(int)(Ki*Error0+Kp*E0+Kd*(Error0-Error1));
EError1=EE1-EE2;
iiIncpid=(int)(KKi*EError0+KKp*EE0+KKd*(EError0-EError1));
EE1=EE0;
EE2=EE1; //存储误差，用于下次计算
sp+=iiIncpid;
if(sp>210)
sp=210;
if(sp<0)
sp=0;
return sp;
#include <stdlib.h>
#include <math.h>
#pragma LINK_INFO DERIVATIVE "mc9s12xs128"
#define K 1
#define N 3
unsigned int i=0,j=0,m=0,n=0;
unsigned char str[]={'0'};
PITMTLD1=0X0F ; //micro time base 0 equals 255 clock cycles
PITLD1=0XFFFF; //time-out period = (PITMTLD + 1) * (PITLD + 1) / fBUS.
//时间计算
//For example, for a 16 MHz bus clock, the maximum time-out period equals:
飞思卡尔智能车(摄像头)核心程序
飞思卡尔智能车（摄像头）核心程序1舵机PD法过弯策略/***************************舵机PID函数*********************************/void Angle_PID(int CENTER){//2.8float angle_kp=7.38,angle_kd=17.2;//系数PD控制if(lost_time==0 ||left_lost_time==0 ||right_lost_time==0 ) //判断如果黑线都丢失了，则不更新数据{angle_error=CENTER-CENTER_INIT;// if(left_lost_time>20 && right_lost_time==0)// angle_error=CENTER_INIT-CENTER+abs(angle_error)/8.5;// else if(left_lost_time==0 && right_lost_time>20)//angle_error=CENTER_INIT-CENTER-abs(angle_error)/8.5;angle=(int)(angle_init+angle_kp*angle_error+angle_kd*(angle_error-angle_last _error));if(angle<=angle_init-250) //防止系数过大导致舵机打死{angle=angle_init-250; //6%-9% ,,2000}if(angle>=angle_init+250){angle=angle_init+250;}angle_last_error=angle_error;PWMDTY23=angle;}}2大津法动态阈值调节/***************************大律法动态阈值*********************************/ void otsu_yuzhi( ){int wh = 128;int inIm[128];int i, t;int L = 128;double p[128];double w1=0;double u0=0;double u1 = 0;double max = 0.0;double w0 = 0;int m =0;double sigma[128];for (i = 0; i < 128; i++)inIm[i] = ADV[i]&0xff;for (i = 0; i < L; i++)p[i] = 0;//计算各灰度出现次数for (i = 0; i < 128; i++)p[inIm[i]]++;//计算各灰度级出现概率for (m = 0; m < L; m++)p[m] = p[m] / wh;for (t = 0; t < L; t++){w0 = 0;for (m = 0; m < t+1; m++)w0 += p[m];w1 = 1 - w0;u0 = 0;for (m = 0; m < t + 1; m++)u0 += m * p[m] / w0;u1 = 0;for (m = t; m < L; m++)u1 += m * p[m] / w1;sigma[t] = w0*w1*(u0-u1)*(u0-u1); }max = 0.0;for (i = 0; i < L-1; i++){if (max < sigma[i]){max = sigma[i];THRESHOLD = i;}}//二值化for (i=0; i<128; i++){ADV[i]=ADV[i]>THRESHOLD?225:0;}}3图像滤波算法/***************************3点滤波*********************************/ void lvbo3() //冒泡法排序后，改点值为中点值{uchar a[3];//定义一个数组既它的元素为7uchar i,j,k,temp;//定义3个变量for(k=3;k<125;k++){a[0]=ADV[k-1];a[1]=ADV[k];a[2]=ADV[k+1];for(i=0;i<2;i++)//进行6轮排序{for(j=0;j<2-i;j++) //每轮进行6-i次交换if(a[j]>a[j+1]){temp=a[j];a[j]=a[j+1];//大的沉底，小的上浮a[j+1]=temp;}}ADV[k]=a[1];}}/***************************5点滤波*********************************/void lvbo5() //冒泡法排序后，改点值为中点值{uchar a[5];//定义一个数组既它的元素为7uchar i,j,k,temp;//定义3个变量for(k=3;k<125;k++){a[0]=ADV[k-2];a[1]=ADV[k-1];a[2]=ADV[k];a[3]=ADV[k+1];a[4]=ADV[k+2];for(i=0;i<4;i++)//进行6轮排序{for(j=0;j<4-i;j++) //每轮进行6-i次交换if(a[j]>a[j+1]){temp=a[j];a[j]=a[j+1];//大的沉底，小的上浮a[j+1]=temp;}}ADV[k]=a[2];}}4边线提取for( i=Center[0]; i>6; i--) //提取左边线{if( ADV[i]>(ADV[i-5]+45) && ADV[i-1]>(ADV[i-6]+45)){left=i-5;break;}}for( i=Center[0]; i<121; i++){if( ADV[i]>(ADV[i+5]+45)&& ADV[i+1]>(ADV[i+6]+45)){right=i+5;break;}}5曲率计算void chuli(){int x1,x2,x3,y1,y2,y3;unsigned int length1,length2,length3;int S;y1=5;y2=10;y3=15;x1=Center[2];x2=Center[1];x3=Center[0];S=abs((y2-y1)*(x3-x1)-(y3-y1)*(x2-x1));length1=sqrt((x2-x1)*(x2-x1)+(y2-y1)*(y2-y1));length2=sqrt((x3-x1)*(x3-x1)+(y3-y1)*(y3-y1));length3=sqrt((x2-x3)*(x2-x3)+(y2-y3)*(y2-y3));if(S<1)S=1;K=abs((length1*length2*length3)/S);//曲率的倒数，即曲率半径Center_Average=mid(Center[0],Center[1],Center[2]);ADV[Left[0]]=0;ADV[Right[0]]=0;Angle_PID(Center_Average);}。
- 1、下载文档前请自行甄别文档内容的完整性，平台不提供额外的编辑、内容补充、找答案等附加服务。
- 2、"仅部分预览"的文档,不可在线预览部分如存在完整性等问题,可反馈申请退款(可完整预览的文档不适用该条件!)。
- 3、如文档侵犯您的权益，请联系客服反馈,我们会尽快为您处理(人工客服工作时间：9:00-18:30)。
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。