單片機(8bit)的16路舵機調速分析與實現

// main.c

[cpp]view plaincopyprint?

1. #include1.H>
2. #include"ControlRobot.h"
3. #include
5. #defineDEBUG_PROTOCOL
7. typedefunsignedcharUCHAR8;
8. typedefunsignedintUINT16;
10. #undefTRUE
11. #undefFALSE
12. #defineTRUE1
13. #defineFALSE0
15. #defineMEMORY_MODEL
17. UINT16MEMORY_MODELdelayVar1;
18. UCHAR8MEMORY_MODELdelayVar2;
20. #defineBAUD_RATE0xF3
22. #defineUSED_PIN2
23. #definePIN_GROUP_10
24. #definePIN_GROUP_21
26. #defineGROUP_1_CONTROL_PINP0
27. #defineGROUP_2_CONTROL_PINP2
29. #defineSTEERING_ENGINE_CYCLE2000
30. #defineSTEERING_ENGINE_INIT_DELAY50
31. #defineSTEERING_ENGINE_FULL_CYCLE((STEERING_ENGINE_CYCLE)-(STEERING_ENGINE_INIT_DELAY))
33. volatileUCHAR8MEMORY_MODELg_angle[2][ROBOT_JOINT_AMOUNT];
34. volatilebitMEMORY_MODELg_fillingBufferIndex=0;
35. volatilebitMEMORY_MODELg_readyBufferIndex=1;
36. volatilebitMEMORY_MODELg_swapBuffer=FALSE;
38. volatileUINT16MEMORY_MODELg_diffAngle[USED_PIN][8];
39. volatileUCHAR8MEMORY_MODELg_diffAngleMask[USED_PIN][8]=
40. {
41. {
42. ROBOT_PIN_MASK_LEFT_SHOULDER_VERTICAL,
43. ROBOT_PIN_MASK_LEFT_SHOULDER_HORIZEN,
44. ROBOT_PIN_MASK_LEFT_ELBOW,
45. ROBOT_PIN_MASK_RIGHT_SHOULDER_VERTICAL,
46. ROBOT_PIN_MASK_RIGHT_SHOULDER_HORIZEN,
47. ROBOT_PIN_MASK_RIGHT_ELBOW,
48. ROBOT_PIN_MASK_LEFT_HIP_VERTICAL,
49. ROBOT_PIN_MASK_RIGHT_HIP_VERTICAL
50. },
51. {
52. ROBOT_PIN_MASK_LEFT_HIP_HORIZEN,
53. ROBOT_PIN_MASK_LEFT_KNEE,
54. ROBOT_PIN_MASK_LEFT_ANKLE,
55. ROBOT_PIN_MASK_LEFT_FOOT,
56. ROBOT_PIN_MASK_RIGHT_HIP_HORIZEN,
57. ROBOT_PIN_MASK_RIGHT_KNEE,
58. ROBOT_PIN_MASK_RIGHT_ANKLE,
59. ROBOT_PIN_MASK_RIGHT_FOOT
60. }
61. };
63. #ifdefDEBUG_PROTOCOL
64. sbitP10=P1^0;//正在填充交換區1
65. sbitP11=P1^1;//正在填充交換區2
66. sbitP12=P1^2;//交換區變換
67. sbitP13=P1^3;//協議是否正確
68. #endif
70. voidDelay10us(UINT16ntimes)
71. {
72. for(delayVar1=0;delayVar1
73. for(delayVar2=0;delayVar2<21;++delayVar2)
74. _nop_();
75. }
77. voidInitPwmPollint()
78. {
79. UCHAR8i;
80. UCHAR8j;
81. UCHAR8k;
82. UINT16temp;
84. for(i=0;i
85. {
86. for(j=0;j<7;++j)
87. {
88. for(k=j;k<8;++k)
89. {
90. if(g_diffAngle[i][j]>g_diffAngle[i][k])
91. {
92. temp=g_diffAngle[i][j];
93. g_diffAngle[i][j]=g_diffAngle[i][k];
94. g_diffAngle[i][k]=temp;
96. temp=g_diffAngleMask[i][j];
97. g_diffAngleMask[i][j]=g_diffAngleMask[i][k];
98. g_diffAngleMask[i][k]=temp;
99. }
100. }
101. }
102. }
104. for(i=0;i
105. {
106. for(j=0;j<8;++j)
107. {
108. if(INVALID_JOINT_VALUE==g_diffAngle[i][j])
109. {
110. g_diffAngle[i][j]=STEERING_ENGINE_FULL_CYCLE;
111. }
112. }
113. }
115. for(i=0;i
116. {
117. for(j=7;j>=1;--j)
118. {
119. g_diffAngle[i][j]=g_diffAngle[i][j]-g_diffAngle[i][j-1];
120. }
121. }
122. }
125. voidInitSerialPort()
126. {
127. AUXR=0x00;
128. ES=0;
129. TMOD=0x20;
130. SCON=0x50;
131. TH1=BAUD_RATE;
132. TL1=TH1;
133. PCON=0x80;
134. EA=1;
135. ES=1;
136. TR1=1;
137. }
139. voidOnSerialPort()interrupt4
140. {
141. staticUCHAR8previousChar=0;
142. staticUCHAR8currentChar=0;
143. staticUCHAR8counter=0;
145. if(RI)
146. {
147. RI=0;
148. currentChar=SBUF;
150. if(PROTOCOL_HEADER[0]==currentChar)//協議標志
151. {
152. previousChar=currentChar;
153. }
154. else
155. {
156. if(PROTOCOL_HEADER[0]==previousChar&&PROTOCOL_HEADER[1]==currentChar)//協議開始
157. {
158. counter=0;
159. previousChar=currentChar;
160. g_swapBuffer=FALSE;
161. }
162. elseif(PROTOCOL_END[0]==previousChar&&PROTOCOL_END[1]==currentChar)//協議結束
163. {
164. previousChar=currentChar;
166. if(ROBOT_JOINT_AMOUNT==counter)//協議接受正確
167. {
168. if(0==g_fillingBufferIndex)
169. {
170. g_readyBufferIndex=0;
171. g_fillingBufferIndex=1;
172. }
173. else
174. {
175. g_readyBufferIndex=1;
176. g_fillingBufferIndex=0;
177. }
179. g_swapBuffer=TRUE;
181. #ifdefDEBUG_PROTOCOL
182. P13=0;
183. #endif
184. }
185. else
186. {
187. g_swapBuffer=FALSE;
189. #ifdefDEBUG_PROTOCOL
190. P13=1;
191. #endif
192. }
194. counter=0;
195. }
196. else//接受協議正文
197. {
198. g_swapBuffer=FALSE;
199. previousChar=currentChar;
201. if(counter
202. g_angle[g_fillingBufferIndex][counter]=currentChar;
204. ++counter;
205. }
206. }//if(PROTOCOL_HEADER[0]==currentChar)
208. SBUF=currentChar;
209. while(!TI)
210. ;
211. TI=0;
212. }//(RI)
213. }
215. voidFillDiffAngle()
216. {
217. //設置舵機要調整的角度
218. g_diffAngle[PIN_GROUP_1][ROBOT_PIN_INDEX_LEFT_SHOULDER_VERTICAL]=g_angle[g_readyBufferIndex][ROBOT_LEFT_SHOULDER_VERTICAL];
219. g_diffAngle[PIN_GROUP_1][ROBOT_PIN_INDEX_LEFT_SHOULDER_HORIZEN]=g_angle[g_readyBufferIndex][ROBOT_LEFT_SHOULDER_HORIZEN];
220. g_diffAngle[PIN_GROUP_1][ROBOT_PIN_INDEX_LEFT_ELBOW]=g_angle[g_readyBufferIndex][ROBOT_LEFT_ELBOW];
222. g_diffAngle[PIN_GROUP_1][ROBOT_PIN_INDEX_RIGHT_SHOULDER_VERTICAL]=g_angle[g_readyBufferIndex][ROBOT_RIGHT_SHOULDER_VERTICAL];
223. g_diffAngle[PIN_GROUP_1][ROBOT_PIN_INDEX_RIGHT_SHOULDER_HORIZEN]=g_angle[g_readyBufferIndex][ROBOT_RIGHT_SHOULDER_HORIZEN];
224. g_diffAngle[PIN_GROUP_1][ROBOT_PIN_INDEX_RIGHT_ELBOW]=g_angle[g_readyBufferIndex][ROBOT_RIGHT_ELBOW];
226. g_diffAngle[PIN_GROUP_1][ROBOT_PIN_INDEX_LEFT_HIP_VERTICAL]=g_angle[g_readyBufferIndex][ROBOT_LEFT_HIP_VERTICAL];
227. g_diffAngle[PIN_GROUP_1][ROBOT_PIN_INDEX_RIGHT_HIP_VERTICAL]=g_angle[g_readyBufferIndex][ROBOT_RIGHT_HIP_VERTICAL];
229. g_diffAngle[PIN_GROUP_2][ROBOT_PIN_INDEX_LEFT_HIP_HORIZEN]=g_angle[g_readyBufferIndex][ROBOT_LEFT_HIP_HORIZEN];
230. g_diffAngle[PIN_GROUP_2][ROBOT_PIN_INDEX_LEFT_KNEE]=g_angle[g_readyBufferIndex][ROBOT_LEFT_KNEE];
231. g_diffAngle[PIN_GROUP_2][ROBOT_PIN_INDEX_LEFT_ANKLE]=g_angle[g_readyBufferIndex][ROBOT_LEFT_ANKLE];
232. g_diffAngle[PIN_GROUP_2][ROBOT_PIN_INDEX_LEFT_FOOT]=g_angle[g_readyBufferIndex][ROBOT_LEFT_FOOT];
234. g_diffAngle[PIN_GROUP_2][ROBOT_PIN_INDEX_RIGHT_HIP_HORIZEN]=g_angle[g_readyBufferIndex][ROBOT_RIGHT_HIP_HORIZEN];
235. g_diffAngle[PIN_GROUP_2][ROBOT_PIN_INDEX_RIGHT_KNEE]=g_angle[g_readyBufferIndex][ROBOT_RIGHT_KNEE];
236. g_diffAngle[PIN_GROUP_2][ROBOT_PIN_INDEX_RIGHT_ANKLE]=g_angle[g_readyBufferIndex][ROBOT_RIGHT_ANKLE];
237. g_diffAngle[PIN_GROUP_2][ROBOT_PIN_INDEX_RIGHT_FOOT]=g_angle[g_readyBufferIndex][ROBOT_RIGHT_FOOT];
239. //復位舵機管腳索引
240. g_diffAngleMask[PIN_GROUP_1][ROBOT_PIN_INDEX_LEFT_SHOULDER_VERTICAL]=ROBOT_PIN_MASK_LEFT_SHOULDER_VERTICAL;
241. g_diffAngleMask[PIN_GROUP_1][ROBOT_PIN_INDEX_LEFT_SHOULDER_HORIZEN]=ROBOT_PIN_MASK_LEFT_SHOULDER_HORIZEN;
242. g_diffAngleMask[PIN_GROUP_1][ROBOT_PIN_INDEX_LEFT_ELBOW]=ROBOT_PIN_MASK_LEFT_ELBOW;
244. g_diffAngleMask[PIN_GROUP_1][ROBOT_PIN_INDEX_RIGHT_SHOULDER_VERTICAL]=ROBOT_PIN_MASK_RIGHT_SHOULDER_VERTICAL;
245. g_diffAngleMask[PIN_GROUP_1][ROBOT_PIN_INDEX_RIGHT_SHOULDER_HORIZEN]=ROBOT_PIN_MASK_RIGHT_SHOULDER_HORIZEN;
246. g_diffAngleMask[PIN_GROUP_1][ROBOT_PIN_INDEX_RIGHT_ELBOW]=ROBOT_PIN_MASK_RIGHT_ELBOW;
248. g_diffAngleMask[PIN_GROUP_1][ROBOT_PIN_INDEX_LEFT_HIP_VERTICAL]=ROBOT_PIN_MASK_LEFT_HIP_VERTICAL;
249. g_diffAngleMask[PIN_GROUP_1][ROBOT_PIN_INDEX_RIGHT_HIP_VERTICAL]=ROBOT_PIN_MASK_RIGHT_HIP_VERTICAL;
251. g_diffAngleMask[PIN_GROUP_2][ROBOT_PIN_INDEX_LEFT_HIP_HORIZEN]=ROBOT_PIN_MASK_LEFT_HIP_HORIZEN;
252. g_diffAngleMask[PIN_GROUP_2][ROBOT_PIN_INDEX_LEFT_KNEE]=ROBOT_PIN_MASK_LEFT_KNEE;
253. g_diffAngleMask[PIN_GROUP_2][ROBOT_PIN_INDEX_LEFT_ANKLE]=ROBOT_PIN_MASK_LEFT_ANKLE;
254. g_diffAngleMask[PIN_GROUP_2][ROBOT_PIN_INDEX_LEFT_FOOT]=ROBOT_PIN_MASK_LEFT_FOOT;
256. g_diffAngleMask[PIN_GROUP_2][ROBOT_PIN_INDEX_RIGHT_HIP_HORIZEN]=ROBOT_PIN_MASK_RIGHT_HIP_HORIZEN;
257. g_diffAngleMask[PIN_GROUP_2][ROBOT_PIN_INDEX_RIGHT_KNEE]=ROBOT_PIN_MASK_RIGHT_KNEE;
258. g_diffAngleMask[PIN_GROUP_2][ROBOT_PIN_INDEX_RIGHT_ANKLE]=ROBOT_PIN_MASK_RIGHT_ANKLE;
259. g_diffAngleMask[PIN_GROUP_2][ROBOT_PIN_INDEX_RIGHT_FOOT]=ROBOT_PIN_MASK_RIGHT_FOOT;
260. }
262. #definePWM_STEERING_ENGINE(group)
263. {
264. counter=STEERING_ENGINE_INIT_DELAY;
265. for(i=0;i<8;++i)
266. counter+=g_diffAngle[PIN_GROUP_##group][i];
268. for(i=0;i<30;++i)
269. {
270. GROUP_##group##_CONTROL_PIN=0xFF;
271. Delay10us(STEERING_ENGINE_INIT_DELAY);
273. for(j=0;j<8;++j)
274. {
275. Delay10us(g_diffAngle[PIN_GROUP_##group][j]);
276. GROUP_##group##_CONTROL_PIN&=~(g_diffAngleMask[PIN_GROUP_##group][j]);
277. }
279. Delay10us(STEERING_ENGINE_CYCLE-counter);
280. }
281. }
283. voidmain()
284. {
285. UCHAR8i;
286. UCHAR8j;
287. UINT16counter;
289. InitSerialPort();
291. P1=0xFF;
293. //初始化舵機角度
294. for(i=0;i
295. {
296. g_angle[0][i]=45;
297. g_angle[1][i]=45;
298. }
300. for(i=0;i
301. for(j=0;j<8;++j)
302. g_diffAngle[i][j]=0;
304. FillDiffAngle();
305. InitPwmPollint();
307. while(1)
308. {
309. #ifdefDEBUG_PROTOCOL
310. if(g_fillingBufferIndex)
311. {
312. P11=0;
313. P10=1;
314. }
315. else
316. {
317. P11=1;
318. P10=0;
319. }
321. if(g_swapBuffer)
322. P12=0;
323. else
324. P12=1;
325. #endif
327. if(g_swapBuffer)
328. {
329. FillDiffAngle();
331. g_swapBuffer=FALSE;
333. InitPwmPollint();
334. }
336. PWM_STEERING_ENGINE(1)
337. PWM_STEERING_ENGINE(2)
338. }
340. }