自平衡小車開源程序(向zlstone致敬)

1、1. /MCU：Mega16；晶振：8MHz；   2. /PWM:4KHz;濾波器頻率:100Hz;系統(tǒng)頻率：100Hz；10ms;   3. /趙國霖：；   4. #include <iom16v.h>   5. #include <macros.h>   6. #include <math.h>   7.   8. /#define checkbit(var,bit)

2、60;    (var&(0x01<<(bit)     /*定義查詢位函數(shù)*/   9. /#define setbit(var,bit)     (var|=(0x01<<(bit)     /*定義置位函數(shù)*/   10. /#define clrbit(var,bit)   &#

3、160; (var&=(0x01<<(bit) /*定義清零位函數(shù)*/   11.   12.   13. /-   14. /輸出端口初始化   15. void PORT_initial(void)  16.   17.     DDRA=0B00000000;  18.     PINA=0X00;&#

4、160; 19.     PORTA=0X00;  20.       21.     DDRB=0B00000000;  22.     PINB=0X00;  23.     PORTB=0X00;  24.       25

5、.     DDRC=0B00010000;  26.     PINC=0X00;  27.     PORTC=0X00;  28.       29.     DDRD=0B11110010;  30.     PIND=0X00;  

6、;31.     PORTD=0X00;  32.   33.   34.   35. /-   36. /定時器1初始化   37. void T1_initial(void)                     

7、38.   39.     TCCR1A|=(1<<COM1A1)|(1<<WGM10)|(1<<COM1B1);               /T1：8位快速PWM模式、匹配時清0，TOP時置位   40.     TCCR1B|=(1<<WGM12)|(1<<CS1

8、1);           /PWM：8分頻：8M/8/256=4KHz;   41.   42.   43.   44. /-   45. /定時器2初始化   46. void T2_initial(void)           /T2：計數(shù)至

9、OCR2時產(chǎn)生中斷   47.   48.     OCR2=0X4E;          /T2：計數(shù)20ms(0X9C)10ms(0X4E)時產(chǎn)生中斷;   49.     TIMSK|=(1<<OCIE2);  50.     TCCR2|=(1<<WGM21)|(1&

10、lt;<CS22)|(1<<CS21)|(1<<CS20);            /CTC模式，1024分頻   51.   52.   53.   54. /-   55. /外部中斷初始化   56. void INT_initial(void)  57.   58. &#

11、160;   MCUCR|=(1<<ISC01)|(1<<ISC00)|(1<<ISC11)|(1<<ISC10);         /INT0、INT1上升沿有效   59.     GICR|=(1<<INT0)|(1<<INT1);         &

12、#160;/外部中斷使能   60.   61.   62.   63. /-   64. /串口初始化；   65. void USART_initial( void )  66.      67.     UBRRH = 0X00;  68.     UB

13、RRL = 51;     /f=8MHz;設(shè)置波特率:9600:51;19200:25;   69.     UCSRB = (1<<RXEN)|(1<<TXEN);        /接收器與發(fā)送器使能；   70.     UCSRC = (1<<U

14、RSEL)|(1<<UCSZ0)|(1<<UCSZ1);     /設(shè)置幀格式: 8 個數(shù)據(jù)位, 1 個停止位；   71.       72.     UCSRB|=(1<<RXCIE);        /USART接收中斷使能   73.  

15、;  74.   75.   76. /-   77. /串口發(fā)送數(shù)據(jù)；   78. void USART_Transmit( unsigned char data )  79.   80.     while ( !( UCSRA & (1<<UDRE);   

16、60;   /等待發(fā)送緩沖器為空；   81.     UDR = data;     /將數(shù)據(jù)放入緩沖器，發(fā)送數(shù)據(jù)；   82.    83.   84.   85. /-   86. /串口接收數(shù)據(jù)中斷，確定數(shù)據(jù)輸出的狀態(tài)；   87. #pragma interrupt_handler

17、0;USART_Receive_Int:12   88. static char USART_State;  89. void USART_Receive_Int(void)  90.   91.     USART_State=UDR;/USART_Receive();   92.   93.   94.   95. /-   96. /計算LH

18、側(cè)輪速：INT0中斷；   97. /-   98. static int speed_real_LH;  99. /-   100. #pragma interrupt_handler SPEEDLHINT_fun:2   101. void SPEEDLHINT_fun(void)  102.   103.     if (0=(PINB&BIT

19、(0)  104.       105.         speed_real_LH-=1;  106.       107.     else  108.       109.      

20、   speed_real_LH+=1;  110.        111.   112.   113.   114. /-   115. /計算RH側(cè)輪速，：INT1中斷；   116. /同時將輪速信號統(tǒng)一成前進方向了；   117. /-   118. static int speed_real_RH; 

21、; 119. /-   120. #pragma interrupt_handler SPEEDRHINT_fun:3   121. void SPEEDRHINT_fun(void)  122.   123.     if (0=(PINB&BIT(1)  124.       125.     &

22、#160;   speed_real_RH+=1;  126.       127.     else  128.       129.         speed_real_RH-=1;  130.      

23、0; 131.   132.   133.   134. /-   135. /ADport采樣：10位，采樣基準電壓Aref   136. /-   137. static int AD_data;  138. /-   139. int ADport(unsigned char port)  140.   141.  

24、60;  ADMUX=port;  142.     ADCSRA|=(1<<ADEN)|(1<<ADSC)|(1<<ADPS1)|(1<<ADPS0);          /采樣頻率為8分頻；   143.     while(!(ADCSRA&(BIT(ADIF);  144.

25、    AD_data=ADCL;  145.     AD_data+=ADCH*256;  146.     AD_data-=512;  147.     return (AD_data);  148.   149.   150.   151. /*   152.

26、 /-   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,&

27、#160;dt=0.01;  158.             /注意：dt的取值為kalman濾波器采樣時間;   159. static float P22 =   160.                 

28、0;            1, 0 ,  161.                              0, 1 

29、0; 162.                           163.       164. static float Pdot4 =0,0,0,0;  165.   166. static&#

30、160;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) &

31、#160;        /gyro_m:gyro_measure   171.   172.     angle+=(gyro_m-q_bias) * dt;  173.       174.     Pdot0=Q_angle - P01 - P10;&#

32、160; 175.     Pdot1=- P11;  176.     Pdot2=- P11;  177.     Pdot3=Q_gyro;  178.       179.     P00 += Pdot0 * dt;  

33、180.     P01 += Pdot1 * dt;  181.     P10 += Pdot2 * dt;  182.     P11 += Pdot3 * dt;  183.       184.    

34、;   185.     angle_err = angle_m - angle;  186.       187.       188.     PCt_0 = C_0 * P00;  189.     PC

35、t_1 = C_0 * P10;  190.       191.     E = R_angle + C_0 * PCt_0;  192.       193.     K_0 = PCt_0 / E; &#

36、160;194.     K_1 = PCt_1 / E;  195.       196.     t_0 = PCt_0;  197.     t_1 = C_0 * P01;  198.   199.   

37、0; P00 -= K_0 * t_0;  200.     P01 -= K_0 * t_1;  201.     P10 -= K_1 * t_0;  202.     P11 -= K_1 * t_1;  203. 

38、0;     204.       205.     angle   += K_0 * angle_err;  206.     q_bias  += K_1 * angle_err;  207.     angle_d

39、ot = gyro_m-q_bias;  208.   209. /*/   210.   211. /* 212. /- 213. /互補濾波 214. /- 215. static float angle,angle_dot;       /外部需要引用的變量 216. /-    217. static 

40、;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次均值

41、； 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；

42、60;227.  228. */    229.   230.   231.   232.   233. /-   234. /AD采樣；   235. /以角度表示；   236. /加速度計：1.2V=1g=90°；滿量程：1.3V3.7V；   237. /陀螺儀：0.5V4.5V=-80°+80°；滿量程5V=200°=256=200

43、76;；   238. /-   239. static float gyro,acceler;  240. /-   241. void AD_calculate(void)  242.   243.       244.     acceler=ADport(2)+28;      &

44、#160;    /角度校正   245.     gyro=ADport(3);   246.       247.     acceler*=0.004069;      /系數(shù)換算：2.5/(1.2*512);   248.     accel

45、er=asin(acceler);  249.     gyro*=0.00341;          /角速度系數(shù)：(3.14/180)* 100/512=0.01364；       250.       251.     Kalman_Filter(acceler

46、,gyro);  252.     /complement_filter(acceler,gyro);   253.   254.       255.   256.   257. /-   258. /PWM輸出   259. /-   260. void PWM_output (int PWM_LH,in

47、t PWM_RH)  261.   262.     if (PWM_LH<0)  263.       264.         PORTD|=BIT(6);  265.         PWM_LH*=-1;  

48、;266.       267.     else  268.       269.         PORTD&=BIT(6);  270.       271.       272. &#

49、160;   if (PWM_LH>252)  273.       274.         PWM_LH=252;  275.       276.       277.       278

50、.     if (PWM_RH<0)  279.       280.         PORTD|=BIT(7);  281.         PWM_RH*=-1;  282.       283.

51、     else  284.       285.         PORTD&=BIT(7);  286.       287.       288.     if (PWM_RH>252)&

52、#160; 289.       290.         PWM_RH=252;  291.       292.       293.           294.    &#

53、160;OCR1AH=0;  295.     OCR1AL=PWM_LH;          /OC1A輸出；   296.       297.     OCR1BH=0;  298.     OCR1BL=PWM_RH;   

54、;       /OC1B輸出；   299.       300.   301.   302.   303.   304.   305. /-   306. /計算PWM輸出值   307. /車輛直徑：76mm; 12*64pulse/rev; 1m=3216pulses;  

55、; 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&#

56、160;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_o

57、utput_RH;  317. static int Turn_Need,Speed_Need;  318. /-      319. void PWM_calculate(void)      320.   321.     if ( 0=(PINA&BIT(1) )    /左

58、轉(zhuǎn)   322.       323.         Turn_Need=-40;  324.       325.     else if ( 0=(PINB&BIT(2) )   /右轉(zhuǎn)   326.  &

59、#160;    327.         Turn_Need=40;  328.       329.     else    /不轉(zhuǎn)   330.       331.      

60、60;  Turn_Need=0;  332.       333.       334.     if ( 0=(PINC&BIT(0) )    /前進   335.       336.     

61、    Speed_Need=-2;  337.       338.     else if ( 0=(PINC&BIT(1) )   /后退    339.       340.        &#

62、160;Speed_Need=2;  341.       342.     else    /不動   343.       344.         Speed_Need=0;  345.      

63、60;346.       347.     K_angle_AD=ADport(4)*0.007;  348.     K_angle_dot_AD=ADport(5)*0.007;  349.     K_position_AD=ADport(6)*0.007;  350.     K_position_dot_A

64、D=ADport(7)*0.007;  351.   352.     position_dot=(speed_real_LH+speed_real_RH)*0.5;  353.       354.     position_dot_filter*=0.85;      /車輪速度濾波   355.  &#

65、160;  position_dot_filter+=position_dot*0.15;   356.       357.     position+=position_dot_filter;  358.     /position+=position_dot;      359.     po

66、sition+=Speed_Need;  360.       361.     if (position<-768)       /防止位置誤差過大導致的不穩(wěn)定   362.       363.         position=

67、-768;  364.       365.     else if  (position>768)  366.       367.         position=768;  368.       

68、369.       370.     PWM = K_angle*angle *K_angle_AD + K_angle_dot*angle_dot *K_angle_dot_AD +   371.                 

69、60;   K_position*position *K_position_AD + K_position_dot*position_dot_filter *K_position_dot_AD;  372.       373.       374.     speed_output_RH = PWM - T

70、urn_Need;  375.     speed_output_LH = - PWM - Turn_Need   376.       377.     /* 378.     speed_diff=speed_real_RH-speed_real_LH;    &#

71、160;    /左右輪速差PI控制； 379.     speed_diff_all+=speed_diff; 380.     speed_diff_adjust=(K_speed_P*speed_diff+K_speed_I*speed_diff_all)/2; 381.     */  382.        

72、   383.     PWM_output (speed_output_LH,speed_output_RH);     384.   385.   386. /-   387. /定時器2中斷處理   388. /-   389. static unsigned char temp;  390. /-  

73、 391. #pragma interrupt_handler T2INT_fun:4   392. void T2INT_fun(void)                          393.   394.    

74、60;AD_calculate();  395.       396.     PWM_calculate();  397.       398.     if(temp>=4)          /10ms即中斷；每秒計算：100/4=25次；

75、60; 399.              400.         if (USART_State=0X30)      /ASCII碼：0X30代表字符'0'   401.         

76、  402.             USART_Transmit(angle*57.3+128);  403.             USART_Transmit(angle_dot*57.3+128);  404.       

77、60;     USART_Transmit(128);      405.           406.         else if(USART_State=0X31)      /ASCII碼：0X30代表字符'1'

78、   407.           408.             USART_Transmit(speed_output_LH+128);  409.             USART_Transmit(speed

79、_output_RH+128);              410.             USART_Transmit(128);  411.           412.    

80、60;    else if(USART_State=0X32)      /ASCII碼：0X30代表字符'2'   413.           414.             USART_Transmit(speed_real

81、_LH+128);  415.             USART_Transmit(speed_real_RH+128);  416.             USART_Transmit(128);  417.        

82、   418.         else if(USART_State=0X33)      /ASCII碼：0X30代表字符'3'   419.           420.          

83、;   USART_Transmit(K_angle+128);  421.             USART_Transmit(K_angle_dot+128);  422.             USART_Transmit(K_position_dot+128); &#

84、160;423.                          424.         temp=0;             

85、60; 425.       426.     speed_real_LH=0;  427.     speed_real_RH=0;      428.     temp+=1;      429.   430.   431.

86、  432.   433. /-   434. int i,j;  435. /-   436. void main(void)  437.   438.     PORT_initial();  439.     T2_initial();  440.     INT_init

87、ial();  441.     USART_initial ();  442.       443.     SEI();  444.       445.     K_position=0.8 * 0.209;    

88、0;/換算系數(shù)：(256/10) * (2*pi/(64*12)=0.20944；/256/10:電壓換算至PWM，256對應(yīng)10V；   446.     K_angle=34 * 25.6;      /換算系數(shù)：256/10 =25.6；   447.     K_position_dot=1.09 * 20.9;  &#

89、160;  /換算系數(shù)：(256/10) * (25*2*pi/(64*12)=20.944；   448.     K_angle_dot=2 * 25.6;       /換算系數(shù)：256/10 =25.6；   449.       450.     for (i

90、=1;i<=500;i+)     /延時啟動PWM，等待卡爾曼濾波器穩(wěn)定   451.       452.         for (j=1;j<=300;j+);  453.           454.    

91、   455.     T1_initial();  456.           457.     while(1)  458.       459.           460.       461.   /MCU：Mega16；晶振：8MHz；/PWM:4KHz;濾波器頻率:100Hz;系統(tǒng)頻率：100Hz；10ms;#include <iom16v.h>#include <macros.h>#include <math.h>/#define checkbit(var,bit) (var&(0x01<<(bit) /*定義查詢位函數(shù)*/#defi