自平衡小车开源程序向zlstone致敬Word文档下载推荐.docx
《自平衡小车开源程序向zlstone致敬Word文档下载推荐.docx》由会员分享，可在线阅读，更多相关《自平衡小车开源程序向zlstone致敬Word文档下载推荐.docx（31页珍藏版）》请在冰豆网上搜索。
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<
COM1A1）|（1<
WGM10）|（1<
COM1B1）;
//T1：
8位快速PWM模式、匹配时清0，TOP时置位
40.
TCCR1B|=（1<
WGM12）|（1<
CS11）;
//PWM：
8分频：
8M/8/256=4KHz;
41.}
42.
43.
44.//-------------------------------------------------------
45.//定时器2初始化
46.void
T2_initial（void）
//T2：
计数至OCR2时产生中断
47.{
48.
OCR2=0X4E;
计数20ms（0X9C）10ms（0X4E）时产生中断;
49.
TIMSK|=（1<
OCIE2）;
50.
TCCR2|=（1<
WGM21）|（1<
CS22）|（1<
CS21）|（1<
CS20）;
//CTC模式，1024分频
51.}
52.
53.
54.//-------------------------------------------------------
55.//外部中断初始化
56.void
INT_initial（void）
57.{
58.
MCUCR|=（1<
ISC01）|（1<
ISC00）|（1<
ISC11）|（1<
ISC10）;
//INT0、INT1上升沿有效
59.
GICR|=（1<
INT0）|（1<
INT1）;
//外部中断使能
60.}
61.
62.
63.//-------------------------------------------------------
64.//串口初始化；
65.void
USART_initial（
void
）
66.{
67.
UBRRH
=
0X00;
68.
UBRRL
51;
//f=8MHz;
设置波特率:
9600:
19200:
25;
69.
UCSRB
（1<
RXEN）|（1<
TXEN）;
//接收器与发送器使能；
70.
UCSRC
URSEL）|（1<
UCSZ0）|（1<
UCSZ1）;
//设置帧格式:
8
个数据位,
1
个停止位；
71.
72.
UCSRB|=（1<
RXCIE）;
//USART接收中断使能
73.}
74.
75.
76.//-------------------------------------------------------
77.//串口发送数据；
78.void
USART_Transmit（
unsigned
char
data
79.{
80.
while
（
!
UCSRA
&
UDRE）））;
//等待发送缓冲器为空；
81.
UDR
data;
//将数据放入缓冲器，发送数据；
82.}
83.
84.
85.//-------------------------------------------------------
86.//串口接收数据中断，确定数据输出的状态；
87.#pragma
interrupt_handler
USART_Receive_Int:
12
88.static
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
SPEEDLHINT_fun:
2
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
speed_real_RH;
119.//-------------------------------------------------------
120.#pragma
SPEEDRHINT_fun:
3
121.void
SPEEDRHINT_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.static
AD_data;
138.//-------------------------------------------------------
139.int
ADport（unsigned
port）
140.{
141.
ADMUX=port;
142.
ADCSRA|=（1<
ADEN）|（1<
ADSC）|（1<
ADPS1）|（1<
ADPS0）;
//采样频率为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.static
float
angle,
angle_dot;
//外部需要引用的变量
156.//-------------------------------------------------------
157.static
const
Q_angle=0.001,
Q_gyro=0.003,
R_angle=0.5,
dt=0.01;
158.
//注意：
dt的取值为kalman滤波器采样时间;
159.static
P[2][2]
160.
1,
0
},
161.
0,
162.
};
163.
164.static
Pdot[4]
={0,0,0,0};
165.
166.static
C_0
1;
167.
168.static
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]=-
177.
Pdot[3]=Q_gyro;
178.
179.
P[0][0]
+=
Pdot[0]
180.
Pdot[1]
181.
P[1][0]
Pdot[2]
182.
P[1][1]
Pdot[3]
183.
184.
185.
angle_err
angle_m
angle;
186.
187.
188.
PCt_0
P[0][0];
189.
PCt_1
190.
191.
E
R_angle
+
PCt_0;
192.
193.
K_0
/
E;
194.
K_1
195.
196.
t_0
197.
t_1
P[0][1];
198.
199.
-=
t_0;
200.
201.
202.
203.
204.
205.
angle
angle_err;
206.
q_bias
207.
angle_dot
gyro_m-q_bias;
208.}
209.//*/
210.
211./*
212.//-------------------------------------------------------
213.//互补滤波
214.//-------------------------------------------------------
215.static
angle,angle_dot;
//外部需要引用的变量
216.//-------------------------------------------------------
217.static
bias_cf;
218.static
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
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.
（PWM_LH<
0）
263.
264.
PORTD|=BIT（6）;
265.
PWM_LH*=-1;
266.
267.
268.
269.
PORTD&
=~BIT（6）;
270.
271.
272.
（PWM_LH>
252）
273.
274.
PWM_LH=252;
275.
276.
277.
278.
（PWM_RH<
279.
280.
PORTD|=BIT（7）;
281.
PWM_RH*=-1;
282.
283.
284.
285.
=~BIT（7）;
286.
287.
288.
（PWM_RH>
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
speed_diff,speed_diff_all,speed_diff_adjust;
310.//static
K_speed_P,K_speed_I;
311.static
K_voltage,K_angle,K_angle_dot,K_position,K_position_dot;
312.static
K_angle_AD,K_angle_dot_AD,K_position_AD,K_position_dot_AD;
313.static
position,position_dot;
314.static
position_dot_filter;
315.static
PWM;
316.static
speed_output_LH,speed_output_RH;
317.static
Turn_Need,Spe
升级会员 