自平衡小车开源程序向zlstone致敬Word文档下载推荐.docx

1、27. PORTC=0X00;28. 29. DDRD=0B11110010;30. PIND=0X00;31. PORTD=0X00;32. 33. 34. 35. /- 36. /定时器1初始化 37. voidT1_initial（void）38. 39. TCCR1A|=（1COM1A1）|（1WGM10）|（1COM1B1）;/T1：8位快速PWM模式、匹配时清0，TOP时置位 40. TCCR1B|=（1WGM12）|（1CS11）;/PWM：8分频：8M/8/256=4KHz;41. 42. 43. 44. /- 45. /定时器2初始化 46. voidT2_initial（v

2、oid）/T2：计数至OCR2时产生中断 47. 48. OCR2=0X4E;计数20ms（0X9C）10ms（0X4E）时产生中断;49. TIMSK|=（1OCIE2）;50. TCCR2|=（1WGM21）|（1CS22）|（1CS21）|（1CS20）;/CTC模式，1024分频 51. 52. 53. 54. /- 55. /外部中断初始化 56. voidINT_initial（void）57. 58. MCUCR|=（1ISC01）|（1ISC00）|（1ISC11）|（1ISC10）;/INT0、INT1上升沿有效 59. GICR|=（1INT0）|（1INT1）;/外部中断

3、使能 60. 61. 62. 63. /- 64. /串口初始化；65. voidUSART_initial（void）66. 67. UBRRH=0X00;68. UBRRL51;/f=8MHz;设置波特率:9600:19200:25;69. UCSRB（1RXEN）|（1TXEN）;/接收器与发送器使能；70. UCSRCURSEL）|（1UCSZ0）|（1UCSZ1）;/设置帧格式:8个数据位,1个停止位；71. 72. UCSRB|=（1RXCIE）;/USART接收中断使能 73. 74. 75. 76. /- 77. /串口发送数据；78. voidUSART_Transmit（u

4、nsignedchardata79. 80. while（!UCSRA&UDRE）;/等待发送缓冲器为空；81. UDRdata;/将数据放入缓冲器，发送数据；82. 83. 84. 85. /- 86. /串口接收数据中断，确定数据输出的状态；87. #pragmainterrupt_handlerUSART_Receive_Int:12 88. staticUSART_State;89. voidUSART_Receive_Int（void）90. 91. USART_State=UDR;/USART_Receive（）;92. 93. 94. 95. /- 96. /计算LH侧轮速：IN

5、T0中断；97. /- 98. staticintspeed_real_LH;99. /- 100. #pragmaSPEEDLHINT_fun:2 101. voidSPEEDLHINT_fun（void）102. 103. if（0=（PINB&BIT（0）104. 105. speed_real_LH-=1;106. 107. else108. 109. speed_real_LH+=1;110. 111. 112. 113. 114. /- 115. /计算RH侧轮速，：INT1中断；116. /同时将轮速信号统一成前进方向了；117. /- 118. staticspeed_real

6、_RH;119. /- 120. #pragmaSPEEDRHINT_fun:3 121. voidSPEEDRHINT_fun（void）122. 123. BIT（1）124. 125. speed_real_RH+=1;126. 127. 128. 129. speed_real_RH-=1;130. 131. 132. 133. 134. /- 135. /ADport采样：10位，采样基准电压Aref 136. /- 137. staticAD_data;138. /- 139. intADport（unsignedport）140. 141. ADMUX=port;142. ADC

7、SRA|=（1ADEN）|（1ADSC）|（1ADPS1）|（1ADPS0）;/采样频率为8分频；143. while（!（ADCSRA&（BIT（ADIF）;144. AD_data=ADCL;145. AD_data+=ADCH*256;146. AD_data-=512;147. return（AD_data）;148. 149. 150. 151. /* 152. /- 153. /Kalman滤波，8MHz的处理时间约1.8ms；154. /- 155. staticfloatangle,angle_dot;/外部需要引用的变量 156. /- 157. staticconstQ_a

8、ngle=0.001,Q_gyro=0.003,R_angle=0.5,dt=0.01;158. /注意：dt的取值为kalman滤波器采样时间;159. staticP22160. 1,0,161. 0,162. ;163. 164. staticPdot4=0,0,0,0;165. 166. staticC_01;167. 168. staticq_bias,angle_err,PCt_0,PCt_1,E,K_0,K_1,t_0,t_1;169. /- 170. voidKalman_Filter（floatangle_m,floatgyro_m）/gyro_m:gyro_measure

9、171. 172. angle+=（gyro_m-q_bias）*dt;173. 174. Pdot0=Q_angle-P01P10;175. Pdot1=-P11;176. Pdot2=-177. Pdot3=Q_gyro;178. 179. P00+=Pdot0180. Pdot1181. P10Pdot2182. P11Pdot3183. 184. 185. angle_errangle_mangle;186. 187. 188. PCt_0P00;189. PCt_1190. 191. ER_angle+PCt_0;192. 193. K_0/E;194. K_1195. 196. t

10、_0197. t_1P01;198. 199. -=t_0;200. 201. 202. 203. 204. 205. angleangle_err;206. q_bias207. angle_dotgyro_m-q_bias;208. 209. /*/ 210. 211. /*212. /-213. /互补滤波214. /-215. staticangle,angle_dot;/外部需要引用的变量216. /-217. staticbias_cf;218. static219. /-220. voidcomplement_filter（floatangle_m_cf,floatgyro_m_

11、cf）221. 222. bias_cf*=0.998;/陀螺仪零飘低通滤波；500次均值；223. bias_cf+=gyro_m_cf*0.002;224. angle_dot=gyro_m_cf-bias_cf;225. angle=（angle+angle_dot*dt）*0.90+angle_m_cf*0.05;226. /加速度低通滤波；20次均值；按100次每秒计算，低通5Hz；227. 228. */229. 230. 231. 232. 233. /- 234. /AD采样；235. /以角度表示；236. /加速度计：1.2V=1g=90；满量程：1.3V3.7V；237.

12、 /陀螺仪：0.5V4.5V=-80+80满量程5V=200=256=200238. /- 239. staticgyro,acceler;240. /- 241. voidAD_calculate（void）242. 243. 244. acceler=ADport（2）+28;/角度校正 245. gyro=ADport（3）;246. 247. acceler*=0.004069;/系数换算：2.5/（1.2*512）;248. acceler=asin（acceler）;249. gyro*=0.00341;/角速度系数：（3.14/180）*100/512=0.01364；250.

13、251. Kalman_Filter（acceler,gyro）;252. /complement_filter（acceler,gyro）;253. 254. 255. 256. 257. /- 258. /PWM输出 259. /- 260. voidPWM_output（intPWM_LH,intPWM_RH）261. 262. （PWM_LH252）273. 274. PWM_LH=252;275. 276. 277. 278. （PWM_RH289. 290. PWM_RH=252;291. 292. 293. 294. OCR1AH=0;295. OCR1AL=PWM_LH;/OC

14、1A输出；296. 297. OCR1BH=0;298. OCR1BL=PWM_RH;/OC1B输出；299. 300. 301. 302. 303. 304. 305. /- 306. /计算PWM输出值 307. /车辆直径：76mm;12*64pulse/rev;1m=3216pulses;308. /-309. /staticspeed_diff,speed_diff_all,speed_diff_adjust;310. /staticK_speed_P,K_speed_I;311. staticK_voltage,K_angle,K_angle_dot,K_position,K_position_dot;312. staticK_angle_AD,K_angle_dot_AD,K_position_AD,K_position_dot_AD;313. staticposition,position_dot;314. staticposition_dot_filter;315. staticPWM;316. staticspeed_output_LH,speed_output_RH;317. staticTurn_Need,Spe