机器人
六足蜘蛛机器人有六个腿,每个腿有两个舵机控制,一个负责水平方向的前后运动,一个负责竖直方向的上下运动(抬腿落腿)。如下图所示
驱动板:
该驱动板为TOROBOT公司产品,编程时与电脑串口连接,靠串口命令驱动舵机工作。见下图及其详细说明书。本机器人有12个舵机,需要占用驱动板的12个S口。
//TOROBOT 驱动板驱动六足蜘蛛机器人
1 //脉冲宽度1166对应60度,右边向上/向后,左边向下/向前
2
3 //脉冲宽度1500对应90度,舵机的初始位置
4
5 //脉冲宽度1833对应120度,右边向下/向前,左边向上/向后
6
7
8
9 int i,j,k;
10
11 int pos1=1166; //对应60度
12
13 int pos2=1277; //对应70度
14
15 int pos3=1388; //对应80度
16
17 int pos4=1500; //对应90度
18
19 int pos5=1611; //对应100度
20
21 int pos6=1722; //对应110度
22
23 int pos7=1833; //对应120度
24
25
26
27 void setup()
28
29 {
30
31 Serial.begin(9600);
32
33 init_robot(); //机器人状态初始化
34
35 }
36
37
38
39 //腿ABC分别对应右前右后左中,上下电机分别接S1,S2,S3,前后电机分别接S4,S5,S6
40
41 //腿DEF分别对应左前左后右中,上下电机分别接S7,S8,S9,前后电机分别接S10,S11,S12
42
43
44
45 void ABC_up() //ABC脚抬起
46
47 {
48
49 Serial.print("#1P1500#2P1500#3P1500T1000\r\n");
50
51 // S1号S2号S3号舵机分别旋转到角度为90度的位置,使用时间1000ms
52
53 delay(1000); // 延时1000ms,舵机刚好执行完上一条命令
54
55 }
56
57
58
59 void ABC_down() //ABC脚落下
60
61 {
62
63 Serial.print("#1P1833#2P1833#3P1166T1000\r\n");
64
65 // S1号S2号S3号舵机分别旋转到角度为120度,120度,60度的位置,使用时间1000ms
66
67 delay(1000); // 延时1000ms,舵机刚好执行完上一条命令
68
69 }
70
71
72
73 void ABC_forward() //AB,C脚往前转到120度和30度
74
75 {
76
77 Serial.print("#4P1833#5P1833#6P1166T1000\r\n");
78
79 // S4号S5号S6号舵机分别旋转到角度分为120,120,60度的位置,使用时间1000ms
80
81 delay(1000); // 延时1000ms,舵机刚好执行完上一条命令
82
83 }
84
85
86
87 void ABC_turn_left() //AB,C脚往前转小角度100度和60度,往右拐,AB旋转角度变小,C不变
88
89 {
90
91 Serial.print("#4P1611#5P1611#6P1166T1000\r\n");
92
93 // S4号S5号S6号舵机分别旋转到角度分为110,110,60度的位置,使用时间1000ms
94
95 delay(1000); // 延时1000ms,舵机刚好执行完上一条命令
96
97 }
98
99
100
101 void ABC_turn_right() //AB,C脚往前转小角度120度和80度,往左拐,AB不变,C旋转角度变小
102
103 {
104
105 Serial.print("#4P1833#5P1833#6P1388T1000\r\n");
106
107 // S4号S5号S6号舵机分别旋转到角度分为120,120,70度的位置,使用时间1000ms
108
109 delay(1000); // 延时1000ms,舵机刚好执行完上一条命令
110
111 }
112
113
114
115 void ABC_backward() //ABC脚往后转
116
117 {
118
119 Serial.print("#4P1500#5P1500#6P1500T1000\r\n");
120
121 // S4号S5号S6号舵机分别旋转到角度分别为90,90,90度的位置,使用时间1000ms
122
123 delay(1000); // 延时1000ms,舵机刚好执行完上一条命令
124
125 }
126
127
128
129 void DEF_up() //DEF脚抬起
130
131 {
132
133 Serial.print("#7P1500#8P1500#9P1500T1000\r\n");
134
135 // S7号S8号S9号舵机分别旋转到角度分别为90,90,90度的位置,使用时间1000ms
136
137 delay(1000); // 延时1000ms,舵机刚好执行完上一条命令
138
139 }
140
141
142
143 void DEF_down() //DEF脚落下
144
145 {
146
147 Serial.print("#7P1166#8P1166#9P1833T1000\r\n");
148
149 // S7号S8号S9号舵机分别旋转到角度分别为60,60,120度的位置,使用时间1000ms
150
151 delay(1000); // 延时1000ms,舵机刚好执行完上一条命令
152
153 }
154
155
156
157 void DEF_forward() //DE,F脚往前转到60度和120度
158
159 {
160
161 Serial.print("#10P1166#11P1166#12P1833T1000\r\n");
162
163 // S10号S11号S12号舵机分别旋转到脉宽为60,60,120度的位置,使用时间1000ms
164
165 delay(1000); // 延时1000ms,舵机刚好执行完上一条命令
166
167 }
168
169
170
171 void DEF_turn_left() //DE,F脚往前转到60度和100度,DE旋转角度不变,F旋转角度变小
172
173 {
174
175 Serial.print("#10P1166#11P1166#12P1611T1000\r\n");
176
177 // S10号S11号S12号舵机分别旋转到脉宽为60,60,110度的位置,使用时间1000ms
178
179 delay(1000); // 延时1000ms,舵机刚好执行完上一条命令
180
181 }
182
183
184
185 void DEF_turn_right() //DE,F脚往前转到70度和120度,DE旋转角度变小,F旋转角度不变
186
187 {
188
189 Serial.print("#10P1388#11P1388#12P1833T1000\r\n");
190
191 // S10号S11号S12号舵机分别旋转到脉宽为80,80,120度的位置,使用时间1000ms
192
193 delay(1000); // 延时1000ms,舵机刚好执行完上一条命令
194
195 }
196
197
198
199
200
201 void DEF_backward() //DEF脚往后转
202
203 {
204
205 Serial.print("#10P1500#11P1500#12P1500T1000\r\n");
206
207 // S10号S11号S12号舵机分别旋转到角度分别为90,90,90度的的位置,使用时间1000ms
208
209 delay(1000); // 延时1000ms,舵机刚好执行完上一条命令
210
211 }
212
213
214
215 void forward_one_step_by_ABC() //前进1步,用ABC
216
217 {
218
219 ABC_up(); //右抬腿
220
221 delay(1000);
222
223
224
225 ABC_forward(); //右前进
226
227 delay(1000);
228
229
230
231 ABC_down(); //右落腿
232
233 delay(1000);
234
235
236
237 DEF_up(); //左抬腿
238
239 delay(1000);
240
241
242
243 ABC_backward(); //右后退
244
245 delay(1000);
246
247
248
249 DEF_down(); //左落腿
250
251 delay(1000);
252
253 }
254
255
256
257 void forward_one_step_by_DEF() //前进1步,用DEF
258
259 {
260
261 DEF_up(); //左抬腿
262
263 delay(1000);
264
265
266
267 DEF_forward(); //左前进
268
269 delay(1000);
270
271
272
273 DEF_down(); //左落腿
274
275 delay(1000);
276
277
278
279 ABC_up(); //右抬腿
280
281 delay(1000);
282
283
284
285 DEF_backward(); //左后退
286
287 delay(1000);
288
289
290
291 ABC_down(); //右落腿
292
293 delay(1000);
294
295 }
296
297
298
299 void init_robot() //不能用init命名函数
300
301 {
302
303 Serial.print("#1P1833#2P1833#3P1166T1000\r\n"); //ABC腿放下
304
305 delay(1000); // 延时1000ms,舵机刚好执行完上一条命令
306
307 Serial.print("#7P1166#8P1166#9P1833T1000\r\n"); //DEF腿放下
308
309 delay(1000); // 延时1000ms,舵机刚好执行完上一条命令
310
311
312
313 Serial.print("#1P1500#2P1500#3P1500T1000\r\n"); //ABC腿抬起
314
315 delay(1000); // 延时1000ms,舵机刚好执行完上一条命令
316
317 Serial.print("#4P1500#5P1500#61500T1000\r\n"); //ABC腿转到中间位置
318
319 delay(1000); // 延时1000ms,舵机刚好执行完上一条命令
320
321 Serial.print("#1P1833#2P1833#3P1166T1000\r\n"); //ABC腿放下
322
323 delay(1000); // 延时1000ms,舵机刚好执行完上一条命令
324
325
326
327 Serial.print("#7P1500#8P1500#9P1500T1000\r\n"); //DEF腿抬起
328
329 delay(1000); // 延时1000ms,舵机刚好执行完上一条命令
330
331 Serial.print("#10P1500#11P1500#12P1500T1000\r\n"); //DEF腿转到中间位置
332
333 delay(1000); // 延时1000ms,舵机刚好执行完上一条命令
334
335 Serial.print("#7P1166#8P1166#9P1833T1000\r\n"); //DEF腿放下
336
337 delay(1000); // 延时1000ms,舵机刚好执行完上一条命令
338
339 }
340
341
342
343 void turn_left_one_step_by_ABC() //左转1步,用ABC
344
345 {
346
347 ABC_up(); //右抬腿
348
349 delay(1000);
350
351
352
353 ABC_turn_left(); //左拐
354
355 delay(1000);
356
357
358
359 ABC_down(); //右落腿
360
361 delay(1000);
362
363
364
365 DEF_up(); //左抬腿
366
367 delay(1000);
368
369
370
371 ABC_backward(); //右后退
372
373 delay(1000);
374
375
376
377 DEF_down(); //左落腿
378
379 delay(1000);
380
381 }
382
383
384
385 void turn_left_one_step_by_DEF() //左转1步,用DEF
386
387 {
388
389 DEF_up(); //左抬腿
390
391 delay(1000);
392
393
394
395 DEF_turn_left(); //左拐
396
397 delay(1000);
398
399
400
401 DEF_down(); //左落腿
402
403 delay(1000);
404
405
406
407 ABC_up(); //右抬腿
408
409 delay(1000);
410
411
412
413 DEF_backward(); //左后退
414
415 delay(1000);
416
417
418
419 ABC_down(); //右落腿
420
421 delay(1000);
422
423 }
424
425
426
427 void turn_right_one_step_by_ABC() //右转1步,用ABC
428
429 {
430
431 ABC_up(); //右抬腿
432
433 delay(1000);
434
435
436
437 ABC_turn_right(); //右拐
438
439 delay(1000);
440
441
442
443 ABC_down(); //右落腿
444
445 delay(1000);
446
447
448
449 DEF_up(); //左抬腿
450
451 delay(1000);
452
453
454
455 ABC_backward(); //右后退
456
457 delay(1000);
458
459
460
461 DEF_down(); //左落腿
462
463 delay(1000);
464
465 }
466
467
468
469 void turn_right_one_step_by_DEF() //右转1步,用DEF
470
471 {
472
473 DEF_up(); //左抬腿
474
475 delay(1000);
476
477
478
479 DEF_turn_right(); //右拐
480
481 delay(1000);
482
483
484
485 DEF_down(); //左落腿
486
487 delay(1000);
488
489
490
491 ABC_up(); //右抬腿
492
493 delay(1000);
494
495
496
497 DEF_backward(); //左后退
498
499 delay(1000);
500
501
502
503 ABC_down(); //右落腿
504
505 delay(1000);
506
507 }
508
509
510
511
512
513 void loop()
514
515 {
516
517 for(i=0;i<5;i++) //前行5步
518
519 {
520
521 forward_one_step_by_ABC();
522
523 delay(1000);
524
525 forward_one_step_by_DEF();
526
527 delay(1000);
528
529 }
530
531
532
533 for(j=0;j<5;j++) //左转5步
534
535 {
536
537 turn_left_one_step_by_ABC();
538
539 delay(1000);
540
541 turn_left_one_step_by_DEF();
542
543 delay(1000);
544
545 }
546
547
548
549 for(k=0;k<5;k++) //右转5步
550
551 {
552
553 turn_right_one_step_by_ABC();
554
555 delay(1000);
556
557 turn_right_one_step_by_DEF();
558
559 delay(1000);
560
561 }
562
563
564
565 while(1) 完成规定工作后机器人停止不动
566
567 {
568
569
570 }
571
572
573
574 }
运行效果:机器人前进5步,然后左转5步,然后右转5步。
更多的运行要求,可在此基础上自己设计程序。