#include "zf_common_headfile.h" #include <math.h> unsigned char Image_Use[LCDH][LCDW]; unsigned char Bin_Image[LCDH][LCDW]; uint16 Threshold; sint16 Center_offset1=0; sint16 Center_offset2=0; sint16 Center_offset3=0; sint16 youbianxian_location[LCDW]={0}; sint16 youbianxian_flag[LCDH]={0} ; sint16 zuobianxian_location[LCDW]={0}; sint16 zuobianxian_flag[LCDH]={0} ; sint16 end_point=0; sint16 start_point; sint16 centerline[LCDH]={0}; sint16 therowmiddlepoint[LCDH]={0}; //******** int16_t Center_offset; float deviation_demo=0; volatile int Ramp_Flag=0; void camera_use(void) { if(mt9v03x_finish_flag == 1) { Transfer_Camera(mt9v03x_image[0], Image_Use[0], MT9V03X_W*MT9V03X_H); Bin_Image_TWO(); Bin_Image_Filter(); edge_search(); Deviation_compute(); mt9v03x_finish_flag = 0; } } void Transfer_Camera(uint8 *p, uint8 *q, int16 pixel_num) { for(int16 i = 0; i < pixel_num; i++) *(q +i) = *(p +i); } void Bin_Image_TWO() { Threshold = GetOSTU(Image_Use); uint16 thre; for(uint8 y = 0; y < MT9V03X_H; y++) //这里考虑到了图像边缘的光照偏弱的问题 { for(uint8 x = 0; x < MT9V03X_W; x++) { thre = Threshold; if (Image_Use[y][x] >thre) //数值越大，显示的内容越多，较浅的图像也能显示出来 Bin_Image[y][x] = 255; //白 else Bin_Image[y][x] = 0; //黑 } } } short GetOSTU (unsigned char tmImage[LCDH][LCDW]) { signed short i, j; unsigned long Amount = 0; unsigned long PixelBack = 0; unsigned long PixelshortegralBack = 0; unsigned long Pixelshortegral = 0; signed long PixelshortegralFore = 0; signed long PixelFore = 0; float OmegaBack, OmegaFore, MicroBack, MicroFore, SigmaB, Sigma; signed short MinValue, MaxValue; signed short Threshold = 0; unsigned char HistoGram[256]; for (j = 0; j < 256; j++) HistoGram[j] = 0; for (j = 0; j < LCDH; j++) { for (i = 0; i < LCDW; i++) { HistoGram[tmImage[j][i]]++; } } for (MinValue = 0; MinValue < 256 && HistoGram[MinValue] == 0; MinValue++); for (MaxValue = 255; MaxValue > MinValue && HistoGram[MinValue] == 0; MaxValue--); if (MaxValue == MinValue) return MaxValue; if (MinValue + 1 == MaxValue) return MinValue; for (j = MinValue; j <= MaxValue; j++) Amount += HistoGram[j]; Pixelshortegral = 0; for (j = MinValue; j <= MaxValue; j++) { Pixelshortegral += HistoGram[j] * j; } SigmaB = -1; for (j = MinValue; j < MaxValue; j++) { PixelBack = PixelBack + HistoGram[j]; PixelFore = Amount - PixelBack; OmegaBack = (float) PixelBack / Amount; OmegaFore = (float) PixelFore / Amount; PixelshortegralBack += HistoGram[j] * j; PixelshortegralFore = Pixelshortegral - PixelshortegralBack; MicroBack = (float) PixelshortegralBack / PixelBack; MicroFore = (float) PixelshortegralFore / PixelFore; Sigma = OmegaBack * OmegaFore * (MicroBack - MicroFore) * (MicroBack - MicroFore); if (Sigma > SigmaB) { SigmaB = Sigma; Threshold = j; } } return Threshold; } void Bin_Image_Filter (void) { sint16 nr; sint16 nc; for (nr = 0; nr < LCDH - 1; nr++) { for (nc = 1; nc < LCDW - 1; nc = nc + 1) { if ((Bin_Image[nr][nc] == 0) && (Bin_Image[nr - 1][nc] + Bin_Image[nr + 1][nc] + Bin_Image[nr][nc + 1] + Bin_Image[nr][nc - 1] > 2)) { Bin_Image[nr][nc] = 1; } else if ((Bin_Image[nr][nc] == 1) && (Bin_Image[nr - 1][nc] + Bin_Image[nr + 1][nc] + Bin_Image[nr][nc + 1] + Bin_Image[nr][nc - 1] < 2)) { Bin_Image[nr][nc] = 0; } } } } int STOP=0; void edge_search(void) { sint16 n,m,middleline = LCDW/2; end_point=0; for(n =LCDH-1; n >=LCDH-5 ; n--) { if(Bin_Image[n][LCDW/2]!=0) { for(m = middleline; m >= 1; m --) { if(Bin_Image[n][m] == 0) { zuobianxian_location[n] = m; zuobianxian_flag[n] = 1; break; } else { zuobianxian_location[n] = 1; zuobianxian_flag[n] = 0; } } for(m = middleline; m <= LCDW - 1; m ++) { if(Bin_Image[n][m] == 0) { youbianxian_location[n] = m; youbianxian_flag[n] = 1; break; } else { youbianxian_location[n] = LCDW-1; youbianxian_flag[n] = 0; } } middleline = (youbianxian_location[n] + zuobianxian_location[n]) / 2; } if(Bin_Image[n][LCDW/2]==0) { for(m = middleline-50; m >= 1; m --) { if(Bin_Image[n][m] == 0) { zuobianxian_location[n] = m; zuobianxian_flag[n] = 1; break; } else { zuobianxian_location[n] = 1; zuobianxian_flag[n] = 0; } } for(m = middleline+50; m <= LCDW - 1; m ++) { if(Bin_Image[n][m] == 0) { youbianxian_location[n] = m; youbianxian_flag[n] = 1; break; } else { youbianxian_location[n] = LCDW-1; youbianxian_flag[n] = 0; } } middleline = (youbianxian_location[n] + zuobianxian_location[n]) / 2; } STOP=Bin_Image[119][LCDW/2]+Bin_Image[118][LCDW/2]+Bin_Image[117][LCDW/2]+Bin_Image[116][LCDW/2]+Bin_Image[115][LCDW/2]; } for(n=115;n>45;n--) { if(Bin_Image[n-1][zuobianxian_location[n]] == 1) { for(m=zuobianxian_location[n];m>=1;m--) { if(Bin_Image[n-1][m] == 0) { zuobianxian_location[n-1]= m; zuobianxian_flag[n-1] = 1; break; } else { zuobianxian_location[n-1]=1; zuobianxian_flag[n-1] = 0; } } } if(Bin_Image[n-1][zuobianxian_location[n]] == 0) { for(m=zuobianxian_location[n];m<=LCDW - 1;m++) { if(m==LCDW-1&&end_point==0) end_point=n; if(Bin_Image[n-1][m] !=0) { zuobianxian_location[n-1] = m-1; zuobianxian_flag[n-1] = 1; break; } else { zuobianxian_location[n-1] = m; zuobianxian_flag[n-1] = 0; } } } if(Bin_Image[n-1][youbianxian_location[n]] !=0) { for(m=youbianxian_location[n];m<=LCDW - 1;m++) { if(Bin_Image[n-1][m] == 0) { youbianxian_location[n-1]= m; youbianxian_flag[n-1] = 1; break; } else { youbianxian_location[n-1]=LCDW - 1; youbianxian_flag[n-1] = 0; } } } if(Bin_Image[n-1][youbianxian_location[n]] == 0) { for(m=youbianxian_location[n];m>=1;m--) { if(m==1&&end_point==0) end_point=n; if(Bin_Image[n-1][m] !=0) { youbianxian_location[n-1] = m+1; youbianxian_flag[n-1] = 1; break; } else { youbianxian_location[n-1] = m; youbianxian_flag[n-1] = 0; } } } centerline[n]=(youbianxian_location[n] + zuobianxian_location[n]) / 2; Bin_Image[n][centerline[n]] = 0; } } void Deviation_compute(void) { float sumofleft=0,sumofright=0; for(sint16 n=70;n>45;n--) { sumofleft += LCDW/2-zuobianxian_location[n]; sumofright += LCDW/2-youbianxian_location[n]; } deviation_demo = (sumofleft+sumofright)/10; if(end_point>=10&&end_point<40) { for(sint16 n=end_point;n<40;n++) { sumofleft += LCDW/2-zuobianxian_location[n]; sumofright += LCDW/2-youbianxian_location[n]; } Center_offset1=(sumofleft+sumofright)/(40-end_point); sumofleft=0;sumofright=0; for(sint16 n=40;n<80;n++) { sumofleft += LCDW/2-zuobianxian_location[n]; sumofright += LCDW/2-youbianxian_location[n]; } Center_offset2=(sumofleft+sumofright)/40; sumofleft=0;sumofright=0; for(sint16 n=80;n<110;n++) { sumofleft += LCDW/2-zuobianxian_location[n]; sumofright += LCDW/2-youbianxian_location[n]; } Center_offset3=(sumofleft+sumofright)/30; sumofleft=0;sumofright=0; } else if(end_point>=40&&end_point<80) { Center_offset1=0; for(sint16 n=end_point;n<80;n++) { sumofleft += LCDW/2-zuobianxian_location[n]; sumofright += LCDW/2-youbianxian_location[n]; } Center_offset2=(sumofleft+sumofright)/(80-end_point); sumofleft=0;sumofright=0; for(sint16 n=80;n<110;n++) { sumofleft += LCDW/2-zuobianxian_location[n]; sumofright += LCDW/2-youbianxian_location[n]; } Center_offset3=(sumofleft+sumofright)/30; sumofleft=0;sumofright=0; } else if(end_point>=80&&end_point<110) { Center_offset1=0; Center_offset2=0; for(sint16 n=end_point;n<110;n++) { sumofleft += LCDW/2-zuobianxian_location[n]; sumofright += LCDW/2-youbianxian_location[n]; } Center_offset3=(sumofleft+sumofright)/(110-end_point); sumofleft=0;sumofright=0; } else if(end_point<10) { for(sint16 n=end_point;n<40;n++) { sumofleft += LCDW/2-zuobianxian_location[n]; sumofright += LCDW/2-youbianxian_location[n]; } Center_offset1=(sumofleft+sumofright)/(40-end_point); sumofleft=0;sumofright=0; for(sint16 n=30;n<70;n++) { sumofleft += LCDW/2-zuobianxian_location[n]; sumofright += LCDW/2-youbianxian_location[n]; } Center_offset2=(sumofleft+sumofright)/40; sumofleft=0;sumofright=0; for(sint16 n=80;n<110;n++) { sumofleft += LCDW/2-zuobianxian_location[n]; sumofright += LCDW/2-youbianxian_location[n]; } Center_offset3=(sumofleft+sumofright)/30; sumofleft=0;sumofright=0; } Center_offset = Center_offset1 * 0 + Center_offset2 * 1 + Center_offset3 * 0; deviation_demo = Center_offset; } void MotorInit(void) { gpio_init(P21_2, GPO, GPIO_LOW, GPO_PUSH_PULL); gpio_init(P21_4, GPO, GPIO_LOW, GPO_PUSH_PULL); pwm_init(ATOM0_CH1_P21_3, 17000, 0); pwm_init(ATOM0_CH3_P21_5, 17000, 0); pwm_init(ATOM1_CH1_P33_9, 50, 0); } void Motor_control(int32 motor1,int32 motor2) { if (motor1>0) { pwm_set_duty(ATOM0_CH1_P21_3,motor1); gpio_set_level(P21_2, 1); } else{ pwm_set_duty(ATOM0_CH1_P21_3,(0-motor1)); gpio_set_level(P21_2, 0); } if (motor2>0) { pwm_set_duty(ATOM0_CH3_P21_5,motor2); gpio_set_level(P21_4, 0); } else{ pwm_set_duty(ATOM0_CH3_P21_5,(0-motor2)); gpio_set_level(P21_4, 1); } if(motor1>8000) motor1=8000; if(motor1<-8000) motor1=-8000; if(motor2>8000) motor2=8000; if(motor2<-8000) motor2=-8000; } void ServoCtrl (uint32 duty) { // if (duty >= 1635) //限制幅值 // duty = 1635; // else if (duty <= 1235) //限制幅值 // duty = 1235; pwm_set_duty(ATOM1_CH1_P33_9, duty); } // //float constrain_float(float amt, float low, float high) //{ // return ((amt)<(low)?(low):((amt)>(high)?(high):(amt))); //} // //float Inc_kp = 30.0; //float Inc_ki = 0.0; float Loc_kp = 1.1f;//1.01,1.05 float Loc_kd = 0.05f;//0.015 float Loc_pd(float error) { float Loc_out; Loc_out = error * Loc_kp - imu660ra_gyro_z * Loc_kd; return Loc_out; } pid_param_t Lpid,Rpid; float constrain_float(float amt, float low, float high) { return ((amt)<(low)?(low):((amt)>(high)?(high):(amt))); } void PidInit(pid_param_t * pid ,float Kp,float Ki,float Kd,int out_max) { pid->Kp= Kp; pid->Ki= Ki; pid->Kd= Kd; pid->imax= out_max; pid->out_p = 0; pid->out_i= 0; pid->out_d= 0; pid->out = 0; pid->integrator= 0; pid->last_error= 0; pid->last_derivative = 0; } float PidIncCtrl(pid_param_t * pid, float error) { pid->out_p = pid->Kp * (error - pid->last_error); pid->out_i = pid->Ki * error; pid->out_d = pid->Kd * ((error - pid->last_error) - pid->last_derivative); pid->last_derivative = error - pid->last_error; pid->last_error = error; pid->out += pid->out_p + pid->out_i + pid->out_d; return constrain_float(pid->out, -pid->imax, pid->imax); } int16 lspeed = 0; int16 rspeed = 0; int16 LMotor=0,RMotor=0,Velocity=0,speed=200; float TMotor = 0; char START=0; int16 ENCODER=0,offset0=0,en_distance=0,en_distance_init=0; void pit(void) { imu660ra_get_gyro(); rspeed = encoder_get_count(TIM5_ENCODER); encoder_clear_count(TIM5_ENCODER); lspeed = -encoder_get_count(TIM4_ENCODER); encoder_clear_count(TIM4_ENCODER); LMotor=PidIncCtrl(&Lpid,(Velocity-absolute(deviation_demo*0.3)-(int)TMotor)-lspeed); RMotor=PidIncCtrl(&Rpid,(Velocity-absolute(deviation_demo*0.3)+(int)TMotor)-rspeed); //LMotor=PidIncCtrl(&Lpid,(Velocity-lspeed)); //RMotor=PidIncCtrl(&Rpid,(Velocity-rspeed)); ENCODER=rspeed; if(offset0==1) { en_distance+=ENCODER; en_distance_init=en_distance/111; } TMotor = Loc_pd(deviation_demo); //LMotor += LInc_pi(Velocity-lspeed-TMotor-absolute(deviation_demo*0.1));//0.1 //RMotor += RInc_pi(Velocity-rspeed+TMotor-absolute(deviation_demo*0.1)); // if(START==1) // { // Velocity=speed-80; //// if(lspeed<30 && rspeed<30) //// { //// if (LMotor>1000) //// {LMotor=1000;} //// if (RMotor>1000) //// {RMotor=1000;} //// } //// else if((lspeed>30 && rspeed>30)) //// { //// START=2; //// } // } // if(START==1) // { // Inc_kp = 8.0; // Inc_ki = 2.0; // //offset0=1; // // Velocity=speed-(80-(rspeed+lspeed)); // Velocity=speed-50; //// if(en_distance_init>=50) //// { //// START=2; //// } // if(lspeed>80 && rspeed>80) // { // START=2; // } // // // } // // if(START==2) // { // Inc_kp = 5.0; // Inc_ki = 20.0; // offset0=0; // en_distance_init=0; // ENCODER=0; // en_distance=0; // Velocity=speed; // } // if(STOP==0) { Velocity=0; //speed=0; LMotor=0; RMotor=0; TMotor=0; } if(LMotor >8000) LMotor =8000; if(RMotor> 8000) RMotor =8000; if(LMotor<-8000) LMotor=-8000; if(RMotor<-8000) RMotor=-8000; Motor_control(LMotor,RMotor);// // Motor_control(1000-deviation_demo*15-absolute(deviation_demo*2),1000+deviation_demo*15-absolute(deviation_demo*2)); //Motor_control(1000,1000); ServoCtrl(10000); } float absolute(float error2) { float real_error2; if(error2>0) real_error2= error2; else real_error2=-error2; return real_error2; } unsigned char data_to_send[64]; void ANO_DT_send_int16byte16(short data1, short data2, short data3, short data4, short data5, short data6, short data7, short data8 ){ unsigned char _cnt=0; unsigned char sum = 0, i = 0; data_to_send[_cnt++] = 0xAA; data_to_send[_cnt++] = 0xAA; data_to_send[_cnt++] = 0xF1; data_to_send[_cnt++] = 16; data_to_send[_cnt++]=(unsigned short)(data1>>8); data_to_send[_cnt++]=(unsigned char)(data1); data_to_send[_cnt++]=(unsigned short)(data2>>8); data_to_send[_cnt++]=(unsigned char)(data2); data_to_send[_cnt++]=(unsigned short)(data3>>8); data_to_send[_cnt++]=(unsigned char)(data3); data_to_send[_cnt++]=(unsigned short)(data4>>8); data_to_send[_cnt++]=(unsigned char)(data4); data_to_send[_cnt++]=(unsigned short)(data5>>8); data_to_send[_cnt++]=(unsigned char)(data5); data_to_send[_cnt++]=(unsigned short)(data6>>8); data_to_send[_cnt++]=(unsigned char)(data6); data_to_send[_cnt++]=(unsigned short)(data7>>8); data_to_send[_cnt++]=(unsigned char)(data7); data_to_send[_cnt++]=(unsigned short)(data8>>8); data_to_send[_cnt++]=(unsigned char)(data8); sum = 0; for(i=0;i<_cnt;i++) sum += data_to_send[i]; data_to_send[_cnt++]=sum; uart_write_string1(UART_2,data_to_send,_cnt); } 为该代码添加十字路口识别及十字路口补线功能