Smart-Freescale-Car/ImageProcess.c at master · livey/Smart-Freescale-Car · GitHub

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
#include "includes.h"

#define Adot 15
#define Bdot 40          ///40

#define SafeSpeed  20
#define BreakSpeed 10
#define SpeedMax   80        //80

#define A0 13
#define A1 7    //低于这个速度则按照最小速度原则


#define Direct_P  310


//   #define maxspeed  110
 //  #define minspeed  20
#define minline   10
#define maxline   59


/*----------------------------------------------------
函数ImageProcess
目的：对提取中心线后的图像进行处理 得到预计方向和速度
输入：二值化后的图像
输出：期望的方向输出    期望的两个电机输出
错误：
-----------------------------------------------------*/


 int16  ExpectDirection;
 uint16 ExpectSpeed_left;
 uint16 ExpectSpeed_right;
 uint16 ExpectSpeed;
 int16 speed;

 uint8 MAX_SPEED;
 uint8 MIN_SPEED;


/*提取 期望的速度与方向*/


void ImageProcess(void)  {


      int offset;
      int ii,sum_all,sum_back;
		  int R;
		  int Speed;

		  int  k1,k2,k3;
		  int a0,a1,a2,a3;
		  long  jj;
		  long  aa,bb;


		  int16 zuobiao[RealRow]={0};


   ///////////////////////////////////////////
            ///////坐标变换/////////
    /////////////变换成实际的坐标/////////////


       for(ii=0;ii<RealRow;ii++)
           zuobiao[ii]=(CenterLine[ii]-Mid_line)*ii/50+(CenterLine[ii]-Mid_line)>>1;


     /*利用图像分割原理*/


	 /*图像参考线动态调整*/  //速度控制

   if(DeadLine==0)

     {
        ExpectDirection=ExpectDirection/abs(ExpectDirection)*220;
        ExpectSpeed=SafeSpeed;

        return;
     }

    else  if( DeadLine<7)
		  {


			  ExpectDirection=(zuobiao[DeadLine]/abs(zuobiao[DeadLine]))*220;
			  ExpectSpeed=SafeSpeed;

			  return;

          }
	 else if(DeadLine<Adot)
		   {


			 a1=zuobiao[DeadLine]+zuobiao[DeadLine-1]+zuobiao[ii-2]+zuobiao[ii-3];
			 a1>>=2;


			   ExpectDirection=Direct_P*a1/DeadLine;                           // 需要校正的系数


		    }
      else if (DeadLine>=Adot)
		    {


				 ii=(DeadLine-60)*(Bdot-Adot)/(60-Adot)+Bdot;


				a1=zuobiao[ii]+zuobiao[ii-1]+zuobiao[ii-2]+zuobiao[ii-3];
				a1>>=2;


         ExpectDirection=Direct_P*a1/ii;

			  }


 // 速度控制

  if(DeadLine<minline)
      ExpectSpeed=MIN_SPEED;

  else

      ExpectSpeed=MAX_SPEED-(maxline-DeadLine)*(MAX_SPEED-MIN_SPEED)/(maxline-minline);


}


int16 abs(int16 x) {

 if(x<0)
   return(-x);

  else return(x);


}