Delta型腿机器狗全动作展示

机器谱

1. 功能说明
       本文示例将实现R322样机Delta型腿机器狗维持身体平衡、原地圆形摆动、原地踏步、蹲起、站立、前进、后退、转向、横向移动、斜向移动等功能。

2. 电子硬件
        本实验中采用了以下硬件：

主控板	Basra主控板（兼容Arduino Uno）‍
扩展板	Bigfish2.1扩展板‍
	SH-SR舵机扩展板
传感器	近红外传感器
	六轴陀螺仪
电池	7.4v锂电池、11.1V动力电池
其它	电压显示器

        电路连接说明：为了便于识别控制Delta型腿机器狗，我们先对机器狗的腿位置编号（如下图所示）：

        ① 硬件连接：

        ② 电压显示器与大电池连接：

        ③ 舵机接线位置：上面3个舵机分别连接在Bigfish扩展板的D4、D3、D8端口。

        Delta型腿机器狗每条腿有4个舵机，4条腿上总共有16个舵机，将这16个舵机分别连接在SH-SR舵机扩展板的舵机接口上。

1号腿 ：s1连接口9    s2连接口8    s3连接口5    s4连接口6
2号腿 ：s1连接口18    s2连接口19    s3连接口20    s4连接口21
3号腿 ：s1连接口0    s2连接口2    s3连接口1    s4连接口3
4号腿 ：s1连接口27    s2连接口25    s3连接口26    s4连接口24

3. 功能实现
       编程环境：Arduino 1.8.19
       下面提供一个Delta型腿机器狗全动作展示（维持身体平衡、原地圆形摆动、原地踏步、蹲起、站立、前进、后退、转向、横向移动、斜向移动）的参考例程（parallel_dog_display.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-06-07 https://www.robotway.com/
   
10. 
    
11.   ------------------------------*/
    
12. 
    
13. /*****
    
14. 
    
15.   Copyright 2017 Robot TIme
    
16. 
    
17.   全动作展示例程
    
18. 
    
19. *****/
    
20. 
    
21. 
    
22. #include "Tlc5940.h"
    
23. 
    
24. #include "tlc_servos.h"
    
25. 
    
26. #include <math.h>
    
27. 
    
28. 
    
29. #include "types.h"
    
30. 
    
31. #include "config.h"
    
32. 
    
33. 
    
34. // 相关函数声明
    
35. 
    
36. /***** 红外相关函数 *****/
    
37. 
    
38. void IRInit(); //红外初始化
    
39. 
    
40. void enableIR(); //红外使能
    
41. 
    
42. void disableIR(); //关闭红外
    
43. 
    
44. void updateIR(); //红外避障更新动作
    
45. 
    
46. /***** 平衡相关函数 *****/
    
47. 
    
48. void switchAdjustStat(uint stat); //切换平衡调节模式 不调节/原地调节/行进间调节
    
49. 
    
50. void readGyroSerial(); //读取陀螺仪串口消息
    
51. 
    
52. void adjustAct(); //平衡调节动作
    
53. 
    
54. /****** 腿部动作相关函数 *****/
    
55. 
    
56. void setTurnLeftFlag(bool flag); //修改左转状态标志位
    
57. 
    
58. void setTurnRightFlag(bool flag); //修改右转状态标志位
    
59. 
    
60. void leg1(); //更新1号腿（左前）位置
    
61. 
    
62. void leg2(); //更新2号腿（左后）位置
    
63. 
    
64. void leg3(); //更新3号腿（右前）位置
    
65. 
    
66. void leg4(); //更新4号腿（右后）位置
    
67. 
    
68. bool calc(Point3d p, bool leg1, bool leg2, bool leg3, bool leg4); //逆解计算函数
    
69. 
    
70. /***** 整机动作相关函数 *****/
    
71. 
    
72. void dogReset(Point3d initPos, uint waitTime); //复位动作
    
73. 
    
74. void dogInit(); //初始化动作
    
75. 
    
76. void upDown(float x, float y, float z1, float z2, uint times); //蹲起动作
    
77. 
    
78. void drawCircle(float ox, float oy, float z, float r, uint times); //原地圆形摆动动作
    
79. 
    
80. void stepping(float x, float y, float z1, float z2, uint times); // 原地踏步动作
    
81. 
    
82. void liftShoulder(uint height, uint times); //原地摆臂动作
    
83. 
    
84. 
    
85. //动作周期计数器
    
86. 
    
87. int cycleCount;
    
88. 
    
89. //复位计数器
    
90. 
    
91. void resetCycleCount()
    
92. 
    
93. {
    
94. 
    
95.   cycleCount = -1;
    
96. 
    
97. }
    
98. 
    
99. void updateCycleCount()
    
100. 
     
101. {
     
102. 
     
103.   cycleCount++;
     
104. 
     
105. }
     
106. 
     
107. 
     
108. //当前运动状态
     
109. 
     
110. dogMode currentMode;
     
111. 
     
112. //切换运动状态
     
113. 
     
114. void setMode(dogMode mode)
     
115. 
     
116. {
     
117. 
     
118.   if (mode == currentMode) return;
     
119. 
     
120.   if (mode == DOG_MODE_TURN_LEFT)
     
121. 
     
122.   {
     
123. 
     
124.     setTurnLeftFlag(true);
     
125. 
     
126.     setTurnRightFlag(false);
     
127. 
     
128.   } else if (mode == DOG_MODE_TURN_RIGHT)
     
129. 
     
130.   {
     
131. 
     
132.     setTurnLeftFlag(false);
     
133. 
     
134.     setTurnRightFlag(true);
     
135. 
     
136.   } else {
     
137. 
     
138.     setTurnLeftFlag(false);
     
139. 
     
140.     setTurnRightFlag(false);
     
141. 
     
142.   }
     
143. 
     
144. 
     
145.   if (mode == DOG_MODE_BACK) //后退时关闭红外传感器
     
146. 
     
147.   {
     
148. 
     
149.     disableIR();
     
150. 
     
151.   } else if (mode == DOG_MODE_STOP) //静止后开始原地姿态调节
     
152. 
     
153.   {
     
154. 
     
155.     switchAdjustStat(ADJUST_STAT_LEG);
     
156. 
     
157.     dogReset({0, 0, Leg_Init_Z_Pos}, 200);
     
158. 
     
159.   }
     
160. 
     
161.   currentMode = mode;
     
162. 
     
163. }
     
164. 
     
165. 
     
166. void updateMode()
     
167. 
     
168. {
     
169. 
     
170.   if (cycleCount == MOTION_TIMES + 1) setMode(DOG_MODE_BACK);
     
171. 
     
172.   if (cycleCount == 3 * MOTION_TIMES) setMode(DOG_MODE_LEFT);
     
173. 
     
174.   if (cycleCount == 4 * MOTION_TIMES) setMode(DOG_MODE_RIGHT);
     
175. 
     
176.   if (cycleCount == 5 * MOTION_TIMES) setMode(DOG_MODE_RIGHT_FRONT);
     
177. 
     
178.   if (cycleCount == 6 * MOTION_TIMES) setMode(DOG_MODE_RIGHT_BACK);
     
179. 
     
180.   if (cycleCount == 7 * MOTION_TIMES) setMode(DOG_MODE_LEFT_BACK);
     
181. 
     
182.   if (cycleCount == 8 * MOTION_TIMES) setMode(DOG_MODE_LEFT_FRONT);
     
183. 
     
184.   if (cycleCount == 9 * MOTION_TIMES) setMode(DOG_MODE_TURN_LEFT);
     
185. 
     
186.   if (cycleCount == 10 * MOTION_TIMES) setMode(DOG_MODE_TURN_RIGHT);
     
187. 
     
188.   if (cycleCount == 11 * MOTION_TIMES) setMode(DOG_MODE_STOP);
     
189. 
     
190. }
     
191. 
     
192. 
     
193. void setup()
     
194. 
     
195. {
     
196. 
     
197.   //陀螺仪连接串口，波特率115200
     
198. 
     
199.   Serial.begin(115200);
     
200. 
     
201. 
     
202.   //舵机驱动板初始化
     
203. 
     
204.   Tlc.init(0);
     
205. 
     
206.   tlc_initServos();   // Note: this will drop the PWM freqency down to 50Hz.
     
207. 
     
208. 
     
209.   //红外传感器初始化
     
210. 
     
211.   IRInit();
     
212. 
     
213. 
     
214.   //大狗身体初始化
     
215. 
     
216.   dogInit();
     
217. 
     
218. 
     
219.   //原地摆臂动作一次
     
220. 
     
221.   liftShoulder(40, 1);
     
222. 
     
223.   delay(500);
     
224. 
     
225.   //原地做圆形摆动3周
     
226. 
     
227.   drawCircle(0, 0, -120, 60, 3);
     
228. 
     
229.   delay(500);
     
230. 
     
231.   //原地蹲起3次
     
232. 
     
233.   upDown(0, 0, -160, -90, 3);
     
234. 
     
235.   delay(500);
     
236. 
     
237.   //原地踏步6次
     
238. 
     
239.   stepping(0, 0, -150, -100, 6);
     
240. 
     
241.   delay(500);
     
242. 
     
243. 
     
244. 
     
245.   resetCycleCount();
     
246. 
     
247.   enableIR();
     
248. 
     
249.   switchAdjustStat(ADJUST_STAT_TRACK);
     
250. 
     
251.   setMode(DOG_MODE_FRONT);
     
252. 
     
253. }
     
254. 
     
255. 
     
256. void loop()
     
257. 
     
258. {
     
259. 
     
260.   //姿态调节
     
261. 
     
262.   adjustAct();
     
263. 
     
264. 
     
265.   if (currentMode == DOG_MODE_STOP) return; //静止模式不进行后续动作
     
266. 
     
267. 
     
268.   updateMode(); //切换运动模式
     
269. 
     
270.   //计算4条腿运动位置
     
271. 
     
272.   leg1();
     
273. 
     
274.   leg4();
     
275. 
     
276.   leg2();
     
277. 
     
278.   leg3();
     
279. 
     
280.   //更新所有舵机位置
     
281. 
     
282.   Tlc.update();
     
283. 
     
284.   //检测红外传感器信息
     
285. 
     
286.   updateIR();
     
287. 
     
288. 
     
289. }
     
290. 
     
291. 
     
292. //串口与陀螺仪通信
     
293. 
     
294. void serialEvent() {
     
295. 
     
296.   readGyroSerial();
     
297. 
     
298. }

复制代码

4. 资料下载
资料内容：
①程序源代码
②样机3D文件
资料下载地址：Delta型腿机器狗-全动作展示 https://www.robotway.com/h-col-242.html