智能车竞赛摄像头组代码

#include <hidef.h> /* common defines and macros */
#include "derivative.h" /* derivative-specific definitions */
#include <MC9S12XS128.h> /* derivative information */
#pragma MESSAGE DISABLE C2705 /* Disable warning C2705 "Possible loss of data" */

///////////////////坑爹板子插接////////////////////////
//Vcc、GND
//PS0-3
//PB0-7 i
//AD0-7
//PP4(HREF)、PP5(VSYN)

/*****************************************************/
////////////////////////常量定义///////////////////////

//////////////////自己加入的宏定义////////////////////
////////////////////////////////////////////////////
#define White 0
#define Black 1
int flag=0;
int flag1=0;
//////////////////////可调参数///////////////////////
/////////////////////////////////////////////////////
#define IMG_ROWS 30 //图像采集行数
#define IMG_COLS 118 //图像采集列数

#define car_center 59 //车模中心值

#define steer_center 3830 //舵机中心值
#define steer_left 3100 //舵机左拐最大值
#define steer_right 4450 //舵机右拐最大值 ;cd

/////////////////////摄像头控制////////////////////
///////////////////////////////////////////////////

//CCD_state//
#define GETVSYNC 0 //获取场中断

#define GETHREF 1 //获取行中断
#define WAITVSYNC 2 //等待场中断
#define WAITHREF 3 //等待行中断

#pragma CODE_SEG DEFAULT //后续代码置于默认区域内

//////////////////自己加入的变量////////////////////
////////////////////////////////////////////////////
/*int border[IMG_ROWS]={135,135,134,133,133,132,130,128,
125,125,125,125,125,124,121,120,
120,120,120,120,120,120,120,120,
119,119,119,118,118,118}; */

/*int border[IMG_ROWS]={140,140,140,140,139,139,139,139,
138,138,135,135,132,132,130,130,
128,128,127,127,126,126,125,125,
124,124,123,123,122,122}; */

int border[IMG_ROWS]={118,118,118,118,118,118,118,118,
118,118,118,118,118,118,118,118,
118,118,118,118,118,118,118,118,
118,118,118,118,118,118};


///////////////////////LED////////////////////////
//////////////////////////////////////////////////
unsigned char Led[] = {0xFB,0xF7,0xDF,0xBF}; //PE2.3.5.6口显示，0对应的LED灯亮
int i=0,j=0,k=0;
int t=0,f=0,s_f=0;
///////////////////////数码管/////////////////////
//////////////////////////////////////////////////
unsigned char LedCode[]={0xc0,0xf9,0xa4,0xb0,0x99,0x92,0x82,0xf8,0x80,0x90}; //数码管显示二进制码
unsigned char LedData[
]={0,0,0,0};
unsigned char LedNum = 0;
int time1 = 0;
int time=0;
int pulse=0;
///////////////////摄像头图像采

集//////////////////
///////////////////////////////////////////////////
uchar ImageData0[IMG_ROWS][IMG_COLS]; //图像存储数组
uchar image_v_num=0; //已采集到的摄像头的当前行
uchar v_counter=0; //已读取的行数
unsigned char CCD_state; //摄像头的给的状态
unsigned int VSync=0; //总场数
int count=0;
int st_flag=0;
int jishu=0;
//////////////////////黑线提取////////////////////
//////////////////////////////////////////////////
unsigned char Lx[IMG_ROWS]; //左引导线中心点列号,没有找到的值为IMG_COLS
unsigned char Rx[IMG_ROWS]; //右引导线中心点列号,没有找到的值为0
int center[IMG_ROWS]; //中点坐标

//////////////////////舵机PD控制/////////////////////
/////////////////////////////////////////////////////
int steer_out=steer_center; //舵机控制输出量
int steer_out1= steer_center;

//int steer_out6=steer_center;
//float steer_p_near=5.0; //对P值赋初值
//float steer_p_mid=14.0 ;
//float steer_p_far=13;

//float steer_p_near=25.0; //对P值赋初值
//float steer_p_mid=13.5;
//float steer_p_far=10.2;
//int SteerKd=2.0; //对D值赋初值
float steer_p_near=6.3; //对P值赋初值
float steer_p_mid=8.3;
float steer_p_far=10.2;

int SteerKd=1;
int near_piancha; //近处中线与车模中心偏差
int mid_piancha; //中间中线与车模中心偏差
int far_piancha; //远处中线与车模中心偏差
int allpiancha; //中线与车模中心总偏差
int hw_flag=1;
int zhidao=450 ;
int wandao= 450 ;
int SteerE0=0;
int SteerE1=0;
int M_flag=1;
int P_flag=0;
/****************************************************/
////////////////全局&初始化程序段/////////////////////

//////////////////////初始化函数/////////////////////
/////////////////////////////////////////////////////

//通用IO口初始化//
void initGPIO(void)
{
DDRB = 0xFF; //B口数码管输出，PORTB
DDRS = 0xFF; //S口输出，共阳数码管选择，PTS
DDRA=0x00;
//PP1、PP3PP5、PP7作为PWM输出时不用定义IO方向
//The PWM forces the I/O state to be an output for an enabled channel.
//If the PWM shutdown feature is enabled this pin is forced to be an input.
//In this case the data direction bit will not change.
//摄像头行场中断输入口
DDRP_DDRP4=0;
DDRP_DDRP5=0;
DDRT_DDRT1=0;
DDRT_DDRT2=0;
DDRT_DDRT0=0;
DDRT_DDRT3=0;
}

//ATD作为通用
IO//
void ATD0_init(void)
{
DDR1AD0=0x00; //AD口摄像头灰度值输入
ATD0DIEN=0x00FF;
}

//行场中断初始化//
void PP_init(void)
{
PIEP_PIEP4=1; //行中断允许

PIEP_PIEP5=1; //场中断允许
PPSP=0xFF; //上升沿触发中断
}

//锁相环初始化，将总线频率调整到80M//
void initPLL80M(void)
{
CLKSEL=0x00; //禁止锁相环，时钟有外部晶振提供，总线频率=外部晶振/2
PLLCTL_CME=1; //允许时钟监控
PLLCTL_SCME=0; //外部晶振失效进入自给时钟
CRGINT_SCMIE=1; //允许时钟状态改变时中断
PLLCTL_PLLON=1; //打开锁相环
SYNR=0xc9; //SYNRDIV=9
REFDV=0x81; //REFDIV=1 Fvoc=2*Fosc*(SYNRDIV+1)/(REFDIV+1) 2*16*10/2=160M
POSTDIV = 0x00; //Fpll=Fvoc/2/POSTDIV
//If POSTDIV = $00 then fPLL is same as fVCO (divide by one).
_asm(nop); //BUS CLOCK=80M (f_bus=f_pll/2) 160M/2=80M;
_asm(nop);
while(!(CRGFLG_LOCK==1)); // 等待锁相环初始化完成
CLKSEL_PLLSEL =1; // 使用锁相环



}

//总线80M，PWM初始化//
void initPWM80M(void)
{
PWME=0x00; //关闭所有PWM通道
PWMPOL=0xFF; //PWM极性选择，选择一个周期开始时为高电平
//Polarity = 1 (PPOLx = 1)
//Duty Cycle = [PWMDTYx / PWMPERx] * 100%
PWMPRCLK=0x22; //CLOCK A，B时钟分频，均选择从总线四分频，80M/4=20M,
PWMSCLA=5; //Clock SA = Clock A / (2 * PWMSCLA),从CLOCK A 10分频，2M（可以选用小于等于10M的其他频率）
PWMSCLB=5; //CLOCK SB从CLOCK B 10分频 ,20M/10=2M;
PWMCTL=0xF0; //01级联，23级联，45级联，67级联（16Bit）
PWMCLK=0xFF; //PWM始终选择，选择CLOCK SA SB为PWM时钟
//For channels 0, 1, 4, and 5 the choices are clock A or clock SA.
//For channels 2, 3, 6, and 7 the choices are clock B or clock SB.
PWMPER01=5000; //电机0 PWM频率=2M/200=10KHz,200为计数值，计数完了进入下一个PWM，计数频率为2M
PWMDTY01=300; //电机0的占空比=PWMDTY01/PWMPER01

PWMPER23=5000; //电机1 PWM频率=2M/200=10KHz
PWMDTY23=0; //电机1的占空比=PWMDTY23/PWMPER23
//PWMx Frequency = Clock (A, B, SA, or SB) / PWMPERx
PWMPER67=40000; //PP7的PWM频率为50Hz，舵机的PWM频率
PWMDTY67=steer_center; //舵机占空比
PWME_PWME1=1;
PWME_PWME3=1;
PWME_PWME7=1; //PWM波开始输出
}

// 实时中断初始化
//
void InitRTI(void)
{
RTICTL =0xAF; //16M/(80*1000)=200Hz 20ms中断一次
CRGINT = 0x80; // 打开实时中断


}
void Moto_PID1(int think_speed)
{
volatile static int err=0,derr=0,dderr=0;
volatile static int last_err=0,last_derr=0 ;
volatile static int M_PWM=0;

err=think_speed-pulse;// err
derr=err-last_err;
dderr=derr-last_derr;
if(err<80&&err>-80)

;
// else{
// if(err<40&&err>-40)
M_PWM+=err*1.3+derr*3.0+ dderr*3.3;
// else M_PWM+=err*5+ ;

// }
if(M_PWM<0)
M_PWM=0;

if(M_PWM>=2500)
M_PWM=2500;
PWMDTY01=M_PWM ;

last_err=err; //记录上次偏差值
last_derr=derr;
}

void Moto_PID() {

if((steer_out>=4000||steer_out<=3600)&&(steer_out1>=4000||steer_out1<=3600))
Moto_PID1(wandao) ;
else Moto_PID1(zhidao) ;
}
/***************************************************/
///////////////////图像处理算法//////////////////////

//摄像头正方向安装
//car_center=59
//IMG_COLS=118

//////////////////中线的中值滤波/////////////////////
/////////////////////////////////////////////////////

int lvbo(unsigned int a,unsigned int b,unsigned int c)
{
//lvbo(center[i-1],center[i+1],center[i])
unsigned int max,min;
if(a==0 || a==IMG_COLS || b==0 || b==IMG_COLS) return c;
//ab排序
max=a;
min=b;
if(a<b)
{
min=a;
max=b;
}
//c在ab中间
if(c>min && c<max) return c;
//否则
else return (max+min)/2;
}

////////////////////////二值化/////////////////////////
///////////////////////////////////////////////////////
void Erzhihua()
{
for(i=0;i<IMG_ROWS;i++)
{
for(j=0;j<IMG_COLS;j++)
{
if(ImageData0[i][j]>border[i]) ImageData0[i][j]=White;
else ImageData0[i][j]=Black;
}
}
}

//////////////////中线的中值滤波///////////////////////
///////////////////////////////////////////////////////
void CenterFilter()
{
int temp=0;
//黑线的中值滤波程序
for(i=IMG_ROWS-2;i>=1;i--)
{
temp=lvbo(center[i-1],center[i+1],center[i]);
center[i]=temp;
}
}

/////////////////////计算中心值//////////////////////
/////////////////////////////////////////////////////
void GetCenter(int i)
{
if(Lx[i]==2 && Rx[i]==116) {center[i]=car_center;} //左右均未搜到，用车中心值代替
if(Lx[i]==2 && Rx[i]!=116) {
if(i>2&&i<8) center[i]=Rx[i]-42;
if(i>13&&i<19) center[i]=Rx[i] -60;
else center[i]=Rx[i]-55 ;
}

if(Lx[i]!=2 && Rx[i]==116) {
if(i>2&&i<8) center[i]=Lx[i]+42;
if(i>13&&i<19) center[i]=Lx[i] +60;
else center[i]=Lx[i]+55;
}
if (Lx[i]!=2 && Rx[i]!=116)center[i]=(Lx[i]+Rx[i])/2;
}

void ImagePro
cessing()
{

//小车前1/2图像部分的处理//
for(i=IMG_ROWS-1;i>=IMG_ROWS*1/2;i--)
{
//从中间往右搜索右线
for(j=car_center;j<=IMG_COLS-3;j++)
{
if( !ImageData0[i][j] && ImageData0[i][j+1] && ImageData0[i][j+2] && ImageData0[i][j+3] )
{
Rx[i]=(j+1);
break;
}
if(j==(IMG_COLS-4)) {
Rx[i]=IMG_COLS-2;
}//没有搜索到右线
}
//从中间往左搜索左线
for(j=car_center;j>=3;j

--)
{
if( !ImageData0[i][j] && ImageData0[i][j-1] && ImageData0[i][j-2] && ImageData0[i][j-3] )
{
Lx[i]=(j-1);
break;
}
if(j==3){
Lx[i]=2; //没有搜索到左线 ////////为什么等于2啊？不应该等于0吗？

}
}
//计算中心线
GetCenter(i);
}
//小车后1/2图像部分的处理//
for(i=IMG_ROWS*1/2-1;i>=0;i--)
{
//从上一行中心点往右搜索右线
for(j=center[i+1];j<=IMG_COLS-3;j++)
{
if( !ImageData0[i][j] && ImageData0[i][j+1] && ImageData0[i][j+2] )
{
Rx[i]=(j+1);
break;
}
if(j==(IMG_COLS-3)) {
Rx[i]=IMG_COLS-2; //没有搜索到右线 //////?

}
}
//从上一行中心点往左搜索左线
for(j=center[i+1];j>=3;j--)
{
if( !ImageData0[i][j] && ImageData0[i][j-1] && ImageData0[i][j-2] )
{
Lx[i]=(j-1);
break;
}
if(j==3) {
Lx[i]=2; //没有搜索到左线

}
}
//计算中心线

GetCenter(i);
}
}

/***************************************************/
////////////////舵机电机控制算法/////////////////////

//获取中间值//
int get_mid(unsigned int a,unsigned int b,unsigned int c)
{
unsigned int x=0;
if(a>b){x=b;b=a;a=x;}
if(b>c){x=c;c=b;b=x;}
if(a>b){x=b;b=a;a=x;}
return b ;
}

/////////////////舵机控制///////////////////////
////////////////////////////////////////////////
void steer_PD()
{

steer_out1=steer_out;
near_piancha= car_center-get_mid((int)center[25],(int)center[26],(int)center[27]);
//若不在跑道内，保持最后的偏差值
//若偏右，则中线靠左，偏差大于0
mid_piancha= car_center-get_mid((int)center[15],(int)center[16],(int)center[17]);
far_piancha= car_center-get_mid((int)center[4],(int)center[5],(int)center[6]);

allpiancha=steer_p_near*(float)(near_piancha)+steer_p_mid*(float)(mid_piancha)+steer_p_far*(float)(far_piancha);
SteerE0=allpiancha;
steer_out=steer_center-SteerE0-SteerKd*(SteerE0-SteerE1); //若偏右，则舵机向左打（正开），steer_out应减小
//加改为减，因逆向行驶改为正向
SteerE1=SteerE0;
if(steer_out>steer_right) steer_out=steer_right; //舵机输出限幅，否则会烧坏舵机
if(steer_o
ut<steer_left) steer_out=steer_left;
PWMDTY67=steer_out;
}

/***************************************************/
//////////////// ///最终控制函数//////////////////////

void final_control()
{
if(CCD_state==GETVSYNC &&VSync%2==0 ) //偶数场进行控制，奇数场采集
{
Erzhihua();
ImageProcessing();
CenterFilter();
steer_PD();
Moto_PID() ;
CCD_state=WAITVSYNC;//等待下一个场中断
}
}

/***************************************************/
///////////////////主函数程序段//////////////////////
void main()
{
/* put your own code here */

DisableInterrupts; //不能

进入中断函数

initPLL80M();
initPWM80M();
InitRTI();
initGPIO();
ATD0_init();
PP_init();
PWMDTY67=steer_center;
PACTL = 0x40;
PACNT = 0; //初始化计数器
CCD_state=WAITVSYNC; //初始化摄像头状态变量，等待场中断

EnableInterrupts; //可以进入中断函数

/
for(;;) {

final_control();
_FEED_COP(); /* feeds the dog */
}



} /* loop forever */





/* please ma、 sure that you never leave main */



/******************************************************/
//////////////////中断函数程序段////////////////////////
//（中断号从Includes/MC9S12XS128.h中查询）
#pragma CODE_SEG __NEAR_SEG NON_BANKED


void interrupt 7 RTI_INT()
{
time++;

if (time>10){
pulse=PACNT;
PACNT=0;
time=0;

}
if(PTT_PTT1==0){
zhidao=550;
wandao=500;
}
else if(PTT_PTT2==0){
zhidao=500;
wandao=475;
}
else if(PTT_PTT3==0) {
zhidao=475;
wandao=475;
}


CRGFLG = 0x80;



}

////////////////灰度值获取//////////////
////////////////////////////////////////
interrupt 56 void V_SYNC() //行场中断服务子函数
{
if(PIFP_PIFP5==1) //场中断
{
PIFP_PIFP5=1;
PIEP_PIEP4=0; //进入场中断，行中断禁止
image_v_num=0; //行数清零
v_counter=0; //采集行清零
VSync++; //记录是奇数场还是偶数场
VSync=VSync%2;
if(VSync%2==0) //偶数场停止采集图像，即禁止行中断，用于控制
PIEP_PIEP4=0;
else
PIEP_PIEP4=1; //奇数场允许行中断，采集图像
CCD_state=GETVSYNC; //获得场中断
}
else if(PIFP_PIFP4==1) //行中断
{
image_v_num++;


//采集图像

if(image_v_num>=30 && image_v_num<=300 && v_counter<IMG_ROWS && image_v_num%5==0)
{


for(i=0;i<IMG_COLS;i++) //采集图像
{
Ima
geData0[v_counter][i]=PT1AD0;
}

v_counter++; //准备下一行
}
PIFP_PIFP4=1;
}

}

#pragma CODE_SEG DEFAULT