小型双轮差速底盘机器人实现寻迹功能

机器谱

1. 功能说明
       寻迹机器人是一种能够跟踪特定物体或线路的机器人。它们通常具有以下功能和特点：
       ① 传感器：寻迹机器人配备了用于感知环境的传感器，如摄像头、灰度传感器等。这些传感器可以探测地面上的标记、颜色、纹理或其他特定特征，以确定要跟踪的目标。

       ② 自主导航：寻迹机器人通常具备自主导航能力，可以根据目标物体的位置和运动轨迹进行移动和调整。它们可能使用轮式、履带或其他移动机构来在地面上移动。

       ③ 跟踪精度：寻迹机器人通常被设计为能够实时跟踪目标物体，并尽可能准确地保持距离和方向。一些高级寻迹机器人还可以通过预测目标物体的运动来提高跟踪的精度。

       ④ 应用场景：寻迹机器人可以应用于多种场景，如工业生产线上的零部件跟踪、物流仓库中的货物识别与追踪、安防领域中的行人监控等。它们在自动化、智能化和效率提升方面具有广泛的应用前景。

      本文示例将实现R023样机小型双轮差速底盘机器人沿直线寻迹行走的一个功能。

2. 结构装配
按照下图所示方式进行安装：

3. 电子硬件
在这个示例中，我们采用了以下硬件，请大家参考：

主控板	Basra主控板（兼容Arduino Uno）‍
扩展板	Bigfish2.1扩展板‍
传感器	灰度传感器
电池	7.4V锂电池

按照下图所示方式进行电路连接：

4. 功能实现
编程环境：Arduino 1.8.19
① 下面提供一个控制轮子转动方向和速度的参考例程（Test1.ino）：

1. /*------------------------------------------------------------------------------------
   
2. 
   
3.   版权说明：Copyright 2023 Robottime(Beijing) Technology Co., Ltd. All Rights Reserved.
   
4. 
   
5.            Distributed under MIT license.See file LICENSE for detail or copy at
   
6. 
   
7.            https://opensource.org/licenses/MIT
   
8. 
   
9.            by 机器谱 2023-07-14 https://www.robotway.com/
   
10. 
    
11.   ------------------------------*/
    
12. 
    
13. #include <Servo.h>//调用舵机库
    
14. 
    
15. 
    
16. Servo myservo;//声明一个舵机类
    
17. 
    
18. 
    
19. void setup()//Arduino的设置函数
    
20. 
    
21. {
    
22. 
    
23. myservo.attach(4);//绑定控制舵机的引脚
    
24. 
    
25. }
    
26. 
    
27. 
    
28. void loop()//Arduino的循环函数
    
29. 
    
30. {
    
31. 
    
32. for(int i=0; i<180; i++){//通过调节i值控制舵机的运行参数
    
33. 
    
34.     myservo.write(i);//输出控制舵机的运行参数
    
35. 
    
36.     delay(500);//延时
    
37. 
    
38. }
    
39. 
    
40. }

复制代码

② 下面提供一个控制轮子前进、停止、左转、右转、左微调、右微调的参考例程（Test2.ino）：

1. /*------------------------------------------------------------------------------------
   
2. 
   
3.   版权说明：Copyright 2023 Robottime(Beijing) Technology Co., Ltd. All Rights Reserved.
   
4. 
   
5.            Distributed under MIT license.See file LICENSE for detail or copy at
   
6. 
   
7.            https://opensource.org/licenses/MIT
   
8. 
   
9.            by 机器谱 2023-07-14 https://www.robotway.com/
   
10. 
    
11.   ------------------------------*/
    
12. 
    
13. #include <Servo.h>
    
14. 
    
15. 
    
16. #define middle1 88//定义对应舵机的中间值，即停止转动的值
    
17. 
    
18. #define middle2 88//此值需要测量，各个舵机不一定相同
    
19. 
    
20. 
    
21. Servo myservo[2];//定义一个舵机类数组
    
22. 
    
23. 
    
24. void setup()
    
25. 
    
26. {
    
27. 
    
28. myservo[0].attach(4);
    
29. 
    
30. myservo[1].attach(3);
    
31. 
    
32. }
    
33. 
    
34. 
    
35. void loop()
    
36. 
    
37. {
    
38. 
    
39. Left();//调用左转函数
    
40. 
    
41. delay(1000);
    
42. 
    
43. 
    
44. Right();
    
45. 
    
46. delay(1000);
    
47. 
    
48. 
    
49. Forwards();
    
50. 
    
51. delay(1000);
    
52. 
    
53. 
    
54. stop();
    
55. 
    
56. delay(1000);
    
57. 
    
58. }
    
59. 
    
60. 
    
61. void Left()//左转函数
    
62. 
    
63. {
    
64. 
    
65. myservo[0].write(middle1);
    
66. 
    
67. myservo[1].write(middle2 + 20);
    
68. 
    
69. }
    
70. 
    
71. 
    
72. void Right()//右转函数
    
73. 
    
74. {
    
75. 
    
76. myservo[0].write(middle1 - 20);
    
77. 
    
78. myservo[1].write(middle2);
    
79. 
    
80. }
    
81. 
    
82. 
    
83. void Forwards()//前进函数
    
84. 
    
85. {
    
86. 
    
87. myservo[0].write(middle1 - 20);
    
88. 
    
89. myservo[1].write(middle2 + 20);
    
90. 
    
91. }
    
92. 
    
93. 
    
94. void stop()//后退函数
    
95. 
    
96. {
    
97. 
    
98. myservo[0].write(middle1);
    
99. 
    
100. myservo[1].write(middle2);
     
101. 
     
102. }

复制代码

③ 下面提供一个将灰度传感器数据显示到LED点阵上的参考例程（Test3.ino）：

1. /*------------------------------------------------------------------------------------
   
2. 
   
3.   版权说明：Copyright 2023 Robottime(Beijing) Technology Co., Ltd. All Rights Reserved.
   
4. 
   
5.            Distributed under MIT license.See file LICENSE for detail or copy at
   
6. 
   
7.            https://opensource.org/licenses/MIT
   
8. 
   
9.            by 机器谱 2023-07-14 https://www.robotway.com/
   
10. 
    
11.   ------------------------------*/
    
12. 
    
13. #include <LedControl.h>//调用点阵库函数
    
14. 
    
15. #include <Servo.h>
    
16. 
    
17. 
    
18. LedControl lc=LedControl(12,11,13,1);//声明点阵类，并设置对应的引脚
    
19. 
    
20. int pin[3] = {A0, A4, A3};//设置传感器的对应的三个引脚
    
21. 
    
22. byte value;//声明传感器值变量
    
23. 
    
24. 
    
25. void setup()
    
26. 
    
27. {
    
28. 
    
29. LedInit();//初始化点阵
    
30. 
    
31. }
    
32. 
    
33. 
    
34. /************************************************************************************
    
35. 
    
36. 此程序用到了for与switch的配合框架，可用于多传感器的实时处理，请细细体会！
    
37. 
    
38. 具体解析：for循环中使用了位处理，这样的结果就是value的一个数据位对应一个传感器的状态，
    
39. 
    
40.   此程序value的类型为byte，则可支持8个传感器，如果要使用更多传感器可定义int等。
    
41. 
    
42.   传感器触发时返回值为0，因此value值与传感器触发的状态对应关系以A0触发为例：
    
43. 
    
44.   A0传感器触发-->二进制：00000110-->十六进制：0x06-->对应case 0x06；
    
45. 
    
46.   所以这样做的好处就是当传感器的状态发生改变时程序可以快速的到达指定的处理方式
    
47. 
    
48. **************************************************************************************/
    
49. 
    
50. void loop()
    
51. 
    
52. {
    
53. 
    
54. value = 0;
    
55. 
    
56. for(int i=0; i<3; i++){//通过循环检测，读取传感器的状态值
    
57. 
    
58.     value |= (digitalRead(pin[i]) << i);//通过位处理得到结果值，digitalRead()用于读取数字值
    
59. 
    
60. }
    
61. 
    
62. 
    
63. switch (value) {//根据结果值进行相应的事件处理
    
64. 
    
65. case 0x00://全部触发
    
66. 
    
67. LedOn(0);//点亮相应的点阵
    
68. 
    
69.     LedOn(1);
    
70. 
    
71.     LedOn(2);
    
72. 
    
73. break;
    
74. 
    
75. case 0x01://触发右边两个
    
76. 
    
77.     LedOn(1);
    
78. 
    
79.     LedOn(2);
    
80. 
    
81.       break;
    
82. 
    
83.     case 0x03://触发右边一个
    
84. 
    
85.       LedOn(2);
    
86. 
    
87.       break;
    
88. 
    
89.     case 0x04://触发左边两个
    
90. 
    
91.     LedOn(0);
    
92. 
    
93.     LedOn(1);
    
94. 
    
95.       break;
    
96. 
    
97.     case 0x05://触发中间一个
    
98. 
    
99.     LedOn(1);
    
100. 
     
101.       break;
     
102. 
     
103.     case 0x06://触发左边一个
     
104. 
     
105.     LedOn(0);
     
106. 
     
107.       break;
     
108. 
     
109.     default:
     
110. 
     
111.       ;
     
112. 
     
113. }
     
114. 
     
115. }
     
116. 
     
117. 
     
118. void LedOn(int key)//根据参数点亮相应的点阵LED
     
119. 
     
120. {
     
121. 
     
122. lc.clearDisplay(0);
     
123. 
     
124. for(int i=0; i<2; i++){
     
125. 
     
126.     for(int j=3*key; j<3*key+2; j++){
     
127. 
     
128.         lc.setLed(0, i, j, true);
     
129. 
     
130.     }
     
131. 
     
132. }
     
133. 
     
134. }
     
135. 
     
136. 
     
137. void LedInit() //点阵初始化函数
     
138. 
     
139. {
     
140. 
     
141. lc.shutdown(0,false);
     
142. 
     
143. lc.setIntensity(0,8);
     
144. 
     
145. lc.clearDisplay(0);
     
146. 
     
147. }

复制代码

④ 下面提供一个小车行走直线的参考例程（Test4.ino）：

1. /*------------------------------------------------------------------------------------
   
2. 
   
3.   版权说明：Copyright 2023 Robottime(Beijing) Technology Co., Ltd. All Rights Reserved.
   
4. 
   
5.            Distributed under MIT license.See file LICENSE for detail or copy at
   
6. 
   
7.            https://opensource.org/licenses/MIT
   
8. 
   
9.            by 机器谱 2023-07-14 https://www.robotway.com/
   
10. 
    
11.   ------------------------------*/
    
12. 
    
13. #include <Servo.h>
    
14. 
    
15. 
    
16. #define middle1 88
    
17. 
    
18. #define middle2 88
    
19. 
    
20. 
    
21. Servo myservo[2];
    
22. 
    
23. int pin[3] = {A0, A4, A3};
    
24. 
    
25. byte value;
    
26. 
    
27. byte value_his = 0;//记录上一次的传感器值
    
28. 
    
29. 
    
30. void setup()
    
31. 
    
32. {
    
33. 
    
34. myservo[0].attach(4);
    
35. 
    
36. myservo[1].attach(3);
    
37. 
    
38. }
    
39. 
    
40. 
    
41. void loop()
    
42. 
    
43. {
    
44. 
    
45. value = 0;
    
46. 
    
47. for(int i=0; i<3; i++){
    
48. 
    
49.     value |= (digitalRead(pin[i]) << i);
    
50. 
    
51. }
    
52. 
    
53. 
    
54. if(value == 0x07){//当传感器都没有触发时默认为上一次的值
    
55. 
    
56.     value = value_his;
    
57. 
    
58. }
    
59. 
    
60. 
    
61. switch (value) {
    
62. 
    
63. case 0x00://全部触发
    
64. 
    
65.     Forwards();
    
66. 
    
67. break;
    
68. 
    
69. case 0x01://触发右边两个
    
70. 
    
71.     while(digitalRead(pin[1])){//通过while循环使小车回到跑道中间
    
72. 
    
73.         Right();
    
74. 
    
75.     }
    
76. 
    
77.       break;
    
78. 
    
79.     case 0x03://触发右边一个
    
80. 
    
81.       while(digitalRead(pin[1])){
    
82. 
    
83.         Right();
    
84. 
    
85.     }
    
86. 
    
87.       break;
    
88. 
    
89.     case 0x04://触发左边两个
    
90. 
    
91.     while(digitalRead(pin[1])){
    
92. 
    
93.         Left();
    
94. 
    
95.     }
    
96. 
    
97.       break;
    
98. 
    
99.     case 0x05://触发中间一个
    
100. 
     
101.     Forwards();
     
102. 
     
103.       break;
     
104. 
     
105.     case 0x06://触发左边一个
     
106. 
     
107.     while(digitalRead(pin[1])){
     
108. 
     
109.         Left();
     
110. 
     
111.     }
     
112. 
     
113.       break;
     
114. 
     
115.     default:
     
116. 
     
117.       stop();
     
118. 
     
119. }
     
120. 
     
121. value_his = value;
     
122. 
     
123. }
     
124. 
     
125. 
     
126. void Left()
     
127. 
     
128. {
     
129. 
     
130. myservo[0].write(middle1);
     
131. 
     
132. myservo[1].write(middle2 + 20);
     
133. 
     
134. }
     
135. 
     
136. 
     
137. void Right()
     
138. 
     
139. {
     
140. 
     
141. myservo[0].write(middle1 - 20);
     
142. 
     
143. myservo[1].write(middle2);
     
144. 
     
145. }
     
146. 
     
147. 
     
148. void Forwards()
     
149. 
     
150. {
     
151. 
     
152. myservo[0].write(middle1 - 20);
     
153. 
     
154. myservo[1].write(middle2 + 20);
     
155. 
     
156. }
     
157. 
     
158. 
     
159. void stop()
     
160. 
     
161. {
     
162. 
     
163. myservo[0].write(middle1);
     
164. 
     
165. myservo[1].write(middle2);
     
166. 
     
167. }

复制代码

⑤ 下面提供一个小车识别十字路口的参考例程（Test5.ino）：

1. /*------------------------------------------------------------------------------------
   
2. 
   
3.   版权说明：Copyright 2023 Robottime(Beijing) Technology Co., Ltd. All Rights Reserved.
   
4. 
   
5.            Distributed under MIT license.See file LICENSE for detail or copy at
   
6. 
   
7.            https://opensource.org/licenses/MIT
   
8. 
   
9.            by 机器谱 2023-07-14 https://www.robotway.com/
   
10. 
    
11.   ------------------------------*/
    
12. 
    
13. #include <Servo.h>
    
14. 
    
15. #include <LedControl.h>
    
16. 
    
17. 
    
18. #define middle1 88
    
19. 
    
20. #define middle2 88
    
21. 
    
22. 
    
23. Servo myservo[2];
    
24. 
    
25. LedControl lc=LedControl(12,11,13,1);
    
26. 
    
27. 
    
28. int pin[3] = {A0, A4, A3};
    
29. 
    
30. byte value;
    
31. 
    
32. byte value_his = 0;
    
33. 
    
34. int time[3];//用于记录传感器的触发时间
    
35. 
    
36. 
    
37. void setup()
    
38. 
    
39. {
    
40. 
    
41. LedInit();
    
42. 
    
43. myservo[0].attach(4);
    
44. 
    
45. myservo[1].attach(3);
    
46. 
    
47. }
    
48. 
    
49. 
    
50. void loop()
    
51. 
    
52. {
    
53. 
    
54. value = 0;
    
55. 
    
56. for(int i=0; i<3; i++){
    
57. 
    
58.     value |= (digitalRead(pin[i]) << i);
    
59. 
    
60.     if(!digitalRead(pin[i])){
    
61. 
    
62.         time[i] = millis();//调用mills函数可以得到此时单片机的运行时间
    
63. 
    
64.     }
    
65. 
    
66. }
    
67. 
    
68. 
    
69. if(TimeDeal())
    
70. 
    
71. {
    
72. 
    
73. if(millis() > 1000){//用于排除刚开机时的误判
    
74. 
    
75.     LedDis();//十字路口显示
    
76. 
    
77. }
    
78. 
    
79. }
    
80. 
    
81. 
    
82. if(value == 0x07){//当传感器都没有触发时默认为上一次的值
    
83. 
    
84.     value = value_his;
    
85. 
    
86. }
    
87. 
    
88. 
    
89. switch (value) {
    
90. 
    
91. case 0x00://全部触发
    
92. 
    
93.     Forwards();
    
94. 
    
95. break;
    
96. 
    
97. case 0x01://触发右边两个
    
98. 
    
99.     while(digitalRead(pin[1])){
    
100. 
     
101.         Right();
     
102. 
     
103.     }
     
104. 
     
105.       break;
     
106. 
     
107.     case 0x03://触发右边一个
     
108. 
     
109.       while(digitalRead(pin[1])){
     
110. 
     
111.         Right();
     
112. 
     
113.     }
     
114. 
     
115.       break;
     
116. 
     
117.     case 0x04://触发左边两个
     
118. 
     
119.     while(digitalRead(pin[1])){
     
120. 
     
121.         Left();
     
122. 
     
123.     }
     
124. 
     
125.       break;
     
126. 
     
127.     case 0x05://触发中间一个
     
128. 
     
129.     Forwards();
     
130. 
     
131.       break;
     
132. 
     
133.     case 0x06://触发左边一个
     
134. 
     
135.     while(digitalRead(pin[1])){
     
136. 
     
137.         Left();
     
138. 
     
139.     }
     
140. 
     
141.       break;
     
142. 
     
143.     default:
     
144. 
     
145.       stop();
     
146. 
     
147. }
     
148. 
     
149. value_his = value;
     
150. 
     
151. lc.clearDisplay(0);
     
152. 
     
153. }
     
154. 
     
155. 
     
156. void Left()
     
157. 
     
158. {
     
159. 
     
160. myservo[0].write(middle1);
     
161. 
     
162. myservo[1].write(middle2 + 20);
     
163. 
     
164. }
     
165. 
     
166. 
     
167. void Right()
     
168. 
     
169. {
     
170. 
     
171. myservo[0].write(middle1 - 20);
     
172. 
     
173. myservo[1].write(middle2);
     
174. 
     
175. }
     
176. 
     
177. 
     
178. void Forwards()
     
179. 
     
180. {
     
181. 
     
182. myservo[0].write(middle1 - 20);
     
183. 
     
184. myservo[1].write(middle2 + 20);
     
185. 
     
186. }
     
187. 
     
188. 
     
189. void stop()
     
190. 
     
191. {
     
192. 
     
193. myservo[0].write(middle1);
     
194. 
     
195. myservo[1].write(middle2);
     
196. 
     
197. }
     
198. 
     
199. 
     
200. bool TimeDeal()//十字路口识别函数
     
201. 
     
202. {
     
203. 
     
204. if(millis() > 500){
     
205. 
     
206. if((abs(time[1] - time[0]) < 100) && (abs(time[1] - time[2]) < 100)){//当中间传感器与另外两个传感器触发的时间小于100毫秒时判定为十字路口
     
207. 
     
208.     return true;
     
209. 
     
210. }
     
211. 
     
212. else
     
213. 
     
214. return false;   
     
215. 
     
216. }
     
217. 
     
218. }
     
219. 
     
220. 
     
221. void LedDis()//十字路口显示函数
     
222. 
     
223. {
     
224. 
     
225. for(int i=3; i<5; i++){
     
226. 
     
227.     for(int j=0; j<8; j++){
     
228. 
     
229.         lc.setLed(0, i, j, true);
     
230. 
     
231.     }
     
232. 
     
233. }
     
234. 
     
235. for(int i=3; i<5; i++){
     
236. 
     
237.     for(int j=0; j<8; j++){
     
238. 
     
239.         lc.setLed(0, j, i, true);
     
240. 
     
241.     }
     
242. 
     
243. }
     
244. 
     
245. }
     
246. 
     
247. 
     
248. void LedInit()
     
249. 
     
250. {
     
251. 
     
252. lc.shutdown(0,false); //start the 8*8 led
     
253. 
     
254. lc.setIntensity(0,8);
     
255. 
     
256. lc.clearDisplay(0);
     
257. 
     
258. }

复制代码

⑥ 下面提供一个小车实现寻迹的完整程序（TrackingCar.ino）：

1. /*------------------------------------------------------------------------------------
   
2. 
   
3.   版权说明：Copyright 2023 Robottime(Beijing) Technology Co., Ltd. All Rights Reserved.
   
4. 
   
5.            Distributed under MIT license.See file LICENSE for detail or copy at
   
6. 
   
7.            https://opensource.org/licenses/MIT
   
8. 
   
9.            by 机器谱 2023-07-14 https://www.robotway.com/
   
10. 
    
11.   ------------------------------*/
    
12. 
    
13. #include <LedControl.h>
    
14. 
    
15. #include <Servo.h>
    
16. 
    
17. 
    
18. #define middle1 88
    
19. 
    
20. #define middle2 88
    
21. 
    
22. 
    
23. LedControl lc=LedControl(12,11,13,1);
    
24. 
    
25. Servo myservo[2];
    
26. 
    
27. int pin[3] = {A0, A4, A3};
    
28. 
    
29. int time[3];
    
30. 
    
31. byte value;
    
32. 
    
33. byte value_his = 0;
    
34. 
    
35. int flag = 0;
    
36. 
    
37. int times = 0;
    
38. 
    
39. 
    
40. void setup()
    
41. 
    
42. {
    
43. 
    
44. LedInit();
    
45. 
    
46. Serial.begin(9600);//串口，用于调试
    
47. 
    
48. myservo[0].attach(4);
    
49. 
    
50. myservo[1].attach(3);
    
51. 
    
52. }
    
53. 
    
54. 
    
55. void loop()
    
56. 
    
57. {
    
58. 
    
59. value = 0;
    
60. 
    
61. for(int i=0; i<3; i++){
    
62. 
    
63.     value |= (digitalRead(pin[i]) << i);
    
64. 
    
65.     if(!digitalRead(pin[i])){
    
66. 
    
67.         time[i] = millis();
    
68. 
    
69.     }
    
70. 
    
71. }
    
72. 
    
73. 
    
74. if(TimeDeal())
    
75. 
    
76. {
    
77. 
    
78. times++;
    
79. 
    
80. Serial.print(times);
    
81. 
    
82. }
    
83. 
    
84. else
    
85. 
    
86. {
    
87. 
    
88. if(times > 1){
    
89. 
    
90. Serial.println();
    
91. 
    
92.     flag += 1;
    
93. 
    
94.     Serial.println(flag);
    
95. 
    
96. }
    
97. 
    
98. times = 0;
    
99. 
    
100. }
     
101. 
     
102. 
     
103. if(flag == 3){
     
104. 
     
105.     while(1){
     
106. 
     
107.         stop();
     
108. 
     
109.     }
     
110. 
     
111. }
     
112. 
     
113. 
     
114. if(value == 0x07){
     
115. 
     
116.     value = value_his;
     
117. 
     
118. }
     
119. 
     
120. 
     
121. switch (value) {
     
122. 
     
123. case 0x00://全部触发
     
124. 
     
125. LedOn(0);
     
126. 
     
127.     LedOn(1);
     
128. 
     
129.     LedOn(2);
     
130. 
     
131.     Forwards();
     
132. 
     
133.     //delay(500);
     
134. 
     
135. break;
     
136. 
     
137. case 0x01://触发右边两个
     
138. 
     
139.     LedOn(1);
     
140. 
     
141.     LedOn(2);
     
142. 
     
143.     while(digitalRead(pin[1])){
     
144. 
     
145.         Right();
     
146. 
     
147.     }
     
148. 
     
149.       break;
     
150. 
     
151.     case 0x03://触发右边一个
     
152. 
     
153.       LedOn(2);
     
154. 
     
155.       while(digitalRead(pin[1])){
     
156. 
     
157.         Right();
     
158. 
     
159.     }
     
160. 
     
161.       break;
     
162. 
     
163.     case 0x04://触发左边两个
     
164. 
     
165.     LedOn(0);
     
166. 
     
167.     LedOn(1);
     
168. 
     
169.     while(digitalRead(pin[1])){
     
170. 
     
171.         Left();
     
172. 
     
173.     }
     
174. 
     
175.       break;
     
176. 
     
177.     case 0x05://触发中间一个
     
178. 
     
179.     LedOn(1);
     
180. 
     
181.     Forwards();
     
182. 
     
183.       break;
     
184. 
     
185.     case 0x06://触发左边一个
     
186. 
     
187.     LedOn(0);
     
188. 
     
189.     while(digitalRead(pin[1])){
     
190. 
     
191.         Left();
     
192. 
     
193.     }
     
194. 
     
195.       break;
     
196. 
     
197.     default:
     
198. 
     
199.       stop();
     
200. 
     
201. }
     
202. 
     
203. value_his = value;
     
204. 
     
205. lc.clearDisplay(0);
     
206. 
     
207. }
     
208. 
     
209. 
     
210. bool TimeDeal()
     
211. 
     
212. {
     
213. 
     
214. if(millis() > 500){
     
215. 
     
216. if((abs(time[1] - time[0]) < 100) && (abs(time[1] - time[2]) < 100)){
     
217. 
     
218.     return true;
     
219. 
     
220. }
     
221. 
     
222. else
     
223. 
     
224. return false;   
     
225. 
     
226. }
     
227. 
     
228. }
     
229. 
     
230. 
     
231. void Left()
     
232. 
     
233. {
     
234. 
     
235. myservo[0].write(middle1);
     
236. 
     
237. myservo[1].write(middle2 + 20);
     
238. 
     
239. }
     
240. 
     
241. 
     
242. void Right()
     
243. 
     
244. {
     
245. 
     
246. myservo[0].write(middle1 - 20);
     
247. 
     
248. myservo[1].write(middle2);
     
249. 
     
250. }
     
251. 
     
252. 
     
253. void Forwards()
     
254. 
     
255. {
     
256. 
     
257. myservo[0].write(middle1 - 20);
     
258. 
     
259. myservo[1].write(middle2 + 20);
     
260. 
     
261. }
     
262. 
     
263. 
     
264. void stop()
     
265. 
     
266. {
     
267. 
     
268. myservo[0].write(middle1);
     
269. 
     
270. myservo[1].write(middle2);
     
271. 
     
272. }
     
273. 
     
274. 
     
275. void LedOn(int key)
     
276. 
     
277. {
     
278. 
     
279. for(int i=0; i<2; i++){
     
280. 
     
281.     for(int j=3*key; j<3*key+2; j++){
     
282. 
     
283.         lc.setLed(0, i, j, true);
     
284. 
     
285.     }
     
286. 
     
287. }
     
288. 
     
289. }
     
290. 
     
291. 
     
292. void LedInit()
     
293. 
     
294. {
     
295. 
     
296. lc.shutdown(0,false); //start the 8*8 led
     
297. 
     
298. lc.setIntensity(0,8);
     
299. 
     
300. lc.clearDisplay(0);
     
301. 
     
302. }

复制代码

5. 资料下载
资料内容：程序源代码
资料卸载地址：小型双轮差速底盘-寻迹与路口 https://www.robotway.com/h-col-113.html