新聞中心

EEPW首頁 > 嵌入式系統(tǒng) > 設(shè)計(jì)應(yīng)用 > 單片機(jī)(8bit)的16路舵機(jī)調(diào)速分析與實(shí)現(xiàn)

單片機(jī)(8bit)的16路舵機(jī)調(diào)速分析與實(shí)現(xiàn)

作者： 時(shí)間：2016-11-18 來源：網(wǎng)絡(luò) 收藏
// 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;//正在填充交換區(qū)1
  65. sbitP11=P1^1;//正在填充交換區(qū)2
  66. sbitP12=P1^2;//交換區(qū)變換
  67. sbitP13=P1^3;//協(xié)議是否正確
  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)//協(xié)議標(biāo)志
  151. {
  152. previousChar=currentChar;
  153. }
  154. else
  155. {
  156. if(PROTOCOL_HEADER[0]==previousChar&&PROTOCOL_HEADER[1]==currentChar)//協(xié)議開始
  157. {
  158. counter=0;
  159. previousChar=currentChar;
  160. g_swapBuffer=FALSE;
  161. }
  162. elseif(PROTOCOL_END[0]==previousChar&&PROTOCOL_END[1]==currentChar)//協(xié)議結(jié)束
  163. {
  164. previousChar=currentChar;
  166. if(ROBOT_JOINT_AMOUNT==counter)//協(xié)議接受正確
  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//接受協(xié)議正文
  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. //設(shè)置舵機(jī)要調(diào)整的角度
  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. //復(fù)位舵機(jī)管腳索引
  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. //初始化舵機(jī)角度
  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. }

評(píng)論


技術(shù)專區(qū)

關(guān)閉