飞思卡尔智能车摄像头组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 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。
- 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{;}}。