自平衡小车开源程序向zlstone致敬.docx

资源描述

自平衡小车开源程序向zlstone致敬.docx

《自平衡小车开源程序向zlstone致敬.docx》由会员分享，可在线阅读，更多相关《自平衡小车开源程序向zlstone致敬.docx（31页珍藏版）》请在冰豆网上搜索。

自平衡小车开源程序向zlstone致敬.docx

自平衡小车开源程序向zlstone致敬

1.//MCU：

Mega16；晶振：

8MHz；

2.//PWM:

4KHz;滤波器频率:

100Hz;系统频率：

100Hz；10ms;

3.//赵国霖：

10.05.10；

4.#include

5.#include

6.#include

8.//#define checkbit（var,bit）（var&（0x01<<（bit））） /*定义查询位函数*/

9.//#define setbit（var,bit）（var|=（0x01<<（bit））） /*定义置位函数*/

10.//#define clrbit（var,bit）（var&=（~（0x01<<（bit）））） /*定义清零位函数*/

11.

12.

13.//-------------------------------------------------------

14.//输出端口初始化

15.void PORT_initial（void）

16.{

17. DDRA=0B00000000;

18. PINA=0X00;

19. PORTA=0X00;

20.

21. DDRB=0B00000000;

22. PINB=0X00;

23. PORTB=0X00;

24.

25. DDRC=0B00010000;

26. PINC=0X00;

27. PORTC=0X00;

28.

29. DDRD=0B11110010;

30. PIND=0X00;

31. PORTD=0X00;

32.}

33.

34.

35.//-------------------------------------------------------

36.//定时器1初始化

37.void T1_initial（void）

38.{

39. TCCR1A|=（1<

8位快速PWM模式、匹配时清0，TOP时置位

40. TCCR1B|=（1<

8分频：

8M/8/256=4KHz;

41.}

42.

43.

44.//-------------------------------------------------------

45.//定时器2初始化

46.void T2_initial（void） //T2：

计数至OCR2时产生中断

47.{

48. OCR2=0X4E; //T2：

计数20ms（0X9C）10ms（0X4E）时产生中断;

49. TIMSK|=（1<

50. TCCR2|=（1<

51.}

52.

53.

54.//-------------------------------------------------------

55.//外部中断初始化

56.void INT_initial（void）

57.{

58. MCUCR|=（1<

59. GICR|=（1<

60.}

61.

62.

63.//-------------------------------------------------------

64.//串口初始化；

65.void USART_initial（ void ）

66.{

67. UBRRH = 0X00;

68. UBRRL = 51; //f=8MHz;设置波特率:

9600:

51;19200:

25;

69. UCSRB = （1<

70. UCSRC = （1<

8 个数据位, 1 个停止位；

71.

72. UCSRB|=（1<

73.}

74.

75.

76.//-------------------------------------------------------

77.//串口发送数据；

78.void USART_Transmit（ unsigned char data ）

79.{

80. while （ !

（ UCSRA & （1<

81. UDR = data; //将数据放入缓冲器，发送数据；

82.}

83.

84.

85.//-------------------------------------------------------

86.//串口接收数据中断，确定数据输出的状态；

87.#pragma interrupt_handler USART_Receive_Int:

88.static char USART_State;

89.void USART_Receive_Int（void）

90.{

91. USART_State=UDR;//USART_Receive（）;

92.}

93.

94.

95.//-------------------------------------------------------

96.//计算LH侧轮速：

INT0中断；

97.//-------------------------------------------------------

98.static int speed_real_LH;

99.//-------------------------------------------------------

100.#pragma interrupt_handler SPEEDLHINT_fun:

101.void SPEEDLHINT_fun（void）

102.{

103. if （0==（PINB&BIT（0）））

104. {

105. speed_real_LH-=1;

106. }

107. else

108. {

109. speed_real_LH+=1;

110. }

111.}

112.

113.

114.//-------------------------------------------------------

115.//计算RH侧轮速，：

INT1中断；

116.//同时将轮速信号统一成前进方向了；

117.//-------------------------------------------------------

118.static int speed_real_RH;

119.//-------------------------------------------------------

120.#pragma interrupt_handler SPEEDRHINT_fun:

121.void SPEEDRHINT_fun（void）

122.{

123. if （0==（PINB&BIT

（1）））

124. {

125. speed_real_RH+=1;

126. }

127. else

128. {

129. speed_real_RH-=1;

130. }

131.}

132.

133.

134.//-------------------------------------------------------

135.//ADport采样：

10位，采样基准电压Aref

136.//-------------------------------------------------------

137.static int AD_data;

138.//-------------------------------------------------------

139.int ADport（unsigned char port）

140.{

141. ADMUX=port;

142. ADCSRA|=（1<

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.static float angle, angle_dot; //外部需要引用的变量

156.//-------------------------------------------------------

157.static const float Q_angle=0.001, Q_gyro=0.003, R_angle=0.5, dt=0.01;

158. //注意：

dt的取值为kalman滤波器采样时间;

159.static float P[2][2] = {

160. { 1, 0 },

161. { 0, 1 }

162. };

163.

164.static float Pdot[4] ={0,0,0,0};

165.

166.static const char C_0 = 1;

167.

168.static float q_bias, angle_err, PCt_0, PCt_1, E, K_0, K_1, t_0, t_1;

169.//-------------------------------------------------------

170.void Kalman_Filter（float angle_m,float gyro_m） //gyro_m:

gyro_measure

171.{

172. angle+=（gyro_m-q_bias） * dt;

173.

174. Pdot[0]=Q_angle - P[0][1] - P[1][0];

175. Pdot[1]=- P[1][1];

176. Pdot[2]=- P[1][1];

177. Pdot[3]=Q_gyro;

178.

179. P[0][0] += Pdot[0] * dt;

180. P[0][1] += Pdot[1] * dt;

181. P[1][0] += Pdot[2] * dt;

182. P[1][1] += Pdot[3] * dt;

183.

184.

185. angle_err = angle_m - angle;

186.

187.

188. PCt_0 = C_0 * P[0][0];

189. PCt_1 = C_0 * P[1][0];

190.

191. E = R_angle + C_0 * PCt_0;

192.

193. K_0 = PCt_0 / E;

194. K_1 = PCt_1 / E;

195.

196. t_0 = PCt_0;

197. t_1 = C_0 * P[0][1];

198.

199. P[0][0] -= K_0 * t_0;

200. P[0][1] -= K_0 * t_1;

201. P[1][0] -= K_1 * t_0;

202. P[1][1] -= K_1 * t_1;

203.

204.

205. angle += K_0 * angle_err;

206. q_bias += K_1 * angle_err;

207. angle_dot = gyro_m-q_bias;

208.}

209.//*/

210.

211./*

212.//-------------------------------------------------------

213.//互补滤波

214.//-------------------------------------------------------

215.static float angle,angle_dot; //外部需要引用的变量

216.//-------------------------------------------------------

217.static float bias_cf;

218.static const float dt=0.01;

219.//-------------------------------------------------------

220.void complement_filter（float angle_m_cf,float gyro_m_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.3V~3.7V；

237.//陀螺仪：

0.5V~4.5V=-80°~+80°；满量程5V=200°=256=200°；

238.//-------------------------------------------------------

239.static float gyro,acceler;

240.//-------------------------------------------------------

241.void AD_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.

251. Kalman_Filter（acceler,gyro）;

252. //complement_filter（acceler,gyro）;

253.}

254.

255.

256.

257.//-------------------------------------------------------

258.//PWM输出

259.//-------------------------------------------------------

260.void PWM_output （int PWM_LH,int PWM_RH）

261.{

262. if （PWM_LH<0）

263. {

264. PORTD|=BIT（6）;

265. PWM_LH*=-1;

266. }

267. else

268. {

269. PORTD&=~BIT（6）;

270. }

271.

272. if （PWM_LH>252）

273. {

274. PWM_LH=252;

275. }

276.

277.

278. if （PWM_RH<0）

279. {

280. PORTD|=BIT（7）;

281. PWM_RH*=-1;

282. }

283. else

284. {

285. PORTD&=~BIT（7）;

286. }

287.

288. if （PWM_RH>252）

289. {

290. PWM_RH=252;

291. }

292.

293.

294. OCR1AH=0;

295. OCR1AL=PWM_LH; //OC1A输出；

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.//static int speed_diff,speed_diff_all,speed_diff_adjust;

310.//static float K_speed_P,K_speed_I;

311.static float K_voltage,K_angle,K_angle_dot,K_position,K_position_dot;

312.static float K_angle_AD,K_angle_dot_AD,K_position_AD,K_position_dot_AD;

313.static float position,position_dot;

314.static float position_dot_filter;

315.static float PWM;

316.static int speed_output_LH,speed_output_RH;

317.static int Turn_Need,Spe

展开阅读全文