单片机(8bit)的16路舵机调速分析与实现

[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_an