SHOW:
|
|
- or go back to the newest paste.
1 | /* | |
2 | * Copyright (c) 2015,小二极客科技有限公司 | |
3 | * All rights reserved. | |
4 | * | |
5 | * 文件名称:wifi-robots | |
6 | * 文件标识: | |
7 | * 摘 要:wifi机器人智能小车控制 | |
8 | * | |
9 | * 当前版本:1.0 | |
10 | * 作 者:小R团队 | |
11 | * 完成日期:2015年4月1日 | |
12 | */ | |
13 | #include <Servo.h> | |
14 | //#include <MsTimer2.h> | |
15 | #include <EEPROM.h> | |
16 | ||
17 | int ledpin = 13;//设置系统启动指示灯 | |
18 | int ENA = 5;//L298使能A | |
19 | int ENB = 6;//L298使能B | |
20 | int INPUT2 = 7;//电机接口1 | |
21 | int INPUT1 = 8;//电机接口2 | |
22 | int INPUT3 = 12;//电机接口3 | |
23 | int INPUT4 = 13;//电机接口4 | |
24 | int num;//定义电机标志位 | |
25 | ||
26 | int Echo = A5; // 定义超音波信号接收脚位 | |
27 | int Trig = A4; // 定义超音波信号发射脚位 | |
28 | int Input_Detect_LEFT = A3; //定义小车左侧红外 | |
29 | int Input_Detect_RIGHT = A2; //定义小车右侧红外 | |
30 | int Input_Detect = A1;//定义小车前方红外 | |
31 | int Carled = A0;//定义小车车灯接口 | |
32 | int Cruising_Flag = 0; | |
33 | int Pre_Cruising_Flag = 0 ; | |
34 | int Left_Speed_Hold = 200;//定义左侧速度变量 | |
35 | int Right_Speed_Hold = 200;//定义右侧速度变量 | |
36 | ||
37 | ||
38 | #define MOTOR_GO_FORWARD {digitalWrite(INPUT1,LOW);digitalWrite(INPUT2,HIGH);digitalWrite(INPUT3,LOW);digitalWrite(INPUT4,HIGH);} //车体前进 | |
39 | #define MOTOR_GO_BACK {digitalWrite(INPUT1,HIGH);digitalWrite(INPUT2,LOW);digitalWrite(INPUT3,HIGH);digitalWrite(INPUT4,LOW);} //车体前进 | |
40 | #define MOTOR_GO_RIGHT {digitalWrite(INPUT1,HIGH);digitalWrite(INPUT2,LOW);digitalWrite(INPUT3,LOW);digitalWrite(INPUT4,HIGH);} //车体前进 | |
41 | #define MOTOR_GO_LEFT {digitalWrite(INPUT1,LOW);digitalWrite(INPUT2,HIGH);digitalWrite(INPUT3,HIGH);digitalWrite(INPUT4,LOW);} //车体前进 | |
42 | #define MOTOR_GO_STOP {digitalWrite(INPUT1,LOW);digitalWrite(INPUT2,LOW);digitalWrite(INPUT3,LOW);digitalWrite(INPUT4,LOW);} //车体前进 | |
43 | ||
44 | ||
45 | void forward(int num) | |
46 | { | |
47 | switch(num) | |
48 | { | |
49 | case 1:MOTOR_GO_FORWARD;return; | |
50 | case 2:MOTOR_GO_FORWARD;return; | |
51 | case 3:MOTOR_GO_BACK;return; | |
52 | case 4:MOTOR_GO_BACK;return; | |
53 | case 5:MOTOR_GO_LEFT;return; | |
54 | case 6:MOTOR_GO_LEFT;return; | |
55 | case 7:MOTOR_GO_RIGHT;return; | |
56 | case 8:MOTOR_GO_RIGHT;return; | |
57 | default:return; | |
58 | } | |
59 | } | |
60 | ||
61 | void back(int num) | |
62 | { | |
63 | switch(num) | |
64 | { | |
65 | case 1:MOTOR_GO_BACK;return; | |
66 | case 2:MOTOR_GO_BACK;return; | |
67 | case 3:MOTOR_GO_FORWARD;return; | |
68 | case 4:MOTOR_GO_FORWARD;return; | |
69 | case 5:MOTOR_GO_RIGHT;return; | |
70 | case 6:MOTOR_GO_RIGHT;return; | |
71 | case 7:MOTOR_GO_LEFT;return; | |
72 | case 8:MOTOR_GO_LEFT;return; | |
73 | default:return; | |
74 | } | |
75 | } | |
76 | void left(int num) | |
77 | { | |
78 | switch(num) | |
79 | { | |
80 | case 1:MOTOR_GO_LEFT;return; | |
81 | case 2:MOTOR_GO_RIGHT;return; | |
82 | case 3:MOTOR_GO_LEFT;return; | |
83 | case 4:MOTOR_GO_RIGHT;return; | |
84 | case 5:MOTOR_GO_FORWARD;return; | |
85 | case 6:MOTOR_GO_BACK;return; | |
86 | case 7:MOTOR_GO_FORWARD;return; | |
87 | case 8:MOTOR_GO_BACK;return; | |
88 | default:return; | |
89 | } | |
90 | } | |
91 | void right(int num) | |
92 | { | |
93 | switch(num) | |
94 | { | |
95 | case 1:MOTOR_GO_RIGHT;return; | |
96 | case 2:MOTOR_GO_LEFT;return; | |
97 | case 3:MOTOR_GO_RIGHT;return; | |
98 | case 4:MOTOR_GO_LEFT;return; | |
99 | case 5:MOTOR_GO_BACK;return; | |
100 | case 6:MOTOR_GO_FORWARD;return; | |
101 | case 7:MOTOR_GO_BACK;return; | |
102 | case 8:MOTOR_GO_FORWARD;return; | |
103 | default:return; | |
104 | } | |
105 | } | |
106 | ||
107 | ||
108 | int Left_Speed[11]={90,106,122,138,154,170,186,203,218,234,255};//左侧速度档位值 | |
109 | int Right_Speed[11]={90,106,122,138,154,170,186,203,218,234,255};//右侧速度档位值 | |
110 | ||
111 | //Servo servo1;// 创建舵机#1号 | |
112 | //Servo servo2;// 创建舵机#2号 | |
113 | Servo servo3;// 创建舵机#3号 | |
114 | Servo servo4;// 创建舵机#4号 | |
115 | Servo servo5;// 创建舵机#5号 | |
116 | Servo servo6;// 创建舵机#6号 | |
117 | Servo servo7;// 创建舵机#7号 | |
118 | Servo servo8;// 创建舵机#8号 | |
119 | ||
120 | ||
121 | ||
122 | //byte angle1=60;//舵机#1初始值 | |
123 | //byte angle2=60;//舵机#2初始值 | |
124 | byte angle3=60;//舵机#3初始值 | |
125 | byte angle4=60;//舵机#4初始值 | |
126 | byte angle5=60;//舵机#5初始值 | |
127 | byte angle6=120;//舵机#6初始值 | |
128 | byte angle7=60;//舵机#7初始值 | |
129 | byte angle8=60;//舵机#8初始值 | |
130 | ||
131 | ||
132 | int buffer[3]; //串口接收数据缓存 | |
133 | int rec_flag; //串口接收标志位 | |
134 | int serial_data; | |
135 | int Uartcount; | |
136 | int IR_R; | |
137 | int IR_L; | |
138 | int IR; | |
139 | unsigned long Pretime; | |
140 | unsigned long Nowtime; | |
141 | unsigned long Costtime; | |
142 | float Ldistance; | |
143 | ||
144 | void Open_Light()//开大灯 | |
145 | { | |
146 | digitalWrite(Carled,HIGH); //拉低电平,正极接电源,负极接Io口 | |
147 | delay(1000); | |
148 | } | |
149 | void Close_Light()//关大灯 | |
150 | { | |
151 | digitalWrite(Carled, LOW); //拉低电平,正极接电源,负极接Io口 | |
152 | delay(1000); | |
153 | } | |
154 | ||
155 | void Avoiding()//红外避障函数 | |
156 | { | |
157 | IR = digitalRead(Input_Detect); | |
158 | if((IR == HIGH)) | |
159 | { | |
160 | forward(num);;//直行 | |
161 | return; | |
162 | } | |
163 | if((IR == LOW)) | |
164 | { | |
165 | MOTOR_GO_STOP;//停止 | |
166 | return; | |
167 | } | |
168 | } | |
169 | ||
170 | ||
171 | ||
172 | void FollowLine() // 巡线模式 | |
173 | { | |
174 | IR_L = digitalRead(Input_Detect_LEFT);//读取左边传感器数值 | |
175 | IR_R = digitalRead(Input_Detect_RIGHT);//读取右边传感器数值 | |
176 | ||
177 | if((IR_L == LOW) && (IR_R == LOW))//两边同时探测到障碍物 | |
178 | { | |
179 | forward(num);//直行 | |
180 | return; | |
181 | ||
182 | } | |
183 | if((IR_L == LOW) && (IR_R == HIGH))//右侧遇到障碍 | |
184 | { | |
185 | left(num);//左转 | |
186 | return; | |
187 | ||
188 | } | |
189 | if((IR_L == HIGH) &&( IR_R == LOW))//左侧遇到障碍 | |
190 | { | |
191 | right(num);//右转 | |
192 | return; | |
193 | ||
194 | } | |
195 | if((IR_L == HIGH) && (IR_R == HIGH))//左右都检测到,就如视频中的那样遇到一道横的胶带 | |
196 | { | |
197 | MOTOR_GO_STOP;//停止 | |
198 | return; | |
199 | } | |
200 | } | |
201 | ||
202 | char Get_Distence()//测出距离 | |
203 | { | |
204 | digitalWrite(Trig, LOW); // 让超声波发射低电压2μs | |
205 | delayMicroseconds(2); | |
206 | digitalWrite(Trig, HIGH); // 让超声波发射高电压10μs,这里至少是10μs | |
207 | delayMicroseconds(10); | |
208 | digitalWrite(Trig, LOW); // 维持超声波发射低电压 | |
209 | Ldistance = pulseIn(Echo, HIGH); // 读差相差时间 | |
210 | Ldistance= Ldistance/5.8/10; // 将时间转为距离距离(单位:公分) | |
211 | // Serial.println(Ldistance); //显示距离 | |
212 | return Ldistance; | |
213 | ||
214 | } | |
215 | ||
216 | void Avoid_wave()//超声波避障函数 | |
217 | { | |
218 | Get_Distence(); | |
219 | if(Ldistance < 15) | |
220 | { | |
221 | MOTOR_GO_STOP; | |
222 | } | |
223 | else | |
224 | { | |
225 | forward(num); | |
226 | } | |
227 | } | |
228 | ||
229 | void Avoid_wave_auto() | |
230 | { | |
231 | Get_Distence(); | |
232 | if(Ldistance < 15) | |
233 | { | |
234 | back(num);; | |
235 | delay(300); | |
236 | MOTOR_GO_STOP; | |
237 | } | |
238 | } | |
239 | ||
240 | void Send_Distance()//超声波距离PC端显示 | |
241 | { | |
242 | int dis= Get_Distence(); | |
243 | Serial.write(0xff); | |
244 | Serial.write(0x03); | |
245 | Serial.write(0x00); | |
246 | Serial.write(dis); | |
247 | Serial.write(0xff); | |
248 | delay(1000); | |
249 | } | |
250 | /* | |
251 | ********************************************************************************************************* | |
252 | ** 函数名称 :Delayed() | |
253 | ** 函数功能 :延时程序 | |
254 | ** 入口参数 :无 | |
255 | ** 出口参数 :无 | |
256 | ********************************************************************************************************* | |
257 | */ | |
258 | void Delayed() //延迟40秒等WIFI模块启动完毕 | |
259 | { | |
260 | int i; | |
261 | for(i=0;i<20;i++) | |
262 | { | |
263 | digitalWrite(ledpin,LOW); | |
264 | delay(1000); | |
265 | digitalWrite(ledpin,HIGH); | |
266 | delay(1000); | |
267 | } | |
268 | } | |
269 | ||
270 | /* | |
271 | ********************************************************************************************************* | |
272 | ** 函数名称 :setup().Init_Steer() | |
273 | ** 函数功能 :系统初始化(串口、电机、舵机、指示灯初始化)。 | |
274 | ** 入口参数 :无 | |
275 | ** 出口参数 :无 | |
276 | ********************************************************************************************************* | |
277 | */ | |
278 | void Init_Steer()//舵机初始化(角度为上次保存数值) | |
279 | { | |
280 | // angle1 = EEPROM.read(0x01);//读取寄存器0x01里面的值 | |
281 | // angle2 = EEPROM.read(0x02);//读取寄存器0x02里面的值 | |
282 | angle3 = EEPROM.read(0x03);//读取寄存器0x03里面的值 | |
283 | angle4 = EEPROM.read(0x04);//读取寄存器0x04里面的值 | |
284 | angle5 = EEPROM.read(0x05);//读取寄存器0x05里面的值 | |
285 | angle6 = EEPROM.read(0x06);//读取寄存器0x06里面的值 | |
286 | angle7 = EEPROM.read(0x07);//读取寄存器0x07里面的值 | |
287 | angle8 = EEPROM.read(0x08);//读取寄存器0x08里面的值 | |
288 | ||
289 | if(angle7 == 255 && angle8 == 255) | |
290 | { | |
291 | // EEPROM.write(0x01,60);//把初始角度存入地址0x01里面 | |
292 | // EEPROM.write(0x02,60);//把初始角度存入地址0x02里面 | |
293 | EEPROM.write(0x03,60);//把初始角度存入地址0x03里面 | |
294 | EEPROM.write(0x04,60);//把初始角度存入地址0x04里面 | |
295 | EEPROM.write(0x05,60);//把初始角度存入地址0x05里面 | |
296 | EEPROM.write(0x06,120);//把初始角度存入地址0x06里面 | |
297 | EEPROM.write(0x07,60);//把初始角度存入地址0x07里面 | |
298 | EEPROM.write(0x08,60);//把初始角度存入地址0x08里面 | |
299 | return; | |
300 | } | |
301 | ||
302 | // servo1.write(angle1);//把保存角度赋值给舵机1 | |
303 | // servo2.write(angle2);//把保存角度赋值给舵机2 | |
304 | servo3.write(angle3);//把保存角度赋值给舵机3 | |
305 | servo4.write(angle4);//把保存角度赋值给舵机4 | |
306 | servo5.write(angle5);//把保存角度赋值给舵机5 | |
307 | servo6.write(angle6);//把保存角度赋值给舵机6 | |
308 | servo7.write(angle7);//把保存角度赋值给舵机7 | |
309 | servo8.write(angle8);//把保存角度赋值给舵机8 | |
310 | num = EEPROM.read(0x10);//读取寄存器0x10里面的值 | |
311 | if(num==0xff)EEPROM.write(0x10,1); | |
312 | } | |
313 | ||
314 | void setup() | |
315 | { | |
316 | pinMode(ledpin,OUTPUT); | |
317 | pinMode(ENA,OUTPUT); | |
318 | pinMode(ENB,OUTPUT); | |
319 | pinMode(INPUT1,OUTPUT); | |
320 | pinMode(INPUT2,OUTPUT); | |
321 | pinMode(INPUT3,OUTPUT); | |
322 | pinMode(INPUT4,OUTPUT); | |
323 | pinMode(Input_Detect_LEFT,INPUT); | |
324 | pinMode(Input_Detect_RIGHT,INPUT); | |
325 | pinMode(Carled, OUTPUT); | |
326 | pinMode(Input_Detect,INPUT); | |
327 | pinMode(Echo,INPUT); | |
328 | pinMode(Trig,OUTPUT); | |
329 | ||
330 | Delayed();//延迟40秒等WIFI模块启动完毕 | |
331 | analogWrite(ENB,Left_Speed_Hold);//给L298使能端B赋值 | |
332 | analogWrite(ENA,Right_Speed_Hold);//给L298使能端A赋值 | |
333 | digitalWrite(ledpin,LOW); | |
334 | //servo1.attach(SDA);//定义舵机1控制口 | |
335 | //servo2.attach(SCL);//定义舵机2控制口 | |
336 | servo3.attach(3);//定义舵机3控制口 | |
337 | servo4.attach(4);//定义舵机4控制口 | |
338 | servo5.attach(2);//定义舵机5控制口 | |
339 | servo6.attach(11);//定义舵机6控制口 | |
340 | servo7.attach(9);//定义舵机7控制口 | |
341 | servo8.attach(10);//定义舵机8控制口 | |
342 | Serial.begin(9600);//串口波特率设置为9600 bps | |
343 | Init_Steer(); | |
344 | } | |
345 | /* | |
346 | ********************************************************************************************************* | |
347 | ** 函数名称 :loop() | |
348 | ** 函数功能 :主函数 | |
349 | ** 入口参数 :无 | |
350 | ** 出口参数 :无 | |
351 | ********************************************************************************************************* | |
352 | */ | |
353 | void Cruising_Mod()//模式功能切换函数 | |
354 | { | |
355 | ||
356 | if(Pre_Cruising_Flag != Cruising_Flag) | |
357 | { | |
358 | if(Pre_Cruising_Flag != 0) | |
359 | { | |
360 | MOTOR_GO_STOP; | |
361 | } | |
362 | ||
363 | Pre_Cruising_Flag = Cruising_Flag; | |
364 | } | |
365 | switch(Cruising_Flag) | |
366 | { | |
367 | ||
368 | case 2:FollowLine(); return;//巡线模式 | |
369 | case 3:Avoiding(); return;//避障模式 | |
370 | case 4:Avoid_wave();return;//超声波避障模式 | |
371 | case 5:Send_Distance();//超声波距离PC端显示 | |
372 | default:return; | |
373 | } | |
374 | ||
375 | } | |
376 | ||
377 | void loop() | |
378 | { | |
379 | while(1) | |
380 | { | |
381 | Get_uartdata(); | |
382 | UartTimeoutCheck(); | |
383 | Cruising_Mod(); | |
384 | } | |
385 | } | |
386 | ||
387 | ||
388 | ||
389 | /* | |
390 | ********************************************************************************************************* | |
391 | ** 函数名称 :Communication_Decode() | |
392 | ** 函数功能 :串口命令解码 | |
393 | ** 入口参数 :无 | |
394 | ** 出口参数 :无 | |
395 | ********************************************************************************************************* | |
396 | */ | |
397 | void Communication_Decode() | |
398 | { | |
399 | if(buffer[0]==0x00) | |
400 | { | |
401 | switch(buffer[1]) //电机命令 | |
402 | { | |
403 | case 0x01:MOTOR_GO_FORWARD; return; | |
404 | case 0x02:MOTOR_GO_BACK; return; | |
405 | case 0x03:MOTOR_GO_LEFT; return; | |
406 | case 0x04:MOTOR_GO_RIGHT; return; | |
407 | case 0x00:MOTOR_GO_STOP; return; | |
408 | default: return; | |
409 | } | |
410 | } | |
411 | else if(buffer[0]==0x01)//舵机命令 | |
412 | { | |
413 | if(buffer[2]<1)return; | |
414 | switch(buffer[1]) | |
415 | { | |
416 | // case 0x01:angle1 = buffer[2];servo1.write(angle1);return; | |
417 | // case 0x02:angle2 = buffer[2];servo2.write(angle2);return; | |
418 | case 0x01:if(buffer[2]>170)return;angle3 = buffer[2];servo3.write(angle3);return; | |
419 | case 0x02:if(buffer[2]>170)return;angle4 = buffer[2];servo4.write(angle4);return; | |
420 | case 0x03:if(buffer[2]>170)return;angle5 = buffer[2];servo5.write(angle5);return; | |
421 | case 0x04:if((buffer[2]<105)||(buffer[2]>178))return;angle6 = buffer[2];servo6.write(angle6);return; | |
422 | case 0x07:if(buffer[2]>170)return;angle7 = buffer[2];servo7.write(angle7);return; | |
423 | case 0x08:if(buffer[2]>170)return;angle8 = buffer[2];servo8.write(angle8);return; | |
424 | default:return; | |
425 | } | |
426 | } | |
427 | ||
428 | else if(buffer[0]==0x02)//调速 | |
429 | { | |
430 | int i,j; | |
431 | if(buffer[2]>10)return; | |
432 | ||
433 | if(buffer[1]==0x01)//左侧调档 | |
434 | { | |
435 | i=buffer[2]; | |
436 | Left_Speed_Hold=Left_Speed[i] ; | |
437 | analogWrite(ENB,Left_Speed_Hold); | |
438 | } | |
439 | if(buffer[1]==0x02)//右侧调档 | |
440 | { | |
441 | j=buffer[2]; | |
442 | Right_Speed_Hold=Right_Speed[j] ; | |
443 | analogWrite(ENA,Right_Speed_Hold); | |
444 | }else return; | |
445 | } | |
446 | else if(buffer[0]==0x33)//读取舵机角度并赋值 | |
447 | { | |
448 | Init_Steer();return; | |
449 | } | |
450 | else if(buffer[0]==0x32)//保存命令 | |
451 | { | |
452 | // EEPROM.write(0x01,angle1); | |
453 | // EEPROM.write(0x02,angle2); | |
454 | EEPROM.write(0x03,angle3); | |
455 | EEPROM.write(0x04,angle4); | |
456 | EEPROM.write(0x05,angle5); | |
457 | EEPROM.write(0x06,angle6); | |
458 | EEPROM.write(0x07,angle7); | |
459 | EEPROM.write(0x08,angle8); | |
460 | return; | |
461 | } | |
462 | else if(buffer[0]==0x13)//模式切换开关 | |
463 | { | |
464 | switch(buffer[1]) | |
465 | { | |
466 | ||
467 | case 0x02: Cruising_Flag = 2; return;//巡线 | |
468 | case 0x03: Cruising_Flag = 3; return;//避障 | |
469 | case 0x04: Cruising_Flag = 4; return;//雷达避障 | |
470 | case 0x05: Cruising_Flag = 5; return;//超声波距离PC端显示 | |
471 | case 0x00: Cruising_Flag = 0; return;//正常模式 | |
472 | default:Cruising_Flag = 0; return;//正常模式 | |
473 | } | |
474 | } | |
475 | else if(buffer[0]==0x05) | |
476 | { | |
477 | switch(buffer[1]) // | |
478 | { | |
479 | case 0x00:Open_Light(); return; | |
480 | case 0x02:Close_Light(); return; | |
481 | default: return; | |
482 | } | |
483 | } | |
484 | else if(buffer[0]==0x40)//存储电机标志 | |
485 | { | |
486 | num=buffer[1]; | |
487 | EEPROM.write(0x10,num); | |
488 | } | |
489 | } | |
490 | /* | |
491 | ********************************************************************************************************* | |
492 | ** 函数名称 :Get_uartdata() | |
493 | ** 函数功能 :读取串口命令 | |
494 | ** 入口参数 :无 | |
495 | ** 出口参数 :无 | |
496 | ********************************************************************************************************* | |
497 | */ | |
498 | void Get_uartdata(void) | |
499 | { | |
500 | static int i; | |
501 | ||
502 | if (Serial.available() > 0) //判断串口缓冲器是否有数据装入 | |
503 | { | |
504 | serial_data = Serial.read();//读取串口 | |
505 | if(rec_flag==0) | |
506 | { | |
507 | if(serial_data==0xff) | |
508 | { | |
509 | rec_flag = 1; | |
510 | i = 0; | |
511 | Costtime = 0; | |
512 | } | |
513 | } | |
514 | else | |
515 | { | |
516 | if(serial_data==0xff) | |
517 | { | |
518 | rec_flag = 0; | |
519 | if(i==3) | |
520 | { | |
521 | Communication_Decode(); | |
522 | } | |
523 | i = 0; | |
524 | } | |
525 | else | |
526 | { | |
527 | buffer[i]=serial_data; | |
528 | i++; | |
529 | } | |
530 | } | |
531 | } | |
532 | } | |
533 | /* | |
534 | ********************************************************************************************************* | |
535 | ** 函数名称 :UartTimeoutCheck() | |
536 | ** 函数功能 :串口超时检测 | |
537 | ** 入口参数 :无 | |
538 | ** 出口参数 :无 | |
539 | ********************************************************************************************************* | |
540 | */ | |
541 | void UartTimeoutCheck(void) | |
542 | { | |
543 | if(rec_flag == 1) | |
544 | { | |
545 | Costtime++; | |
546 | if(Costtime == 100000) | |
547 | { | |
548 | rec_flag = 0; | |
549 | } | |
550 | } | |
551 | } |