分享STM32平衡小车参考程序

1. //MCU：Mega16；晶振：8MHz；
   
2. //PWM:4KHz;滤波器频率:100Hz;系统频率：100Hz；10ms;
   
3. #include <iom16v.h>
   
4. #include <macros.h>
   
5. #include <math.h>
   
6. 
   
7. //#define checkbit(var,bit)     (var&(0x01<<(bit)))     /*定义查询位函数*/
   
8. //#define setbit(var,bit)     (var|=(0x01<<(bit)))     /*定义置位函数*/
   
9. //#define clrbit(var,bit)     (var&=(~(0x01<<(bit)))) /*定义清零位函数*/
   
10. 
    
11. 
    
12. //-------------------------------------------------------
    
13. //输出端口初始化
    
14. void PORT_initial(void)
    
15. {
    
16.         DDRA=0B00000000;
    
17.         PINA=0X00;
    
18.         PORTA=0X00;
    
19.        
    
20.         DDRB=0B00000000;
    
21.         PINB=0X00;
    
22.         PORTB=0X00;
    
23.        
    
24.         DDRC=0B00010000;
    
25.         PINC=0X00;
    
26.         PORTC=0X00;
    
27.        
    
28.         DDRD=0B11110010;
    
29.         PIND=0X00;
    
30.         PORTD=0X00;
    
31. }
    
32. 
    
33. 
    
34. //-------------------------------------------------------
    
35. //定时器1初始化
    
36. void T1_initial(void)                                       
    
37. {
    
38.         TCCR1A|=(1<<COM1A1)|(1<<WGM10)|(1<<COM1B1);                                //T1：8位快速PWM模式、匹配时清0，TOP时置位
    
39.         TCCR1B|=(1<<WGM12)|(1<<CS11);                        //PWM：8分频：8M/8/256=4KHz;
    
40. }
    
41. 
    
42. 
    
43. //-------------------------------------------------------
    
44. //定时器2初始化
    
45. void T2_initial(void)                        //T2：计数至OCR2时产生中断
    
46. {
    
47.         OCR2=0X4E;                        //T2：计数20ms(0X9C)10ms(0X4E)时产生中断;
    
48.         TIMSK|=(1<<OCIE2);
    
49.         TCCR2|=(1<<WGM21)|(1<<CS22)|(1<<CS21)|(1<<CS20);                        //CTC模式，1024分频
    
50. }
    
51. 
    
52. 
    
53. //-------------------------------------------------------
    
54. //外部中断初始化
    
55. void INT_initial(void)
    
56. {
    
57.         MCUCR|=(1<<ISC01)|(1<<ISC00)|(1<<ISC11)|(1<<ISC10);                        //INT0、INT1上升沿有效
    
58.         GICR|=(1<<INT0)|(1<<INT1);                        //外部中断使能
    
59. }
    
60. 
    
61. 
    
62. //-------------------------------------------------------
    
63. //串口初始化；
    
64. void USART_initial( void )
    
65. {       
    
66.         UBRRH = 0X00;
    
67.         UBRRL = 51;                //f=8MHz;设置波特率:9600:51;19200:25;
    
68.         UCSRB = (1<<RXEN)|(1<<TXEN);                //接收器与发送器使能；
    
69.         UCSRC = (1<<URSEL)|(1<<UCSZ0)|(1<<UCSZ1);                //设置帧格式: 8 个数据位, 1 个停止位；
    
70.        
    
71.         UCSRB|=(1<<RXCIE);                //USART接收中断使能
    
72. }
    
73. 
    
74. 
    
75. //-------------------------------------------------------
    
76. //串口发送数据；
    
77. void USART_Transmit( unsigned char data )
    
78. {
    
79.         while ( !( UCSRA & (1<<UDRE)));                //等待发送缓冲器为空；
    
80.         UDR = data;                //将数据放入缓冲器，发送数据；
    
81. }
    
82. 
    
83. 
    
84. //-------------------------------------------------------
    
85. //串口接收数据中断，确定数据输出的状态；
    
86. #pragma interrupt_handler USART_Receive_Int:12
    
87. static char USART_State;
    
88. void USART_Receive_Int(void)
    
89. {
    
90.         USART_State=UDR;//USART_Receive();
    
91. }
    
92. 
    
93. 
    
94. //-------------------------------------------------------
    
95. //计算LH侧轮速：INT0中断；
    
96. //-------------------------------------------------------
    
97. static int speed_real_LH;
    
98. //-------------------------------------------------------
    
99. #pragma interrupt_handler SPEEDLHINT_fun:2
    
100. void SPEEDLHINT_fun(void)
     
101. {
     
102.         if (0==(PINB&BIT(0)))
     
103.         {
     
104.                 speed_real_LH-=1;
     
105.         }
     
106.         else
     
107.         {
     
108.                 speed_real_LH+=1;
     
109.         }
     
110. }
     
111. 
     
112. 
     
113. //-------------------------------------------------------
     
114. //计算RH侧轮速，：INT1中断；
     
115. //同时将轮速信号统一成前进方向了；
     
116. //-------------------------------------------------------
     
117. static int speed_real_RH;
     
118. //-------------------------------------------------------
     
119. #pragma interrupt_handler SPEEDRHINT_fun:3
     
120. void SPEEDRHINT_fun(void)
     
121. {
     
122.         if (0==(PINB&BIT(1)))
     
123.         {
     
124.                 speed_real_RH+=1;
     
125.         }
     
126.         else
     
127.         {
     
128.                 speed_real_RH-=1;
     
129.         }
     
130. }
     
131. 
     
132. 
     
133. //-------------------------------------------------------
     
134. //ADport采样：10位，采样基准电压Aref
     
135. //-------------------------------------------------------
     
136. static int AD_data;
     
137. //-------------------------------------------------------
     
138. int ADport(unsigned char port)
     
139. {
     
140.         ADMUX=port;
     
141.         ADCSRA|=(1<<ADEN)|(1<<ADSC)|(1<<ADPS1)|(1<<ADPS0);                        //采样频率为8分频；
     
142.         while(!(ADCSRA&(BIT(ADIF))));
     
143.         AD_data=ADCL;
     
144.         AD_data+=ADCH*256;
     
145.         AD_data-=512;
     
146.         return (AD_data);
     
147. }
     
148. 
     
149. 
     
150. //*
     
151. //-------------------------------------------------------
     
152. //Kalman滤波，8MHz的处理时间约1.8ms；
     
153. //-------------------------------------------------------
     
154. static float angle, angle_dot;                 //外部需要引用的变量
     
155. //-------------------------------------------------------
     
156. static const float Q_angle=0.001, Q_gyro=0.003, R_angle=0.5, dt=0.01;
     
157.                         //注意：dt的取值为kalman滤波器采样时间;
     
158. static float P[2][2] = {
     
159.                                                         { 1, 0 },
     
160.                                                         { 0, 1 }
     
161.                                                 };
     
162.        
     
163. static float Pdot[4] ={0,0,0,0};
     
164. 
     
165. static const char C_0 = 1;
     
166. 
     
167. static float q_bias, angle_err, PCt_0, PCt_1, E, K_0, K_1, t_0, t_1;
     
168. //-------------------------------------------------------
     
169. void Kalman_Filter(float angle_m,float gyro_m)                        //gyro_m:gyro_measure
     
170. {
     
171.         angle+=(gyro_m-q_bias) * dt;
     
172.        
     
173.         Pdot[0]=Q_angle - P[0][1] - P[1][0];
     
174.         Pdot[1]=- P[1][1];
     
175.         Pdot[2]=- P[1][1];
     
176.         Pdot[3]=Q_gyro;
     
177.        
     
178.         P[0][0] += Pdot[0] * dt;
     
179.         P[0][1] += Pdot[1] * dt;
     
180.         P[1][0] += Pdot[2] * dt;
     
181.         P[1][1] += Pdot[3] * dt;
     
182.        
     
183.        
     
184.         angle_err = angle_m - angle;
     
185.        
     
186.        
     
187.         PCt_0 = C_0 * P[0][0];
     
188.         PCt_1 = C_0 * P[1][0];
     
189.        
     
190.         E = R_angle + C_0 * PCt_0;
     
191.        
     
192.         K_0 = PCt_0 / E;
     
193.         K_1 = PCt_1 / E;
     
194.        
     
195.         t_0 = PCt_0;
     
196.         t_1 = C_0 * P[0][1];
     
197. 
     
198.         P[0][0] -= K_0 * t_0;
     
199.         P[0][1] -= K_0 * t_1;
     
200.         P[1][0] -= K_1 * t_0;
     
201.         P[1][1] -= K_1 * t_1;
     
202.        
     
203.        
     
204.         angle        += K_0 * angle_err;
     
205.         q_bias        += K_1 * angle_err;
     
206.         angle_dot = gyro_m-q_bias;
     
207. }
     
208. //*/
     
209. 
     
210. /*
     
211. //-------------------------------------------------------
     
212. //互补滤波
     
213. //-------------------------------------------------------
     
214. static float angle,angle_dot;                 //外部需要引用的变量
     
215. //-------------------------------------------------------       
     
216. static float bias_cf;
     
217. static const float dt=0.01;
     
218. //-------------------------------------------------------
     
219. void complement_filter(float angle_m_cf,float gyro_m_cf)
     
220. {
     
221.         bias_cf*=0.998;                        //陀螺仪零飘低通滤波；500次均值；
     
222.         bias_cf+=gyro_m_cf*0.002;
     
223.         angle_dot=gyro_m_cf-bias_cf;
     
224.         angle=(angle+angle_dot*dt)*0.90+angle_m_cf*0.05;       
     
225.         //加速度低通滤波；20次均值；按100次每秒计算，低通5Hz；
     
226. }
     
227. */       
     
228. 
     
229. 
     
230. 
     
231. 
     
232. //-------------------------------------------------------
     
233. //AD采样；
     
234. //以角度表示；
     
235. //加速度计：1.2V=1g=90°；满量程：1.3V~3.7V；
     
236. //陀螺仪：0.5V~4.5V=-80°~+80°；满量程5V=200°=256=200°；
     
237. //-------------------------------------------------------
     
238. static float gyro,acceler;
     
239. //-------------------------------------------------------
     
240. void AD_calculate(void)
     
241. {
     
242.        
     
243.         acceler=ADport(2)+28;                        //角度校正
     
244.         gyro=ADport(3);       
     
245.        
     
246.         acceler*=0.004069;                //系数换算：2.5/(1.2*512);
     
247.         acceler=asin(acceler);
     
248.         gyro*=0.00341;                        //角速度系数：(3.14/180)* 100/512=0.01364；       
     
249.        
     
250.         Kalman_Filter(acceler,gyro);
     
251.         //complement_filter(acceler,gyro);
     
252. }
     
253.        
     
254. 
     
255. 
     
256. //-------------------------------------------------------
     
257. //PWM输出
     
258. //-------------------------------------------------------
     
259. void PWM_output (int PWM_LH,int PWM_RH)
     
260. {
     
261.         if (PWM_LH<0)
     
262.         {
     
263.                 PORTD|=BIT(6);
     
264.                 PWM_LH*=-1;
     
265.         }
     
266.         else
     
267.         {
     
268.                 PORTD&=~BIT(6);
     
269.         }
     
270.        
     
271.         if (PWM_LH>252)
     
272.         {
     
273.                 PWM_LH=252;
     
274.         }
     
275.        
     
276.        
     
277.         if (PWM_RH<0)
     
278.         {
     
279.                 PORTD|=BIT(7);
     
280.                 PWM_RH*=-1;
     
281.         }
     
282.         else
     
283.         {
     
284.                 PORTD&=~BIT(7);
     
285.         }
     
286.        
     
287.         if (PWM_RH>252)
     
288.         {
     
289.                 PWM_RH=252;
     
290.         }
     
291.        
     
292.                
     
293.         OCR1AH=0;
     
294.         OCR1AL=PWM_LH;                        //OC1A输出；
     
295.        
     
296.         OCR1BH=0;
     
297.         OCR1BL=PWM_RH;                        //OC1B输出；
     
298.        
     
299. }
     
300. 
     
301. 
     
302. 
     
303. 
     
304. //-------------------------------------------------------
     
305. //计算PWM输出值
     
306. //车辆直径：76mm; 12*64pulse/rev; 1m=3216pulses;
     
307. //-------------------------------------------------------       
     
308. //static int speed_diff,speed_diff_all,speed_diff_adjust;
     
309. //static float K_speed_P,K_speed_I;
     
310. static float K_voltage,K_angle,K_angle_dot,K_position,K_position_dot;
     
311. static float K_angle_AD,K_angle_dot_AD,K_position_AD,K_position_dot_AD;
     
312. static float position,position_dot;
     
313. static float position_dot_filter;
     
314. static float PWM;
     
315. static int speed_output_LH,speed_output_RH;
     
316. static int Turn_Need,Speed_Need;
     
317. //-------------------------------------------------------       
     
318. void PWM_calculate(void)       
     
319. {
     
320.         if ( 0==(~PINA&BIT(1)) )        //左转
     
321.         {
     
322.                 Turn_Need=-40;
     
323.         }
     
324.         else if ( 0==(~PINB&BIT(2)) )         //右转
     
325.         {
     
326.                 Turn_Need=40;
     
327.         }
     
328.         else        //不转
     
329.         {
     
330.                 Turn_Need=0;
     
331.         }
     
332.        
     
333.         if ( 0==(~PINC&BIT(0)) )        //前进
     
334.         {
     
335.                 Speed_Need=-2;
     
336.         }
     
337.         else if ( 0==(~PINC&BIT(1)) )        //后退
     
338.         {
     
339.                 Speed_Need=2;
     
340.         }
     
341.         else        //不动
     
342.         {
     
343.                 Speed_Need=0;
     
344.         }
     
345.        
     
346.         K_angle_AD=ADport(4)*0.007;
     
347.         K_angle_dot_AD=ADport(5)*0.007;
     
348.         K_position_AD=ADport(6)*0.007;
     
349.         K_position_dot_AD=ADport(7)*0.007;
     
350. 
     
351.         position_dot=(speed_real_LH+speed_real_RH)*0.5;
     
352.        
     
353.         position_dot_filter*=0.85;                //车轮速度滤波
     
354.         position_dot_filter+=position_dot*0.15;       
     
355.        
     
356.         position+=position_dot_filter;
     
357.         //position+=position_dot;       
     
358.         position+=Speed_Need;
     
359.        
     
360.         if (position<-768)                //防止位置误差过大导致的不稳定
     
361.         {
     
362.                 position=-768;
     
363.         }
     
364.         else if  (position>768)
     
365.         {
     
366.                 position=768;
     
367.         }
     
368.        
     
369.         PWM = K_angle*angle *K_angle_AD + K_angle_dot*angle_dot *K_angle_dot_AD +
     
370.                                         K_position*position *K_position_AD + K_position_dot*position_dot_filter *K_position_dot_AD;
     
371.        
     
372.        
     
373.         speed_output_RH = PWM - Turn_Need;
     
374.         speed_output_LH = - PWM - Turn_Need ;
     
375.        
     
376.         /*
     
377.         speed_diff=speed_real_RH-speed_real_LH;                        //左右轮速差PI控制；
     
378.         speed_diff_all+=speed_diff;
     
379.         speed_diff_adjust=(K_speed_P*speed_diff+K_speed_I*speed_diff_all)/2;
     
380.         */
     
381.                
     
382.         PWM_output (speed_output_LH,speed_output_RH);       
     
383. }
     
384. 
     
385. //-------------------------------------------------------
     
386. //定时器2中断处理
     
387. //-------------------------------------------------------
     
388. static unsigned char temp;
     
389. //-------------------------------------------------------
     
390. #pragma interrupt_handler T2INT_fun:4
     
391. void T2INT_fun(void)                                               
     
392. {
     
393.         AD_calculate();
     
394.        
     
395.         PWM_calculate();
     
396.        
     
397.         if(temp>=4)                        //10ms即中断；每秒计算：100/4=25次；
     
398.         {               
     
399.                 if (USART_State==0X30)                //ASCII码：0X30代表字符'0'
     
400.                 {
     
401.                         USART_Transmit(angle*57.3+128);
     
402.                         USART_Transmit(angle_dot*57.3+128);
     
403.                         USART_Transmit(128);       
     
404.                 }
     
405.                 else if(USART_State==0X31)                //ASCII码：0X30代表字符'1'
     
406.                 {
     
407.                         USART_Transmit(speed_output_LH+128);
     
408.                         USART_Transmit(speed_output_RH+128);                       
     
409.                         USART_Transmit(128);
     
410.                 }
     
411.                 else if(USART_State==0X32)                //ASCII码：0X30代表字符'2'
     
412.                 {
     
413.                         USART_Transmit(speed_real_LH+128);
     
414.                         USART_Transmit(speed_real_RH+128);
     
415.                         USART_Transmit(128);
     
416.                 }
     
417.                 else if(USART_State==0X33)                //ASCII码：0X30代表字符'3'
     
418.                 {
     
419.                         USART_Transmit(K_angle+128);
     
420.                         USART_Transmit(K_angle_dot+128);
     
421.                         USART_Transmit(K_position_dot+128);
     
422.                 }                               
     
423.                 temp=0;                               
     
424.         }
     
425.         speed_real_LH=0;
     
426.         speed_real_RH=0;       
     
427.         temp+=1;       
     
428. }
     
429. 
     
430. 
     
431. 
     
432. //-------------------------------------------------------
     
433. int i,j;
     
434. //-------------------------------------------------------
     
435. void main(void)
     
436. {
     
437.         PORT_initial();
     
438.         T2_initial();
     
439.         INT_initial();
     
440.         USART_initial ();
     
441.        
     
442.         SEI();
     
443.        
     
444.         K_position=0.8 * 0.209;                //换算系数：(256/10) * (2*pi/(64*12))=0.20944；//256/10:电压换算至PWM，256对应10V；
     
445.         K_angle=34 * 25.6;                //换算系数：256/10 =25.6；
     
446.         K_position_dot=1.09 * 20.9;                //换算系数：(256/10) * (25*2*pi/(64*12))=20.944；
     
447.         K_angle_dot=2 * 25.6;                //换算系数：256/10 =25.6；
     
448.        
     
449.         for (i=1;i<=500;i++)                //延时启动PWM，等待卡尔曼滤波器稳定
     
450.         {
     
451.                 for (j=1;j<=300;j++);;
     
452.                
     
453.         }
     
454.         T1_initial();
     
455.                
     
456.         while(1)
     
457.         {
     
458.                 ;
     
459.         }
     
460. }
     

复制代码