废的摄像头代码

智能车 评论 
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
//2022/07/17
//最后一版的1.0，经过二十天的折磨终于实现大道至简了，我觉得这是反应最快的思路了
#include "headfile.h"

#define col  MT9V03X_W //宽 120
#define row  MT9V03X_H //高 80

uint8 threshold_max;
uint8 mid_x,mid_y,pre_x,pre_y,temp_x,temp_y;
uint16 n,count_noBeacon=0,zero=0;
uint32 sum_x,sum_y;
uint8 flag_stick=0,flag_beacon=1,flag_row=0;

void camera(void){
  
  uint8 i,j,flag;
  
  flag=0;
  threshold_max = 180;
  sum_x=0,sum_y=0;
  n=0;
  
  pre_x = mid_x;
  pre_y = mid_y;
  
  if(mt9v03x_finish_flag){
    mt9v03x_finish_flag=0;
    ips200_displayimage032(mt9v03x_image[0],col,row);
    
    //找中心点
    for(i=17;i<row;i++){
      for(j=0;j<col;j++){
        if(mt9v03x_image[i][j]>threshold_max){
          sum_x = sum_x + j;
          sum_y = sum_y + i;
          n++;
          flag=1;
          if(n>400)flag_row=1;
        }
      }
    }
    if(flag){
      mid_x = sum_x/n;
      mid_y = sum_y/n;
      
      ips200_showfloat(0,19,mid_x,6,3);
      ips200_showfloat(80,19,mid_y,6,3);
      for(i=mid_x-10;i<mid_x+10;i++)
        ips200_drawpoint(i,mid_y,RED);
      for(i=mid_y-10;i<mid_y+10;i++)
        ips200_drawpoint(mid_x,i,RED);
    }else{
      mid_x=0;
      mid_y=0;
      
      ips200_showstr(0,19,"  not find points !");
    }
    
    point_deal();//处理点的坐标（畸变影响）
    flag_stick = stick();//是否一直怼着信标灯/停住出现卡死状态
  }
}

//处理点的坐标（畸变影响）
void point_deal(void){
  //判断是否找到信标灯
  if(mid_x==0){
    count_noBeacon++;
    if(count_noBeacon>=3){
      flag_beacon=0;//没找到灯
    }else{
      //在没找到点比较少时，将发现的点的坐标给予现在的坐标
      if(pre_x!=0&&pre_y!=0){
        temp_x = pre_x;
        temp_y = pre_y;
      }
      mid_x = temp_x;
      mid_y = temp_y;
    }
  }
  else{
    count_noBeacon=0;
    flag_beacon=1;//找到信标灯
  }
  
  //处理x
  if(mid_x<=110){
    mid_x=mid_x-20;
    if(mid_x<=0)mid_x=0;
  }else{
    mid_x=mid_x+20;
  }
  
  //处理y（当很靠近灯的时候）
  if(flag_row){
    mid_y=mid_y-5;
    flag_row=0;
  }
}

//一直怼着信标灯/停住出现卡死状态
uint8 stick(void){
  uint8 bias; 
  bias=mid_x-pre_x;
  if(bias==0){
    zero++;
  }else{
    zero=0;
  }
  if(zero>=20){//(zero>=3&&flag_beacon==0)//有20个重复的画面并且未找到信标灯
    zero=0;
    return 1;//卡死了
  }else{
    return 0;//没卡死
  }
}



#ifndef _imgDeal_H_
#define _imgDeal_H_

#include "headfile.h"


// **************************** 宏定 义 **************************** 

#define col MT9V03X_W //120 //MT9V03X_W 宽 
#define row   MT9V03X_H//80 //MT9V03X_H 高 


// **************************** 变量定义 ****************************

extern uint8 threshold_max;
extern uint8 mid_x,mid_y,pre_x,pre_y,temp_x,temp_y;
extern uint16 n,count_noBeacon,zero;
extern uint32 sum_x,sum_y;
extern uint8 flag_stick,flag_beacon,flag_row;

// **************************** 函数定义 ****************************
extern void camera(void);
extern void point_deal(void);
extern uint8 stick(void);

#endif
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
489
490
491
492
493
494
495
496
497
498
499
500
501
502
503
504
505
506
507
508
509
510
511
512
513
514
515
516
517
518
519
520
521
522
523
524
525
526
527
528
529
530
531
532
533
534
535
536
537
538
539
540
541
542
543
2022/07/15
#include "headfile.h"
#include<string.h>
#include<math.h>

#define col  MT9V03X_W //宽 120
#define row  MT9V03X_H //高 80

#define white 255
#define black 0
#define GrayScale 256

#define maxnum 1000
#define offset 0

uint8 mid_x,mid_y;
uint8 pre_x,pre_y;
uint8 zero=0,count_beacon=0;
uint8 threshold;//=70;//灰20-70；白70-126 -127-0
uint8 bin_img[row][col]; //二值化图 
uint8 domains[row][col]; //连通域图
//
//uint8 area_start[maxnum];
//uint8 area_end[maxnum];
//uint8 area_row[maxnum];
//uint8 num_area;   
//uint8 area_label[maxnum];
//uint8 point[col+20][2];
//int equalLabels[maxnum][2];

//可以不加二值化浪费时间，直接通过阈值来判断
//只改变与偏差有关的，与中线有关的间接有关的都不改变
//滤波等，思考……效率问题
//#define RED					0xF800													// 红色
//#define BLUE				0x001F													// 蓝色
//#define YELLOW				0xFFE0													// 黄色
//#define GREEN				0x07E0													// 绿色
//#define WHITE				0xFFFF													// 白色
//#define BLACK				0x0000													// 黑色 
//#define GRAY				0X8430													// 灰色
//#define BROWN				0XBC40													// 棕色
//#define PURPLE				0XF81F													// 紫色
//#define PINK				0XFE19	

//main函数
void camera(void)
{   
  uint8 stick_flag;//卡死标志
  if(mt9v03x_finish_flag)
  {
    mt9v03x_finish_flag=0;
    threshold=100;
    ips200_displayimage032(mt9v03x_image[0],col,row);
    
    Binarization(threshold);
//    ips200_displayimage032(bin_img[0],col, row);
    
    
    
//    Find_Beacon(); 
//    stick_flag=stick();
//    if(stick_flag==1&&flag_beacon==0){
//      //卡死在那,令车略微向后退
////      duty_l=duty_r=-7000;
//      duty_l=-duty_l;
//      duty_r=-duty_r;
//    }
//    if(stick_flag==1&&fabs(mid_x-pre_x)<6&&fabs(mid_y-pre_y)<6){
//      duty_l=-duty_l-5000;
//      duty_r=-duty_r-5000;
//    }
//    if(flag_beacon==0){
//      duty_l=0;
//      duty_r=duty_r+5000;
//    }
//    if(flag_beacon==1){
//      duty_l=duty_l+5000;
//      duty_r=duty_r+5000;
//    }
  }
}

//图像二值化 
void Binarization(uint8 threshold)
{
  uint8 i,j,m,n;
  uint8 flag_red=0;//信标灯没有阈值大于180的点
  
  for(i=0; i<row; i++)
  { 
    for(j=0; j<col; j++)
    {
//      domains[i][j]=0;
      if(mt9v03x_image[i][j]>threshold)
        bin_img[i][j]=white;
      else
        bin_img[i][j]=black;
      if(mt9v03x_image[i][j]>160){
        ips200_drawpoint(j,i,RED);
        flag_red=1;
        for(m=j-10;m<j+10;m++)
          ips200_drawpoint(m,i,RED);
        for(n=i-10;n<i+10;n++)
          ips200_drawpoint(j,n,RED);
      }
    }
  }
  ips200_showfloat(0,16,flag_red,6,3);
}


uint8 flag_beacon=1,flag_row=0;//判断是否找到信标灯
void Find_Beacon(void){
//  Connect_domains();
  pre_x=mid_x;
  pre_y=mid_y;
  Find_CenterPoint();

//  if(bin_img[pre_x][pre_y]==black&&flag_beacon==1){
//    DirG_Output= -1200;
//    flag_beacon=0;
//  }
}

//一直怼着信标灯/停住出现卡死状态
uint8 stick(void){
  uint8 bias; 
  bias=mid_x-pre_x;
  if(bias==0){
    zero++;
  }else{
    zero=0;
  }
  if(zero>=20){//(zero>=3&&flag_beacon==0)//有20个重复的画面并且未找到信标灯
    zero=0;
    return 1;//卡死了
  }else{
    return 0;//没卡死
  }
}
//找中心点
void Find_CenterPoint(void){
  uint8 i,j;
  uint8 point_count=0;
  uint8 x,y,row_current=0;
  uint8 left,right,top,bottom;
  uint8 n=0;
  uint16 sum_x=0,sum_y=0;
  uint8 x_pre=0,y_pre=0;
  uint8 flag=0;//没有找到第一个白点
  
  for(i=0;i<row;i++){
    for(j=0;j<col;j++){
      if(row_current!=i){
        point_count=0;
        row_current=i;
        x_pre=y_pre=0;
        flag=0;
      }
      if(bin_img[i][j]==white){
        point_count++;
      }else{
        if(point_count>=1&&flag==0){
          x = j-1-point_count/2;
          y = i;
          
          while(bin_img[y][x-left]==white){
            left++;
          }
          while(bin_img[y][x+right]==white){
            right++;
          }
          while(bin_img[y-top][x]==white){
            top++;
          }
          while(bin_img[y+bottom][x]==white){
            bottom++;
          }
          if((bottom-top)<2&&(top-bottom)<2&&(left-right)<2&&(right-left)<2){
            
//              point[n][0]=x;
//              point[n++][1]=y;
//              ips200_drawpoint(x,y,RED);
//              ips200_drawpoint(x+1,y,RED);
//              ips200_drawpoint(x-1,y,RED);
//              ips200_drawpoint(x,y-1,RED);
//              ips200_drawpoint(x,y+1,RED);
            
//            if(right-left>=18)flag_row=1;
//            sum_x += x;
//            sum_y += y;
//            n++;
//            flag_beacon=1;
            
            if(x_pre==0&&y_pre==0){
              if(right-left>=18)flag_row=1;
              x_pre=x;
              y_pre=y;
              sum_x += x;
              sum_y += y;
              n++;
            }else if(x-x_pre<=1){
              if(right-left>=18)flag_row=1;
              sum_x += x;
              sum_y += y;
              n++;
              x_pre=x;
              y_pre=y;
            }else{
              flag=1;
            }
          }     
        }
        point_count=0;
        left=right=top=bottom=0;
      }
      if(j==col-1&&bin_img[i][col-1]==white){
          x = j-1-point_count/2;
          y = i;
          
          while(bin_img[y][x-left]==white){
            left++;
          }
          while(bin_img[y][x+right]==white){
            right++;
          }
          while(bin_img[y-top][x]==white){
            top++;
          }
          while(bin_img[y+bottom][x]==white){
            bottom++;
          }
          if((bottom-top)<2&&(top-bottom)<2&&(left-right)<2&&(right-left)<2){
            
//              point[n][0]=x;
//              point[n++][1]=y;
//              ips200_drawpoint(x,y,RED);
//              ips200_drawpoint(x+1,y,RED);
//              ips200_drawpoint(x-1,y,RED);
//              ips200_drawpoint(x,y-1,RED);
//              ips200_drawpoint(x,y+1,RED);
            
//            if(right-left>=18)flag_row=1;
//            sum_x += x;
//            sum_y += y;
//            n++;
//            flag_beacon=1;
            
            if(x_pre==0&&y_pre==0){
              if(right-left>=18)flag_row=1;
              x_pre=x;
              y_pre=y;
              sum_x += x;
              sum_y += y;
              n++;
            }else if(x-x_pre<=1){
              if(right-left>=18)flag_row=1;
              sum_x += x;
              sum_y += y;
              n++;
              x_pre=x;
              y_pre=y;
            }else{
              flag=1;
            }
            
            point_count=0;
            left=right=top=bottom=0;
          }     
        }
    }
    
  }
  
  mid_x = sum_x/n;
  mid_y = sum_y/n;
  ips200_drawpoint(mid_x,mid_y,RED);
  ips200_drawpoint(mid_x+1,mid_y,RED);
  ips200_drawpoint(mid_x-1,mid_y,RED);
  ips200_drawpoint(mid_x-1,mid_y-1,RED);
  ips200_drawpoint(mid_x,mid_y-1,RED);
  ips200_drawpoint(mid_x+1,mid_y-1,RED);
  ips200_drawpoint(mid_x-1,mid_y+1,RED);
  ips200_drawpoint(mid_x,mid_y+1,RED);
  ips200_drawpoint(mid_x+1,mid_y+1,RED);
  
  if(mid_x==0){
    count_beacon++;
    if(count_beacon>=3){
      flag_beacon=0;//没找到灯
    }else{
      mid_x=pre_x;
      mid_y=pre_y;
    }
  }
  else{
      count_beacon=0;
      flag_beacon=1;//找到信标灯
//      mid_x=mid_x;
//      mid_y=mid_y;
  }
  
  if(mid_x<=110){
    mid_x=mid_x-20;
    if(mid_x<=0)mid_x=0;
  }else{
    mid_x=mid_x+20;
  }
  
  if(flag_row){
    mid_y=mid_y-5;
    flag_row=0;
  }
}


//大津算法
//uint8 OTSU(void)
//{
//  uint16 i,j;
//  uint8 img_row=row,img_col=col; 
//  uint16 histogram[GrayScale]; //灰度直方图
//  
//  //第一步：初始化灰度直方图
//  for(i=0; i<GrayScale; i++)
//    histogram[i]=0; 
//  
//  //第二步：统计每个灰度值出现得次数
//  for(i=0; i<img_row; i++)
//    for(j=0; j<img_col; j++)
//      ++histogram[mt9v03x_image[i][j]];
//  
//  
//  //第三步： 获取最小最大灰度值
//  uint16 minGray,maxGray;
//  for(minGray=0;minGray<256&&histogram[minGray]==0;minGray++);
//  for(maxGray=255;maxGray>minGray&&histogram[maxGray]==0;maxGray--);
//  
//  //第四步：分情况讨论
//  // 最大=最小，图像只有一种颜色
//  if(maxGray==minGray)
//    return maxGray;
//  //最大=最小+1，图像只有两种颜色
//  if(maxGray==minGray+1)
//    return minGray;
//  
//  //第五步：统计[最小,最大]范围内的像素总数
//  uint16 PixelSum=0;
//  for(i=minGray; i<maxGray+1; i++)
//    PixelSum += histogram[i];
//  
//  //第六步：统计[最小,最大]范围内的灰度值总数(灰度值*出现次数)
//  uint16 GraySum=0;
//  for(i=minGray; i<maxGray+1; i++)
//    GraySum += histogram[i]*i;
//    
//
//  //第七步： 大津法优化
//  float w0; //前景像素点占整幅图像的百分比 
//  float w1; //背景像素点占整幅图像的百分比 
//  uint16 w0num; //前景像素点数 
//  uint16 w1num; // 背景像素点数 
//  uint16 u0gray; //前景灰度值 
//  uint16 u1gray; //背景灰度值 
//  float u0; //w0平均灰度
//  float u1; //w1平均灰度
//  float deltaTmp=0,deltaMax=-1;
//  uint8 th;
//  
//  u0gray = u1gray = w0num = w1num = 0;
//  for(i=minGray; i<maxGray+1; i++)
//  {
//    w0num += histogram[i];
//    w1num = PixelSum-w0num;
//    w0 = w0num*1.0/PixelSum;
//    w1 = w1num*1.0/PixelSum;
//    u0gray += histogram[i]*i;
//    u1gray = GraySum-u0gray;
//    u0 = u0gray*1.0/w0num;
//    u1 = u1gray*1.0/w1num;
//    deltaTmp = (float)(w0 * w1 * (u0 - u1)*(u0 - u1)); 
//    //类间方差公式 g = w0 * w1 * (u0 - u1) ^ 2
//    
//    //第八步：遍历最大类间方差（因为呈正态分布） 
//    if(deltaTmp>deltaMax)
//    {
//      deltaMax = deltaTmp;
//      th = i;
//    }
//  }
//  return th; 
//  
//}
//
////连通域
//void Connect_domains(void){
//  uint16 i,j;
//  uint8 s,e,r,l;
//  searchArea();
//  markArea();
//  
//  for(i=0;i<num_area;i++){
//    r=area_row[i];
//    s=area_start[i];
//    e=area_end[i];
//    l=area_label[i];
//    for(j=s;j<=e;j++){
//      domains[r][j]=l;
////      ips200_drawpoint(j,r,RED);
////      ips200_drawpoint(j+1,r,RED);
//    }		
//  }
//}
//
//void searchArea(void){
//  uint8 i,j;
//  uint16 ns=0,ne=0,nr=0;
//  num_area = 0;
//  
//  for(i=0;i<row;i++){
//    if(bin_img[i][0]==white){
//      num_area++;
//      area_start[ns++]=0;
//    }
//    for(j=1;j<col;j++){
//      if(bin_img[i][j-1]==black&&bin_img[i][j]==white){
//        num_area++;
//        area_start[ns++]=j;
//      }else if(bin_img[i][j]==black&&bin_img[i][j-1]==white){
//        area_end[ne++]=j-1;
//        area_row[nr++]=i;
//      }
//    }
//    if(bin_img[i][col-1]==white){
//      area_end[ne++]=col-1;
//      area_row[nr++]=i;
//    }
//  }
//}
//
//void markArea(void){
//  uint8 i,j;
////  int nel=0;
//  uint8 label=1;
//  uint8 row_current=0;
//  uint8 index_currentFirstArea=0;
//  uint8 index_preFirstArea=0;
//  uint8 index_preLastArea=0;
//  
//  for(i=0;i<num_area;i++){
//    area_label[i]=0;
//  }
//  
//  for(i=0;i<num_area;i++){
//    //轮到下一行时，变量更新 
//    if(row_current!=area_row[i]){
//      row_current = area_row[i];
//      index_preFirstArea = index_currentFirstArea;
//      index_preLastArea = i-1;
//      index_currentFirstArea = i;
//    }
//    
//    if(row_current!=area_row[index_preFirstArea]+1){
//      //相邻行不存在子区域 
//      area_label[i]=label++;
//    }else{
//      //当前行与上一行进行比较 
//      for(j=index_preFirstArea;j<=index_preLastArea;j++){
//        if(area_start[i]<=area_end[j]+offset&&area_end[i]>=area_start[j]-offset){
//          //相连的情况 
//          if(area_label[i]==0){
//            //没有标记情况 
//            area_label[i]=area_label[j];
//          }
////          else if(area_label[i]!=area_label[j]){
////            //已经标记过，保存等价对 
////            equalLabels[nel][0]=area_label[i];
////            equalLabels[nel++][1]=area_label[j];
////          }
//        }else if(area_end[i]<area_start[j]-offset){
//          //不相连的情况 
//          if(index_preFirstArea<j-1)
//            index_preFirstArea=j-1;
//          j=index_preLastArea;
//        }
//      }
//    }
//    //这一行循环完与上一行不存在联系 
//    if(area_label[i]==0){
//      area_label[i] = ++label;
//    }
//  }	
//}


#ifndef _imgDeal_H_
#define _imgDeal_H_

#include "headfile.h"
#include<string.h>
#include<math.h>

// **************************** 宏定 义 **************************** 

#define col MT9V03X_W //120 //MT9V03X_W 宽 
#define row   MT9V03X_H//80 //MT9V03X_H 高 

#define white 255
#define black 0
#define GrayScale 256

#define maxnum 1000
#define offset 0

// **************************** 变量定义 ****************************

extern uint8 threshold;
extern uint8 mid_x,mid_y;
extern uint8 pre_x,pre_y;
extern uint8 zero,count_beacon;
extern uint8 bin_img[row][col]; 

extern uint8 area_start[maxnum];
extern uint8 area_end[maxnum];
extern uint8 area_row[maxnum];
extern uint8 num_area;           
extern uint8 area_label[maxnum];
extern uint8 point[col+20][2];
extern uint8 flag_beacon,flag_row;
//extern uint8 stick_flag;
//extern uint8 equalLabels[maxnum][2];

// **************************** 函数定义 ****************************
extern void camera(void);
extern void Find_Beacon(void);
extern void Binarization(uint8 threshold);
extern uint8 OTSU(void);
extern void searchArea(void);
extern void markArea(void);
extern void Connect_domains(void);
extern void Find_CenterPoint(void);
extern uint8 stick(void);
#endif
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
#include "headfile.h"
#include<string.h>
#include<math.h>

#define col  MT9V03X_W //宽 120
#define row  MT9V03X_H //高 80

#define white 255
#define black 0
#define GrayScale 256

#define maxnum 1000
#define offset 0

uint8 mid_x,mid_y;
uint8 pre_x,pre_y;
uint8 zero=0;
uint8 bias;
uint8 stick_flag=0;//判断车是否被卡死
uint8 count_beacon=0;
uint8 flag_beacon=0;
uint8 begin_beacon=0;
uint8 automatic_start=0;
uint8 flag1=0;
uint8 threshold;//=70;//灰20-70；白70-126 -127-0
uint8 bin_img[row][col]; //二值化图 
uint8 domains[row][col]; //连通域图

uint8 area_start[maxnum];
uint8 area_end[maxnum];
uint8 area_row[maxnum];
uint8 num_area;   
uint8 area_label[maxnum];
uint8 point[col+20][2];
//int equalLabels[maxnum][2];

//可以不加二值化浪费时间，直接通过阈值来判断
//只改变与偏差有关的，与中线有关的间接有关的都不改变
//滤波等，思考……效率问题
//#define RED					0xF800													// 红色
//#define BLUE				0x001F													// 蓝色
//#define YELLOW				0xFFE0													// 黄色
//#define GREEN				0x07E0													// 绿色
//#define WHITE				0xFFFF													// 白色
//#define BLACK				0x0000													// 黑色 
//#define GRAY				0X8430													// 灰色
//#define BROWN				0XBC40													// 棕色
//#define PURPLE				0XF81F													// 紫色
//#define PINK				0XFE19	

//main函数
void camera(void)
{    
  if(mt9v03x_finish_flag)
  {
    mt9v03x_finish_flag=0;
    
    stick_flag = stick();
    threshold=70;
//    ips200_showfloat(0,15,threshold,6,3);
//    if(threshold<70)threshold=70;
    Binarization(threshold);
    ips200_displayimage032(bin_img[0],col, row);
    Find_Beacon(); 
  }
}

void Find_Beacon(void){
//  Connect_domains();
  pre_x=mid_x;
  pre_y=mid_y;
  Find_CenterPoint();
//  if(mid_x==0&&mid_y==0){
//    mid_x=pre_x;
//    mid_y=pre_y;
//  }
}

//一直怼着信标灯,出现卡死状态
uint8 stick(void){
  bias = mid_y-pre_y;
  if(bias==0){
    zero++;
  }else{
    zero=0;
  }
  if(zero>=3&&flag1==1){
    zero=0;
    return 1;
  }else{
    return 0;
  }
}
//找中心点
void Find_CenterPoint(void){
  uint8 i,j;
  uint8 point_count=0;
  uint8 x,y,row_current=0;
  uint8 left,right,top,bottom;
  uint8 n=0;
  uint16 sum_x=0,sum_y=0;
  
  for(i=0;i<row;i++){
    for(j=0;j<col;j++){
      if(row_current!=i){
        point_count=0;
        row_current=i;
      }
      if(bin_img[i][j]==white){
        point_count++;
      }else{
        if(point_count>=2){
          x = j-1-point_count/2;
          y = i;
          
          while(bin_img[y][x-left]==white){
            left++;
          }
          while(bin_img[y][x+right]==white){
            right++;
          }
          while(bin_img[y-top][x]==white){
            top++;
          }
          while(bin_img[y+bottom][x]==white){
            bottom++;
          }
          if((bottom-top)<2&&(top-bottom)<2&&(left-right)<2&&(right-left)<2){
            
//              point[n][0]=x;
//              point[n++][1]=y;
//              ips200_drawpoint(x,y,RED);
//              ips200_drawpoint(x+1,y,RED);
//              ips200_drawpoint(x-1,y,RED);
//              ips200_drawpoint(x,y-1,RED);
//              ips200_drawpoint(x,y+1,RED);
            sum_x += x;
            sum_y += y;
            n++;
            flag1=1;
            
          }
        }
        point_count=0;
        left=right=top=bottom=0;
      }
      
    }
  }
  mid_x = sum_x/n;//信标灯纵坐标
  mid_y = sum_y/n;//信标灯横坐标
  ips200_drawpoint(mid_x,mid_y,RED);
  ips200_drawpoint(mid_x+1,mid_y,RED);
  ips200_drawpoint(mid_x-1,mid_y,RED);
  ips200_drawpoint(mid_x,mid_y-1,RED);
  ips200_drawpoint(mid_x,mid_y+1,RED);
  
  if(mid_x==0){
    count_beacon++;
    if(count_beacon>=3){
      flag_beacon=0;//没找到信标灯
      mid_x=0;
      mid_y=0;
    }else{
      begin_beacon=1;//信标灯打开
      count_beacon=0;
      flag_beacon=1;//找到信标灯
      automatic_start=1;//自动发车标志位
    }
}

//图像二值化 
void Binarization(uint8 threshold)
{
  uint8 i,j;
  
  for(i=0; i<row; i++)
  { 
    for(j=0; j<col; j++)
    {
      domains[i][j]=0;
      if(mt9v03x_image[i][j]>threshold)
        bin_img[i][j]=white;
      else
        bin_img[i][j]=black;
    }
  }
}

//大津算法
uint8 OTSU(void)
{
  uint16 i,j;
  uint8 img_row=row,img_col=col; 
  uint16 histogram[GrayScale]; //灰度直方图
  
  //第一步：初始化灰度直方图
  for(i=0; i<GrayScale; i++)
    histogram[i]=0; 
  
  //第二步：统计每个灰度值出现得次数
  for(i=0; i<img_row; i++)
    for(j=0; j<img_col; j++)
      ++histogram[mt9v03x_image[i][j]];
  
  
  //第三步： 获取最小最大灰度值
  uint16 minGray,maxGray;
  for(minGray=0;minGray<256&&histogram[minGray]==0;minGray++);
  for(maxGray=255;maxGray>minGray&&histogram[maxGray]==0;maxGray--);
  
  //第四步：分情况讨论
  // 最大=最小，图像只有一种颜色
  if(maxGray==minGray)
    return maxGray;
  //最大=最小+1，图像只有两种颜色
  if(maxGray==minGray+1)
    return minGray;
  
  //第五步：统计[最小,最大]范围内的像素总数
  uint16 PixelSum=0;
  for(i=minGray; i<maxGray+1; i++)
    PixelSum += histogram[i];
  
  //第六步：统计[最小,最大]范围内的灰度值总数(灰度值*出现次数)
  uint16 GraySum=0;
  for(i=minGray; i<maxGray+1; i++)
    GraySum += histogram[i]*i;
    

  //第七步： 大津法优化
  float w0; //前景像素点占整幅图像的百分比 
  float w1; //背景像素点占整幅图像的百分比 
  uint16 w0num; //前景像素点数 
  uint16 w1num; // 背景像素点数 
  uint16 u0gray; //前景灰度值 
  uint16 u1gray; //背景灰度值 
  float u0; //w0平均灰度
  float u1; //w1平均灰度
  float deltaTmp=0,deltaMax=-1;
  uint8 th;
  
  u0gray = u1gray = w0num = w1num = 0;
  for(i=minGray; i<maxGray+1; i++)
  {
    w0num += histogram[i];
    w1num = PixelSum-w0num;
    w0 = w0num*1.0/PixelSum;
    w1 = w1num*1.0/PixelSum;
    u0gray += histogram[i]*i;
    u1gray = GraySum-u0gray;
    u0 = u0gray*1.0/w0num;
    u1 = u1gray*1.0/w1num;
    deltaTmp = (float)(w0 * w1 * (u0 - u1)*(u0 - u1)); 
    //类间方差公式 g = w0 * w1 * (u0 - u1) ^ 2
    
    //第八步：遍历最大类间方差（因为呈正态分布） 
    if(deltaTmp>deltaMax)
    {
      deltaMax = deltaTmp;
      th = i;
    }
  }
  return th; 
  
}

//连通域
void Connect_domains(void){
  uint16 i,j;
  uint8 s,e,r,l;
  searchArea();
  markArea();
  
  for(i=0;i<num_area;i++){
    r=area_row[i];
    s=area_start[i];
    e=area_end[i];
    l=area_label[i];
    for(j=s;j<=e;j++){
      domains[r][j]=l;
//      ips200_drawpoint(j,r,RED);
//      ips200_drawpoint(j+1,r,RED);
    }		
  }
}

void searchArea(void){
  uint8 i,j;
  uint16 ns=0,ne=0,nr=0;
  num_area = 0;
  
  for(i=0;i<row;i++){
    if(bin_img[i][0]==white){
      num_area++;
      area_start[ns++]=0;
    }
    for(j=1;j<col;j++){
      if(bin_img[i][j-1]==black&&bin_img[i][j]==white){
        num_area++;
        area_start[ns++]=j;
      }else if(bin_img[i][j]==black&&bin_img[i][j-1]==white){
        area_end[ne++]=j-1;
        area_row[nr++]=i;
      }
    }
    if(bin_img[i][col-1]==white){
      area_end[ne++]=col-1;
      area_row[nr++]=i;
    }
  }
}

void markArea(void){
  uint8 i,j;
//  int nel=0;
  uint8 label=1;
  uint8 row_current=0;
  uint8 index_currentFirstArea=0;
  uint8 index_preFirstArea=0;
  uint8 index_preLastArea=0;
  
  for(i=0;i<num_area;i++){
    area_label[i]=0;
  }
  
  for(i=0;i<num_area;i++){
    //轮到下一行时，变量更新 
    if(row_current!=area_row[i]){
      row_current = area_row[i];
      index_preFirstArea = index_currentFirstArea;
      index_preLastArea = i-1;
      index_currentFirstArea = i;
    }
    
    if(row_current!=area_row[index_preFirstArea]+1){
      //相邻行不存在子区域 
      area_label[i]=label++;
    }else{
      //当前行与上一行进行比较 
      for(j=index_preFirstArea;j<=index_preLastArea;j++){
        if(area_start[i]<=area_end[j]+offset&&area_end[i]>=area_start[j]-offset){
          //相连的情况 
          if(area_label[i]==0){
            //没有标记情况 
            area_label[i]=area_label[j];
          }
//          else if(area_label[i]!=area_label[j]){
//            //已经标记过，保存等价对 
//            equalLabels[nel][0]=area_label[i];
//            equalLabels[nel++][1]=area_label[j];
//          }
        }else if(area_end[i]<area_start[j]-offset){
          //不相连的情况 
          if(index_preFirstArea<j-1)
            index_preFirstArea=j-1;
          j=index_preLastArea;
        }
      }
    }
    //这一行循环完与上一行不存在联系 
    if(area_label[i]==0){
      area_label[i] = ++label;
    }
  }	
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
2022/7/4
    
#include "headfile.h"
#include<string.h>
#include<math.h>

#define col  MT9V03X_W //宽 120
#define row  MT9V03X_H //高 80
#define white 255
#define black 0
#define GrayScale 256

int mid_x,mid_y;
int threshold;//=70;//灰20-70；白70-126 -127-0
uint8 bin_img[row][col]; //二值化图 

//可以不加二值化浪费时间，直接通过阈值来判断
//只改变与偏差有关的，与中线有关的间接有关的都不改变
//滤波等，思考……效率问题
//#define RED					0xF800													// 红色
//#define BLUE				0x001F													// 蓝色
//#define YELLOW				0xFFE0													// 黄色
//#define GREEN				0x07E0													// 绿色
//#define WHITE				0xFFFF													// 白色
//#define BLACK				0x0000													// 黑色 
//#define GRAY				0X8430													// 灰色
//#define BROWN				0XBC40													// 棕色
//#define PURPLE				0XF81F													// 紫色
//#define PINK				0XFE19	

//main函数
void camera(void)
{    
  if(mt9v03x_finish_flag)
  {
//    threshold=OTSU();
//    ips200_showfloat(0,15,threshold,6,3);
//    if(threshold<90)threshold=90;
//    threshold=255;
    //加一个：黑白天阈值限定
//    Binarization(threshold);
//    ips200_displayimage032(bin_img[0],col, row);
//    Find_Beacon();
//    ips200_showfloat(0,15,mt9v03x_image[60][100],6,3);
//    ips200_displayimage032(mt9v03x_image[0],col, row);
//    ips200_drawpoint(100,60,RED);
//    Scan_Point1();
  }
}

void Find_Beacon(void)
{
	int i,j;
    int n_row=0,n_col=0,sum_row=0,sum_col=0;//记录白块的行和列的个数，行或列的总和
    int n1,n2;
	int point_row[col];//记录每行白点所在列 
	int point_col[row][5];//记录每行所有连续白点的中心线 
    int point_temp[row][2];//记下每行产生连续白点行数和产生的连续白点数 
    
    for(i=0;i<row;i++)
    {
        point_temp[i][0] = -1;
        point_temp[i][1] = 0;
        point_col[i][0] = -1;
        point_col[i][1] = -1;
        point_col[i][2] = -1;
        point_col[i][3] = -1;
        point_col[i][4] = -1;
    }
	
	for(i=0;i<row;i++)
	{
		n_row = 0,sum_row=0;
//		memset(point_row,-1,sizeof(point_row));
		for(j=0;j<col;j++)
		{
			if((mt9v03x_image[i][j-1]>threshold)&&(mt9v03x_image[i][j]>threshold)&&(mt9v03x_image[i][j+1]>threshold))
			{
				point_row[n_row++] = j;//记下连续白点的列	
				point_temp[i][0] = i;  //记下存在白点的行  
//                                ips200_drawpoint(j,i,RED);
			}			
		}
		
		if(n_row!=0){//可以使用120个点的如果点与点间又3个点的间隔就算另一个图
			point_temp[i][1] = 1;
			n1 = 0,n2=0; //记录第几个白块(存列数),记录有多少个连续的白点 
			for(j=0;j<n_row;j++)
			{
				sum_row += point_row[j];
				n2++;
                if(point_row[j+1]-point_row[j]>2) 
                {
                	point_temp[i][1]++;
                	point_col[i][n1++] = sum_row/n2;
                	sum_row = 0;
                	n2 = 0;
				}   				      
			}		
            n_col++;
            ips200_drawpoint(point_col[i][0],i,BLUE);
            ips200_drawpoint(point_col[i][0],i+1,BLUE);
            ips200_drawpoint(point_col[i][0],i-1,BLUE);
		}
	}
	
        
	if(n_col==0&&n_row==0){//找不到点 
		mid_x = col/2;
		mid_y = row/2;
	}else{ 
        n_row = 0,n_col=0;	
        sum_row=0,sum_col=0;
    
        for(i=1;i<row;i++)
        {
        	if(point_temp[i][1]==1)
        	{
        		sum_col += point_col[i][0];
        		sum_row += point_temp[i][0];
        		n_row++;
        		if(point_col[i+1][0]==-1&&point_col[i-1][0]==-1&&fabs(point_col[i+1]-point_col[i])>2&&fabs(point_col[i-1]-point_col[i])>2)
        		{
        			mid_x = sum_col/n_row;
        			mid_y = sum_row/n_row;
        			sum_row = 0;
        			sum_col = 0;
        			n_row = 0;
				}
			}
        }
	} 
//        ips200_showfloat(0,13,n_row,4,3);
//        ips200_showfloat(0,14,sum_col,4,3);
//        ips200_showfloat(0,15,sum_row,4,3);
        ips200_showfloat(0,16,mid_x,4,3);
        ips200_showfloat(0,17,mid_y,4,3);
        
    ips200_drawpoint(mid_y,mid_x,RED);//列，行
    ips200_drawpoint(mid_y,mid_x+1,RED);
    ips200_drawpoint(mid_y+1,mid_x,RED);
    ips200_drawpoint(mid_y+1,mid_x+1,RED);
}

//图像二值化 
void Binarization(int threshold)
{
  int i,j;
  
  for(i=0; i<row; i++)
  { 
    for(j=0; j<col; j++)
    {
      if(mt9v03x_image[i][j]>threshold)
        bin_img[i][j]=white;
      else
        bin_img[i][j]=black;
    }
  }
}

//大津算法
int OTSU(void)
{
  int i,j;
  int img_row=row,img_col=col; 
  int histogram[GrayScale]; //灰度直方图
  
  //第一步：初始化灰度直方图
  for(i=0; i<GrayScale; i++)
    histogram[i]=0; 
  
  //第二步：统计每个灰度值出现得次数
  for(i=0; i<img_row; i++)
    for(j=0; j<img_col; j++)
      ++histogram[mt9v03x_image[i][j]];
  
  
  //第三步： 获取最小最大灰度值
  int minGray,maxGray;
  for(minGray=0;minGray<256&&histogram[minGray]==0;minGray++);
  for(maxGray=255;maxGray>minGray&&histogram[maxGray]==0;maxGray--);
  
  //第四步：分情况讨论
  // 最大=最小，图像只有一种颜色
  if(maxGray==minGray)
    return maxGray;
  //最大=最小+1，图像只有两种颜色
  if(maxGray==minGray+1)
    return minGray;
  
  //第五步：统计[最小,最大]范围内的像素总数
  int PixelSum=0;
  for(i=minGray; i<maxGray+1; i++)
    PixelSum += histogram[i];
  
  //第六步：统计[最小,最大]范围内的灰度值总数(灰度值*出现次数)
  int GraySum=0;
  for(i=minGray; i<maxGray+1; i++)
    GraySum += histogram[i]*i;
    

  //第七步： 大津法优化
  double w0; //前景像素点占整幅图像的百分比 
  double w1; //背景像素点占整幅图像的百分比 
  int w0num; //前景像素点数 
  int w1num; // 背景像素点数 
  int u0gray; //前景灰度值 
  int u1gray; //背景灰度值 
  double u0; //w0平均灰度
  double u1; //w1平均灰度
  double deltaTmp=0,deltaMax=-1;
  int th;
  
  u0gray = u1gray = w0num = w1num = 0;
  for(i=minGray; i<maxGray+1; i++)
  {
    w0num += histogram[i];
    w1num = PixelSum-w0num;
    w0 = w0num*1.0/PixelSum;
    w1 = w1num*1.0/PixelSum;
    u0gray += histogram[i]*i;
    u1gray = GraySum-u0gray;
    u0 = u0gray*1.0/w0num;
    u1 = u1gray*1.0/w1num;
    deltaTmp = (float)(w0 * w1 * (u0 - u1)*(u0 - u1)); 
    //类间方差公式 g = w0 * w1 * (u0 - u1) ^ 2
    
    //第八步：遍历最大类间方差（因为呈正态分布） 
    if(deltaTmp>deltaMax)
    {
      deltaMax = deltaTmp;
      th = i;
    }
  }
  return th; 
  
}

void Binarization(int threshold)
{
  int i,j;
  
  for(i=0; i<row; ++i)
  { 
    for(j=0; j<col; ++j)
    {
      if(mt9v03x_image[i][j]>threshold)
        bin_img[i][j]=white;
      else
        bin_img[i][j]=black;
    }
  }
  mt9v03x_finish_flag = 0;
}

void Find_Beacon(void)
{
	int i,j;
    int n_row=0,n_col=0,sum_row=0,sum_col=0;//记录白块的行和列的个数，行或列的总和
    int n1,n2;
	int point_row[col];//记录每行白点所在列 
	int point_col[row][5];//记录每行所有连续白点的中心线 
    int point_temp[row][2];//记下每行产生连续白点行数和产生的连续白点数 
    
    for(i=0;i<row;i++)
    {
        point_temp[i][0] = -1;
        point_temp[i][1] = 0;
        point_col[i][0] = -1;
        point_col[i][1] = -1;
        point_col[i][2] = -1;
        point_col[i][3] = -1;
        point_col[i][4] = -1;
    }
	
	for(i=0;i<row;i++)
	{
		n_row = 0,sum_row=0;
//		memset(point_row,-1,sizeof(point_row));
		for(j=0;j<col;j++)
		{
			if((mt9v03x_image[i][j-1]>threshold)&&(mt9v03x_image[i][j]>threshold)&&(mt9v03x_image[i][j+1]>threshold))
			{
				point_row[n_row++] = j;//记下连续白点的列	
				point_temp[i][0] = i;  //记下存在白点的行  
//                                ips200_drawpoint(j,i,RED);
			}			
		}
		
		if(n_row!=0){//可以使用120个点的如果点与点间又3个点的间隔就算另一个图
			point_temp[i][1] = 1;
			n1 = 0,n2=0; //记录第几个白块(存列数),记录有多少个连续的白点 
			for(j=0;j<n_row;j++)
			{
				sum_row += point_row[j];
				n2++;
                if(point_row[j+1]-point_row[j]>2) 
                {
                	point_temp[i][1]++;
                	point_col[i][n1++] = sum_row/n2;
                	sum_row = 0;
                	n2 = 0;
				}   				      
			}		
            n_col++;
            ips200_drawpoint(point_col[i][0],i,BLUE);
            ips200_drawpoint(point_col[i][0],i+1,BLUE);
            ips200_drawpoint(point_col[i][0],i-1,BLUE);
		}
	}
	
        
	if(n_col==0&&n_row==0){//找不到点 
		mid_x = col/2;
		mid_y = row/2;
	}else{ 
        n_row = 0,n_col=0;	
        sum_row=0,sum_col=0;
    
        for(i=1;i<row;i++)
        {
        	if(point_temp[i][1]==1)
        	{
        		sum_col += point_col[i][0];
        		sum_row += point_temp[i][0];
        		n_row++;
        		if(point_col[i+1][0]==-1&&point_col[i-1][0]==-1&&fabs(point_col[i+1]-point_col[i])>2&&fabs(point_col[i-1]-point_col[i])>2)
        		{
        			mid_x = sum_col/n_row;
        			mid_y = sum_row/n_row;
        			sum_row = 0;
        			sum_col = 0;
        			n_row = 0;
				}
			}
        }
	} 
//        ips200_showfloat(0,13,n_row,4,3);
//        ips200_showfloat(0,14,sum_col,4,3);
//        ips200_showfloat(0,15,sum_row,4,3);
//        ips200_showfloat(0,16,mid_x,4,3);
//        ips200_showfloat(0,17,mid_y,4,3);
        
    ips200_drawpoint(mid_y,mid_x,RED);//列，行
    ips200_drawpoint(mid_y,mid_x+1,RED);
    ips200_drawpoint(mid_y+1,mid_x,RED);
    ips200_drawpoint(mid_y+1,mid_x+1,RED);
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
2022/7/2
    
void Find_Beacon(void)
{
	int i,j;
        int n_row=0,n_col=0,sum_row=0,sum_col=0;//记录白块的行和列的个数，行或列的总和
        int n_
	int point_row[col];//记录每行点的状态（黑/白)
	int point_col[row][5];//记录每行有几个白块
        int point_temp[row][2];//记录每行是否有白块，并记下行数和产生的白块数
        
        for(i=0;i<row;i++)
        {
          point_temp[i][0] = -1;
          point_temp[i][1] = 0;
        }
	
	for(i=0;i<row;i++)
	{
		n_row = 0,sum_row=0;
//		memset(point_row,-1,sizeof(point_row));
		for(j=0;j<col;j++)
		{
			if((mt9v03x_image[i][j-1]>threshold)&&(mt9v03x_image[i][j]>threshold)&&(mt9v03x_image[i][j+1]>threshold))
			{
				point_row[n_row++] = j;//记下连续白点的列	
				point_temp[i][0] = i;  //记下存在白点的行
                              
//                                ips200_drawpoint(j,i,RED);
			}			
		}
		
		if(n_row!=0){//可以使用120个点的如果点与点间又3个点的间隔就算另一个图
			for(j=1;j<n_row;j++)
			{
                          if(point_row[j]-point_row[j-1]>2)
                            
				
			}		
			point_col[i] = sum_row/n_row;
                          n_col++;
                        ips200_drawpoint(point_col[i],i,BLUE);
                        ips200_drawpoint(point_col[i],i+1,BLUE);
                        ips200_drawpoint(point_col[i],i-1,BLUE);
		}
	}
	
        
	if(n_col==0&&n_row==0){
		//找不到点 
		mid_x = col/2;
		mid_y = row/2;
	}else{
          
                n_row = 0,n_col=0;	
                sum_row=0,sum_col=0;
                for(i=1;i<row-1;i++)
                {
                      if(point_temp[i]!=0&&point_temp[i+1]!=0&&point_temp[i-1]!=0)
                      {   
                            sum_row += point_temp[i];             
                            n_row++;
                            sum_col += point_col[i];
                      }
                }
		mid_x = sum_col/n_row;
		mid_y = sum_row/n_row;
	} 
//        ips200_showfloat(0,13,n_row,4,3);
//        ips200_showfloat(0,14,sum_col,4,3);
//        ips200_showfloat(0,15,sum_row,4,3);
//        ips200_showfloat(0,16,mid_x,4,3);
//        ips200_showfloat(0,17,mid_y,4,3);
        
        ips200_drawpoint(mid_y,mid_x,RED);//列，行
        ips200_drawpoint(mid_y,mid_x+1,RED);
        ips200_drawpoint(mid_y+1,mid_x,RED);
        ips200_drawpoint(mid_y+1,mid_x+1,RED);
} 

 if(sumhang>110)
          {
               KP1=15;
               KD1=0.1;
               KP2=15;
               KD1=0.1;
               setspeed2= 20+(sumlie-93)*2.4;
               setspeed1= 20-(sumlie-93)*2.4;


               speedave=speed1+speed2;
               if(speedave<80)               {
                 setspeed1= 200-(sumlie-93)*2.4;//1.5
                 setspeed2= 200+(sumlie-93)*2.4;//1.5;
               }

               MotorDuty1 =speedout1 - 1.8*(gyroz+9);
               MotorDuty2 =speedout2 + 1.8*(gyroz+9);
          }
          else if(sumhang>=33 &&sumhang <=110)          {


                                   KP1=29;
                                    KD1=0.1;
                                    KP2=29;
                                    KD1=0.1;
                                    setspeed2= 0+(sumlie-93)*2.4;
                                    setspeed1= 0-(sumlie-93)*2.4;

                                                 speedave=speed1+speed2;
                                                 if(speedave<80)//
                                                 {
                                                     setspeed1= 200-(sumlie-93)*2.4;//1.5
                                                     setspeed2= 200+(sumlie-93)*2.4;//1.5;
                                                  }
                                                 MotorDuty1 =speedout1 - 1.8*(gyroz+9);
                                                 MotorDuty2 =speedout2 + 1.8*(gyroz+9);
          }
          else if(sumhang>=40 &&sumhang <55)//滑行
          {

              KP1=15;
               KD1=0.1;
             KP2=15;
            KD1=0.1;
           setspeed2= 200+(sumlie-93)*2.4;
           setspeed1= 200-(sumlie-93)*2.4;
           MotorDuty1 =speedout1 - 1.8*(gyroz+9);
           MotorDuty2 =speedout2 + 1.8*(gyroz+9);
          }

          else if(sumhang<25)//加速
          {
              KP1=15;
              KD1=0.1;
              KP2=15;
              KD1=0.1;
              setspeed2= 330+(sumlie-93)*2.4+sumhang*10;//250
              setspeed1= 330-(sumlie-93)*2.4+sumhang*10;

              MotorDuty1 =speedout1 - 2.5*(gyroz+9);
              MotorDuty2 =speedout2 + 2.5*(gyroz+9);
          }
          else//冲
          {
                       KP1=15;
                       KD1=0.1;
                       KP2=15;
                       KD1=0.1;
                       setspeed2= 580+(sumlie-93)*2.4;//430//460
                       setspeed1= 580-(sumlie-93)*2.4;//430

                       MotorDuty1 =speedout1 - 3.6*(gyroz+9);
                       MotorDuty2 =speedout2 + 3.6*(gyroz+9);
          }


          if(MotorDuty1>6000) MotorDuty1 =  6000;//限幅*********需测量
          if(MotorDuty2>6000) MotorDuty2 =  6000;//限幅

      }
      else // 没有发现有白色点
      {

                    nolight=1;
          beaconFlashCnt++;
          
          if(beaconFlashCnt>5)                    {
              KP1=15;
              KD1=0.1;
              KP2=15;
              KD2=0.1;
              setspeed1= 260;
              setspeed2= 0;
              MotorDuty1 = speedout1 - 0.5*(gyroz+9);      MotorDuty2 = speedout2 + 0.5*(gyroz+9);

              //sumhang=1;
          }
       }
          if(1)
      {
           
MotorCtrl(-MotorDuty1, MotorDuty2);

      }
          else
          {
              MotorDuty1 =0;
              MotorDuty2 =0;
              MotorCtrl(-MotorDuty2 ,-MotorDuty1);
          }
    }
  }
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
2022/7/1
    
#ifndef _imgDeal_H_
#define _imgDeal_H_

#include "headfile.h"

// **************************** 宏定 义 **************************** 

#define col MT9V03X_W //120 //MT9V03X_W 宽 
#define row   MT9V03X_H//80 //MT9V03X_H 高 

#define white 255
#define black 0

// **************************** 变量定义 ****************************
extern uint8 bin_img[row][col]; 
extern uint8 gray_img[row][col]; 
extern int point[row];

extern int threshold;
extern int mid_x,mid_y;

// **************************** 函数定义 ****************************
extern void camera(void);
extern void Binarization(int threshold);
extern int OTSU(void);
extern void Scan_Point1(void);
extern void Bin_Image_Filter(void);
extern void sign_Beacon(int x,int y,int top,int bottom);
//extern void sign_Beacon(int x,int y);

#endif


#include "headfile.h"

#define col  MT9V03X_W //宽 120
#define row  MT9V03X_H //高 80
#define white 255
#define black 0
#define GrayScale 256

uint8 bin_img[row][col]; //二值化图 
uint8 gray_img[row][col]; //灰度图 

int mid_x,mid_y;
int threshold;//=70;//灰20-70；白70-126 -127-0

//可以不加二值化浪费时间，直接通过阈值来判断
//只改变与偏差有关的，与中线有关的间接有关的都不改变
//滤波等，思考……效率问题
//#define RED					0xF800													// 红色
//#define BLUE				0x001F													// 蓝色
//#define YELLOW				0xFFE0													// 黄色
//#define GREEN				0x07E0													// 绿色
//#define WHITE				0xFFFF													// 白色
//#define BLACK				0x0000													// 黑色 
//#define GRAY				0X8430													// 灰色
//#define BROWN				0XBC40													// 棕色
//#define PURPLE				0XF81F													// 紫色
//#define PINK				0XFE19	

//main函数
void camera(void)
{    
  if(mt9v03x_finish_flag)
  {
    //            threshold=OTSU();
    threshold=70;
    //加一个：黑白天阈值限定
    Binarization(threshold);
    ips200_displayimage032(bin_img[0],col, row);
    
    //            ips200_displayimage032(mt9v03x_image[0],col, row);
    Scan_Point1();
  }
}



//简单找点
void Scan_Point1(void)
{
  int i,j,k=1;
  int left,right,top,bottom;
  int sum=0;
  int point[row];
  int flag1=1,flag2=1;
  
  for(i=0;i<row;i++)
    point[i] = 0;
  
  //第一步：找到左右边界进行折半取中值
  for(i=0;i<row;i++)
  {
    left=col,right=0;
    for(j=0;j<col; )
    {
      if(bin_img[i][j]==255)
      {
        if(left==col)left=j;
        right = j+1;
        flag1=0;
        continue;
      }
      j++;
    }
    
    if(left<right)point[i]=(left+right)>>1;
    else point[i]=0xff;
    
  } 
  //第二步：找到上下边界，折半得中点坐标
  top=row,bottom=0,k=0;
  for(i=0;i<row;)
  {
    if(point[i]!=0xff)
    {
      if(top==row)top=i;
      bottom=i+1;
      k += 1;
      flag2=0;
      continue;
    }
    i++;
  }
  //第三步：判断是否存在在信标灯
  if(top<=bottom)
  {
    for(i=top;i<=bottom;i++)
    {
      if(point[i]!=0xff)sum += point[i];
    }
    mid_x = sum/k;
    mid_y = top+(bottom-top)>>1;
  }
  if(flag1==0&&flag2==0){
    ips200_drawpoint(mid_x,mid_y,RED);//列，行
    ips200_drawpoint(mid_x+1,mid_y+1,RED);//列，行
    ips200_drawpoint(mid_x,mid_y+1,RED);//列，行
    ips200_drawpoint(mid_x+1,mid_y,RED);//列，行
    sign_Beacon(mid_x,mid_y,top-10,bottom+10);
  }
  
  if(flag1==1&&flag2==1){
    mid_x=120;
  }
}



//标出信标灯的位置
void sign_Beacon(int x,int y,int top,int bottom)
{
  int i,j;
  if(top<0)top=0;
  if(bottom>120)bottom=120;
  for(i=top;i<bottom;++i)
  {
    ips200_drawpoint(x,i,RED);//列，行
    for(j=0;j<col;++j)
      ips200_drawpoint(j,y,RED);//列，行
  } 
}

//图像二值化 
void Binarization(int threshold)
{
  int i,j;
  
  for(i=0; i<row; ++i)
  { 
    for(j=0; j<col; ++j)
    {
      if(mt9v03x_image[i][j]>threshold)
        bin_img[i][j]=white;
      else
        bin_img[i][j]=black;
    }
  }
  mt9v03x_finish_flag = 0;
}


//过滤噪点
void Bin_Image_Filter(void)
{
  int i,j;
  
  for(i=1; i<row-1;++i)
  {
    for(j=1; j<col-1;++j)
    {
      if((bin_img[i][j]==0)&&(bin_img[i-1][j]+bin_img[i-1][j+1]+bin_img[i+1][j]+bin_img[i][j+1]+bin_img[i][j+2]>3))
        bin_img[i][j]=1;
      else if((bin_img[i][j]==1)&&(bin_img[i-1][j]+bin_img[i-1][j+1]+bin_img[i+1][j]+bin_img[i][j+1]+bin_img[i][j+2]<2))
        bin_img[i][j]=0;
    }
  }
}

//大津算法
int OTSU(void)
{
  int i,j;
  int img_row=row,img_col=col; 
  int histogram[GrayScale]; //灰度直方图
  
  //第一步：初始化灰度直方图
  for(i=0; i<GrayScale; ++i)
    histogram[i]=0; 
  
  //第二步：统计每个灰度值出现得次数
  for(i=0; i<img_row; ++i)
    for(j=0; j<img_col; ++j)
      ++histogram[mt9v03x_image[i][j]];
  
  
  //第三步： 获取最小最大灰度值
  int minGray,maxGray;
  for(minGray=0;minGray<256&&histogram[minGray]==0;++minGray);
  for(maxGray=255;maxGray>minGray&&histogram[minGray]==0;--maxGray);
  
  //第四步：分情况讨论
  // 最大=最小，图像只有一种颜色
  if(maxGray==minGray)
    return maxGray;
  //最大=最小+1，图像只有两种颜色
  if(maxGray==minGray+1)
    return minGray;
  
  //第五步：统计[最小,最大]范围内的像素总数
  int PixelSum=0;
  for(i=minGray; i<maxGray+1; ++i)
    PixelSum += histogram[i];
  
  //第六步：统计[最小,最大]范围内的灰度值总数(灰度值*出现次数)
  int GraySum=0;
  for(i=minGray; i<maxGray+1; ++i)
    GraySum += histogram[i]*i;
  
  //第七步： 大津法优化
  double w0; //前景像素点占整幅图像的百分比 
  double w1; //背景像素点占整幅图像的百分比 
  int w0num; //前景像素点数 
  int w1num; // 背景像素点数 
  int u0gray; //前景灰度值 
  int u1gray; //背景灰度值 
  double u0; //w0平均灰度
  double u1; //w1平均灰度
  double deltaTmp=0,deltaMax=-1;
  int th;
  
  u0gray = u1gray = w0num = w1num = 0;
  for(i=minGray; i<maxGray; i++)
  {
    w0num += histogram[i];
    w1num = PixelSum-w0num;
    w0 = w0num*1.0/PixelSum;
    w1 = w1num*1.0/PixelSum;
    u0gray += histogram[i]*i;
    u1gray = GraySum-u0gray;
    u0 = u0gray*1.0/w0num;
    u1 = u1gray*1.0/w1num;
    deltaTmp = (float)(w0 * w1 * (u0 - u1)*(u0 - u1)); 
    //类间方差公式 g = w0 * w1 * (u0 - u1) ^ 2
    
    //第八步：遍历最大类间方差（因为呈正态分布） 
    if(deltaTmp>deltaMax)
    {
      deltaMax = deltaTmp;
      th = i;
    }
  }
  return th; 
  
}

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
//简单找点
void Scan_Point1(void) 
{
	int i,j;
	int sum,num;
	int point[row];
//	int right,left,bottom,top;
	int flag,top,bottom;
	
	//第一步：找到左右边界进行折半取中值
	flag=0;
	for(i=0;i<row;++i)
	{
		sum=0,num=0;
		for(j=0;j<col-1;++j)
		{
			if(bin_img[i][j]==255&&bin_img[i][j+1]==255){
				sum += j;
				++num;
			}else if(bin_img[i][j-1]==255&&bin_img[i][j]==0&&bin_img[i][j+1]==0)
				break; 
		}
		if(sum)
			point[i] = sum/num;
		else{
			point[i] = 0xff;
			++flag;
		}		
	}
	//第二步：找到上下边界，折半得中点坐标
	if(flag<row)
	{
		sum=0,num=0,flag=1;
		for(i=0;i<row-1;++i)
		{
			if(point[i] != 0xff){
				sum += point[i];
				++num;
                                if(flag){
                                      top = i;
                                      flag = 0;
                                }
			}else if(point[i]==0xff&&point[i-1]!=0xff&&point[i+1]==0xff){
				bottom = i;
				break; 
			}				
		}
                if(num)
                {
                    mid_x = sum/num;
                    mid_y = top + num/2;
		
                    ips200_drawpoint(mid_x,mid_y,RED);//列，行
                    ips200_drawpoint(mid_x+1,mid_y+1,RED);//列，行
                    ips200_drawpoint(mid_x,mid_y+1,RED);//列，行
                    ips200_drawpoint(mid_x+1,mid_y,RED);//列，行
                    sign_Beacon(mid_x,mid_y,top-10,bottom+10);
                }
	}else{
		mid_x=120;
	}	
}

void Scan_Point1(void) 
{
	int i,j;
	int sum,num;
	int point[row];
//	int right,left,bottom,top;
	int flag,top,bottom;
	
	//第一步：找到左右边界进行折半取中值
	flag=0;
	for(i=0;i<row;++i)
	{
		sum=0,num=0;
		for(j=0;j<col;++j)
		{
//			if(flag){
//				if(mt9v03x_image[i][j]>=70){
//					left = j;
//					flag = 0;
//				}
//			}else{
//				if(mt9v03x_image[i][j-1]>=70&&mt9v03x_image[i][j]<70&&mt9v03x_image[i][j+1]<70){
//					right = j;
//					flag = 1;
//					break;
//				}
//			}
			if(mt9v03x_image[i][j]>69){
				sum = sum + j;
				++num;
			}else if(mt9v03x_image[i-1][j]>=70&&mt9v03x_image[i][j]<70&&mt9v03x_image[i][j+1]<70)
				break; 
		}
//		if(flag==0&&j==col)right = col-1;
//		if(left<right)
//			point[i] = (left+right)>>1;
//		else
//			point[i] = 0xff;
//		left = right = 0;
		if(sum)
			point[i]=sum/num;
		else{
			point[i] = 0xff;
			++flag;
		}		
	}
	//第二步：找到上下边界，折半得中点坐标
	if(flag!=row)
	{
		sum=0,num=0,flag=1;
		for(i=0;i<row;++i)
		{
			if(point[i] != 0xff){
				sum += point[i];
				num++;
			if(flag){
				top = i;
				flag = 0;
			}
			}else if(point[i]==0xff&&point[i-1]!=0xff&&point[i+1]==0xff){
				bottom = i;
				break; 
			}				
		}
		mid_x = sum/num;
		mid_y = first + num/2;
		
		ips200_drawpoint(mid_x,mid_y,RED);//列，行
        sign_Beacon(mid_x,mid_y,top,bottom);
	}else{
		mid_x=120;
	}	
}

//标出信标灯的位置
void sign_Beacon(int x,int y,int top,int bottom)
{
	int i,j;
	for(i=top-10;i<bottom+10;++i)
	{
		ips200_drawpoint(x,i,RED);//列，行
		for(j=0;j<col;++j)
			ips200_drawpoint(j,y,RED);//列，行
	 } 
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
#include "headfile.h"

#define col  MT9V03X_W //宽 120
#define row  MT9V03X_H //高 80
#define white 255
#define black 0
#define GrayScale 256

uint8 bin_img[row][col]; //二值化图 
uint8 gray_img[row][col]; //灰度图 

int mid_x,mid_y;

//可以不加二值化浪费时间，直接通过阈值来判断
//只改变与偏差有关的，与中线有关的间接有关的都不改变
//滤波等，思考……效率问题
//#define RED					0xF800													// 红色
//#define BLUE				0x001F													// 蓝色
//#define YELLOW				0xFFE0													// 黄色
//#define GREEN				0x07E0													// 绿色
//#define WHITE				0xFFFF													// 白色
//#define BLACK				0x0000													// 黑色 
//#define GRAY				0X8430													// 灰色
//#define BROWN				0XBC40													// 棕色
//#define PURPLE				0XF81F													// 紫色
//#define PINK				0XFE19	

//main函数
void camera(void)
{
      int threshold;//=70;//灰20-70；白70-126 -127-0
      
      if(mt9v03x_finish_flag)
      {
//            threshold=OTSU();
            threshold=70;
            //加一个：黑白天阈值限定
            Binarization(threshold);
            ips200_displayimage032(bin_img[0],col, row);
            Scan_Point1();
      }
      
      
}




//图像二值化 
void Binarization(int threshold)
{
	int i,j;
      
	for(i=0; i<row; ++i)
	{ 
		for(j=0; j<col; ++j)
		{
			if(mt9v03x_image[i][j]>threshold)
				bin_img[i][j]=white;
			else
				bin_img[i][j]=black;
		}
	}
	mt9v03x_finish_flag = 0;
}

//大津算法
int OTSU(void)
{
	int i,j;
	int img_row=row,img_col=col; 
	int histogram[GrayScale]; //灰度直方图
	
	//第一步：初始化灰度直方图
	for(i=0; i<GrayScale; ++i)
		histogram[i]=0; 
	
	//第二步：统计每个灰度值出现得次数
	for(i=0; i<img_row; ++i)
              for(j=0; j<img_col; ++j)
                  ++histogram[mt9v03x_image[i][j]];

	
	//第三步： 获取最小最大灰度值
	int minGray,maxGray;
	for(minGray=0;minGray<256&&histogram[minGray]==0;++minGray);
	for(maxGray=255;maxGray>minGray&&histogram[minGray]==0;--maxGray);
	
	//第四步：分情况讨论
	// 最大=最小，图像只有一种颜色
	if(maxGray==minGray)
		return maxGray;
	//最大=最小+1，图像只有两种颜色
	if(maxGray==minGray+1)
		return minGray;
	
	//第五步：统计[最小,最大]范围内的像素总数
	int PixelSum=0;
	for(i=minGray; i<maxGray+1; ++i)
		PixelSum += histogram[i];
	
	//第六步：统计[最小,最大]范围内的灰度值总数(灰度值*出现次数)
	int GraySum=0;
	for(i=minGray; i<maxGray+1; ++i)
		GraySum += histogram[i]*i;
		
	//第七步： 大津法优化
	double w0; //前景像素点占整幅图像的百分比 
        double w1; //背景像素点占整幅图像的百分比 
        int w0num; //前景像素点数 
	int w1num; // 背景像素点数 
        int u0gray; //前景灰度值 
        int u1gray; //背景灰度值 
        double u0; //w0平均灰度
        double u1; //w1平均灰度
        double deltaTmp=0,deltaMax=-1;
        int th;
    
        u0gray = u1gray = w0num = w1num = 0;
        for(i=minGray; i<maxGray; i++)
        {
            w0num += histogram[i];
            w1num = PixelSum-w0num;
            w0 = w0num*1.0/PixelSum;
            w1 = w1num*1.0/PixelSum;
            u0gray += histogram[i]*i;
            u1gray = GraySum-u0gray;
            u0 = u0gray*1.0/w0num;
            u1 = u1gray*1.0/w1num;
            deltaTmp = (float)(w0 * w1 * (u0 - u1)*(u0 - u1)); 
		//类间方差公式 g = w0 * w1 * (u0 - u1) ^ 2
		
            //第八步：遍历最大类间方差（因为呈正态分布） 
            if(deltaTmp>deltaMax)
            {
                  deltaMax = deltaTmp;
                  th = i;
            }
         }
	return th; 
        
}
//简单找点
//void Scan_Point1(void)
//{
//	int i,j,k;
//	int left,right,top,bottom;
//        int sum=0;
//        int point[row];
//        int flag1=1,flag2=1;
//        
//        for(i=0;i<row;i++)
//              point[i] = 0;
//        
//        //第一步：找到左右边界进行折半取中值
//	for(i=0;i<row;i++)
//	{
//                left=col,right=0;
//		for(j=0;j<col-3; )
//		{
//			//连续四个点确定为信标
////                        if(bin_img[i][j]=255&&bin_img[i][j+1]==255&&bin_img[i][j+2]==255&&bin_img[i][j+3]==255)
////			{
////				if(left==col)left=j-1;
////				right = j+3;
////                                j += 4;
////                                continue;
////			}
//                        if(bin_img[i][j]==255)
//			{
//				if(left==col)left=j-1;
//				right = j;
//                                j += 1;
//                                flag1=0;
//                                continue;
//			}
//                        j++;
//		}
//                
//                if(left<right)point[i]=(left+right)/2;
//                else point[i]=0xff;
//                
//	} 
//        //第二步：找到上下边界，折半得中点坐标
//        top=row,bottom=0,k=0;
//        for(i=0;i<row-1;)
//        {
////              if(point[i]!=0xff&&point[i+1]!=0xff&&point[i+2]!=0xff&&point[i+3]!=0xff)
////              {
////                    if(top==row)top=i;
////                    bottom=i+3;
////                    i += 4;
////                    k += 4;
////                    continue;
////              }
//              if(point[i]!=0xff)
//              {
//                    if(top==row)top=i;
//                    bottom=i+1;
//                    i += 1;
//                    k += 1;
//                    flag2=0;
//                    continue;
//              }
//              i++;
//        }
//        //第三步：判断是否存在在信标灯
//        if(top<=bottom)
//        {
//              for(i=top;i<=bottom;i++)
//              {
//                  if(point[i]!=0xff)sum += point[i];
//              }
//              mid_x = sum/k;
//              mid_y = top+(bottom-top)/2;
//        }
//        if(flag1==0&&flag2==0){
//          ips200_drawpoint(mid_x,mid_y,RED);//列，行
//        ips200_drawpoint(mid_x+1,mid_y+1,RED);//列，行
//        ips200_drawpoint(mid_x,mid_y+1,RED);//列，行
//        ips200_drawpoint(mid_x+1,mid_y,RED);//列，行
//        sign_Beacon(mid_x,mid_y);
//        }
//        
//        if(flag1==1&&flag2==1){
//          mid_x=120;
//        }
//}
//  


//过滤噪点
void Bin_Image_Filter(void)
{
	int i,j;
	
	for(i=1; i<row-1;++i)
	{
            for(j=1; j<col-1;++j)
            {
                if((bin_img[i][j]==0)&&(bin_img[i-1][j]+bin_img[i-1][j+1]+bin_img[i+1][j]+bin_img[i][j+1]+bin_img[i][j+2]>3))
			bin_img[i][j]=1;
		else if((bin_img[i][j]==1)&&(bin_img[i-1][j]+bin_img[i-1][j+1]+bin_img[i+1][j]+bin_img[i][j+1]+bin_img[i][j+2]<2))
			bin_img[i][j]=0;
            }
	}
}


//标出信标灯的位置
void sign_Beacon(int x,int y)
{
	int i,j;
	for(i=0;i<row;i++)
	{
		ips200_drawpoint(x,i,RED);//列，行
		for(j=0;j<col;j++)
		{
			ips200_drawpoint(j,y,RED);//列，行
		}
	 } 
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
#include "headfile.h"

#define col MT9V03X_W	//宽 
#define row MT9V03X_H	//高 
#define white 255
#define black 0

int bin_img[row][col]; //二值化图 
int gray_img[row][col]; //灰度图 

//图像二值化 
void Binarization(void)
{
	int i,j;
	int threshold=OTSU();
	
	for(i=0;i<row;i++)
	{
		for(j=0;j<col;j++)
		{
			if(mt9v03x_image[i][j]>=threshold)
				bin_img[i][j]=white;
			else
				bin_img[i][j]=black;
			gray_img[i][j]=mt9v03x_image[i][j];
		}
	}
	mt9v03x_finish_flag = 0;
}

//大津法求阈值 -> 优化！速度慢！光线影响！(亮导致阈值高，暗导致阈值低) 
int OTSU(void)
{
	int i,j;
	int GrayScale=256;
	int pixelSum=row*col/4,graySum=0; //像素点总和，灰度值总和 
	int pixCount[GrayScale]; //灰度直方图
	float pixPro[GrayScale]; //归一化处理 
	
	//第一步：初始化 
	for(i=0;i<GrayScale;i++)
	{
		histogram[i] = 0;
		normalization[i] = 0; 
	} 
	
	//第二步：统计0-255灰度值出现次数
	for(i=0;i<row;i+=2)
	{
		for(j=0;j<col;j+=2)
		{
			histogram[gray_img[i][j]]++;
			graySum +=  gray_img[i][j];
		}
	}
	
	//第三步：灰度直方图归一化 [0,1] 
	for(i=0;i<GrayScale;i++)
	{
		normalization[i] = histogram[i]*1.0/pixelSum;
	}
	
	//第四步： 大津法公式套用
	float w0 = 0; //前景像素点占整幅图像的比例
    float w1 = 0; //背景景像素点占整幅图像的比例 
    double u0tmp;
    double u1tmp;
    double u0; //w0平均灰度
    double u1; //w1平均灰度
    double deltaTmp;
    double deltaMax = 0;
    int th = 0;
	
	w0 = w1 = u0tmp = u1tmp = u0 = u1 = deltaTmp = 0;
	for(i=0;i<GrayScale;i++)
	{
//		for (int j = 0; j < GrayScale; j++) 
//		{
//            if (j <= i) //前景
//			{ 
//                w0 += pixPro[j];
//                u0tmp += j * pixPro[j];
//            } 
//			else //背景
//			{ 
//                w1 += pixPro[j];
//                u1tmp += j * pixPro[j];
//            }
//        }
		
		w0 += pixPro[j];
		u0tmp += j * pixPro[j];
		w1 = 1-w0;
		u1tmp = graySum/pixelSum-u0tmp;
        
        u0 = u0tmp / w0;
        u1 = u1tmp / w1;
        deltaTmp = (float)(w0 * w1 * (u0 - u1)*(u0 - u1)); //类间方差公式 g = w0 * w1 * (u0 - u1) ^ 2
        
		//第五步：0-255中找到大津法打分最高的阈值(正态分布) 
		if (deltaTmp > deltaMax) 
		{
            deltaMax = deltaTmp;
            th = i;
        }
        if (deltaTmp < deltaMax) break;
    }

	return th;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
#include<iostream>
using namespace std;
//uint8 imgDeal1[MT9V03X_H][MT9V03X_W];
//uint8 arr[9];
int arr[9];
int MT9V03X_H=10,MT9V03X_W=10;
//int mt9v03x_image[MT9V03X_H][MT9V03X_W];
//噪声处理 
// *********************************************************************/
//快速排序 
// *********************************************************************/
void Quick_Sort(int begin,int end) 
{
	int mid,i,j,temp;
	if(begin>end)
		return;
	mid=filter[begin];
	i=begin;
	j=end;
	while(i!=j)
	{
		while(filter[j]>=mid&&j>i)
			j--;
		while(filter[i]<=mid&&j>i)
			i++;
		if(i<j)
		{
			temp=filter[i];
			filter[i]=filter[j];
			filter[j]=temp;
		}
	}
	filter[begin]=filter[i];
	filter[i]=mid;
	Quick_Sort(begin,i-1);
	Quick_Sort(i+1,end);
}

int filter[9];
//中值滤波：斑点噪声和椒盐噪声 
//用于当找到细小白点进行判断是否为灯，将白点过滤 
// *********************************************************************/
int MedianFilter(void)
{
	int mid;
	//第一步：3*3的观察窗，快速排序或堆排序 (使用灰度图像) 
	Quick_Sort(0,8);
	//第二步：求出中值 
	mid = filter[4];
	//第三步：替换白点 
	return mid;
}

int point[row];
//求信标灯各行的中值 
void find_LR(void)  
{
	int i,j;
	int left,right;
	
	for(i=0;i<row;i++)
	{
		left=col,right=0;
		for(j=0;j<col-1;j++)
		{
			//同一行要连续两个白点显示 
			if(!bin_img[i][j]&&!bin_img[i][j+1])
			{
				if(left==col)left=j; //左边缘确定 
				right = j+1; //右边缘确定 
			} 
		}
		if(left<right)point[i]=(left+right)/2;
		else point[i]=col;
	}
}
//求信标灯的坐标(mid_x,mid_y) 
void find_XY(void)
{
	int i,j,flag1=1,flag2=1,m=0,n=0,sum=0;
	int mid_x,mid_y;
	
	for(i=0;i<row-3;i++)
	{
		if(point[i]!=col&&point[i+1]!=col)//连续四个点确定为信标灯
		{
			if(flag1)
			{
				m=i;
				flag1=0;
			} 
			else if(flag2)n++;
		} 
		else
		{
			if(n>=3)flag2=0;
			else
				flag1=1;
		}
	}
	
	for(j=0;j<n;i++)sum+=point[m+j];
	mid_x = sum/(n+1);
	mid_y = m+(n>>1);
}

//判断是否是信标灯：因为环境中也存在红外光但信标灯是闪烁的，故和其他红外区分 
int isBeacon()
{
	int i,j;
	int threshold;
	int bin_img1[row][col],bin_img2[row][col];
	
	threshold=OTSU();
	for(i=0;i<row;i++)
	{
		for(j=0;j<col;j++)
		{
			if(mt9v03x_image[i][j]>=threshold)
				bin_img1[i][j]=white;
			else
				bin_img1[i][j]=black;
		}
	}
	mt9v03x_finish_flag = 0;
	
	systick_delay_ms(2);
	
	threshold=OTSU();
	for(i=0;i<row;i++)
	{
		for(j=0;j<col;j++)
		{
			if(mt9v03x_image[i][j]>=threshold)
				bin_img2[i][j]=white;
			else
				bin_img2[i][j]=black;
		}
	}
	mt9v03x_finish_flag = 0;
	
	for(i=0;i<row;i++)
	{
		for(j=0;j<col;j++)
		{
			bin_img[i][j]=bin_img1[i][j]^bin_img2[i][j];
		}
	}
}


//均值滤波
// *********************************************************************/
int main()
{
//	Quick_Sort(0,18);
//	for(int i=0;i<19;i++)
//		cout<<arr[i]<<" ";

	cout<<endl;
}	
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
#include "headfile.h"

#define col MT9V03X_W //120 //MT9V03X_W 宽 
#define row   MT9V03X_H//80 //MT9V03X_H 高 
#define white 255
#define black 0

uint8 bin_img[row][col]; //二值化图 
uint8 gray_img[row][col]; //灰度图 
int pixCount[256]; //灰度直方图
float pixPro[256]; //归一化处理 


void camera(void)
{
      Binarization();
      ips200_displayimage032(bin_img[0],MT9V03X_W, MT9V03X_H); 
//      isBeacon();
      find_LR();
//      find_XY();
}

//图像二值化 
void Binarization(void)
{
	int i,j;
//	int threshold=OTSU();
        int threshold=129;
	
	for(i=0;i<row;i++)
	{
		for(j=0;j<col;j++)
		{
			if(mt9v03x_image[i][j]>=threshold)
				bin_img[i][j]=white;
			else
				bin_img[i][j]=black;
			gray_img[i][j]=mt9v03x_image[i][j];
		}
	}
	mt9v03x_finish_flag = 0;
}

int point[row];
//求信标灯各行的中值 
void find_LR(void)  
{
	int i,j;
	int left,right;
	
	for(i=0;i<row;i++)
	{
		left=col,right=0;
		for(j=0;j<col-1;j++)
		{
			//同一行要连续两个白点显示 
			if(!bin_img[i][j]&&!bin_img[i][j+1])
			{
				if(left==col)left=j; //左边缘确定 
				right = j+1; //右边缘确定 
                                ips200_drawpoint(left,i,RED);
			} 
		}
		if(left<right)point[i]=(left+right)/2;
		else point[i]=col;
                ips200_drawpoint(right,i,RED);
	}
}

//求信标灯的坐标(mid_x,mid_y) 
void find_XY(void)
{
	int i,j,flag1=1,flag2=1,m=0,n=0,sum=0;
	int mid_x,mid_y;
	
	for(i=0;i<row-3;i++)
	{
		if(point[i]!=col&&point[i+1]!=col)//连续四个点确定为信标灯
		{
			if(flag1)
			{
				m=i;
				flag1=0;
			} 
			else if(flag2)n++;
		} 
		else
		{
			if(n>=3)flag2=0;
			else
				flag1=1;
		}
	}
	
	for(j=0;j<n;i++)sum+=point[m+j];
	mid_x = sum/(n+1);
	mid_y = m+(n>>1);
        ips200_drawpoint(mid_x,mid_y,BLUE);
}

void Scan_Point2(void)
{
	int i,j;
	int left,right;
         int threshold=70;
         
	for(i=0;i<row;i++)
	{
                left=col,right=0;
		for(j=1;j<col-1;j++)
		{
			
			if(mt9v03x_image[i][j-1]>threshold&&mt9v03x_image[i][j]>threshold&&mt9v03x_image[i][j+1]>threshold)
			{
				if(left==col)left=j-1;
				ips200_drawpoint(left,i,RED);//列，行
                                ips200_showint8(120,139,left);
                                ips200_showint8(120,140,i);
			}
		}
	}
	 
} 

//判断是否是信标灯：因为环境中也存在红外光但信标灯是闪烁的，故和其他红外区分 
void isBeacon(void)
{
	int i,j;
	int threshold;
	int bin_img1[row][col],bin_img2[row][col];
	
	threshold=OTSU();
	for(i=0;i<row;i++)
	{
		for(j=0;j<col;j++)
		{
			if(mt9v03x_image[i][j]>=threshold)
				bin_img1[i][j]=white;
			else
				bin_img1[i][j]=black;
		}
	}
	mt9v03x_finish_flag = 0;
	
	systick_delay_ms(2);
	
	threshold=OTSU();
	for(i=0;i<row;i++)
	{
		for(j=0;j<col;j++)
		{
			if(mt9v03x_image[i][j]>=threshold)
				bin_img2[i][j]=white;
			else
				bin_img2[i][j]=black;
		}
	}
	mt9v03x_finish_flag = 0;
	
	for(i=0;i<row;i++)
	{
		for(j=0;j<col;j++)
		{
			bin_img[i][j]=bin_img1[i][j]^bin_img2[i][j];
		}
	}
}


//大津法求阈值 -> 优化！速度慢！光线影响！(亮导致阈值高，暗导致阈值低) 
//不能用，直立对光线要求高，大津法1ms会导致闪频等，若使用需要加滤波
//int OTSU(void)
//{
//	int i,j;
//	int GrayScale=256;
//	int pixelSum=row*col/4,graySum=0; //像素点总和，灰度值总和 
////	int pixCount[256]; //灰度直方图
////	float pixPro[256]; //归一化处理 
//	
//	//第一步：初始化 
//	for(i=0;i<GrayScale;i++)
//	{
//		pixCount[i] = 0;
//		pixPro[i] = 0; 
//	} 
//	
//	//第二步：统计0-255灰度值出现次数
//	for(i=0;i<row;i+=2)
//	{
//		for(j=0;j<col;j+=2)
//		{
//			pixCount[mt9v03x_image[i][j]]++;
//			graySum +=  mt9v03x_image[i][j];
//		}
//	}
//	
//	//第三步：灰度直方图归一化 [0,1] 
//	for(i=0;i<GrayScale;i++)
//	{
//		pixPro[i] = pixCount[i]*1.0/pixelSum;
//	}
//	
//	//第四步： 大津法公式套用
//	float w0 = 0; //前景像素点占整幅图像的比例
//        float w1 = 0; //背景景像素点占整幅图像的比例 
//        double u0tmp;
//        double u1tmp;
//        double u0; //w0平均灰度
//        double u1; //w1平均灰度
//        double deltaTmp;
//        double deltaMax = 0;
//        int th = 0;
//	
//	w0 = w1 = u0tmp = u1tmp = u0 = u1 = deltaTmp = 0;
//	for(i=0;i<GrayScale;i++)
//	{
////		for (int j = 0; j < GrayScale; j++) 
////		{
////            if (j <= i) //前景
////			{ 
////                w0 += pixPro[j];
////                u0tmp += j * pixPro[j];
////            } 
////			else //背景
////			{ 
////                w1 += pixPro[j];
////                u1tmp += j * pixPro[j];
////            }
////        }
//		
//	w0 += pixPro[j];
//	u0tmp += j * pixPro[j];
//	w1 = 1-w0;
//	u1tmp = graySum/pixelSum-u0tmp;
//        
//        u0 = u0tmp / w0;
//        u1 = u1tmp / w1;
//        deltaTmp = (float)(w0 * w1 * (u0 - u1)*(u0 - u1)); //类间方差公式 g = w0 * w1 * (u0 - u1) ^ 2
//        
//	//第五步：0-255中找到大津法打分最高的阈值(正态分布) 
//	if (deltaTmp > deltaMax) 
//	{
//            deltaMax = deltaTmp;
//            th = i;
//        }
//        if (deltaTmp < deltaMax) break;
//    }
//
//	return th;
//}



//快速排序
//void Quick_Sort(int begin,int end) 
//{
//	int mid,i,j,temp;
//	if(begin>end)
//		return;
//	mid=arr[begin];
//	i=begin;
//	j=end;
//	while(i!=j)
//	{
//		while(arr[j]>=mid&&j>i)
//			j--;
//		while(arr[i]<=mid&&j>i)
//			i++;
//		if(i<j)
//		{
//			temp=arr[i];
//			arr[i]=arr[j];
//			arr[j]=temp;
//		}
//	}
//	arr[begin]=arr[i];
//	arr[i]=mid;
//	Quick_Sort(begin,i-1);
//	Quick_Sort(i+1,end);
//}

EurekaStudy Notes & Supplement

Strong is beautiful!