飞思卡尔智能车摄像头组freescale程序代码

合集下载

飞思卡尔智能车智能车源代码光电组(有注解)

智能车源代码光电组(有注解)#include <hidef.h> /* common defines and macros */#include <mc9s12dg256.h> /* derivative information */#include "math.h"#include "PWM.h"#include "A TD.h"#include "LQprintp.h"#pragma LINK_INFO DERIV A TIVE "mc9s12dg256b"unsigned int i = 0;unsigned int j = 0;unsigned int t = 0;byte ad_value[13];uchar data[13];int sum = 0;uchar start_flag = 0;uchar num = 0;uchar lw=0;unsigned int per = 65530;int SPWM = 0;int L_SPWM = 0;unsigned int SPmax = 1000;int MPWM = 0;uchar current_corrd = 0;static unsigned int mem_num = 0;//***********************PID**************** ***************static unsigned int Kp=25;static unsigned int Kp2=60;static unsigned int Ki=9;static unsigned int Kd=30;static unsigned int rKp=100;static unsigned int rKp2=60;static unsigned int rKi=0;static unsigned int rKd=60;unsigned short E = 5;unsigned char q = 1;int Mp = 0;int Mi = 0;int Md = 0;int Mp2 = 0;int P_Speed = 0;int L_u[3];//****************************************** ****************//***********************舵机PID变量**********************static unsigned int s_sKp=35; //直道PID的P值static unsigned int s_sKp2=0; //直道PID 的二阶P值static unsigned int s_sKi=0; //直道PID的I值static unsigned int s_sKd=10; //直道PID的D值//****用于防止PID溢出******unsigned short s_sE = 5;unsigned char s_sq = 1;//*************************//****分别存放P I D 值*****int s_sMp = 0;int s_sMi = 0;int s_sMd = 0;int s_sMp2 = 0;//*************************int sL_u[3]; //存放前3次理论速度与实际速度的差值int last_corrd[3][10] ={{0, 0, 0, 0, 0, 0, 0, 0, 0, 0},{0, 0, 0, 0, 0, 0, 0, 0, 0, 0},{0, 0, 0, 0, 0, 0, 0, 0, 0, 0}};//****************************************** ***************//*********************PID调试中断**************************unsigned char cp = 0;unsigned char ci = 0;unsigned char cd = 0;unsigned int search_PACN10;unsigned int np = 0;unsigned int sp[500];//****************************************** ****************//*********************存储前20点的数据*********************int L_num[10] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0};int sum_corrd = 0;int wb = 0; //记录当前状态黑为0，白为1；//****************************************** ****************//********************红外滤波*****************************int corrd[10] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0};int T_corrd = 0;int is_white = 0;int numb = 0;//****************************************** ****************//***********************数据统计***************************int corrd_sate[23] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};int SPWM_sate[15] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};int MPWM_sate[11] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}; uchar corrd_time[512];uchar SPWM_time[512];uchar MPWM_time[512];uchar PACN_time[512];//****************************************** ****************void setbusclock(void){CLKSEL=0X00; //disengage PLL to systemPLLCTL_PLLON=1; //turn on PLL SYNR=1;REFDV=1;//pllclock=2*osc*(1+SYNR)/(1+REFDV)=32MHz;_asm(nop); //BUS CLOCK=16M_asm(nop);while(!(CRGFLG_LOCK==1)); //when pll is steady ,then use it;CLKSEL_PLLSEL =1; //engage PLL to system;}void Dly_ms(int ms){int ii,jj;if (ms<1) ms=1;for(ii=0;ii<ms;ii++)for(jj=0;jj<2670;jj++);//busclk:16MHz--1ms}static void SCI_Init(void){SCI0CR2=0x2c; //enable Receive Full Interrupt,RX enable,Tx enableSCI0BDH=0x00; //出口波特率为9600SCI0BDL=0x68;//SCI0BDL=busclk/(16*SCI0BDL)//busclk 8MHz, 9600bps,SCI0BDL=0x34//busclk 16MHz, 9600bps,SCI0BDL=0x68//busclk 24MHz, 9600bps,SCI0BDL=0x9C} //busclk 32MHz, 9600bps,SCI0BDL=0xD0static void IOC_Init(void){ PERT = 0XFF;//PPST=0XFF;DDRT=0XFe;PBCTL=0X50;//PT0 PIN,PACN10 16BIT,FALLing edge,NOT INTERRUPT//PACN0=200;//PACN1=0xFF;TCTL4=0x01;//40表示ICx禁止, 1表示上升沿, 2表示下降沿, 3表示任何沿//TIE =0x00;//每一位对应相应通道中断允许,0表示禁止中断TIOS =0xfe;//每一位对应通道的: 0输入捕捉,1输出比较}unsigned int get_Speed(){int Speed;Speed = PACN10;PACN10 = 0;return Speed;}void s_PID_MPWM(){L_u[2] = L_u[1];L_u[1] = L_u[0];L_u[0] = P_Speed - search_PACN10;Mp = Kp * L_u[0];Mi = Mi + Ki * L_u[0];Md = Kd * (L_u[0] - 2 * L_u[1] + L_u[2]);Mp2 = Kp2 * (L_u[0] - L_u[1]);if(((Ki * L_u[0]) > E)||((Ki * L_u[0]) < -E)) q = 0; else q = 1;MPWM = MPWM + Mp + q * Mi + Md + Mp2;if(MPWM > 1000) MPWM = 1000;if(MPWM < -1000) MPWM = -1000;Set_MPWM(MPWM);}void r_PID_MPWM(){L_u[2] = L_u[1];L_u[1] = L_u[0];L_u[0] = P_Speed - search_PACN10;Mp = rKp * L_u[0];Mi = Mi + rKi * L_u[0];Md = rKd * (L_u[0] - 2 * L_u[1] + L_u[2]);Mp2 = rKp2 * (L_u[0] - L_u[1]);if(((rKi * L_u[0]) > E)||((rKi * L_u[0]) < -E)) q = 0; else q = 1;MPWM = MPWM + Mp + q * Mi + Md + Mp2;if(MPWM > 1000) MPWM = 1000;if(MPWM < -1000) MPWM = -1000;Set_MPWM(MPWM);}//*******************舵机PID控制函数*************************void s_PID_SPWM(){sL_u[2] = sL_u[1];sL_u[1] = sL_u[0];sL_u[0] = ((current_corrd+last_corrd[0][1]) - (last_corrd[0][2]+last_corrd[0][3]))/2;//*********计算PID值***********if(sL_u[0]<20 && sL_u[0]>-20){s_sMp = s_sKp * sL_u[0];s_sMi = s_sMi + s_sKi * sL_u[0];s_sMd = s_sKd * (sL_u[0] - 2 * sL_u[1] + sL_u[2]);s_sMp2 = s_sKp2 * (sL_u[0] - sL_u[1]);//*****************************//***********I项溢出防止*******if(((s_sKi * sL_u[0]) > s_sE)||((s_sKi * sL_u[0]) < -s_sE)) s_sq = 0;else s_sq = 1;//*****************************SPWM = SPWM + s_sMp + s_sq * s_sMi + s_sMd + s_sMp2;//*********PWM溢出防止*********if(SPWM > 70) SPWM = 70;if(SPWM < -70) SPWM = -70;//*****************************}else sL_u[0] = sL_u[1];Set_SPWM(SPWM);}//****************************************** ****************void show_SPWM_data(){for(j = 0; j < 15; j++){printp("%10d", SPWM_sate[j]);}}void show_MPWM_data(){for(j = 0; j < 11; j++){printp("%10d", MPWM_sate[j]);}}void show_corrd_data(){for(j = 0; j < 23; j++){printp("%6d", corrd_sate[j]);}}void show_corrd_time(){for(j = 0; j < 512; j++){printp("%c", corrd_time[j]);}}void show_SPWM_time(){for(j = 0; j < 512; j++){printp("%c", SPWM_time[j]);}}void show_MPWM_time(){for(j = 0; j < 512; j++){printp("%c", MPWM_time[j]);}}void show_PACN_time(){for(j = 0; j < 512; j++){printp("%c", PACN_time[j]);}}void main(void) {/* put your own code here */setbusclock();SCI_Init();AD_Init();IOC_Init();Ini_PWM();EnableInterrupts;for(;;){Dly_ms(10);//*************红外滤波*************************for(j=0; j<10; j++){adc_get(ad_value);for(i = 1; i<12; i++){if(ad_value[i]>=160){data[i] = 1;num = num + 1;}else data[i] = 0;sum = sum + data[i] * 2*i;//printp("%10d", data[i]);}if(num == 0){corrd[j] = -1;}else{corrd[j] = sum/num;}sum = 0;num = 0;}is_white = 0;for(i=0;i<10;i++){if(corrd[i] < 0){is_white++;}for(j=i;j<10;j++){T_corrd = corrd[j];corrd[j] = corrd[i];corrd[i] = T_corrd;}}if(is_white > 6){numb = 0;}sum = 0;j = 0;for(i=3; i<8; i++){if(corrd[i] > -1){sum += corrd[i];j++;}}if(j!=0){current_corrd = sum/j;numb = j;}L_num[9] = L_num[8];L_num[8] = L_num[7];L_num[7] = L_num[6];L_num[6] = L_num[5];L_num[5] = L_num[4];L_num[4] = L_num[3];L_num[3] = L_num[2];L_num[2] = L_num[1];L_num[1] = L_num[0];L_num[0] = numb;//****************************************************wb = L_num[1]|L_num[2]|L_num[3]|L_num[4]|L_num[5]| L_num[6]|L_num[7]|L_num[8]|L_num[9];if(numb == 0){DDRB = 0x80;if(search_PACN10 > 10 && wb>0){P_Speed = 20;if(lw == 0){SPWM = -70;}else{SPWM = 72;}//if(sum_corrd<220&&sum_corrd>-220)Set_MPWM(0);// else// Set_MPWM(700);}else{P_Speed = 20;if(lw == 0){SPWM = -70;}else{SPWM = 72;}Set_MPWM(1000);}//Set_MPWM(0);}else{DDRB = 0x00;// printp("%d ", current_corrd);// if(PTJ & 0x80) Set_MPWM(0);//elseif(wb == 0){Set_MPWM(1000);}else{switch(current_corrd){case 2: SPWM = -70;P_Speed = 32;r_PID_MPWM();lw = 0;break;case 3: SPWM = -60;P_Speed = 32;r_PID_MPWM();lw = 0;break;case 4: SPWM = -50;P_Speed = 32;r_PID_MPWM();lw = 0;break;case 5: SPWM = -40;P_Speed =32;r_PID_MPWM();lw = 0;break;case 6: SPWM = -30;P_Speed = 32;r_PID_MPWM();lw = 0;break;case 7: SPWM = -25;P_Speed = 32;r_PID_MPWM();lw = 0;break;case 8: SPWM = -20;P_Speed = 32;r_PID_MPWM();lw = 0;break;case 9: SPWM = -15;P_Speed = 32;s_PID_MPWM();lw = 0;break;case 10: SPWM = -10; P_Speed = 32;s_PID_MPWM();lw = 0;break;case 11: SPWM = -5; P_Speed = 32;s_PID_MPWM();lw = 0;break;case 12: SPWM = 0; P_Speed = 32;s_PID_MPWM();break;case 13: SPWM = 5; P_Speed = 32;s_PID_MPWM();lw = 1;break;case 14: SPWM = 10; P_Speed = 32;s_PID_MPWM();lw = 1;break;case 15: SPWM = 15; P_Speed = 32;s_PID_MPWM();lw = 1;break;case 16: SPWM = 20; P_Speed = 32;r_PID_MPWM();lw = 1;break;case 17: SPWM = 25; P_Speed = 32;r_PID_MPWM();lw = 1;break;case 18: SPWM = 30; P_Speed = 32;r_PID_MPWM();lw = 1;break;case 19: SPWM = 40; P_Speed = 32;r_PID_MPWM();lw = 1;break;case 20: SPWM = 50; P_Speed = 32;r_PID_MPWM();lw = 1;break;case 21: SPWM = 60; P_Speed = 32;r_PID_MPWM();lw = 1;break;case 22: SPWM = 70; P_Speed = 32;r_PID_MPWM();lw = 1;break;default: break;}//Set_MPWM(500);}}last_corrd[0][3] = last_corrd[0][2];last_corrd[0][2] = last_corrd[0][1];last_corrd[0][1] = last_corrd[0][0];last_corrd[0][0] = current_corrd;corrd_sate[current_corrd]++;SPWM_sate[SPWM/10+7]++;MPWM_sate[MPWM/10]++;corrd_time[t]=current_corrd;SPWM_time[t]=SPWM+70;MPWM_time[t]=(MPWM+1000)/10;s_PID_SPWM();//Set_SPWM(SPWM);//Set_MPWM(500);search_PACN10 = PACN10;//printp("%10d", search_PACN10);PACN_time[t]=search_PACN10;PACN10 = 0;sum_corrd = 0;sum = 0;start_flag = 0;num = 0;t++;if(t==512){MPWM=0;for(;;);}} /* wait forever *//* please make sure that you never leave this function */}void interrupt 20 SCI0RX(void) {byte result,temp;DisableInterrupts;temp=SCI0SR1; /*clear flag*/result=SCI0DRL;if(result=='s'||result=='S') show_SPWM_data();if(result=='M'||result=='m') show_MPWM_data(); if(result=='c'||result=='C') show_corrd_data();if(result=='1'||result=='!') show_corrd_time();if(result=='2'||result=='@') show_SPWM_time();if(result=='3'||result=='#') show_MPWM_time();if(result=='4'||result=='$') show_PACN_time();if(result=='a'||result=='A'){MPWM=0;for(;;)EnableInterrupts;;}EnableInterrupts;}。

飞思卡尔摄像头程序

}
/*************************************************************/
/* 行场中断初始化函数 */
/*************************************************************/
#include <hidef.h>
#include "derivative.h"
#include <mc9s12xs128.h>
#define ROW 40 //数字摄像头所采集的二维数组行数
#define COLUMN 120 //数字摄像头所采集的二维数组列数
PLLCTL_PLLON=1; //turn on PLL
SYNR =0xc0 | 0x09;
REFDV=0x80 | 0x01;
POSTDIV=0x00; //pllclock=2*osc*(1+SYNR)/(1+REFDV)=160MHz;
#define ROW_START 10 //数字摄像头二维数组行开始行值
#define ROW_MAX 200 //数字摄像头所采集的二维数组行最大值
#define THRESHOLD 0x68 //图像阈值，根据所采集图像亮度值大小的实际情况调整（OV7620所采集的亮度值大小为0--255）
/*************************************************************/
void main(void)
{
/* put your own code here */

飞思卡尔智能车摄像头代码

系统初始化区域
**************************************************************************/
void SystemInit(void)
{ AbsoluteTime = 0; // 初始化系统时钟
DDRB = 0xff; // 初始化PORTB，用于数据输出
}
else
{
ie+=e;
speed_0=speed_0-(Kp*e/10)-(int)(Ki*ie/1000)+Kd*(e-e_1);
SetSpeed();
}
}
void SetSpeed(void)
{ if(speed_0>2400)
speed_0=2400;
int e=0,e_1=0;
int ie=0;
int Kp=35;
int Kd=0;
int Ki=10;//5;
int PACN0_i;
int SpeedMin=90,SpeedMax=400;
char a;
/***********************************************************************
PWMCNT23 = 0xFF;
PWMCNT45 = 0xFF;
PWMCNT67 = 0xFF;
PWMPER01 = 60000; // 驱动前轮舵机，周期20ms，频率50Hz
PWMPER23 = 2400; //周期0.1ms，频率10kHz
PWMPER67 = 2400;
PBCTL_PBEN=1;//16位累加器B使能

飞思卡尔智能车光电源代码

1.#include <hidef.h>2.#include <mc9s12dg128.h>3.#pragma LINK_INFO DERIV ATIVE "mc9s12dg128b"4.//=========================public variable=====================5.//-----------------------turning variable------------------6.unsigned char sam_g[15]; //道路检测值7.unsigned int angle_data; //舵机转角8.int car_positn; //赛车当前位置参数9.int pre_positn;10.unsigned int black_sensor_number; //检测到黑线的传感器个数11.int positn_temp[10];12.unsigned int t=0;13.//---------------------speed variable---------------------14.unsigned char dir_flag; //方向标志，为1表示检测到有效路径，可以给驱动力15.unsigned char brake_flag; //刹车标志位判断当前是否需要刹车16.unsigned int car_driver; //驱动力控制17.unsigned int pulse_count; //速度检测统计脉冲个数18.unsigned int ideal_speed; //车的理想速度19.unsigned int times; //丢失黑线的次数20.int speed_error; //理想与实际速度偏差值21.int pre_error; //速度PID 前一次的速度误差值ideal_speed- pulse_count22.int pre_d_error; //速度PID 前一次的速度误差之差d_error-pre_d_error23.int pk; //速度PID值24.//---------------------start_line variable-------------------25.unsigned char start_line_acc; //统计检测起跑线次数26.unsigned char finish_flag; //起跑线标志位，为1表示检测到起跑线3次27.//----------------------dis_play variable--------------------28.unsigned int start_flag,start_count;29.//---------------------table-------------------------30.unsigned charspeed_table11[13]={270,260,250,240,200,180,180,180,170,140,140,100,90}; //15.0s31.unsigned char speed_table21[13]= {25,24,23,20,19,17,17,17,15,12,11,10,9}; //15.0s32.unsigned char speed_table12[13]= {29,28,27,26,25,20,20,20,19,17,15,10,9}; //15.0s33.unsigned char speed_table22[13]= {27,26,25,24,20,18,18,18,17,14,14,10,9}; //15.0s34.unsigned int circle; //控制赛车跑几圈停车35.#define kp 2000//200036.#define ki 5//537.#define kd 10//1038.#define Angle_Center 4344 //舵机中心位置39.#define lose_limit 30000 //丢失黑线后滑翔时间40.void data_init(void);41.void crg_init(void); // 锁相环初始化42.void pwm_init(void); // PWM信号初始化43.void ect_init(void); // ECT初始化44.void sam_position(void); //读结果45.void check_start(void); //起跑线检测函数46.void car_position(void); //计算car_positn47.void angle(void); //计算转角48.void speed(void); //计算速度49.void driver(void); //驱动50.void pre_start(void);51.void delay(void);52.void found_start(void);53.void stop(void);54.void pid(void);55.unsigned int absolute(int);56.//========================main loop============================57.void main(void)58.{59. data_init(); //设置基本数据60. crg_init(); //锁向环初始化61. ect_init(); //ECT62. pwm_init(); //初始化PWM63. pre_start();64. EnableInterrupts;65. for(;;)66. {67.sam_position(); //读采样值68.check_start(); //检测起跑线69.car_position(); //计算car_positn70.angle(); //计算转角71.speed(); //计算速度72.driver(); //拐弯驱动73. }74.}75.//--------------------data_init--------------------76.void data_init(void)77.{78. start_line_acc=0;79. finish_flag=0;80. DDRA=0X00;81. DDRB=0X00;82. times=0;83.}84.//-------------------pre_start------------------85.void pre_start(void)86.{87.unsigned int i;88.PWMDTY01=Angle_Center;89.PWMDTY67=0;90.for(i=0;i<10;i++) delay();91.PWMDTY23=0;92.}93.//----------------------crg_init-------------------94.void crg_init(void)95.{96.SYNR=0x02;97.REFDV=0x01;98.while((CRGFLG & 0x08)==0 );99.CLKSEL =0x80;100.}101.//--------------------pwm_init------------------------102.void pwm_init(void)103. {104.PWMCTL=0xB0; // 设置通道76、32、10级连105.PWME=0x00; // 通道禁止输出;106.PWMPRCLK=0x12;//预分频:A_CLK=busclk/2^2=6M B_CLK=BUSCLK/2^1=12M 107.PWMSCLA=0x01; //SA_CLK=A_CLK/(2*1)==3MHz108.PWMSCLB=0X01; //SB_CLK=B_CLK/(2*1)==6MHz109.PWMPOL=0x8A; //极性选择起始为高电平;110.PWMCLK=0x8A; //PWM01 选择SA_CLK PWM23 67选择SB_CLK111.PWMCNT0=0x00;112.PWMCNT1=0x00;113.PWMCNT2=0x00;114.PWMCNT3=0x00;115.PWMCNT6=0x00;116.PWMCNT7=0x00;117.PWMPER01=60000; // 周期==(1/3M)*(60000)=20ms118.PWMPER23=10000; // F=6M/10000==600Hz119.PWMPER67=10000; // F=6M/10000==600Hz120.PWMCAE=0x00; //左对齐方式121.PWME=0x82; // 通道1,7输出使能;122.}123.//-----------------------ect_init-------------------------124.void ect_init(void)125.{126.TCTL4=0x01; // Set the rising endge for PT0.127.PACN10=0x0000;128.PBCTL=0x40; //pt0 and pt1 级联成16位计数器129.MCCNT=60000; //60000*24M/16=40ms130.MCCTL=0xC7;131.TSCR1=0x10;132.}133.void sam_position(void)134.{135.sam_g[1]= PORTA_PA4;136.sam_g[2]= PORTA_PA3;137.sam_g[3]= PORTA_PA2;138.sam_g[4]= PORTA_PA1;139.sam_g[5]= PORTA_PA0;140.sam_g[6]= PORTB_PB0;141.sam_g[7]= PORTB_PB1;142.sam_g[8]= PORTB_PB2;143.sam_g[9]= PORTB_PB3;144.sam_g[10]= PORTB_PB4;145.sam_g[11]= PORTB_PB5;146.sam_g[12]= PORTB_PB6;147.sam_g[13]= PORTB_PB7;148.}149.//----------------------check_start---------------------150.void check_start(void)151.{152.unsigned char i,j=0;153.start_flag=0;154.for(i=1;i<14;i++)155.if(sam_g^sam_g[i+1])156. j++;157. if(j>=4)158. {159. if(sam_g[5] &&((!sam_g[4])&&(!sam_g[6])) &&((!sam_g[3])&&(!sam_g[7]))&&(sam_g[1]&&sam_g[10])160.) start_flag=1;161.else if(sam_g[5]&&sam_g[6] &&((!sam_g[4])&&(!sam_g[7]))&&((!sam_g[3])&&(!sam_g[8]))&&(sam_g[1]&&sam_g[10])162.) start_flag=1;163.else if( sam_g[6] &&((!sam_g[5])&&(!sam_g[7])) &&((!sam_g[4])&&(!sam_g[8])) &&(sam_g[1]&&sam_g[11])164.) start_flag=1;165.else if( sam_g[6]&&sam_g[7] &&((!sam_g[5])&&(!sam_g[8])) &&((!sam_g[4])&&(!sam_g[9])) &&(sam_g[2]&&sam_g[11])166.) start_flag=1;167.else if( sam_g[7] &&((!sam_g[6])&&(!sam_g[8])) &&((!sam_g[5])&&(!sam_g[9])) &&(sam_g[2]&&sam_g[12])168.) start_flag=1;169.else if( sam_g[7]&&sam_g[8] &&((!sam_g[6])&&(!sam_g[9]))&&((!sam_g[5])&&(!sam_g[10])) &&(sam_g[3]&&sam_g[12])170.) start_flag=1;171.else if( sam_g[8] &&((!sam_g[7])&&(!sam_g[9])) &&((!sam_g[6])&&(!sam_g[10])) &&(sam_g[3]&&sam_g[13])172.) start_flag=1;173.else if( sam_g[8]&&sam_g[9]&&((!sam_g[7])&&(!sam_g[10])) &&((!sam_g[6])&&(!sam_g[11])) &&(sam_g[4]&&sam_g[13])174.) start_flag=1;175.else if( sam_g[9] &&((!sam_g[8])&&(!sam_g[10])) &&((!sam_g[7])&&(!sam_g[11])) &&(sam_g[4]&&sam_g[13])176.) start_flag=1;177.}178.if(start_flag)179. art_count++;180.else181. art_count=0;182.if(start_count==2)183.{184.found_start();185.start_count=0;186.}187.if(start_line_acc==2)188.{189.finish_flag=1;190.}191.}192.//--------------------------car_position------------------------。

飞思卡尔摄像头组智能车程序代码

//---------------------------------------------------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----------------------------------。

飞思卡尔单片机的程序

#include "derivative.h"//-----------------------------------------------------static void SCI_Init(void){SCI0CR2=0x2c; //enable Receive Full Interrupt,RX enable,Tx enableSCI0BDH=0x00; //busclk 8MHz,19200bps,SCI0BDL=0x1aSCI0BDL=0x68; //SCI0BDL=busclk/(16*SCI0BDL)//busclk 8MHz, 9600bps,SCI0BDL=0x34//busclk 8MHz, 9600bps,SCI0BDL=0x68//busclk 24MHz, 9600bps,SCI0BDL=0x9C} //busclk 32MHz, 9600bps,SCI0BDL=0xD0//busclk 40MHz, 9600bps,SCI0BD =0x104//-----------------------------------------------------static void Port_Init(void){DDRA = 0xff; //LCD1100,PA0--4,PA67 D1D2PORTA= 0x00;DDRB = 0xff; //LED PTB0--7,PORTB= 0xff; //LEDs onDDRE = 0xFF; //MOTOR CONTROLPORTE= 0x00; //PDDRH = 0x00; // PORTH inputPTIH = 0X00; // KEY,PH0--5PERH = 0xff; // PORTH pull upPPSH = 0x00; // Port H Polarity Select Register-falling edgePIEH = 0x02; // PORTH interrut disable but 1,DDRJ = 0X01; // PJ0判断行同步脉冲到达//PPSJ = 0x01; // Port J Polarity Select Register-rising EDGEPPSJ = 0x00; // Port J Polarity Select Register-falling EDGEPIEJ = 0X00; // VIDEO SYNC INTERRUPT DISABLED,BUT NOT IN MAIN() PERJ = 0xff;DDRP = 0xff;PERP = 0xff;PTP_PTP0 = 0;}//-----------------------------------------------------static void PWM_Init(void){//SB,B for ch2367//SA,A for ch0145PWMPRCLK = 0X55; //clockA,CLK B 32分频:500khzPWMSCLA = 0x02; //对clock SA 进行2*PWMSCLA=4分频；pwm clock=clockA/4=125KHz;PWMSCLB = 0X02; //clk SB=clk B/(2*pwmsclb)=125KHZ//pwm1PWMCNT1 = 0;PWMCAE_CAE1=0;PWMPOL_PPOL1=0;PWMPER1 =125;PWMDTY1 =100;PWMCLK_PCLK1 = 1;PWME_PWME1 = 0;}void AD_Init(void){A TD0CTL1=0x00; //7:1-外部触发,65:00-8位精度,4:放电,3210:chA TD0CTL2=0x40; //禁止外部触发, 中断禁止A TD0CTL3=0xa0; //右对齐无符号,每次转换4个序列, No FIFO, Freeze模式下继续转A TD0CTL4=0x01; //765:采样时间为4个AD时钟周期,ATDClock=[BusClock*0.5]/[PRS+1]A TD0CTL5=0x30; //6:0特殊通道禁止,5:1连续转换,4:1多通道轮流采样A TD0DIEN=0x00; //禁止数字输入}//-----------------------------------------------------//IOC7/PT7用于计算CS3144产生的脉冲数static void IOC_Init(void){TCTL3=0xc0;//c-输入捕捉7任何沿有效,TCTL4=0xc0;//40表示ICx禁止, 1表示上升沿, 2表示下降沿, 3表示任何沿TIE =0x00;//每一位对应相应通道中断允许,0表示禁止中断TIOS =0x00;//每一位对应通道的: 0输入捕捉,1输出比较TCTL3_EDG7x=1;//c-输入捕捉7任何沿有效,}//产生40ms的定式中断，读取IOC7的计数值static void Timer_Init(void){//TSCR1=0X80;//TIMER INT ENABLED//TSCR1=0x90;//计数器使能TEN|快速清零TFFCATSCR1=0X00; //禁止TIM//TSCR2=0X80;//DIV 1->2.5ms,enable time overflow interrrupt//TSCR2=0X82;//DIV 4->10ms//TSCR2=0X83;//DIV 8->20ms//TSCR2=0X84;//DIV 16->40msTSCR2=0X85;//DIV 32->80ms//TSCR2=0X86;//DIV 64->160ms//TSCR2=0X87;//DIV 128->320ms,enable time overflow interrruptTCNT =0; //PACTL=0X50; //PT7 PIN,PACN32 16BIT,FALLing edge,NOT INTERRUPT //PBCTL=0X40;//PBCN10 16BIT,INT DISABLED//ICPAR=0; //8BIT DISABLED}//-----------------------------------------------------// setup of the RTI interrupt frequencystatic void RTI_Init(void){//RTICTL=0x10; //2^10x40ms=4.96s//RTICTL=0X74; //SET PRESCALER,div rate=(m+1)x2^(n+9),(m=1-7,n=0-15)//tick=16Mhz/((4+1)x2^(7+9))=48.83,(/sec)//16000000/64k=244.140625 ,与晶振频率相关，与分频无关RTICTL=0x77; //8x2^16 =>32,75ms,30.5175Hz//RTICTL=0x7f; //16x2^16 =>,65.536ms,15.26Hz//RTICTL=0x1F; //16x2^10--1ms//CRGINT=0X80; //enable RTI InterruptCRGINT=0X00; //disable RTI Interrupt}static void Time_Start(void){RTI_Init();CRGINT=0X80; //enable RTI Interrupt}//-----------------------------------------------------// PLL初始化子程序BUS Clock=16Mvoid setbusclock(void){CLKSEL=0X00; // disengage PLL to systemPLLCTL_PLLON=1; // turn on PLLSYNR=0x00 | 0x01; // VCOFRQ[7:6];SYNDIV[5:0]// fVCO= 2*fOSC*(SYNDIV + 1)/(REFDIV + 1)// fPLL= fVCO/(2 × POSTDIV)// fBUS= fPLL/2// VCOCLK Frequency Ranges VCOFRQ[7:6]// 32MHz <= fVCO <= 48MHz 00// 48MHz < fVCO <= 80MHz 01// Reserved 10// 80MHz < fVCO <= 120MHz 11REFDV=0x80 | 0x01; // REFFRQ[7:6];REFDIV[5:0]// fREF=fOSC/(REFDIV + 1)// REFCLK Frequency Ranges REFFRQ[7:6]// 1MHz <= fREF <= 2MHz 00// 2MHz < fREF <= 6MHz 01// 6MHz < fREF <= 12MHz 10// fREF > 12MHz 11// pllclock=2*osc*(1+SYNR)/(1+REFDV)=32MHz;POSTDIV=0x00; // 4:0, fPLL= fVCO/(2xPOSTDIV)// If POSTDIV = $00 then fPLL is identical to fVCO (divide by one)._asm(nop); // BUS CLOCK=16M_asm(nop);while(!(CRGFLG_LOCK==1)); //when pll is steady ,then use it;CLKSEL_PLLSEL =1; //engage PLL to system;}//-----------------------------------------------------#pragma CODE_SEG DEFAULTvoid Init_Dev(void){setbusclock();Port_Init();SCI_Init();PWM_Init();AD_Init();Timer_Init();Time_Start();IOC_Init();}//-----------------------------------------------------。

飞思卡尔智能车程序代码

#define sciflag 1;
#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);

飞思卡尔智能车完整程序

#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。

飞思科尔光电组程序

我是光电组的，下面是程序，小车可以跑起来，但速度有待提升#include <hidef.h> /* common defines and macros */#include <MC9S12XS128.h> /* derivative information */#pragma LINK_INFO DERIVATIVE "mc9s12xs128"//static unsigned chardirection_turn[12]={888,1098,1208,1328,1481,1612,1730,1856,1988,2100,22 22,2368};//Chapter 12//Periodic Interrupt Timer (S12PIT24B4CV1) Page349//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}; //转向给定值初始化short speed_set[7]={250,300,350,400,350,300,250}; //速度给定值short speed_flag=0; //速度档位标志位short speed[3]={0,0,0}; //速度检测函数short pulse_count=0; //编码器脉冲计数值short speed_expect=0; //理想速度short kp=2; //比例环节short ki=0; //积分环节short kd=1; //微分环节short ek1=0; //误差1short ek2=0; //误差2short ek3=0; //误差3short speed_add=0; //速度增量//========================================================== =============================void PLL_Init() //时钟初始化{REFDV=0x81; /*PLLCLK=2*OSCCLK*(SYNR+1)/(REFDV+1)*/SYNR=2; /* 锁相环时钟=2*16*(2+1)/(1+1)=48MHZ */while(!(CRGFLG&0x08)); /* 总线时钟=48/2=24MHZ */CLKSEL=0x80;}void PWM_Init() //PWM初始化{PWME=0x00; //关闭PWM使能PWMPRCLK=0x66; //A,B时钟均为总线的64分频,375KHZ//PWMSCLA=0x01; //clockSA=clockA/(2*PWMSCLA) = 1500KHZ //PWMSCLB=0X01; //clockSB=clockB/(2*PWMSCLB) =1500KHZ PWMCLK=0x00;PWMPOL=0xFF; //PWM输出起始电平为高电平PWMCAE=0x00; //输出左对齐PWMCTL=0xf0; //通道01，23，45，67级联PWMPER01=5999; //舵机频率为62.5HzPWMDTY01=647; //占空比a=（PWMDTY01+1）/(PWMPER01+1) PWMPER23=1000; //PWM通道3周期为375HZPWMDTY23=0; //占空比a=（PWMDTY01+1）/(PWMPER01+1)占空比50% ~~ 150PWMPER45=1000; //PWM通道5周期为0.10ms 10KZH300=0.00010/(1/3000000)PWMDTY45=300; //占空比a=（PWMDTY01+1）/(PWMPER01+1)PWMPER67=375; //频率为1000HzPWMDTY67=200; //PWME=0xff; //使能pwm}void Pit0_Init() //PIT初始化{PITCFLMT_PITE=0; //关PIT使能PITCE_PCE0=1; //通道0使能PITMUX_PMUX0=0; //通道0接微时钟0PITMTLD0=99; //微时钟0值设置为7fPITLD0=3839; //time-out period = (PITMTLD + 1) * (PITLD + 1) / fBUS.//时间计算100*3840/24000000=0.016s PITINTE_PINTE0=1; //通道0中断时能PITCFLMT_PITE=1; //PIT使能}void ECT_Init(){TIOS=0x00; /* OC0路为输出比较，OC1路为输入捕捉*/TSCR2=0x06; /* 定时器溢出中断禁止，计数器自由运行禁止复位，64分频*/TSCR1=0x80; /* 定时器使能*/TIE=0x01; /* 输出比较相应中断使能*/TCTL4=0x01;}void dly_1ms(){int i,j;for(i=0;i<200;i++){for(j=0;j<1000;j++){;}}}void sam_position() //车位检测函数{int i=0,j=0;unsigned char m=0,n=0;n=PORTA;for(i;i<10;i++){m=PORTA;if(n==m)j++;}if(j>6)light=n;}void check_start() //检测起始线{if((light&&4)||(light&&16))start_flag++;}void check_start(){}void turning() //舵机转向函数{switch(light){case 1:if(turn_value==direction_turn[1]) //出界判断算法{turn_value=direction_turn[0];speed_expect=speed_set[0];}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]){turn_value=direction_turn[1];speed_expect=speed_set[1];}else if(turn_value==direction_turn[1]){turn_value=direction_turn[1];speed_expect=speed_set[1];}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]){turn_value=direction_turn[2];speed_expect=speed_set[2];}else if(turn_value==direction_turn[2]){turn_value=direction_turn[2];speed_expect=speed_set[2];}else if(turn_value==direction_turn[3]){turn_value=direction_turn[2];speed_expect=speed_set[2];}break;case 8:if(turn_value==direction_turn[2]){turn_value=direction_turn[3];speed_expect=speed_set[3];}else if(turn_value==direction_turn[3]){turn_value=direction_turn[3];speed_expect=speed_set[3];}else if(turn_value==direction_turn[4]){turn_value=direction_turn[3];speed_expect=speed_set[3];}break;case 16:if(turn_value==direction_turn[3]){turn_value=direction_turn[4];speed_expect=speed_set[4];}else if(turn_value==direction_turn[4]){turn_value=direction_turn[4];speed_expect=speed_set[4];}else if(turn_value==direction_turn[5]){turn_value=direction_turn[4];speed_expect=speed_set[4];}break;case 32:if(turn_value==direction_turn[4]){turn_value=direction_turn[5];speed_expect=speed_set[5];}else if(turn_value==direction_turn[5]){turn_value=direction_turn[5];speed_expect=speed_set[5];}else if(turn_value==direction_turn[6]){turn_value=direction_turn[5];speed_expect=speed_set[5];}break;case 64:if(turn_value==direction_turn[5]) //出界判断算法{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];}break;default:break;}PWMDTY01=turn_value;}void check_speed() //速度检测函数{ek3=ek2; //计算速度差值ek2=ek1;ek1=speed_expect-pulse_count;speed[2]=speed[1]; //当前速度放在[0],之前放在[1],[2]speed[1]=speed[0];speed[0]=pulse_count;pulse_count=0;}void speed_down() //制动函数{PWMDTY23=300; //电机反向供电PWMDTY45=0;}void speed_pid() //PID算法{speed_add=kp*(ek1-ek2)+ki*ek1+kd*(ek1-2*ek2+ek3); //PID增量式}void driver() //驱动电机控制函数{//if(((turn_value>705)||(turn_value<560))&&(speed[0]>200)) //当前速度若远超给定速度// {//speed_down();// }//else// {PWMDTY23=0;speed_pid();PWMDTY45=PWMDTY45+speed_add;// }if(PWMDTY45>600)PWMDTY45=600;}void main(){DisableInterrupts; /* 关中断*/PLL_Init();PWM_Init();Pit0_Init();ECT_Init();turn_value=direction_turn[3];DDRA=0x00;DDRB=0xFF;PORTB=0X00;EnableInterrupts;for(;;){//sam_position();//turning();}}#pragma CODE_SEG NON_BANKEDvoid interrupt 8 Timer0_ISR(void){pulse_count++;TFLG1_C0F=1; /* TC0端有中断产生*/ }void interrupt 66 PIT0_ISR(void){sam_position(); //车位检测函数turning(); //舵机转向函数check_speed(); //速度检测函数driver(); // 驱动电机控制函数PITTF_PTF0=1; /* PIT0端有中断产生,清除标志位*/ }。

飞思卡尔电磁组程序范例

if(ch_4<255){
flag4=9; f4=1;
}
if(ch_4>=255){
flag4=0; f4=0;
}
if(ch_5<255){
flag5=1100;f5=1;
}
if(ch_5>=255){
flag5=0; f5=0;
}
if(ch_6<255){
flag6=13; f6=1;
unsigned char* SensorData, unsigned short SensorCount,
unsigned char* CCDData, int CCDWidth, int CCDHeight,
unsigned short* MotorPWM, unsigned short* SteerPWM);
void SignalProcess();
void PWMout(int, int,int);
void Memoryroad(void);
void Roadplay(void);
void PIDinit(void);
void delay(int cnt);
void SteerContral(void);
sPID.Kp=1500; //比例常数 Proportional Const
sPID.Ki=600; //积分常数 Integral Const
sPID.Kd=60; //微分常数 Derivative Const
stError=0; //Error[-1]
int NextPoint;
int ThisPoint; //设定目标 Desired value

飞思卡尔程序

#include <hidef.h> /* common defines and macros */ #include <MC9S12XS128.h> /* derivative information */ #pragma LINK_INFO DERIVATIVE "mc9s12xs128" #pragma MESSAGE DISABLE C12056 /*屏蔽掉这个信息
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; //输出

飞思卡尔详细代码

主程序/*******************************************************************/ /* 飞思卡尔智能小车主函数/*/* 硬件平台: MC9S12XS128 晶振：12MHZ/* 软件平台: CodeWarriorIDE/* 描述：包括各个功能模块的调用/* 公司: 九江学院/* 作者日期注释 *//*~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~/* 耕耘者 2012-10-16/*******************************************************************/#include <mc9s12xs128.h>#include <hidef.h>#include "derivative.h"#include"pwm.h"#include"image.h"#include"sci.h"#include"KeyScan.h"#include"nokia5110.h"#include"picture.h"#include"pid.h"unsigned char StartDelay = 0;unsigned char DelayEn = 0;unsigned int Time2s = 0;unsigned int CarTime = 0;unsigned int CarTimeTemp = 0;unsigned int CarPulse = 0;unsigned int Speed;unsigned char IsCountSpeed = 0;unsigned char IsDisplayOn = 0;unsigned char THRESHOLD = 0X7D;unsigned int PIDFlag = 0;unsigned char PIDOn = 0;unsigned char keyflag = 1;unsigned char StartLineFlag = 0;/****************************************************************************** ** 函数名称: void PWMDuo_Duty(int duo_duty)** 功能描述: 舵机控制子函数******************************************************************************/ void PWMDuo_Duty(int duo_duty){PWMDTY01 = duo_duty;PWME_PWME1 = 1; // 使能PWM3}/***************************************************************************** ** 函数名称: void PWMdian_Duty(unsigned char motorValue)** 功能描述: 控制PWMDTY23的值从而控制输出的占空比使得电机获得不同的速度正转******************************************************************************/ void PWMdian_Duty(unsigned char motorValue){PWMDTY23 = motorValue;}/****************************************************************************** ** 函数名称: void PWMdian_DutyStop(unsigned char motorValue)** 功能描述: 控制PWMDTY45的值从而控制输出的占空比使得电机获得不同的速度反转******************************************************************************/ void PWMdian_DutyStop(unsigned char motorValue){PWMDTY45 = motorValue;}/****************************************************************************** ** 函数名称: unsigned char GetSpeed(unsigned int CurSpeed)** 功能描述: 获得小车行驶时的速度大小从而反馈给PID实现PID控制******************************************************************************/ unsigned char GetSpeed(unsigned int CurSpeed){CurSpeed = CurSpeed/10;if(CurSpeed>60&&CurSpeed<80){Dianji_data = 45;}if(CurSpeed>=80&&CurSpeed<130){Dianji_data = 50;}if(CurSpeed>=130&&CurSpeed<180){Dianji_data = 55;}if(CurSpeed>=180&&CurSpeed<220){Dianji_data = 60;}if(CurSpeed>=220&&CurSpeed<235){Dianji_data = 65;}if(CurSpeed>=235&&CurSpeed<270){Dianji_data = 70;}if(CurSpeed>=270&&CurSpeed<380){Dianji_data = 40;}return Dianji_data;}/***************************************************************************** ** 函数名称: void main(void)** 功能描述: 调用功能函数******************************************************************************/ void main(void){unsigned char clearFlag = 1;unsigned int PWM_Value;PLL_Init(); // 时钟初始化锁相环TIM_Init(); //定时器初始化IO_Init(); //IO端口初始化SCI_Init(); //串口初始化Init_pwm(); //pwm初始化PIT_Init();LCD_Init(); //nokia5110初始化LCD_clear(); //nokia5110清屏EnableInterrupts; //中断使能LCD_draw_bmp_pixel(0,0,picture,84,48);for(;;){if(IsScanKeyTime == 1){IsScanKeyTime = 0;KeyPro();KeyFunction();}if(PWMFlag!=0){DelayEn = 1;if(StartDelay == 1){StartDelay = 0;PWME_PWME3 = 1;}}else{PWME_PWME3 = 0;Time2s = 0; //保证在按按键之前time2s的值一直为0 }if(IsCountSpeed == 1){IsCountSpeed = 0;CarPulse = PulseNum;Speed = CarPulse*157/10; //每圈100个脉冲转轮周长157cmPulseNum = 0;}if(IsSetMode == 0){IsSetMode = 1;LCD_clear_half();Display_Gray(36,3,3);Display_GUI(0,4,14);}Display_Speed(36,5,4);//窜口发送（注意开了窜口发送会很占用系统资源）Traver_UartValue();PWM_Value = IncPIDCalc(GetSpeed(Speed)); //pid调节语句Dianji_data += PWM_Value;Get_SideLine();Car_Cotrol();PWMDuo_Duty(Duoji_data);PWMdian_Duty(Dianji_data);}}/***************************************************************************** ** 函数名称: void interrupt 66 PIT0(void)** 功能描述: 中断服务函数产出时间片******************************************************************************/ #pragma CODE_SEG __NEAR_SEG NON_BANKEDvoid interrupt 66 PIT0(void){static unsigned char PIDData = 0;PITTF_PTF0=1;++CarTimeTemp;++PIDFlag;if(flag%10 == 0){IsScanKeyTime = 1;}if(CarTimeTemp == 20) //100ms进行一次速度计算并清除计数值{IsCountSpeed = 1;CarTimeTemp = 0;}if(flag%30 == 0){IsDisplayOn = 1;}if(++flag == 6500){flag = 0;StartLineFlag = 1;}if(PIDFlag == 2000){PIDFlag = 0;PIDOn = 1;}if(DelayEn==1){Time2s++;if(Time2s==800) //5s定时{Time2s = 0;DelayEn=0;StartDelay = 1;}}}图像处理#include "image.h"#include <mc9s12xs128.h>#include "sci.h"#include "pid.h"#include "KeyScan.h"#define ROW 40#define COLUMN 120#define ROW_START 10#define ROW_MAX 200//#define THRESHOLD 0x7Dextern unsigned char THRESHOLD;unsigned char SampleFlag=0; //奇偶场标记unsigned int m=0;unsigned int Line;unsigned int hang;unsigned char a_Temp=0,b_Temp=0;unsigned char Dianji_data = 50;unsigned int Duoji_data;unsigned char Buffer[ROW][COLUMN]={0};unsigned char image_center[40]={0};unsigned int zuo_danxian=1,you_danxian=1;unsigned long WholeRoad = 0;unsigned long Road = 0;unsigned char smallSFlage = 0;unsigned int effectLine = 0;unsigned char flagg;//定每场采哪几行。

飞思卡尔智能车程序

#include <hidef.h> /* common defines and macros */#include <MC9S12XS128.h> /* derivative-specific definitions */#pragma LINK_INFO DERIV ATIVE "MC9S12XS128.h"/////////////////////////////////////////////////////word AD_Value[16];//AD转换结int Done_cache[20]={0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};//记忆int Angle_cache[20]={50,50,50,50,50,50,50,50,50,50,50,50,50,50,50,50,50,50,50,50};//记忆//////////////////////////////////////////////////////#define PAEN 6/////////////////////////////////////////////////////unsigned int i=0,Xmax=0,Ymax=0,Xmax2=0,Ymax2=0;unsigned int flag=0;unsigned int Snake_flag=0;unsigned int SenA=50;unsigned int Out_flag=0;unsigned int DuoJi_angle;unsigned int Turn_flag=0;unsigned int Keep_flag=0;unsigned int Run_Time_Count=0;///////////////////////////////////////////////////////////////////unsigned int Speed=0, Count=0;//count=2即4秒测一次速度unsigned int ii=0,tt=0;int Goal_speed=15;int E1=0;int E2=0;int DE=0;int B1=0;int B2=0;int OUT=0;//////////////////////////////////////////////////////#include "Initiate.h"#include "Basic.h"#include "5110.h"#include "Control.h"//////////////////////////////////////////////////////////////////////void main(void){SetBusCLK_64M();Dly_ms(600);DisableInterrupts;PWMINIT();initPIT(); //内部中断SciInit() ; //串口LCD_init(); //LCDADCInit(); //ADCInit_Event_Count(); //测速初始化PWMDTY0=0;PWMDTY1=0;PWMDTY23=1450;Dly_ms(100);DDRB=0x00;while(PORTB_PB6==1) ; //等待开始键按下PWMDTY1=0;PWMDTY0=0;DuoJi_angle=1450;PWMDTY23=DuoJi_angle;Dly_ms(600);EnableInterrupts;while(1);}//////////////////////////////////////////////////////////////////////////////// void Scan_A2(){Xmax=0;Ymax=0;Xmax2=0;Ymax2=0;for(i=0;i<8;i++){if(AD_Value[i]<=10) AD_Value[i]=0;}for(i=0;i<8;i++) ///////////max{if(AD_Value[i]>Ymax){Ymax=AD_Value[i];Xmax=i;}}for(i=0;i<8;i++) ///////////max2{if(AD_Value[i]>Ymax2 && AD_Value[i]<Ymax){Ymax2=AD_Value[i];Xmax2=i;}}if(Ymax!=0){if(Xmax>Xmax2) SenA=(90*(Xmax+Xmax2)+Ymax-Ymax2)/12; //SenA else if(Xmax<Xmax2){if((90*(Xmax+Xmax2))<(Ymax-Ymax2)) SenA=0;else SenA=(90*(Xmax+Xmax2)+Ymax2-Ymax)/12;}else SenA=(90*(Xmax+Xmax2))/12;if(SenA>100) SenA=100;if(Xmax<1 /*&& Xmax2==1 && Ymax2<40*/ ) //////Out_flag 0/1/2{Out_flag=1;}else if(Xmax>6 /*&& Xmax2==6 && Ymax2<40*/ ){Out_flag=2;}else Out_flag=0;}switch(Out_flag){case 1 : SenA=0;break;case 2 : SenA=100;break;}}///////////////////////////////////////////////////////////////////////////////////void Change_angle(){if(SenA>50) DuoJi_angle=(65*(SenA-50)+14500)/10;else DuoJi_angle=(14500-65*(50-SenA))/10;PWMDTY23=DuoJi_angle;}/////////////////////////////////////////////////////////////////////////////////////void Rem_angle() //记忆传感器角度20个{if(SenA<40) Done_cache[20]=1;else if(SenA>60) Done_cache[20]=2;else Done_cache[20]=0;Angle_cache[20]=SenA;for(ii=0;ii<20;ii++){Done_cache[ii]=Done_cache[ii+1];Angle_cache[ii]=Angle_cache[ii+1];}}///////////////////////////////////////////////////////////////////////////////////void Mod(){if(Done_cache[0]==0 && Done_cache[1]==0 && Done_cache[2]==0 && Done_cache[3]==0 && Done_cache[4]==0 && Done_cache[5]==0 && Done_cache[6]==0 && Done_cache[7]==0 && Done_cache[8]==0 && Done_cache[9]==0 && Done_cache[10]==0 && Done_cache[11]==0 && Done_cache[12]==0 && Done_cache[13]==0 && Done_cache[14]==0 && Done_cache[15]==0 && Done_cache[16]==0 && Done_cache[17]==1 && Done_cache[18]==1 && Done_cache[19]==1 && Done_cache[20]==1 && Angle_cache[19]<Angle_cache[18] && Angle_cache[18]<Angle_cache[17] ){Keep_flag=1;//直道左弯}else if(Done_cache[0]==0 && Done_cache[1]==0 && Done_cache[2]==0 && Done_cache[3]==0 && Done_cache[4]==0 && Done_cache[5]==0 && Done_cache[6]==0 && Done_cache[7]==0 && Done_cache[8]==0 && Done_cache[9]==0 && Done_cache[10]==0 && Done_cache[11]==0 && Done_cache[12]==0 && Done_cache[13]==0 && Done_cache[14]==0 && Done_cache[15]==0 && Done_cache[16]==0 && Done_cache[17]==2 &&Done_cache[18]==2 && Done_cache[19]==2 && Done_cache[20]==2 && Angle_cache[19]>Angle_cache[18] && Angle_cache[18]>Angle_cache[17] ){Keep_flag=2;//直道右弯}else if(Done_cache[0]==1 && Done_cache[1]==1 && Done_cache[2]==1 && Done_cache[3]==1 && Done_cache[4]==1 && Done_cache[5]==1 && Done_cache[6]==1 && Done_cache[7]==1 && Done_cache[8]==1 && Done_cache[9]==1 && Done_cache[10]==1 && Done_cache[11]==1 && Done_cache[12]==1 && Done_cache[13]==1 && Done_cache[14]==1 && Done_cache[15]==1 && Done_cache[16]==1 && Done_cache[17]==1 && Done_cache[18]==1 && Done_cache[19]==1 && Done_cache[20]==1 ){Keep_flag=3;//左弯}else if(Done_cache[0]==2 && Done_cache[1]==2 && Done_cache[2]==2 && Done_cache[3]==2 && Done_cache[4]==2 && Done_cache[5]==2 && Done_cache[6]==2 && Done_cache[7]==2 && Done_cache[8]==2 && Done_cache[9]==2 && Done_cache[10]==2 && Done_cache[11]==2 && Done_cache[12]==2 && Done_cache[13]==2 && Done_cache[14]==2 && Done_cache[15]==2 && Done_cache[16]==2 && Done_cache[17]==2 && Done_cache[18]==2 && Done_cache[19]==2 && Done_cache[20]==2 ){Keep_flag=4;//左弯}else Keep_flag=0;}///////////////////////////////////////////////////////////////////////////////////*误差----------E = 实际值-设定值误差变化------dE = 本次误差-上次误差中间变量1-----B1= E + dE中间变量2-----B2= E*E + dE*dE输出----------OUT= B2/B1一次修改后: IF E=0 AND dE=0 THAN OUT=0 ELSE OUT=B2/B1二次修改后: IF B1=0 THAN OUT=0 ELSE OUT=B2/B1*///////////////////////////////////////////////////////////void Speed_pid(){E2=Speed-Goal_speed;DE=E2-E1;B1=E2+DE;B2=E2*E2+DE*DE;E1=E2;if(B1==0) OUT=0;else OUT=B2/B1;PWMDTY1=OUT;}//////////////////////内部中断///////////////////////////////////////////////#pragma CODE_SEG __NEAR_SEG NON_BANKEDvoid interrupt 66 PIT0(void) //2ms{PITTF_PTF0=1;//清中断标志位if(Keep_flag==1 || Keep_flag==2 || Keep_flag==3 || Keep_flag==4){tt++;if(tt==160){Keep_flag=0;tt=0;}}while(ATD0STA T2_CCF0==0); // 等待转换结束while(A TDOSTAT2_CCF0==0) AD_GetValue();Count++;if(Count>1){Speed=PACNT;Scan_A2();Rem_angle();PACNT=0;Count=0;}if(Keep_flag==0){Mod();Goal_speed=17;}else if(Keep_flag==1){//SenA=0;Goal_speed=6;}else if(Keep_flag==2){//SenA=100;Goal_speed=6;}else if(Keep_flag==3 || Keep_flag==4){Goal_speed=15;}PID();Change_angle();}void interrupt 67 PIT1(void) //100ms {PITTF_PTF1=1;//清中断标志位Run_Time_Count++;//PWMDTY23=DuoJi_Middle;//DuoJi_Middle++;/*LCD_Show_Number(30,0,DuoJi_angle);LCD_Show_Number(0,0,SenA);LCD_Show_Number(0,1,Xmax);LCD_Show_Number(30,1,PWMDTY1);LCD_Show_Number(0,2,Ymax);LCD_Show_Number(30,2,Speed);LCD_Show_Number(0,3,Xmax2);LCD_Show_Number(0,4,Ymax2);LCD_Show_Number(0,5,Out_flag);LCD_Show_Number(30,5,flag);*/LCD_Show_Number(0,0,PWMDTY1);LCD_Show_Number(30,0,SenA);LCD_Show_Number(0,1,Speed);LCD_Show_Number(30,1,Goal_speed);LCD_Show_Number(0,2,Keep_flag);LCD_Show_Number(30,2,tt);LCD_Show_Number(0,3,Done_cache[0]);LCD_Show_Number(30,3,Done_cache[5]);LCD_Show_Number(0,4,Done_cache[10]);LCD_Show_Number(30,4,Done_cache[15]);LCD_Show_Number(0,5,Done_cache[19]);LCD_Show_Number(30,5,Done_cache[20]);if(Run_Time_Count>180) stop();}/***************************************************************************/ /*****常用操作**************************************************************/ /* enable global interrupts */#define GIE (SREG |= BIT(7))/* disable global interrupts */#define GID (SREG &= ~BIT(7))#define SLEEP() asm("sleep")/* enables an unsigned char to be used as a series of booleans */#define BIT(x) (1 << (x))#define SETBIT(x, y) (x |= y)#define CLEARBIT(x, y) (x &= ~y)#define CHECKBIT(x, y) (x & y)// ***** Define I/O pins *****#define BIT7 0x80#define BIT6 0x40#define BIT5 0x20#define BIT4 0x10#define BIT3 0x08#define BIT2 0x04#define BIT1 0x02#define BIT0 0x01#define true 1#define True 1#define false 0#define False 0#define SCLK BIT4#define SDIN BIT3#define LCD_DC BIT2#define LCD_CE BIT0#define LCD_RST BIT1#define LCD_PORT PTT#define LCD_DIR DDRT//#define LCD_IN aa/************************************************************************/void LCD_init(void);void LCD_clear(void);void delay_1us(void);void LCD_set_XY(unsigned char x,unsigned char y);void LCD_write_char(unsigned char c);void LCD_write_english_string(unsigned char X,unsigned char Y,char *s);void LCD_write_chinese_string(unsigned char X, unsigned char Y,unsigned char ch_with,unsigned char num,unsigned char line,unsigned char row);void LCD_draw_bmp_pixel(unsigned char X,unsigned char Y,unsigned char *map,unsigned char Pix_x,unsigned char Pix_y);void LCD_write_byte(unsigned char dat, unsigned char command);const tabpoint[9]={0x00,0x01,0x02,0x04,0x08,0x10,0x20,0x40,0x80};//***********************电机PID控制变量声明和初始化*******************************static unsigned int Kp=100;static unsigned int Kp2=100;static unsigned int Ki=3;static unsigned int Kd=30;unsigned short E = 5;unsigned char q = 1;int Mp = 0;int Mi = 0;int Md = 0;int Mp2 = 0;int MPWM=0;int L_u[3];/*************************电机PID调节***************************************************************************/ void PID(){L_u[2] = L_u[1];L_u[1] = L_u[0];L_u[0] = Goal_speed - Speed;//p_speed是根据传感器的检测情况在软件中给定的理论速度，search_PACN10就是PACN10表实际速度Mp = Kp * L_u[0];//偏差的放大倍数，KP值大可以加快调节，但会使稳定性下降Mi = Mi + Ki * L_u[0];//积分累加，可以消稳态误差，但是会使稳定下降，动态响应变慢Md = Kd * (L_u[0] - 2 * L_u[1] + L_u[2]);//改善动态性能，减少超调，太大会有干扰Mp2 = Kp2 * (L_u[0] - L_u[1]);if(((Ki * L_u[0]) > E)||((Ki * L_u[0]) < -E)) //初始化q=1,E=5 用这个if来决定是否需要用积分来调节，如果刚开始的积分已经很大了，就不需要用积分来调节了q = 0;else q = 1;MPWM = MPWM + Mp + q * Mi + Md +Mp2;// MPWM = MPWM + Mp+ Md +Mp2;if(MPWM > 1000) MPWM = 1000;//保证不溢出if(MPWM < 0) MPWM = 0;MPWM=MPWM/3;if(MPWM>100) MPWM=100;if(MPWM<0) MPWM=0;PWMDTY01=MPWM;Mp = 0;//调节一次后要把PID值清0Mi = 0;Md = 0;Mp2 = 0;}/////////////////////////////////////////////////////////////////////////////////////////////unsigned int Key_Scan2() // 返回1，2，4，8{unsigned int i=0;DDRB=0x00;i=PORTB&0x0f;return i;}////////////////////////////////////////////////////////////////////////////////////////////////// void AD_GetValue(){AD_Value[0]=A TD0DR0; //读取结果寄存器的值AD_Value[1]=A TD0DR1;AD_Value[2]=A TD0DR2;AD_Value[3]=A TD0DR3;AD_Value[4]=A TD0DR4;AD_Value[5]=A TD0DR5;AD_Value[6]=A TD0DR6;AD_Value[7]=A TD0DR7;AD_Value[8]=A TD0DR8; //读取结果寄存器的值AD_Value[9]=A TD0DR9;AD_Value[10]=A TD0DR10;AD_Value[11]=ATD0DR11;AD_Value[12]=A TD0DR12;AD_Value[13]=A TD0DR13;AD_Value[14]=A TD0DR14;AD_Value[15]=A TD0DR15;}////////////////////////////////////////////////////////////////////////////////////////////////// void Show_AD(){LCD_Show_Number(0,0,AD_Value[0]);LCD_Show_Number(30,0,AD_V alue[1]);LCD_Show_Number(0,1,AD_Value[2]);LCD_Show_Number(30,1,AD_V alue[3]);LCD_Show_Number(0,2,AD_Value[4]);LCD_Show_Number(30,2,AD_V alue[5]);LCD_Show_Number(0,3,AD_Value[6]);LCD_Show_Number(30,3,AD_V alue[7]);LCD_Show_Number(0,4,AD_Value[8]);LCD_Show_Number(30,4,AD_V alue[9]);LCD_Show_Number(0,5,AD_Value[10]);LCD_Show_Number(30,5,AD_V alue[11]);LCD_Show_Number(0,6,AD_Value[12]);LCD_Show_Number(30,6,AD_V alue[13]);LCD_Show_Number(60,0,AD_V alue[14]);LCD_Show_Number(60,1,AD_V alue[15]);LCD_Show_Number(60,2,AD_V alue[16]);}////////////////////////////////////////////////////////////////////////////////////////////////void SetBusCLK_64M(void){CLKSEL=0X00; //disengage PLL to systemPLLCTL_PLLON=1; //turn on PLLSYNR =0xc0 | 0x07;REFDV=0x80 | 0x01;POSTDIV=0x00; //pllclock=2*osc*(1+SYNR)/(1+REFDV)=128MHz;_asm(nop); //BUS CLOCK=64M_asm(nop);while(!(CRGFLG_LOCK==1)); //when pll is steady ,then use it;CLKSEL_PLLSEL =1; //engage PLL to system;}/////////////////////////////////////////////////////////////////////////////////////////////////void Dly_ms(int ms){int ii,jj;if (ms<1) ms=1;for(ii=0;ii<ms;ii++)for(jj=0;jj<5341;jj++); //64MHz--1ms}////////////////////////////////////////////////////////////////////////////////////////////////unsigned char SciRead() //串口接受函数{if(SCI1SR1_RDRF==1) //表明数据从位移寄存器传输到SCI数据寄存器{SCI1SR1_RDRF=1; //读取数据寄存器会将RDRF清除重新置位return SCI1DRL; //返回数据寄存器的数据}}void uart_putchar(unsigned char message)//串口发送函数{while(SCI1SR1_TC!=1); //这句话很重要，第一个发送完在发送第二个,否则发送错误//PORTB=~PORTB; //改变一次发送一次SCI1DRL=message;}///////////////////////////////////////////////////////////////////////////////////void stop(){DisableInterrupts;PWMDTY0=50;PWMDTY1=0;Dly_ms(900);PWMDTY0=0;PWMDTY1=0;while(PORTB_PB6==1) ; //等待开始键按下{while(1){LCD_Show_Number(0,0,Angle_cache[0]);LCD_Show_Number(27,0,Angle_cache[1]);LCD_Show_Number(54,0,Angle_cache[2]);LCD_Show_Number(0,1,Angle_cache[3]);LCD_Show_Number(27,1,Angle_cache[4]);LCD_Show_Number(54,1,Angle_cache[5]);LCD_Show_Number(0,2,Angle_cache[6]);LCD_Show_Number(27,2,Angle_cache[7]);LCD_Show_Number(54,2,Angle_cache[8]);LCD_Show_Number(0,3,Angle_cache[9]);LCD_Show_Number(27,3,Angle_cache[10]);LCD_Show_Number(54,3,Angle_cache[11]);LCD_Show_Number(0,4,Angle_cache[12]);LCD_Show_Number(27,4,Angle_cache[13]);LCD_Show_Number(54,4,Angle_cache[14]);LCD_Show_Number(0,5,Angle_cache[15]);LCD_Show_Number(27,5,Angle_cache[16]);LCD_Show_Number(54,5,Angle_cache[17]);/////////////////////////////////////////////////显示记忆数据}}}/////////////////////////*while(1){for(i=1150;i<1750;i++){PWMDTY23=i;LCD_Show_Number(0,1,i);Dly_ms(100);}} */////////////////////////////****************************************************************************** **************************************/void LCD_init(void){//先设置为输出SETBIT(LCD_DIR,SCLK);// DDRD|=SCLK;SETBIT(LCD_DIR,SDIN);//DDRC|=SDIN;SETBIT(LCD_DIR,LCD_DC);//DDRC|=LCD_DC;SETBIT(LCD_DIR,LCD_CE);//DDRC|=LCD_CE;SETBIT(LCD_DIR,LCD_RST);//DDRC|=LCD_RST;// 产生一个让LCD复位的低电平脉冲CLEARBIT(LCD_PORT,LCD_RST);//LCD_RST = 0;delay_1us();SETBIT(LCD_PORT,LCD_RST);//LCD_RST = 1;// 关闭LCDCLEARBIT(LCD_PORT,LCD_CE);//LCD_CE = 0;delay_1us();// 使能LCDSETBIT(LCD_PORT,LCD_CE);//LCD_CE = 1;delay_1us();LCD_write_byte(0x21, 0); // 使用扩展命令设置LCD模式LCD_write_byte(0xc8, 0); // 设置偏置电压LCD_write_byte(0x06, 0); // 温度校正LCD_write_byte(0x13, 0); // 1:48LCD_write_byte(0x20, 0); // 使用基本命令LCD_clear(); // 清屏LCD_write_byte(0x0c, 0); // 设定显示模式，正常显示LCD_set_XY(0,1);// 关闭LCDCLEARBIT(LCD_PORT,LCD_CE);//LCD_CE = 0;}/****************************************************************************** **************************************/void LCD_clear(void){unsigned int i;LCD_write_byte(0x0c, 0);LCD_write_byte(0x80, 0);for (i=0; i<500; i++){LCD_write_byte(0, 1);}}/****************************************************************************** **************************************/void delay_1us(void) //1us延时函数{unsigned int i;for(i=0;i<5;i++);}/********************************************************************************************************************/void LCD_set_XY(unsigned char X, unsigned char Y){LCD_write_byte(0x40 | Y, 0);// columnLCD_write_byte(0x80 | X, 0);// row}/****************************************************************************** **************************************/LCD_write_point(unsigned char X, unsigned char Y){unsigned int temp=0;if(poy!=X){potemp1=0x00;potemp2=0x00;potemp3=0x00;potemp4=0x00;potemp5=0x00;potemp6=0x00;}poy=X;if(Y<=8){if(pox!=1) potemp1=0x00;pox=1;temp=potemp1|tabpoint[Y];LCD_set_XY(X,8);LCD_write_byte(temp, 1);potemp1=temp;}else if(Y<=16){if(pox!=2) potemp2=0x00;pox=2;temp=potemp2|tabpoint[Y-8];LCD_set_XY(X,9);LCD_write_byte(temp, 1);potemp2=temp;}else if(Y<=24)if(pox!=3) potemp3=0x00;pox=3;temp=potemp3|tabpoint[Y-16]; LCD_set_XY(X,10);LCD_write_byte(temp, 1); potemp3=temp;}else if(Y<=32){if(pox!=4) potemp4=0x00;pox=4;temp=potemp4|tabpoint[Y-24]; LCD_set_XY(X,11);LCD_write_byte(temp, 1); potemp4=temp;}else if(Y<=40){if(pox!=5) potemp5=0x00;pox=5;temp=potemp5|tabpoint[Y-32]; LCD_set_XY(X,12);LCD_write_byte(temp, 1); potemp5=temp;}else if(Y<=48){if(pox!=6) potemp6=0x00;pox=6;temp=potemp6|tabpoint[Y-40]; LCD_set_XY(X,13);LCD_write_byte(temp, 1); potemp6=temp;}//for(temp=0;temp<50000;temp++);/****************************************************************************** **************************************/void LCD_write_char(unsigned char c){unsigned char line;c -= 32;for (line=0; line<6; line++){LCD_write_byte(font6x8[c][line], 1);}}void LCD_write_char2(unsigned char X, unsigned char Y,unsigned char c){unsigned char line;LCD_set_XY(X,Y);c -= 32;for (line=0; line<6; line++){LCD_write_byte(font6x8[c][line], 1);}}/****************************************************************************** **************************************//*-----------------------------------------------------------------------LCD_write_english_String : 英文字符串显示函数输入参数：*s ：英文字符串指针；X、Y : 显示字符串的位置,x 0-83 ,y 0-5-----------------------------------------------------------------------*/void LCD_write_english_string(unsigned char X,unsigned char Y,char *s){LCD_set_XY(X,Y);while (*s){LCD_write_char(*s);s++;}}/********************************************************************************************************************//*-----------------------------------------------------------------------LCD_write_chinese_string: 在LCD上显示汉字输入参数：X、Y ：显示汉字的起始X、Y坐标；ch_with ：汉字点阵的宽度num ：显示汉字的个数；line ：汉字点阵数组中的起始行数row ：汉字显示的行间距-----------------------------------------------------------------------*//*void LCD_write_chinese_string(unsigned char X, unsigned char Y,unsigned char ch_with,unsigned char num,unsigned char line,unsigned char row){unsigned char i,n;LCD_set_XY(X,Y); //设置初始位置for (i=0;i<num;){for (n=0; n<ch_with*2; n++) //写一个汉字{if (n==ch_with) //写汉字的下半部分{if (i==0) LCD_set_XY(X,Y+1);else{LCD_set_XY((X+(ch_with+row)*i),Y+1);}}LCD_write_byte(HZK[line+i][n],1);}i++;LCD_set_XY((X+(ch_with+row)*i),Y);}}*//****************************************************************************** **************************************//*-----------------------------------------------------------------------LCD_draw_map : 位图绘制函数输入参数：X、Y ：位图绘制的起始X、Y坐标；*map ：位图点阵数据；Pix_x ：位图像素（长）Pix_y ：位图像素（宽）-----------------------------------------------------------------------*/void LCD_draw_bmp_pixel(unsigned char X,unsigned char Y,unsigned char *map,unsigned char Pix_x,unsigned char Pix_y){unsigned int i,n;unsigned char row;if (Pix_y%8==0){row=Pix_y/8; //计算位图所占行数}else{row=Pix_y/8+1;}for (n=0;n<row;n++){LCD_set_XY(X,Y);for(i=0; i<Pix_x; i++){LCD_write_byte(map[i+n*Pix_x], 1);}Y++; //换行}}/****************************************************************************** **************************************//*-----------------------------------------------------------------------LCD_write_byte : 写数据到LCD输入参数：data ：写入的数据；command ：写数据/命令选择；-----------------------------------------------------------------------*/void LCD_write_byte(unsigned char dat, unsigned char command){unsigned char i;CLEARBIT(LCD_PORT,LCD_CE);// 使能LCD_CE = 0if (command == 0){CLEARBIT(LCD_PORT,LCD_DC);// 传送命令LCD_DC = 0;}else{SETBIT(LCD_PORT,LCD_DC);// 传送数据LCD_DC = 1;}for(i=0;i<8;i++){//delay_1us();if(dat&0x80){SETBIT(LCD_PORT,SDIN);//SDIN = 1;}else{CLEARBIT(LCD_PORT,SDIN);//SDIN = 0;}CLEARBIT(LCD_PORT,SCLK);//SCLK = 0;dat = dat << 1;SETBIT(LCD_PORT,SCLK);//SCLK = 1;}SETBIT(LCD_PORT,LCD_CE);//LCD_CE = 1;}void LCD_Show_Number (unsigned char X,unsigned char Y,unsigned int number) {LCD_set_XY(X,Y);LCD_write_char2(X,Y,number/1000+48);LCD_write_char2(X+6,Y,number%1000/100+48);LCD_write_char2(X+12,Y,number%100/10+48);LCD_write_char2(X+18,Y,number%10+48);}。

飞思卡尔智能车(摄像头)核心程序

飞思卡尔智能车（摄像头）核心程序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);}。

飞思卡尔智能车程序

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 stablizeCLKSEL=0x80;// open PLL}Void init_ECT(void);{TIOS_IOS3=0; // set PT3 as input captureTCTL4=0b11000000; // set pt3 as any edge triggeredICPAR_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 OnlyINTCR_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 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=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 PAD4ATD0DIEN_IEN3 = 1; //code switch 1 on RP1,Enable Digital Input PAD3if(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 line dc.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 sets cloop: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（注：可编辑下载，若有不当之处，请指正，谢谢!）。

飞思卡尔智能车程序汇总

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);//////增添电机 //////////增添电机 ////}}。

1、下载文档前请自行甄别文档内容的完整性，平台不提供额外的编辑、内容补充、找答案等附加服务。
2、"仅部分预览"的文档,不可在线预览部分如存在完整性等问题,可反馈申请退款(可完整预览的文档不适用该条件!)。
3、如文档侵犯您的权益，请联系客服反馈,我们会尽快为您处理(人工客服工作时间：9:00-18:30)。

extern int left,w,top,h;extern HDC m_hdc;CBrush brush3(RGB(0,255,0));CBrush brush4(RGB(255,0,0));CBrush brush5(RGB(255,255,0));#else#include <mc9s12xdp512.h>#include "math.h"// #include "LQfun.h"#endif#ifdef ccd#define MAX_VIDEO_LINE 39#define MAX_VIDEO_POINT 187#else//#define MAX_VIDEO_LINE 26// #define MAX_VIDEO_POINT 301#define MAX_VIDEO_LINE 78#define MAX_VIDEO_POINT 57#endifextern unsigned char g_VideoImageDate[MAX_VIDEO_LINE][MAX_VIDEO_POINT];#define INT8U unsigned char#define INT8S signed char#define INT16U unsigned int#define INT16S int#define INT32S int#define NO_DATA_180 254//#define INT32U unsigned intunsigned char LIMIT=((MAX_VIDEO_POINT)/2);unsigned char MIDDLE[MAX_VIDEO_LINE];#define MAX_BLACK_NUM 7INT8S n;INT8U limit_cur;INT8U BackLineValidFlag;INT8U l_BlackStartDot;INT8U k;INT8U k2;INT8U NextFlag;INT16S g_SearchStart;INT16S g_SearchEnd;INT8U g_SearchDirection;INT16S g_BlackMiddle[2];INT8U g_BottomMiddle=MAX_VIDEO_POINT/2;INT8U g_BlackPoint[2][MAX_BLACK_NUM];INT8U g_BlackPoint_qipao[2][MAX_BLACK_NUM];INT8U cnt;INT8U cnt_large[MAX_VIDEO_LINE];INT8U cnt_qipao;INT8U g_BottomValidLine;extern INT8U g_width[MAX_VIDEO_LINE];extern INT8U g_BlackPositionCenter[MAX_VIDEO_LINE];//INT16U g_center;INT8U g_ValidLine;INT16U g_BlackLineTotal;INT8S g_error;INT8S ValidLineError;INT8S g_WidthError;INT8S g_MinWidth;INT8S g_MaxWidth;extern INT8U g_IfCross;unsigned char haveleft,haveright;unsigned char left_n,right_n,left_or_right,left_center_x,right_center_x; INT8U cur_center;//float g_DirectionControl;//float g_DirectionControl_y;//INT32S g_DirectionControlLine;void init_pic_all(){INT8U i;for(i = 0; i < MAX_VIDEO_LINE; i++) //78{//MIDDLE[MAX_VIDEO_LINE-1-i]=50+(90-50)*i/(MAX_VIDEO_LINE-1);MIDDLE[MAX_VIDEO_LINE-1-i]=30+(50-30)*i/(MAX_VIDEO_LINE-1);//150+powf(i/MAX_VIDEO_LINE,3.)/powf(1.,3.)*40;//MIDDLE[MAX_VIDEO_LINE-1-i]=90+(100-90)*i/(MAX_VIDEO_LINE-1);//150+powf(i/MA X_VIDEO_LINE,3.)/powf(1.,3.)*40;}}void init_process(void){//return;INT8U i = 0;for(i = 0; i < MAX_VIDEO_LINE; i ++) //39{g_BlackPositionCenter[i] = NO_DATA_180;g_width[i] = 0;cnt_large[i]=0;}NextFlag=0;g_IfCross=0;cnt_qipao=0;haveright=0;haveleft=0;g_SearchStart = g_BottomMiddle - LIMIT;g_SearchEnd = g_BottomMiddle + LIMIT;if(g_SearchStart < 0){g_SearchStart = 0;}if(g_SearchEnd > MAX_VIDEO_POINT-1){g_SearchEnd = MAX_VIDEO_POINT - 1;}}INT8U last(INT8U line_num,INT8U array_num){//NextFlag=0;INT8U qs;INT8U flag=0;n=line_num;// for(n = MAX_VIDEO_LINE - 1; (n > MAX_VIDEO_LINE -3) ; n --){#ifdef PCCRectbq=CRect(left+g_SearchStart*w,top+n*h,left+g_SearchStart*w+w,top+n*h+1*h);FillRect(m_hdc,bq,brush3);bq=CRect(left+g_SearchEnd*w,top+n*h,left+g_SearchEnd*w+w,top+n*h+1*h);FillRect(m_hdc,bq,brush3);#endiffor(k = 0; k < MAX_BLACK_NUM; k ++){g_BlackPoint[0][k] = NO_DATA_180;g_BlackPoint[1][k] = 0;}cnt = 0;m = g_SearchStart;INT8U process_line(INT8U linenum,INT8U array_num) {INT8U qs;INT8U true_road_cen;//if(linenum==2){// linenum++;// linenum--;//}//if(1 == NextFlag)if(NextFlag>1){//for(n = g_BottomValidLine - 1; n >= 0; n --)n=linenum;{for(k = 0;k < MAX_BLACK_NUM;k ++){g_BlackPoint[0][k] = NO_DATA_180;g_BlackPoint[1][k] = 0;}k = n + 1;while(g_BlackPositionCenter[k] == NO_DATA_180){k ++ ;}k2 = k + 1;while(g_BlackPositionCenter[k2] == NO_DATA_180){k2 ++;}//PORTB=0x00;if(k2-n>3)return 1;// PORTB=0x55;if(g_width[k2] >= g_width[k]){g_WidthError = (g_width[k2] - g_width[k])*(k2-n)/(k2-k) + 4;g_MinWidth = g_width[k] - g_WidthError;if(g_MinWidth < 2){g_MinWidth = 2;}g_MaxWidth = g_width[k] + g_WidthError;if(g_MaxWidth > 20){g_MaxWidth = 20;}}else{g_WidthError = (g_width[k] - g_width[k2])*(k2-n)/(k2-k)+4;//g_width[k] - g_width[k2] + 6;g_MinWidth = g_width[k] - g_WidthError;if(g_MinWidth < 2){g_MinWidth = 2;}g_MaxWidth = g_width[k] + g_WidthError ;if(g_MaxWidth > 20){g_MaxWidth = 20;}}V alidLineError = (g_MinWidth+g_MaxWidth)/2;//20 * (k - n);//(g_MinWidth+g_MaxWidth)*3./2.;//20 * (k - n);if(ValidLineError > 20){ValidLineError = 20;}else if(ValidLineError<10){ValidLineError=10; //ValidLineError keep}if(g_BlackPositionCenter[k2] > g_BlackPositionCenter[k]){g_SearchDirection = 1;g_error = ((g_BlackPositionCenter[k2] - g_BlackPositionCenter[k]) + ValidLineError);g_SearchStart = g_BlackPositionCenter[k] - g_error - g_MaxWidth;if((g_BlackPositionCenter[k2] - g_BlackPositionCenter[k]) > 6){g_SearchEnd = g_BlackPositionCenter[k] + g_MaxWidth / 2;}else{g_SearchEnd = g_BlackPositionCenter[k] + g_error + g_MaxWidth;}if(g_SearchStart < 0){g_SearchStart = 0;}if(g_SearchEnd >= MAX_VIDEO_POINT){g_SearchEnd = MAX_VIDEO_POINT - 1;}}else //if(g_BlackPositionCenter[k] >= g_BlackPositionCenter[k2]){g_SearchDirection = 0;g_error = ((g_BlackPositionCenter[k] - g_BlackPositionCenter[k2]) + ValidLineError);g_SearchStart = g_BlackPositionCenter[k] + g_error + g_MaxWidth;if((g_BlackPositionCenter[k] - g_BlackPositionCenter[k2]) > 6){g_SearchEnd = g_BlackPositionCenter[k] - g_MaxWidth / 2;}else{g_SearchEnd = g_BlackPositionCenter[k] - g_error - g_MaxWidth;}if(g_SearchEnd < 0){g_SearchEnd = 0;}if(g_SearchStart >= MAX_VIDEO_POINT){g_SearchStart = MAX_VIDEO_POINT - 1;}}#ifdef PCCRectbq=CRect(left+g_SearchStart*w,top+n*h,left+g_SearchStart*w+w,top+n*h+1*h);FillRect(m_hdc,bq,brush4);bq=CRect(left+g_SearchEnd*w,top+n*h,left+g_SearchEnd*w+w,top+n*h+1*h);FillRect(m_hdc,bq,brush4);#endifcnt = 0;cnt_qipao=0;if(0 == g_SearchDirection){m = g_SearchStart;while((m > g_SearchEnd) && (cnt < MAX_BLACK_NUM)){if(g_VideoImageDate[array_num][m] < MIDDLE[n]){l_BlackStartDot = m;while((g_VideoImageDate[array_num][m - 1] < MIDDLE[n]) && (m > g_SearchEnd)){m --;}if(((l_BlackStartDot - m + 1) >= g_MinWidth)){//cnt_large[n]++;#ifdef PCCRectbq=CRect(left+m*w,top+n*h,left+l_BlackStartDot*w+w,top+n*h+1*h);FillRect(m_hdc,bq,CBrush(RGB(100,100,0)));#endif/*if((l_BlackStartDot - m +1)>18&&n>20&&abs(((l_BlackStartDot + m) /2)-MAX_VIDEO_POINT/2)<MAX_VIDEO_POINT/3){cnt_qipao++;}*/if(cnt_qipao<MAX_BLACK_NUM){g_BlackPoint_qipao[0][cnt_qipao]= (l_BlackStartDot +m) / 2;g_BlackPoint_qipao[1][cnt_qipao] = l_BlackStartDot - m + 1;cnt_qipao++; //?????ùóD′óμ?}if(((l_BlackStartDot - m + 1) < g_MaxWidth)){// #ifdef PC// CRect bq=CRect(left+m*w,top+n*h,left+l_BlackStartDot*w+w,top+n*h+1*h);// FillRect(m_hdc,bq,brush5);// #endifg_BlackPoint[0][cnt] = (l_BlackStartDot + m) / 2;g_BlackPoint[1][cnt] = l_BlackStartDot - m + 1;cnt ++;}}else{;}if(m > 1){m -= 2;}else{m = 0;}}else{m --;}}}else if(1 == g_SearchDirection){m = g_SearchStart;while((m < g_SearchEnd) && (cnt < MAX_BLACK_NUM)){if(g_VideoImageDate[array_num][m] < MIDDLE[n]){l_BlackStartDot = m;while((g_VideoImageDate[array_num][m + 1] < MIDDLE[n]) && (m < g_SearchEnd)){m ++;}if( ((m - l_BlackStartDot + 1) >= g_MinWidth)){// if((m - l_BlackStartDot +1)>18&&n>20&&abs(((l_BlackStartDot + m) /2)-MAX_VIDEO_POINT/2)<MAX_VIDEO_POINT/3)//{// cnt_qipao++;//}#ifdef PCCRectbq=CRect(left+m*w,top+n*h,left+l_BlackStartDot*w+w,top+n*h+1*h);FillRect(m_hdc,bq,CBrush(RGB(100,100,0)));#endifif(cnt_qipao<MAX_BLACK_NUM){g_BlackPoint_qipao[0][cnt_qipao]= (l_BlackStartDot + m) / 2;g_BlackPoint_qipao[1][cnt_qipao] = m -l_BlackStartDot + 1;cnt_qipao++;}if(((m - l_BlackStartDot + 1) < g_MaxWidth)){#ifdef PCCRectbq=CRect(left+l_BlackStartDot*w,top+n*h,left+m*w+w,top+n*h+1*h);//FillRect(m_hdc,bq,brush5);#endifg_BlackPoint[0][cnt] = (l_BlackStartDot + m) / 2;g_BlackPoint[1][cnt] = m - l_BlackStartDot + 1;cnt ++;}}else{;}m += 2;}else{m ++ ;}}}limit_cur=g_error;for(k2 = 0; k2 < cnt; k2 ++){if((g_BlackPoint[0][k2] < (g_BlackPositionCenter[k] + g_error)) && (g_BlackPoint[0][k2] > (g_BlackPositionCenter[k] - g_error))){qs=(g_BlackPoint[0][k2]>g_BlackPositionCenter[k])?g_BlackPoint[0][k2]-g_BlackPositionCente r[k]:g_BlackPositionCenter[k]-g_BlackPoint[0][k2];if(qs<limit_cur){g_BlackPositionCenter[n] = g_BlackPoint[0][k2];g_width[n] = g_BlackPoint[1][k2];limit_cur=qs;//cur_center=k2;//k2 = MAX_BLACK_NUM;}}else if((g_BlackPoint[0][k2] > (g_BlackPositionCenter[k] + g_error))){;// do nothing}}if(g_width[n]>0&&n>18){//if(cnt_qipao>2){//AfxMessageBox("cnt_qipao>2");cur_center=254;for(k2=0;k2<cnt_qipao;k2++){if(g_BlackPositionCenter[n]==g_BlackPoint_qipao[0][k2]){if(cnt_qipao>(k2+1)&&k2>0){/*if(//abs(g_BlackPoint_qipao[1][k2+1]-g_BlackPoint_qipao[1][k2-1])<8//&&//abs((g_BlackPoint_qipao[0][k2+1]+g_BlackPoint_qipao[0][k2-1])-2*g_BlackPoint_qipao[0][k2 ])<8 //??3?//&&abs(g_BlackPoint_qipao[0][k2+1]-g_BlackPoint_qipao[0][k2])-(g_BlackPoint_qipao[1][k2+1]+g _BlackPoint_qipao[1][k2])/2<(g_BlackPoint_qipao[1][k2]*2) //????×?′ó&&abs(g_BlackPoint_qipao[0][k2-1]-g_BlackPoint_qipao[0][k2])-(g_BlackPoint_qipao[1][k2-1]+g_ BlackPoint_qipao[1][k2])/2<(g_BlackPoint_qipao[1][k2]*2) //????×?′ó&&abs(g_BlackPoint_qipao[0][k2+1]-g_BlackPoint_qipao[0][k2])-(g_BlackPoint_qipao[1][k2+1]+g _BlackPoint_qipao[1][k2])/2>g_BlackPoint_qipao[1][k2]/2 //????×?D?&&abs(g_BlackPoint_qipao[0][k2-1]-g_BlackPoint_qipao[0][k2])-(g_BlackPoint_qipao[1][k2-1]+g_ BlackPoint_qipao[1][k2])/2>g_BlackPoint_qipao[1][k2]/2 //????×?D?&&abs(g_VideoImageDate[array_num][g_BlackPoint_qipao[0][k2+1]]-g_VideoImageDate[array_nu m][g_BlackPoint_qipao[0][k2]+(abs(g_BlackPoint_qipao[0][k2+1]-g_BlackPoint_qipao[0][k2])-(g_ BlackPoint_qipao[1][k2+1]+g_BlackPoint_qipao[1][k2])/2)/2])>10&&abs(g_VideoImageDate[array_num][g_BlackPoint_qipao[0][k2-1]]-g_VideoImageDate[array_nu m][g_BlackPoint_qipao[0][k2]-(abs(g_BlackPoint_qipao[0][k2-1]-g_BlackPoint_qipao[0][k2])-(g_Bl ackPoint_qipao[1][k2-1]+g_BlackPoint_qipao[1][k2])/2)/2])>10) */if(g_BlackPoint_qipao[0][k2+1]<g_BlackPoint_qipao[0][k2-1]){qs=g_BlackPoint_qipao[0][k2+1];g_BlackPoint_qipao[0][k2+1]=g_BlackPoint_qipao[0][k2-1];g_BlackPoint_qipao[0][k2-1]=qs;qs=g_BlackPoint_qipao[1][k2+1];g_BlackPoint_qipao[1][k2+1]=g_BlackPoint_qipao[1][k2-1];g_BlackPoint_qipao[1][k2-1]=qs;}if(//abs(g_BlackPoint_qipao[1][k2+1]-g_BlackPoint_qipao[1][k2-1])<8 //×óóò3¤?àμè//&&//abs((g_BlackPoint_qipao[0][k2+1]+g_BlackPoint_qipao[0][k2-1])-2*g_BlackPoint_qipao[0][k2 ])<8 //??3?//&&abs(g_BlackPoint_qipao[0][k2+1]-g_BlackPoint_qipao[0][k2])-(g_BlackPoint_qipao[1][k2+1]+g _BlackPoint_qipao[1][k2])/2<(g_BlackPoint_qipao[1][k2]*2) //????×?′ó&&abs(g_BlackPoint_qipao[0][k2-1]-g_BlackPoint_qipao[0][k2])-(g_BlackPoint_qipao[1][k2-1]+g_ BlackPoint_qipao[1][k2])/2<(g_BlackPoint_qipao[1][k2]*2) //????×?′ó&&abs(g_BlackPoint_qipao[0][k2+1]-g_BlackPoint_qipao[0][k2])-(g_BlackPoint_qipao[1][k2+1]+g _BlackPoint_qipao[1][k2])/2>g_BlackPoint_qipao[1][k2]/2 //????×?D?&&abs(g_BlackPoint_qipao[0][k2-1]-g_BlackPoint_qipao[0][k2])-(g_BlackPoint_qipao[1][k2-1]+g_ BlackPoint_qipao[1][k2])/2>g_BlackPoint_qipao[1][k2]/2 //????×?D?&&abs(g_VideoImageDate[array_num][g_BlackPoint_qipao[0][k2+1]]-g_VideoImageDate[array_nu m][g_BlackPoint_qipao[0][k2]+g_BlackPoint_qipao[1][k2]/2+(abs(g_BlackPoint_qipao[0][k2+1]-g_ BlackPoint_qipao[0][k2])-(g_BlackPoint_qipao[1][k2+1]+g_BlackPoint_qipao[1][k2])/2)/2])>10 //?D??oúá?±?°×&&abs(g_VideoImageDate[array_num][g_BlackPoint_qipao[0][k2-1]]-g_VideoImageDate[array_nu m][g_BlackPoint_qipao[0][k2]-g_BlackPoint_qipao[1][k2]/2-(abs(g_BlackPoint_qipao[0][k2-1]-g_Bl ackPoint_qipao[0][k2])-(g_BlackPoint_qipao[1][k2-1]+g_BlackPoint_qipao[1][k2])/2)/2])>10 //?D??oúá?±?°×&&g_VideoImageDate[array_num-1][g_BlackPoint_qipao[0][k2]-g_BlackPoint_qipao[1][k2]/2-(abs(g_BlackPoint_qipao[0][k2-1]-g_Bl ackPoint_qipao[0][k2])-(g_BlackPoint_qipao[1][k2-1]+g_BlackPoint_qipao[1][k2])/2)/2]>MIDDLE[n]&&g_VideoImageDate[array_num+1][g_BlackPoint_qipao[0][k2]-g_BlackPoint_qipao[1][k2]/2-(abs(g_BlackPoint_qipao[0][k2-1]-g_Bl ackPoint_qipao[0][k2])-(g_BlackPoint_qipao[1][k2-1]+g_BlackPoint_qipao[1][k2])/2)/2]>MIDDLE[n]&&g_VideoImageDate[array_num-1][g_BlackPoint_qipao[0][k2]+g_BlackPoint_qipao[1][k2]/2+(abs(g_BlackPoint_qipao[0][k2+1]-g_BlackPoint_qipao[0][k2])-(g_BlackPoint_qipao[1][k2+1]+g_BlackPoint_qipao[1][k2])/2)/2]>MIDDLE[n]&&g_VideoImageDate[array_num+1][g_BlackPoint_qipao[0][k2]+g_BlackPoint_qipao[1][k2]/2+(abs(g_BlackPoint_qipao[0][k2+1]-g_ BlackPoint_qipao[0][k2])-(g_BlackPoint_qipao[1][k2+1]+g_BlackPoint_qipao[1][k2])/2)/2]>MIDDLE[n]){g_IfCross=1;#ifdef PCTextOut(m_hdc,left+100,top+240,"ew",8);CString sh=_T("");sh.Format("n=%d,1=%d,2=%d,3=%d",n,g_BlackPoint_qipao[0][k2-1],g_BlackPoint_qipao[0][k2 ],g_BlackPoint_qipao[0][k2+1]);CRectbq=CRect(left+(g_BlackPoint_qipao[0][k2-1]-g_BlackPoint_qipao[1][k2-1]/2)*w,top+n*h,left+(g _BlackPoint_qipao[0][k2-1]+g_BlackPoint_qipao[1][k2-1]/2)*w+w,top+n*h+1*h);FillRect(m_hdc,bq,brush5);bq=CRect(left+(g_BlackPoint_qipao[0][k2]-g_BlackPoint_qipao[1][k2]/2)*w,top+n*h,left+(g_Bl ackPoint_qipao[0][k2]+g_BlackPoint_qipao[1][k2]/2)*w+w,top+n*h+1*h);FillRect(m_hdc,bq,brush5);bq=CRect(left+(g_BlackPoint_qipao[0][k2+1]-g_BlackPoint_qipao[1][k2+1]/2)*w,top+n*h,left+( g_BlackPoint_qipao[0][k2+1]+g_BlackPoint_qipao[1][k2+1]/2)*w+w,top+n*h+1*h);FillRect(m_hdc,bq,brush5);intx=//g_BlackPoint_qipao[0][k2]+(abs(g_BlackPoint_qipao[0][k2+1]-g_BlackPoint_qipao[0][k2])-(g_BlackPoint_qipao[1][k2+1]+g_BlackPoint_qipao[1][k2])/2);g_BlackPoint_qipao[0][k2]+g_BlackPoint_qipao[1][k2]/2+(abs(g_BlackPoint_qipao[0][k2+1]-g_ BlackPoint_qipao[0][k2])-(g_BlackPoint_qipao[1][k2+1]+g_BlackPoint_qipao[1][k2])/2)/2;bq=CRect(left+(x)*w,top+n*h,left+(x)*w+w,top+n*h+1*h);FillRect(m_hdc,bq,brush5);x=//g_BlackPoint_qipao[0][k2]+(abs(g_BlackPoint_qipao[0][k2+1]-g_BlackPoint_qipao[0][k2])-(g_BlackPoint_qipao[1][k2+1]+g_BlackPoint_qipao[1][k2])/2);g_BlackPoint_qipao[0][k2]-g_BlackPoint_qipao[1][k2]/2-(abs(g_BlackPoint_qipao[0][k2-1]-g_Bl ackPoint_qipao[0][k2])-(g_BlackPoint_qipao[1][k2-1]+g_BlackPoint_qipao[1][k2])/2)/2;bq=CRect(left+(x)*w,top+n*h,left+(x)*w+w,top+n*h+1*h);FillRect(m_hdc,bq,brush5);#endif}}else if( g_IfCross==0&&cnt_qipao==k2+2){true_road_cen=k2;left_or_right=0;if(g_BlackPoint_qipao[0][k2+1]<g_BlackPoint_qipao[0][k2]){left_or_right=1;qs=g_BlackPoint_qipao[0][k2+1];g_BlackPoint_qipao[0][k2+1]=g_BlackPoint_qipao[0][k2];g_BlackPoint_qipao[0][k2]=qs;qs=g_BlackPoint_qipao[1][k2+1];g_BlackPoint_qipao[1][k2+1]=g_BlackPoint_qipao[1][k2];g_BlackPoint_qipao[1][k2]=qs;}if(//abs(g_BlackPoint_qipao[1][k2+1]-g_BlackPoint_qipao[1][k2-1])<8 //×óóò3¤?àμè//&&//abs((g_BlackPoint_qipao[0][k2+1]+g_BlackPoint_qipao[0][k2-1])-2*g_BlackPoint_qipao[0][k2 ])<8 //??3?//&&abs(g_BlackPoint_qipao[0][k2+1]-g_BlackPoint_qipao[0][k2])-(g_BlackPoint_qipao[1][k2+1]+g _BlackPoint_qipao[1][k2])/2<(g_BlackPoint_qipao[1][true_road_cen]*2) //????×?′ó&&abs(g_BlackPoint_qipao[0][k2+1]-g_BlackPoint_qipao[0][k2])-(g_BlackPoint_qipao[1][k2+1]+g _BlackPoint_qipao[1][k2])/2>g_BlackPoint_qipao[1][true_road_cen]/2 //????×?D?&&abs(g_VideoImageDate[array_num][g_BlackPoint_qipao[0][true_road_cen]]-g_VideoImageDate[ array_num][g_BlackPoint_qipao[0][k2]+g_BlackPoint_qipao[1][k2]/2+(abs(g_BlackPoint_qipao[0][k2+1]-g_BlackPoint_qipao[0][k2])-(g_BlackPoint_qipao[1][k2+1]+g_BlackPoint_qipao[1][k2])/2)/2])>10&&g_VideoImageDate[array_num-1][g_BlackPoint_qipao[0][k2]+g_BlackPoint_qipao[1][k2]/2+(abs(g_BlackPoint_qipao[0][k2+ 1]-g_BlackPoint_qipao[0][k2])-(g_BlackPoint_qipao[1][k2+1]+g_BlackPoint_qipao[1][k2])/2)/2]>MIDDLE[n]&&g_VideoImageDate[array_num+1][g_BlackPoint_qipao[0][k2]+g_BlackPoint_qipao[1][k2]/2+(abs(g_BlackPoint_qipao[0][k2+ 1]-g_BlackPoint_qipao[0][k2])-(g_BlackPoint_qipao[1][k2+1]+g_BlackPoint_qipao[1][k2])/2)/2]>MIDDLE[n]){if( left_or_right==0){ haveright=1;right_n=n;right_center_x=true_road_cen;//AfxMessageBox("right1");}else{ haveleft=1;left_n=n;left_center_x=true_road_cen;//AfxMessageBox("left1");}//AfxMessageBox("right");#ifdef PCbq=CRect(left+(g_BlackPoint_qipao[0][k2]-g_BlackPoint_qipao[1][k2]/2)*w,top+n*h,left+(g_Bl ackPoint_qipao[0][k2]+g_BlackPoint_qipao[1][k2]/2)*w+w,top+n*h+1*h);FillRect(m_hdc,bq,brush5);bq=CRect(left+(g_BlackPoint_qipao[0][k2+1]-g_BlackPoint_qipao[1][k2+1]/2)*w,top+n*h,left+( g_BlackPoint_qipao[0][k2+1]+g_BlackPoint_qipao[1][k2+1]/2)*w+w,top+n*h+1*h);FillRect(m_hdc,bq,brush5);intx=//g_BlackPoint_qipao[0][k2]+(abs(g_BlackPoint_qipao[0][k2+1]-g_BlackPoint_qipao[0][k2])-(g_BlackPoint_qipao[1][k2+1]+g_BlackPoint_qipao[1][k2])/2);g_BlackPoint_qipao[0][k2]+g_BlackPoint_qipao[1][k2]/2+(abs(g_BlackPoint_qipao[0][k2+1]-g_ BlackPoint_qipao[0][k2])-(g_BlackPoint_qipao[1][k2+1]+g_BlackPoint_qipao[1][k2])/2)/2;bq=CRect(left+(x)*w,top+n*h,left+(x)*w+w,top+n*h+1*h);FillRect(m_hdc,bq,brush5);#endif}}else if( g_IfCross==0&&cnt_qipao==k2+1&&k2>0){true_road_cen=k2;left_or_right=0;if(g_BlackPoint_qipao[0][k2]<g_BlackPoint_qipao[0][k2-1]){left_or_right=1;qs=g_BlackPoint_qipao[0][k2-1];g_BlackPoint_qipao[0][k2-1]=g_BlackPoint_qipao[0][k2];g_BlackPoint_qipao[0][k2]=qs;qs=g_BlackPoint_qipao[1][k2-1];g_BlackPoint_qipao[1][k2-1]=g_BlackPoint_qipao[1][k2];g_BlackPoint_qipao[1][k2]=qs;}if(abs(g_BlackPoint_qipao[0][k2-1]-g_BlackPoint_qipao[0][k2])-(g_BlackPoint_qipao[1][k2-1]+g_ BlackPoint_qipao[1][k2])/2<(g_BlackPoint_qipao[1][true_road_cen]*2) //????×?′ó&&abs(g_BlackPoint_qipao[0][k2-1]-g_BlackPoint_qipao[0][k2])-(g_BlackPoint_qipao[1][k2-1]+g_ BlackPoint_qipao[1][k2])/2>g_BlackPoint_qipao[1][true_road_cen]/2 //????×?D?&&abs(g_VideoImageDate[array_num][g_BlackPoint_qipao[0][true_road_cen]]-g_VideoImageDate[ array_num][g_BlackPoint_qipao[0][k2]-g_BlackPoint_qipao[1][k2]/2-(abs(g_BlackPoint_qipao[0][k2-1]-g_Bl ackPoint_qipao[0][k2])-(g_BlackPoint_qipao[1][k2-1]+g_BlackPoint_qipao[1][k2])/2)/2])>10&&g_VideoImageDate[array_num-1][g_BlackPoint_qipao[0][k2]-g_BlackPoint_qipao[1][k2]/2-(abs(g_BlackPoint_qipao[0][k2-1]-g_BlackPoint_qipao[0][k2])-(g_BlackPoint_qipao[1][k2-1]+g_BlackPoint_qipao[1][k2])/2)/2]>MIDDLE[n]&&g_VideoImageDate[array_num+1][g_BlackPoint_qipao[0][k2]-g_BlackPoint_qipao[1][k2]/2-(abs(g_BlackPoint_qipao[0][k2-1]-g_Bl ackPoint_qipao[0][k2])-(g_BlackPoint_qipao[1][k2-1]+g_BlackPoint_qipao[1][k2])/2)/2]>MIDDLE[n]){if(left_or_right==0){haveleft=1;left_n=n;left_center_x=true_road_cen;//AfxMessageBox("left2");}else{haveright=1;right_n=n;right_center_x=true_road_cen;//AfxMessageBox("right2");}//AfxMessageBox("left");#ifdef PCCRectbq=CRect(left+(g_BlackPoint_qipao[0][k2-1]-g_BlackPoint_qipao[1][k2-1]/2)*w,top+n*h,left+(g _BlackPoint_qipao[0][k2-1]+g_BlackPoint_qipao[1][k2-1]/2)*w+w,top+n*h+1*h);FillRect(m_hdc,bq,brush5);bq=CRect(left+(g_BlackPoint_qipao[0][k2]-g_BlackPoint_qipao[1][k2]/2)*w,top+n*h,left+(g_Bl ackPoint_qipao[0][k2]+g_BlackPoint_qipao[1][k2]/2)*w+w,top+n*h+1*h);FillRect(m_hdc,bq,brush5);intx=g_BlackPoint_qipao[0][k2]-g_BlackPoint_qipao[1][k2]/2-(abs(g_BlackPoint_qipao[0][k2-1]-g_BlackPoint_qipao[0][k2])-(g_BlackPoint_qipao[1][k2-1]+g_BlackPoint_qipao[1][k2])/2)/2;bq=CRect(left+(x)*w,top+n*h,left+(x)*w+w,top+n*h+h);FillRect(m_hdc,bq,CBrush(RGB(100,100,0)));#endif}}}}}}cnt_large[n]=0;if(cur_center<254&&cnt>1){if(cur_center==0&&(0 == g_SearchDirection) ) cnt_large[n]=1;else if(cur_center==0&&(1 == g_SearchDirection) ) cnt_large[n]=2;else if(cur_center==cnt-1&&(0 == g_SearchDirection) ) cnt_large[n]=2;else if(cur_center==cnt-1&&(1 == g_SearchDirection) ) cnt_large[n]=1;else cnt_large[n]=3;}if(((g_BlackPositionCenter[k] < 5) || (g_BlackPositionCenter[k] >= MAX_VIDEO_POINT-5)) ){// not_use_ava=1;if(((k - n) > 3)) {g_BlackPositionCenter[n] = NO_DATA_180;g_width[n] = 0;//n = -1;return 1;}}}}return 0;}void final(){signed char jj;g_ValidLine = 0;g_BlackLineTotal = 0;for(n = (MAX_VIDEO_LINE - 1);n>=0; n --){if(g_BlackPositionCenter[n] != NO_DATA_180){#ifdef PCCRectbq=CRect(left+g_BlackPositionCenter[n]*w,top+n*h,left+g_BlackPositionCenter[n]*w+w,top+n *h+1*h);FillRect(m_hdc,bq,brush3);bq=CRect(left+(g_BlackPositionCenter[n]+g_width[n]/2)*w,top+n*h,left+(g_BlackPositionCente r[n]+g_width[n]/2)*w+w,top+n*h+1*h);FillRect(m_hdc,bq,brush3);bq=CRect(left+(g_BlackPositionCenter[n]-g_width[n]/2)*w,top+n*h,left+(g_BlackPositionCenter [n]-g_width[n]/2)*w+w,top+n*h+1*h);FillRect(m_hdc,bq,brush3);#endifg_V alidLine ++;g_BlackLineTotal += g_BlackPositionCenter[n];}if(n == MAX_VIDEO_LINE - 10){if(g_ValidLine != 0){g_BottomMiddle = g_BlackLineTotal / g_ValidLine;#ifdef PCCString numstr=_T("");numstr.Format("%d",g_BottomMiddle);TextOut(m_hdc,left+180,top+200,numstr,numstr.GetLength());#endif//break;}else{for(jj=MAX_VIDEO_LINE - 10;jj>=0;jj--){if(g_BlackPositionCenter[jj] != NO_DATA_180){ g_BottomMiddle=g_BlackPositionCenter[jj];break;}}}}else{;}}if(g_IfCross==0&&haveleft==1&&haveright==1&&abs(left_n-right_n)<2&&abs(left_center_x-right_center_x)<4){g_IfCross=1;}#ifdef PCif(g_IfCross==1)AfxMessageBox("qipao");#endifif(g_V alidLine == 0){if(g_BlackMiddle[0] < MAX_VIDEO_POINT/2-5){g_BlackMiddle[1] = 0;g_BlackMiddle[0] = 1;}else if(g_BlackMiddle[0] >= MAX_VIDEO_POINT/2+5){g_BlackMiddle[1] = MAX_VIDEO_POINT-1;g_BlackMiddle[0] = MAX_VIDEO_POINT-2;}else{g_BlackMiddle[1] = g_BlackMiddle[0];}}else{g_BlackMiddle[1] = g_BlackLineTotal / g_V alidLine;g_BlackMiddle[0] = g_BlackMiddle[1];}if(g_BlackMiddle[1] < 0){g_BlackMiddle[1] = 0;}else if(g_BlackMiddle[1] > MAX_VIDEO_POINT-1){g_BlackMiddle[1] = MAX_VIDEO_POINT-1;}else{;}}。

飞思卡尔智能车摄像头组freescale程序代码

飞思卡尔智能车 智能车源代码光电组(有注解)

飞思卡尔 摄像头 程序

飞思卡尔智能车摄像头代码

飞思卡尔智能车光电源代码

飞思卡尔摄像头组智能车程序代码

飞思卡尔单片机的程序

飞思卡尔智能车程序代码

飞思卡尔智能车完整程序

飞思科尔光电组程序

飞思卡尔电磁组程序范例

飞思卡尔程序

飞思卡尔详细代码

飞思卡尔智能车程序

飞思卡尔智能车(摄像头)核心程序

飞思卡尔智能车程序

飞思卡尔智能车程序汇总

飞思卡尔智能车智能车源代码光电组(有注解)

飞思卡尔摄像头程序