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

飞思卡尔智能车（摄像头）核心程序 1舵机PD法过弯策略 /***************************舵机PID函数*********************************/ void Angle_PID(int CENTER) { //2.8 float angle_kp=7.38,angle_kd=17.2;//系数 PD控制 if(lost_time==0 ||left_lost_time==0 ||right_lost_time==0 ) //判断如果黑线都丢失了，则不更新数据 { angle_error=CENTER-CENTER_INIT; // if(left_lost_time20 right_lost_time==0) // angle_error=CENTER_INIT-CENTER+abs(angle_error)/8.5; // else if(left_lost_time==0 right_lost_time20) //angle_error=CENTER_INIT-CENTER-abs(angle_error)/8.5; angle=(int)(angle_init+angle_kp*angle_error+angle_kd*(angle_error-angle_last_error)); if(angle=angle_init-250) //防止系数过大导致舵机打死 { angle=angle_init-250; //6%-9% ,,2000 } if(angle=angle_init+250) { angle=angle_init+250; } angle_last_error=angle_error; PWMDTY23=angle; } } 2大津法动态阈值调节 /***************************大律法动态阈值*********************************/ void otsu_yuzhi( ) { int wh = 128; int inIm[128]; int i, t; int L = 128; double p[128]; double w1=0; double u0=0; double u1 = 0; double max = 0.0; double w0 = 0; int m =0; double sigma[128]; for (i = 0; i 128; i++) inIm[i] = ADV[i]0xff; for (i = 0; i L; i++) p[i] = 0; //计算各灰度出现次数 for (i = 0; i 128; i++) p[inIm[i]]++; //计算各灰度级出现概率 for (m = 0; m L; m++) p[m] = p[m] / wh; for (t = 0; t L; t++) { w0 = 0; for (m = 0; m t+1; m++) w0 += p[m]; w1 = 1 - w0; u0 = 0; for (m = 0; m t + 1; m++) u0 += m * p[m] / w0; u1 = 0; for (m = t; m L; m++) u1 += m *