机器人制作开源方案 | Delta型腿机器狗实现原地动作

机器谱

1. 功能说明
       本文示例将实现R322样机Delta型腿机器狗原地摆臂、原地圆形摆动、原地蹲起、原地踏步的功能。

原地摆臂

原地圆形摆动

原地蹲起

原地踏步

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

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

电路连接说明：Delta型腿机器狗的电路连接方式可参考上文【R322】Delta型腿机器狗-全动作展示【https://www.robotway.com/h-col-242.html】

3. 功能实现
       编程环境：Arduino 1.8.0
下面提供一个Delta型腿机器狗原地动作（原地摆臂、原地圆形摆动、原地蹲起、原地踏步）的参考例程，具体实验效果可参考演示视频。
① 原地摆臂例程（parallel_dog_liftLeg.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-26 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.   //原地摆臂动作10次后停止
     
220. 
     
221.   liftShoulder(40, 10);
     
222. 
     
223.   while(1);
     
224. 
     
225. }
     
226. 
     
227. 
     
228. void loop()
     
229. 
     
230. {
     
231. 
     
232. }

复制代码

② 原地圆形摆动例程（parallel_dog_drawCircle.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-26 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.   //原地做圆形摆动10周后停止
     
220. 
     
221.   drawCircle(0, 0, -120, 60, 10);
     
222. 
     
223.   while(1);
     
224. 
     
225. }
     
226. 
     
227. 
     
228. void loop()
     
229. 
     
230. {
     
231. 
     
232. }

复制代码

③ 原地蹲起例程（parallel_dog_updown.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-26 https://www.robotway.com/
   
10. 
    
11.   ------------------------------*/
    
12. 
    
13. /*****
    
14. 
    
15.   Copyright 2017 Robot TIme
    
16. 
    
17.   原地蹲起例程
    
18. 
    
19. *****/
    
20. 
    
21. #include "Tlc5940.h"
    
22. 
    
23. #include "tlc_servos.h"
    
24. 
    
25. #include <math.h>
    
26. 
    
27. #include "types.h"
    
28. 
    
29. #include "config.h"
    
30. 
    
31. // 相关函数声明
    
32. 
    
33. /***** 红外相关函数 *****/
    
34. 
    
35. void IRInit(); //红外初始化
    
36. 
    
37. void enableIR(); //红外使能
    
38. 
    
39. void disableIR(); //关闭红外
    
40. 
    
41. void updateIR(); //红外避障更新动作
    
42. 
    
43. /***** 平衡相关函数 *****/
    
44. 
    
45. void switchAdjustStat(uint stat); //切换平衡调节模式 不调节/原地调节/行进间调节
    
46. 
    
47. void readGyroSerial(); //读取陀螺仪串口消息
    
48. 
    
49. void adjustAct(); //平衡调节动作
    
50. 
    
51. /****** 腿部动作相关函数 *****/
    
52. 
    
53. void setTurnLeftFlag(bool flag); //修改左转状态标志位
    
54. 
    
55. void setTurnRightFlag(bool flag); //修改右转状态标志位
    
56. 
    
57. void leg1(); //更新1号腿（左前）位置
    
58. 
    
59. void leg2(); //更新2号腿（左后）位置
    
60. 
    
61. void leg3(); //更新3号腿（右前）位置
    
62. 
    
63. void leg4(); //更新4号腿（右后）位置
    
64. 
    
65. bool calc(Point3d p, bool leg1, bool leg2, bool leg3, bool leg4); //逆解计算函数
    
66. 
    
67. /***** 整机动作相关函数 *****/
    
68. 
    
69. void dogReset(Point3d initPos, uint waitTime); //复位动作
    
70. 
    
71. void dogInit(); //初始化动作
    
72. 
    
73. void upDown(float x, float y, float z1, float z2, uint times); //蹲起动作
    
74. 
    
75. void drawCircle(float ox, float oy, float z, float r, uint times); //原地圆形摆动动作
    
76. 
    
77. void stepping(float x, float y, float z1, float z2, uint times); // 原地踏步动作
    
78. 
    
79. void liftShoulder(uint height, uint times); //原地摆臂动作
    
80. 
    
81. //动作周期计数器
    
82. 
    
83. int cycleCount;
    
84. 
    
85. //复位计数器
    
86. 
    
87. void resetCycleCount()
    
88. 
    
89. {
    
90. 
    
91.   cycleCount = -1;
    
92. 
    
93. }
    
94. 
    
95. void updateCycleCount()
    
96. 
    
97. {
    
98. 
    
99.   cycleCount++;
    
100. 
     
101. }
     
102. 
     
103. //当前运动状态
     
104. 
     
105. dogMode currentMode;
     
106. 
     
107. //切换运动状态
     
108. 
     
109. void setMode(dogMode mode)
     
110. 
     
111. {
     
112. 
     
113.   if (mode == currentMode) return;
     
114. 
     
115.   if (mode == DOG_MODE_TURN_LEFT)
     
116. 
     
117.   {
     
118. 
     
119.     setTurnLeftFlag(true);
     
120. 
     
121.     setTurnRightFlag(false);
     
122. 
     
123.   } else if (mode == DOG_MODE_TURN_RIGHT)
     
124. 
     
125.   {
     
126. 
     
127.     setTurnLeftFlag(false);
     
128. 
     
129.     setTurnRightFlag(true);
     
130. 
     
131.   } else {
     
132. 
     
133.     setTurnLeftFlag(false);
     
134. 
     
135.     setTurnRightFlag(false);
     
136. 
     
137.   }
     
138. 
     
139.   if (mode == DOG_MODE_BACK) //后退时关闭红外传感器
     
140. 
     
141.   {
     
142. 
     
143.     disableIR();
     
144. 
     
145.   } else if (mode == DOG_MODE_STOP) //静止后开始原地姿态调节
     
146. 
     
147.   {
     
148. 
     
149.     switchAdjustStat(ADJUST_STAT_LEG);
     
150. 
     
151.     dogReset({0, 0, Leg_Init_Z_Pos}, 200);
     
152. 
     
153.   }
     
154. 
     
155.   currentMode = mode;
     
156. 
     
157. }
     
158. 
     
159. void updateMode()
     
160. 
     
161. {
     
162. 
     
163.   if (cycleCount == MOTION_TIMES + 1) setMode(DOG_MODE_BACK);
     
164. 
     
165.   if (cycleCount == 3 * MOTION_TIMES) setMode(DOG_MODE_LEFT);
     
166. 
     
167.   if (cycleCount == 4 * MOTION_TIMES) setMode(DOG_MODE_RIGHT);
     
168. 
     
169.   if (cycleCount == 5 * MOTION_TIMES) setMode(DOG_MODE_RIGHT_FRONT);
     
170. 
     
171.   if (cycleCount == 6 * MOTION_TIMES) setMode(DOG_MODE_RIGHT_BACK);
     
172. 
     
173.   if (cycleCount == 7 * MOTION_TIMES) setMode(DOG_MODE_LEFT_BACK);
     
174. 
     
175.   if (cycleCount == 8 * MOTION_TIMES) setMode(DOG_MODE_LEFT_FRONT);
     
176. 
     
177.   if (cycleCount == 9 * MOTION_TIMES) setMode(DOG_MODE_TURN_LEFT);
     
178. 
     
179.   if (cycleCount == 10 * MOTION_TIMES) setMode(DOG_MODE_TURN_RIGHT);
     
180. 
     
181.   if (cycleCount == 11 * MOTION_TIMES) setMode(DOG_MODE_STOP);
     
182. 
     
183. }
     
184. 
     
185. void setup()
     
186. 
     
187. {
     
188. 
     
189.   //陀螺仪连接串口，波特率115200
     
190. 
     
191.   Serial.begin(115200);
     
192. 
     
193.   //舵机驱动板初始化
     
194. 
     
195.   Tlc.init(0);
     
196. 
     
197.   tlc_initServos();   // Note: this will drop the PWM freqency down to 50Hz.
     
198. 
     
199.   //红外传感器初始化
     
200. 
     
201.   IRInit();
     
202. 
     
203.   //大狗身体初始化
     
204. 
     
205.   dogInit();
     
206. 
     
207.   //原地蹲起10次后停止
     
208. 
     
209.   upDown(0, 0, -160, -90, 10);
     
210. 
     
211.   while(1);
     
212. 
     
213. }
     
214. 
     
215. void loop()
     
216. 
     
217. {
     
218. 
     
219. }

复制代码

④ 原地踏步例程（parallel_dog_stepping.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-26 https://www.robotway.com/
   
10. 
    
11.   ------------------------------*/
    
12. 
    
13. /*****
    
14. 
    
15.   Copyright 2017 Robot TIme
    
16. 
    
17.   原地蹲起例程
    
18. 
    
19. *****/
    
20. 
    
21. #include "Tlc5940.h"
    
22. 
    
23. #include "tlc_servos.h"
    
24. 
    
25. #include <math.h>
    
26. 
    
27. #include "types.h"
    
28. 
    
29. #include "config.h"
    
30. 
    
31. // 相关函数声明
    
32. 
    
33. /***** 红外相关函数 *****/
    
34. 
    
35. void IRInit(); //红外初始化
    
36. 
    
37. void enableIR(); //红外使能
    
38. 
    
39. void disableIR(); //关闭红外
    
40. 
    
41. void updateIR(); //红外避障更新动作
    
42. 
    
43. /***** 平衡相关函数 *****/
    
44. 
    
45. void switchAdjustStat(uint stat); //切换平衡调节模式 不调节/原地调节/行进间调节
    
46. 
    
47. void readGyroSerial(); //读取陀螺仪串口消息
    
48. 
    
49. void adjustAct(); //平衡调节动作
    
50. 
    
51. /****** 腿部动作相关函数 *****/
    
52. 
    
53. void setTurnLeftFlag(bool flag); //修改左转状态标志位
    
54. 
    
55. void setTurnRightFlag(bool flag); //修改右转状态标志位
    
56. 
    
57. void leg1(); //更新1号腿（左前）位置
    
58. 
    
59. void leg2(); //更新2号腿（左后）位置
    
60. 
    
61. void leg3(); //更新3号腿（右前）位置
    
62. 
    
63. void leg4(); //更新4号腿（右后）位置
    
64. 
    
65. bool calc(Point3d p, bool leg1, bool leg2, bool leg3, bool leg4); //逆解计算函数
    
66. 
    
67. /***** 整机动作相关函数 *****/
    
68. 
    
69. void dogReset(Point3d initPos, uint waitTime); //复位动作
    
70. 
    
71. void dogInit(); //初始化动作
    
72. 
    
73. void upDown(float x, float y, float z1, float z2, uint times); //蹲起动作
    
74. 
    
75. void drawCircle(float ox, float oy, float z, float r, uint times); //原地圆形摆动动作
    
76. 
    
77. void stepping(float x, float y, float z1, float z2, uint times); // 原地踏步动作
    
78. 
    
79. void liftShoulder(uint height, uint times); //原地摆臂动作
    
80. 
    
81. //动作周期计数器
    
82. 
    
83. int cycleCount;
    
84. 
    
85. //复位计数器
    
86. 
    
87. void resetCycleCount()
    
88. 
    
89. {
    
90. 
    
91.   cycleCount = -1;
    
92. 
    
93. }
    
94. 
    
95. void updateCycleCount()
    
96. 
    
97. {
    
98. 
    
99.   cycleCount++;
    
100. 
     
101. }
     
102. 
     
103. //当前运动状态
     
104. 
     
105. dogMode currentMode;
     
106. 
     
107. //切换运动状态
     
108. 
     
109. void setMode(dogMode mode)
     
110. 
     
111. {
     
112. 
     
113.   if (mode == currentMode) return;
     
114. 
     
115.   if (mode == DOG_MODE_TURN_LEFT)
     
116. 
     
117.   {
     
118. 
     
119.     setTurnLeftFlag(true);
     
120. 
     
121.     setTurnRightFlag(false);
     
122. 
     
123.   } else if (mode == DOG_MODE_TURN_RIGHT)
     
124. 
     
125.   {
     
126. 
     
127.     setTurnLeftFlag(false);
     
128. 
     
129.     setTurnRightFlag(true);
     
130. 
     
131.   } else {
     
132. 
     
133.     setTurnLeftFlag(false);
     
134. 
     
135.     setTurnRightFlag(false);
     
136. 
     
137.   }
     
138. 
     
139.   if (mode == DOG_MODE_BACK) //后退时关闭红外传感器
     
140. 
     
141.   {
     
142. 
     
143.     disableIR();
     
144. 
     
145.   } else if (mode == DOG_MODE_STOP) //静止后开始原地姿态调节
     
146. 
     
147.   {
     
148. 
     
149.     switchAdjustStat(ADJUST_STAT_LEG);
     
150. 
     
151.     dogReset({0, 0, Leg_Init_Z_Pos}, 200);
     
152. 
     
153.   }
     
154. 
     
155.   currentMode = mode;
     
156. 
     
157. }
     
158. 
     
159. void updateMode()
     
160. 
     
161. {
     
162. 
     
163.   if (cycleCount == MOTION_TIMES + 1) setMode(DOG_MODE_BACK);
     
164. 
     
165.   if (cycleCount == 3 * MOTION_TIMES) setMode(DOG_MODE_LEFT);
     
166. 
     
167.   if (cycleCount == 4 * MOTION_TIMES) setMode(DOG_MODE_RIGHT);
     
168. 
     
169.   if (cycleCount == 5 * MOTION_TIMES) setMode(DOG_MODE_RIGHT_FRONT);
     
170. 
     
171.   if (cycleCount == 6 * MOTION_TIMES) setMode(DOG_MODE_RIGHT_BACK);
     
172. 
     
173.   if (cycleCount == 7 * MOTION_TIMES) setMode(DOG_MODE_LEFT_BACK);
     
174. 
     
175.   if (cycleCount == 8 * MOTION_TIMES) setMode(DOG_MODE_LEFT_FRONT);
     
176. 
     
177.   if (cycleCount == 9 * MOTION_TIMES) setMode(DOG_MODE_TURN_LEFT);
     
178. 
     
179.   if (cycleCount == 10 * MOTION_TIMES) setMode(DOG_MODE_TURN_RIGHT);
     
180. 
     
181.   if (cycleCount == 11 * MOTION_TIMES) setMode(DOG_MODE_STOP);
     
182. 
     
183. }
     
184. 
     
185. void setup()
     
186. 
     
187. {
     
188. 
     
189.   //陀螺仪连接串口，波特率115200
     
190. 
     
191.   Serial.begin(115200);
     
192. 
     
193.   //舵机驱动板初始化
     
194. 
     
195.   Tlc.init(0);
     
196. 
     
197.   tlc_initServos();   // Note: this will drop the PWM freqency down to 50Hz.
     
198. 
     
199.   //红外传感器初始化
     
200. 
     
201.   IRInit();
     
202. 
     
203.   //大狗身体初始化
     
204. 
     
205.   dogInit();
     
206. 
     
207.   //原地蹲起10次后停止
     
208. 
     
209.   upDown(0, 0, -160, -90, 10);
     
210. 
     
211.   while(1);
     
212. 
     
213. }
     
214. 
     
215. void loop()
     
216. 
     
217. {
     
218. 
     
219. }

复制代码

4. 资料下载
资料内容：原地动作-程序源代码
资料下载地址：Delta型腿机器狗-原地动作【https://www.robotway.com/h-col-242.html】