串联仿生毛毛虫的制作分享

机器谱

1. 运动功能说明

该样机是一款纯关节串联的蠕虫形机器人，可以像虫子一样的上下蠕动前进，样子呈波浪状。

2. 结构说明

该样机由5个关节模组串联构成，结构比较单纯。

3. 运动功能实现

      本样机只需要让5个舵机按照一定的时序，做出波浪形摆动的动作，即可实现蠕动爬行。

舵机和关节模组的控制方法请参考如何驱动模拟舵机【https://www.robotway.com/h-col-129.html】，以及舵机关节模组【https://www.robotway.com/h-col-121.html】。

3.1 电子硬件      

在这个示例中，采用了以下硬件，请大家参考：

主控板	Basra（兼容Arduino Uno）
扩展板	Bigfish2.1
电池	7.4V锂电池

3.2 编写程序

编程环境：Arduino 1.8.19

（1）实现尾部关节运动（Tail_Swing.ino），代码如下：

1. /*------------------------------------------------------------------------------------
   
2. 
   
3.   版权说明：Copyright 2022 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 机器谱 2022-9-14 https://www.robotway.com/
   
10. 
    
11.   ------------------------------
    
12. 
    
13.   实验功能： 实现机器毛毛虫尾部关节运动。
    
14. 
    
15.   -----------------------------------------------------
    
16. 
    
17.   实验接线：尾部关节模组的舵机接D8。                                    
    
18. 
    
19. ------------------------------------------------------------------------------------*/
    
20. 
    
21. #include <Servo.h>
    
22. 
    
23. #include <MsTimer2.h>
    
24. 
    
25. int servoPort[5]={8,3,11,7,4}; //毛毛虫的舵机连接顺序及接口号，从尾巴开始。
    
26. 
    
27. Servo myServo[5];
    
28. 
    
29. long timeCount;
    
30. 
    
31. #define DELTATIME 10
    
32. 
    
33. 
    
34. void setup() {         
    
35. 
    
36.   Serial.begin(9600);      
    
37. 
    
38.   for(int i = 0; i < 5; i++)
    
39. 
    
40.     myServo[i].attach(servoPort[i]);   
    
41. 
    
42.   MsTimer2::set(DELTATIME, Timer);
    
43. 
    
44.   MsTimer2::start();
    
45. 
    
46.   delay(100);
    
47. 
    
48. }
    
49. 
    
50. 
    
51. 
    
52. void loop() {
    
53. 
    
54.   ServoMove(0,60,120,2000); //0号舵机在2000ms内从60°转到120°
    
55. 
    
56. }
    
57. 
    
58. 
    
59. void Timer() {
    
60. 
    
61.   timeCount++;
    
62. 
    
63. }
    
64. 
    
65. 
    
66. void ServoMove(int which, int start, int finish, long t)
    
67. 
    
68. {
    
69. 
    
70.   static int a;
    
71. 
    
72.   static long count = 0;
    
73. 
    
74.   static int i = 0;
    
75. 
    
76.   static boolean begin = true;
    
77. 
    
78. 
    
79.   if(begin){
    
80. 
    
81.     if( ( start - finish ) > 0 )
    
82. 
    
83.       a = -1;
    
84. 
    
85.     else
    
86. 
    
87.       a = 1;
    
88. 
    
89. 
    
90.     count = timeCount;
    
91. 
    
92.     begin = false;
    
93. 
    
94.   }
    
95. 
    
96.   else{
    
97. 
    
98.     if( ( timeCount - count ) < (t/DELTATIME) ){
    
99. 
    
100.       if( ( timeCount - count ) > ( i * (t/DELTATIME) / (abs(start-finish)) ) ){
     
101. 
     
102.         myServo[which].write( start + i * a );  
     
103. 
     
104.         delay(1);
     
105. 
     
106.         i++;     
     
107. 
     
108.         Serial.println(start + i * a);
     
109. 
     
110.       }
     
111. 
     
112.     }
     
113. 
     
114.     else{
     
115. 
     
116.       i = 0;
     
117. 
     
118.       begin = true;
     
119. 
     
120.       count = 0;
     
121. 
     
122.     }   
     
123. 
     
124.   }
     
125. 
     
126. }

复制代码

       此段程序之所以看起来那么复杂，是因为它搭建了一个程序框架，在这个框架中，大家可以通过改变参数，来决定哪个舵机运动，以及怎么运动，不需要重写整个程序。

（2）编写并烧录以下程序，它可以让毛毛虫全身的关节一起运动（Body_Swing.ino），代码如下：

1. /*------------------------------------------------------------------------------------
   
2. 
   
3.   版权说明：Copyright 2022 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 机器谱 2022-9-14 https://www.robotway.com/
   
10. 
    
11.   ------------------------------
    
12. 
    
13.   实验功能： 实现机器毛毛虫全身关节运动。
    
14. 
    
15.   -----------------------------------------------------
    
16. 
    
17.   实验接线：从尾巴开始，对应舵机接口依次D8、D3、D11、D7、D4。                                    
    
18. 
    
19. ------------------------------------------------------------------------------------*/
    
20. 
    
21. 
    
22. #include <Servo.h>
    
23. 
    
24. #include <MsTimer2.h>
    
25. 
    
26. 
    
27. int servoPort[5]={8,3,11,7,4};
    
28. 
    
29. Servo myServo[5];
    
30. 
    
31. long servoCount[5];
    
32. 
    
33. boolean begin[5] = {true,true,true,true,true};
    
34. 
    
35. boolean complete[5] = {false,false,false,false,false};
    
36. 
    
37. int direct[5] = {1,1,1,1,1};
    
38. 
    
39. int delta[5] = {0,0,0,0,0};
    
40. 
    
41. 
    
42. long timeCount;
    
43. 
    
44. #define DELTATIME 10
    
45. 
    
46. 
    
47. int up = 30; //关节起始角度
    
48. 
    
49. int down = 120; //关节终止角度
    
50. 
    
51. int turnTime = 1000; //关节运动时间
    
52. 
    
53. 
    
54. String inputString = "";         // a string to hold incoming data
    
55. 
    
56. boolean stringComplete = false;   // whether the string is complete
    
57. 
    
58. 
    
59. void setup() {         
    
60. 
    
61.   Serial.begin(9600);      
    
62. 
    
63.   for(int i = 0; i < 5; i++)
    
64. 
    
65.     myServo[i].attach(servoPort[i]);   
    
66. 
    
67.   MsTimer2::set(DELTATIME, Timer);
    
68. 
    
69.   MsTimer2::start();
    
70. 
    
71.   delay(100);
    
72. 
    
73. }
    
74. 
    
75. 
    
76. 
    
77. void loop() {
    
78. 
    
79.   static int phase[5] = {0,0,0,0,0};
    
80. 
    
81. 
    
82.   if (stringComplete) {
    
83. 
    
84.     Serial.println(inputString);
    
85. 
    
86. 
    
87.     up = inputString.substring(0,inputString.indexOf(',')).toInt();
    
88. 
    
89.     inputString = inputString.substring(inputString.indexOf(',')+1,inputString.length());
    
90. 
    
91. 
    
92.     down = inputString.substring(0,inputString.indexOf(',')).toInt();
    
93. 
    
94.     turnTime = inputString.substring(inputString.indexOf(',')+1,inputString.length()).toInt();
    
95. 
    
96. 
    
97.     Serial.println(up);
    
98. 
    
99.     Serial.println(down);
    
100. 
     
101.     Serial.println(turnTime);
     
102. 
     
103. 
     
104.     inputString = "";
     
105. 
     
106.     stringComplete = false;
     
107. 
     
108.   }
     
109. 
     
110. 
     
111.   if(phase[0] == 0)
     
112. 
     
113.     if(ServoMove(0,up,down,turnTime))
     
114. 
     
115.       phase[0] = 1;
     
116. 
     
117.   if(phase[0] == 1)
     
118. 
     
119.     if(ServoMove(0,down,up,turnTime))
     
120. 
     
121.       phase[0] = 0;
     
122. 
     
123. 
     
124.   if(phase[1] == 0)
     
125. 
     
126.     if(ServoMove(1,down,up,turnTime))
     
127. 
     
128.       phase[1] = 1;
     
129. 
     
130.   if(phase[1] == 1)
     
131. 
     
132.     if(ServoMove(1,up,down,turnTime))
     
133. 
     
134.       phase[1] = 0;
     
135. 
     
136. 
     
137.   if(phase[2] == 0)
     
138. 
     
139.     if(ServoMove(2,up,down,turnTime))
     
140. 
     
141.       phase[2] = 1;
     
142. 
     
143.   if(phase[2] == 1)
     
144. 
     
145.     if(ServoMove(2,down,up,turnTime))
     
146. 
     
147.       phase[2] = 0;
     
148. 
     
149. 
     
150.   if(phase[3] == 0)
     
151. 
     
152.     if(ServoMove(3,down,up,turnTime))
     
153. 
     
154.       phase[3] = 1;
     
155. 
     
156.   if(phase[3] == 1)
     
157. 
     
158.     if(ServoMove(3,up,down,turnTime))
     
159. 
     
160.       phase[3] = 0;
     
161. 
     
162. 
     
163.   if(phase[4] == 0)
     
164. 
     
165.     if(ServoMove(4,up,down,turnTime))
     
166. 
     
167.       phase[4] = 1;
     
168. 
     
169.   if(phase[4] == 1)
     
170. 
     
171.     if(ServoMove(4,down,up,turnTime))
     
172. 
     
173.       phase[4] = 0;
     
174. 
     
175. }
     
176. 
     
177. 
     
178. void Timer() {
     
179. 
     
180.   timeCount++;
     
181. 
     
182. }
     
183. 
     
184. 
     
185. void serialEvent() {
     
186. 
     
187.   while (Serial.available()) {
     
188. 
     
189.     char inChar = (char)Serial.read();
     
190. 
     
191.     inputString += inChar;
     
192. 
     
193.       if (inChar == '\n')
     
194. 
     
195.       stringComplete = true;
     
196. 
     
197.     }
     
198. 
     
199. }
     
200. 
     
201. 
     
202. boolean ServoMove(int which, int start, int finish, long t)
     
203. 
     
204. {
     
205. 
     
206.   if(begin[which]){
     
207. 
     
208.     if( ( start - finish ) > 0 )
     
209. 
     
210.       direct[which] = -1;
     
211. 
     
212.     else
     
213. 
     
214.       direct[which] = 1;
     
215. 
     
216. 
     
217.     servoCount[which] = timeCount;
     
218. 
     
219.     begin[which] = false;
     
220. 
     
221.     complete[which] = false;
     
222. 
     
223.   }
     
224. 
     
225.   else{
     
226. 
     
227.     if( ( timeCount - servoCount[which] ) < (t/DELTATIME) ){
     
228. 
     
229.       if( ( timeCount - servoCount[which] ) > ( delta[which] * (t/DELTATIME) / (abs(start-finish)) ) ){
     
230. 
     
231.         myServo[which].write( start + delta[which] * direct[which] );
     
232. 
     
233.         delay(1);
     
234. 
     
235.         delta[which]++;     
     
236. 
     
237.         //Serial.println(start + i * a);
     
238. 
     
239.       }
     
240. 
     
241.     }
     
242. 
     
243.     else{
     
244. 
     
245.       delta[which] = 0;
     
246. 
     
247.       begin[which] = true;
     
248. 
     
249.       servoCount[which] = 0;
     
250. 
     
251.       complete[which] = true;
     
252. 
     
253.     }   
     
254. 
     
255.   }
     
256. 
     
257. 
     
258.   return (complete[which]);
     
259. 
     
260. }

复制代码

烧录后，打开Serial Monitor，输入up,down,turnTime的值,可直接改变全部关节的运动参数，例如，输入50,100,2000。

       关节的运动状态会实时发生变化。其中50代表摆动的起始角度50°，100代表摆动的终止角度100°，2000代表运动持续时间是2000毫秒。

（2） 上一例程中，机器毛毛虫相邻关节的运动相位差为180°，根据样机的运动姿态分析，各个关节的运动相位差为90°左右的时候运动效应该比较好。

将相邻关节的运动相位差调整为90°（Worm_Go.ino），代码如下：

1. /*------------------------------------------------------------------------------------
   
2. 
   
3.   版权说明：Copyright 2022 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 机器谱 2022-9-14 https://www.robotway.com/
   
10. 
    
11.   ------------------------------
    
12. 
    
13.   实验功能： 实现机器毛毛虫向前蠕动爬行。
    
14. 
    
15.   -----------------------------------------------------
    
16. 
    
17.   实验接线：从尾巴开始，对应舵机接口依次D8、D3、D11、D7、D4。                                    
    
18. 
    
19. ------------------------------------------------------------------------------------*/
    
20. 
    
21. #include <Servo.h>
    
22. 
    
23. #include <MsTimer2.h>
    
24. 
    
25. 
    
26. int servoPort[5]={8,3,11,7,4};
    
27. 
    
28. Servo myServo[5];
    
29. 
    
30. long servoCount[5];
    
31. 
    
32. boolean begin[5] = {true,true,true,true,true};
    
33. 
    
34. boolean complete[5] = {false,false,false,false,false};
    
35. 
    
36. int direct[5] = {1,1,1,1,1};
    
37. 
    
38. int delta[5] = {0,0,0,0,0};
    
39. 
    
40. 
    
41. long timeCount;
    
42. 
    
43. #define DELTATIME 10
    
44. 
    
45. 
    
46. int up = 30;
    
47. 
    
48. int down = 120;
    
49. 
    
50. int turnTime = 1000;
    
51. 
    
52. 
    
53. String inputString = "";         // a string to hold incoming data
    
54. 
    
55. boolean stringComplete = false;   // whether the string is complete
    
56. 
    
57. 
    
58. void setup() {         
    
59. 
    
60.   Serial.begin(9600);      
    
61. 
    
62.   for(int i = 0; i < 5; i++)
    
63. 
    
64.   myServo[i].attach(servoPort[i]);   
    
65. 
    
66.   MsTimer2::set(DELTATIME, Timer);
    
67. 
    
68.   MsTimer2::start();
    
69. 
    
70.   delay(100);
    
71. 
    
72. }
    
73. 
    
74. 
    
75. void loop() {
    
76. 
    
77. static int phase[5] = {0,0,0,0,0};
    
78. 
    
79. 
    
80. if (stringComplete) {
    
81. 
    
82. Serial.println(inputString);
    
83. 
    
84. 
    
85. up = inputString.substring(0,inputString.indexOf(',')).toInt();
    
86. 
    
87. inputString = inputString.substring(inputString.indexOf(',')+1,inputString.length());
    
88. 
    
89. 
    
90. down = inputString.substring(0,inputString.indexOf(',')).toInt();
    
91. 
    
92. turnTime = inputString.substring(inputString.indexOf(',')+1,inputString.length()).toInt();
    
93. 
    
94. 
    
95. Serial.println(up);
    
96. 
    
97. Serial.println(down);
    
98. 
    
99. Serial.println(turnTime);
    
100. 
     
101. 
     
102. inputString = "";
     
103. 
     
104. stringComplete = false;
     
105. 
     
106. }
     
107. 
     
108. 
     
109. if(phase[0] == 0)
     
110. 
     
111. if(ServoMove(0,up,down,turnTime))
     
112. 
     
113. phase[0] = 1;
     
114. 
     
115. if(phase[0] == 1)
     
116. 
     
117. if(ServoMove(0,down,up,turnTime))
     
118. 
     
119. phase[0] = 0;
     
120. 
     
121. 
     
122. if(servoCount[0]>0 && (timeCount - servoCount[0])>(turnTime/(4*DELTATIME))){
     
123. 
     
124. if(phase[1] == 0)
     
125. 
     
126. if(ServoMove(1,up,down,turnTime))
     
127. 
     
128. phase[1] = 1;
     
129. 
     
130. if(phase[1] == 1)
     
131. 
     
132. if(ServoMove(1,down,up,turnTime))
     
133. 
     
134. phase[1] = 0;
     
135. 
     
136. }
     
137. 
     
138. 
     
139. if(servoCount[1]>0 && (timeCount - servoCount[1])>(turnTime/(4*DELTATIME))){
     
140. 
     
141. if(phase[2] == 0)
     
142. 
     
143. if(ServoMove(2,up,down,turnTime))
     
144. 
     
145. phase[2] = 1;
     
146. 
     
147. if(phase[2] == 1)
     
148. 
     
149. if(ServoMove(2,down,up,turnTime))
     
150. 
     
151. phase[2] = 0;
     
152. 
     
153. }
     
154. 
     
155. 
     
156. if(servoCount[2]>0 && (timeCount - servoCount[2])>(turnTime/(4*DELTATIME))){
     
157. 
     
158. if(phase[3] == 0)
     
159. 
     
160. if(ServoMove(3,up,down,turnTime))
     
161. 
     
162. phase[3] = 1;
     
163. 
     
164. if(phase[3] == 1)
     
165. 
     
166. if(ServoMove(3,down,up,turnTime))
     
167. 
     
168. phase[3] = 0;
     
169. 
     
170. }
     
171. 
     
172. 
     
173. if(servoCount[3]>0 && (timeCount - servoCount[3])>(turnTime/(4*DELTATIME))){
     
174. 
     
175. if(phase[4] == 0)
     
176. 
     
177. if(ServoMove(4,up,down,turnTime))
     
178. 
     
179. phase[4] = 1;
     
180. 
     
181. if(phase[4] == 1)
     
182. 
     
183. if(ServoMove(4,down,up,turnTime))
     
184. 
     
185. phase[4] = 0;
     
186. 
     
187. }
     
188. 
     
189. }
     
190. 
     
191. 
     
192. void Timer() {
     
193. 
     
194. timeCount++;
     
195. 
     
196. }
     
197. 
     
198. 
     
199. void serialEvent() {
     
200. 
     
201. while (Serial.available()) {
     
202. 
     
203. char inChar = (char)Serial.read();
     
204. 
     
205. inputString += inChar;
     
206. 
     
207.     if (inChar == '\n')
     
208. 
     
209. stringComplete = true;
     
210. 
     
211.   }
     
212. 
     
213. }
     
214. 
     
215. 
     
216. boolean ServoMove(int which, int start, int finish, long t)
     
217. 
     
218. {
     
219. 
     
220. if(begin[which]){
     
221. 
     
222. if( ( start - finish ) > 0 )
     
223. 
     
224. direct[which] = -1;
     
225. 
     
226. else
     
227. 
     
228. direct[which] = 1;
     
229. 
     
230. 
     
231. servoCount[which] = timeCount;
     
232. 
     
233. begin[which] = false;
     
234. 
     
235. complete[which] = false;
     
236. 
     
237. }
     
238. 
     
239. else{
     
240. 
     
241. if( ( timeCount - servoCount[which] ) < (t/DELTATIME) ){
     
242. 
     
243. if( ( timeCount - servoCount[which] ) > ( delta[which] * (t/DELTATIME) / (abs(start-finish)) ) ){
     
244. 
     
245. myServo[which].write( start + delta[which] * direct[which] );
     
246. 
     
247. delay(1);
     
248. 
     
249. delta[which]++;
     
250. 
     
251. //Serial.println(start + i * a);
     
252. 
     
253. }
     
254. 
     
255. }
     
256. 
     
257. else{
     
258. 
     
259. delta[which] = 0;
     
260. 
     
261. begin[which] = true;
     
262. 
     
263. //servoCount[which] = 0;
     
264. 
     
265. complete[which] = true;
     
266. 
     
267. }
     
268. 
     
269. }
     
270. 
     
271. 
     
272. return (complete[which]);
     
273. 
     
274. }

复制代码

       烧录后，打开Serial Monitor，输入up,down,turnTime的值，并实时观察运动变化。你可以通过这种方法调整机器人的运动姿态，直至对爬行效果满意为止。

4. 扩展样机

本样机没有设计左右摆动的结构，因此不能左右转向，有兴趣的朋友可以尝试为其加上左右摆动的结构，让它的运动方式更加丰富。

5. 资料下载
资料内容：
①样机3D文件
②样机例程源代码

资料下载链接https://www.robotway.com/h-col-149.html