標(biāo)題: Atmega16+mpu6050平衡小車源碼 卡爾曼濾波 [打印本頁(yè)]
作者: swkdnf    時(shí)間: 2017-12-12 20:18
標(biāo)題: Atmega16+mpu6050平衡小車源碼 卡爾曼濾波
代碼測(cè)試通過，卡爾曼濾波
Anyway2.0_新驅(qū)動(dòng)_客戶例程->default->Anyway_main.hex  此文件為燒寫代碼


單片機(jī)源程序如下:
  1. //方能儀器平衡車程序V2.0***************************
  2. //Landforcer 方能儀器版權(quán)所有
  3. /*使用到的單片機(jī)模塊有：
  4. 1、一路定時(shí)器中斷用于給定卡爾曼濾波周期，大約10ms
  5. 2、七路AD，其中ADC0用于檢測(cè)轉(zhuǎn)向電位器輸出（備用擴(kuò)展，小車有接口），
  6. ADC1用于檢測(cè)左輪電機(jī)的電流(備用)，
  7. ADC2連接外部滑動(dòng)變阻器RK0，用于調(diào)整小車的平衡角度，也就是調(diào)零，ADC3連接
  8. RK1用于調(diào)節(jié)角度的比例系數(shù)，ADC4連接RK2用于調(diào)節(jié)角度的微分系數(shù)，ADC5連接
  9. RK3用于調(diào)節(jié)位置的比例系數(shù)，ADC6連接RK4用于調(diào)節(jié)位置的微分系數(shù)，ADC7用于檢測(cè)
  10. 右輪電機(jī)的電流(備用)
  11. 3、兩路外部中斷，用于檢測(cè)編碼器的輸出脈沖數(shù)，進(jìn)而測(cè)得車速
  12. 4、兩路PWM輸出，PWM驅(qū)動(dòng)直流伺服電機(jī)
  13. 5、一般IO口，通過發(fā)送高低電平控制電機(jī)的正反轉(zhuǎn)，
  14. 6、iic接口用于讀取MPU6050六軸傳感器的數(shù)據(jù)
  15. 使用到的外部硬件模塊有：MPU6050六軸傳感器
  16. 電機(jī)驅(qū)動(dòng)模塊，車輪檢測(cè)編碼器模塊和主控板*/
  17. //*************************AnyWay調(diào)試說明**********************//
  18. /*RK0為小車的平衡角度調(diào)節(jié)，也就是零點(diǎn)調(diào)節(jié)，
  19.   RK1為角度比例系數(shù)，順時(shí)針變小，逆時(shí)針變大，系數(shù)越大車子越有力，但是系數(shù)太大會(huì)導(dǎo)致抖動(dòng)造成自激，建議大小為2.0V
  20.   RK2為角度微分系數(shù)，順時(shí)針變小，逆時(shí)針變大，系數(shù)越大車子反應(yīng)越靈敏，能一定程度上消除RK1導(dǎo)致的自激，但是不能太大，建議為3.6V
  21.   RK3為位置微分系數(shù)，順時(shí)針變小，逆7時(shí)針變大，系數(shù)越大車子越穩(wěn)定，但是太大會(huì)導(dǎo)致自激，建議為0.6V
  22.   RK4為位置比例系數(shù)，順時(shí)針變小，逆時(shí)針變大，系數(shù)越大車子回到原點(diǎn)的速度越快，但是太大也會(huì)導(dǎo)致自激，建議為1.0V
  23. */
  24. #include<avr/io.h>
  25. #include<avr/interrupt.h>
  26. //#include<avr/signal.h>
  27. #include "Anyway_uart.h"
  28. #include "Anyway_Kalman.h"
  29. #include "Anyway_mup6050.h"
  30. #include"Anyway_twi.h"
  31. #include <math.h>
  32. #define uchar unsigned char
  33. #define uint unsigned int
  34. /////////////////變量定義區(qū)
  35. extern float angle, angle_dot;
  36. volatile unsigned int AD_data;
  37. volatile float PWM;
  38. volatile unsigned char flager,reger;
  39. volatile int speed_output_RH,speed_output_LH,NUMBER;
  40. volatile int speed_real_LH,speed_real_RH;
  41. volatile float gyro,acc,adc_v,angle_t;
  42. volatile int Kp_angle,Kd_angle,Kd_position;
  43. volatile float Kp_position;
  44. volatile float position,position_dot;
  45. volatile float position_dot_filter;
  46. volatile int Turn_Need,Speed_Need;
  47. unsigned char cdis[14]={0,}; //讀緩存區(qū)
  48. unsigned int aaa,ggg;
  49. #define xtal 16 //以MHz為單位，不同的系統(tǒng)時(shí)鐘要修改。

  51. void delay_1ms(void)
  52. {
  53. unsigned int i;
  54. for(i=0;i<(unsigned int)(xtal*143-2);i++);
  55. }

  57. void delay_nms(unsigned int num)
  58. {
  59. unsigned int i;
  60. for(i=0;i<num;i++)
  61. delay_1ms();
  62. }
  63. ///////////////////中斷初始化
  64. void T1_initial(void)                                       
  65. {
  66.         TCCR1A|=(1<<COM1A1)|(1<<WGM10)|(1<<COM1B1);                                //T1：8位快速PWM模式、匹配時(shí)清0，TOP時(shí)置位
  67.         TCCR1B|=(1<<WGM12)|(1<<CS11);                        //PWM：8分頻：16M/8/256=8KHz;
  68. }

  70. //-------------------------------------------------------
  71. //定時(shí)器2初始化
  72. void T2_initial(void)                        //T2：計(jì)數(shù)至OCR2時(shí)產(chǎn)生中斷
  73. {
  74.         OCR2=0x30;//0X5E;                        //T2：10ms時(shí)產(chǎn)生中斷;
  75.         TIMSK|=(1<<OCIE2);
  76.         TCCR2|=(1<<WGM21)|(1<<CS22)|(1<<CS21)|(1<<CS20);                        //CTC模式，1024分頻
  77. }


  80. //-------------------------------------------------------
  81. //外部中斷初始化
  82. void INT_initial(void)
  83. {
  84.         MCUCR|=(1<<ISC01)|(1<<ISC00)|(1<<ISC11)|(1<<ISC10);                        //INT0、INT1上升沿有效
  85.         GICR|=(1<<INT0)|(1<<INT1);                        //外部中斷使能
  86. }
  87. ///////////////////////ad轉(zhuǎn)換
  88. int mega16_ad(uchar Ch1)
  89. {
  90.      uint addata;
  91.          ADMUX = 0x00 | Ch1;//這個(gè)地方注意設(shè)置，外部基準(zhǔn)源
  92.          ADCSRA|=(1<<ADEN)|(1<<ADSC)|(1<<ADPS2)|(1<<ADPS1)|(1<<ADPS0);;
  93.          while(!(ADCSRA & (1 << ADIF)));
  94.          addata=ADCL;
  95.          addata=addata+ADCH*256;
  96.         // AD_data-=512;
  97.          ADCSRA &= ~(1 << ADIF);
  98.      ADCSRA &= ~(1 << ADEN);
  99.         return addata;
  100. }
  101. void AD_calculate(void)
  102. {aaa=cdis[2]*256+cdis[3]+32768;        
  103. //acc=(mega16_ad(0)-mega16_ad(2))*3.286/800;
  104. acc=(32768-aaa*1.0)/16384;//+(512-mega16_ad(2))*0.004;;
  105. if(acc>1)                 ///這個(gè)地方必須限幅，不然會(huì)出問題
  106. acc=1;
  107. else if(acc<-1)
  108. acc=-1;
  109. acc=57.3*acc;
  110. //gyro=-(mega16_ad(1)-380)*4.9;
  111. ggg=cdis[10]*256+cdis[11]+32768;
  112. gyro=(32768-ggg*1.0)/131;
  113. angle_t=mega16_ad(7)/5.3;
  114. //Kalman_Filter(acc,gyro);
  115. angle=0.95*(angle)+0.05*acc;
  116. angle_dot=gyro;

  118. }
  119. ////////////////////////////////PWM發(fā)送
  120. void PWM_output (int PWM_LH,int PWM_RH)
  121. {
  122.         if (PWM_LH<0)
  123.         {
  124.                 PORTB|=1<<7;
  125.                 PWM_LH*=-1;
  126.         }
  127.         else
  128.         {
  129.                 PORTB&=~(1<<7);
  130.         }
  131.         
  132.         if (PWM_LH>252)
  133.         {
  134.                 PWM_LH=252;
  135.         }
  136.         
  137.         
  138.         if (PWM_RH<0)
  139.         {
  140.                 PORTB|=1<<6;
  141.                 PWM_RH*=-1;
  142.         }
  143.         else
  144.         {
  145.                 PORTB&=~(1<<6);
  146.         }
  147.         
  148.         if (PWM_RH>252)
  149.         {
  150.                 PWM_RH=252;
  151.         }
  152.         
  153.                
  154.         OCR1AH=0;
  155.         OCR1AL=PWM_LH;                        //OC1A輸出；
  156.         
  157.         OCR1BH=0;
  158.         OCR1BL=PWM_RH;                        //OC1B輸出；
  159.         
  160. }
  161. ///////////////////////////////////PWM 計(jì)算
  162. void PWM_calculate(void)        
  163. {
  164.         
  165. //        PORTB|=(1<<6);
  166. /*        if (0x10==(PINB&1<<4) )        //前進(jìn)
  167.         {
  168.                 Speed_Need=-4;
  169.         }
  170.         else if (0x20==(PINB&1<<5) )        //后退
  171.         {
  172.                 Speed_Need=4;
  173.         }
  174.         else        //不動(dòng)
  175.         {
  176.                 Speed_Need=0;
  177.         }
  178. */
  179. //position_dot=(speed_real_LH-speed_real_RH)*0.5;     ///B
  180. //                position_dot=(-speed_real_LH-speed_real_RH)*0.5;///D
  181. //        position_dot=(speed_real_LH+speed_real_RH)*0.5; ///A
  182.   position_dot=(-speed_real_LH+speed_real_RH)*0.5;    ///C        
  183.         position_dot_filter*=0.85;                //車輪速度濾波
  184.         position_dot_filter+=position_dot*0.15;        
  185.         
  186.         position+=position_dot_filter;
  187.         position+=Speed_Need;
  188.         position+=Turn_Need;
  189.                 if (position<-768)                //防止位置誤差過大導(dǎo)致的不穩(wěn)定
  190.         {
  191.                 position=-768;
  192.         }
  193.         else if  (position>768)
  194.         {
  195.                 position=768;
  196.         }
  197.                 PWM =-Kp_angle*angle-Kd_angle*angle_dot/*+Kp_position*position*/+Kd_position*(position_dot_filter-Speed_Need);        

  199.         speed_output_RH = PWM+Turn_Need;
  200.         speed_output_LH = PWM-Turn_Need;
  201.         PWM_output (speed_output_LH,speed_output_RH);        
  202. }
  203. ////////////////////////////////////主程序
  204. int main()
  205. {//uchar a;
  206.         DDRB=0B11000000;
  207. //        PORTB=0;
  208.         DDRB &= ~(1<<0 | 1<<1|1<<2|1<<3|1<<4|1<<5);
  209.         PORTB |= 1<<0 | 1<<1|1<<2|1<<3|1<<4|1<<5;
  210.     DDRA=0;
  211.     PORTA=0;
  212.            DDRD=0B11110010;//這個(gè)地方不能加PIN，不然會(huì)出問題
  213. DDRC |=((1<<PC0)|(1<<PC1));
  214. PORTC|=((1<<PC0)|(1<<PC1));
  215. TWBR=0x0a; //I2C 初始化，400kbps
  216. TWSR|=(1<<TWPS0);

  218. T1_initial();
  219. T2_initial();

  221. USART_Init();

  223. //        PIND=0X00;
  224. //        PORTD=0X00;
  225. //        PORTD|=~(1<<6);
  226.   // SFIOR|=1<<PUD;
  227.   INT_initial();
  228.    sei();
  229. MPU_Init();
  230.          Kp_angle=mega16_ad(3)/10.0;
  231.         Kd_angle=mega16_ad(4)/250.0;
  232.         Kp_position=mega16_ad(5)/500.0;
  233.         Kd_position=mega16_ad(6)/5.0;
  234.         angle_t=mega16_ad(7)/5.3;
  235.    while(1)
  236. {
  237. if(flager==1)
  238. {MPU_6050_Read(cdis);
  239. AD_calculate();
  240. /*flager=0;
  241. AD_calculate();
  242. PWM_calculate();
  243. MPU_6050_Read(cdis);
  244. speed_real_LH=0;//速度計(jì)數(shù)清零
  245. speed_real_RH=0;
  246. }*/
  247. /*
  248. Putc(0xfd);
  249. Putc(0);
  250. Putc(angle*2+120);
  251. */
  252. Putc(0xff);
  253. Putc(angle+128); //紅
  254. Putc(angle_t+10);//綠
  255. Putc(acc+128);   //藍(lán)
  256.   }
  257.    }
  258.    return 0;
  259. }

  261. ISR(TIMER2_COMP_vect)//定時(shí)器2中斷服務(wù)函數(shù)
  262. {
  263. flager=1;
  264. NUMBER++;
  265. //MPU_6050_Read(cdis);
  266. //AD_calculate();
  267. }
  268. ISR(INT0_vect)
  269. {
  270.         if (0==(PINB&1<<0))
  271. ……………………

  273. …………限于本文篇幅 余下代碼請(qǐng)從51黑下載附件…………
復(fù)制代碼

所有資料51hei提供下載:
Anyway2.0_新驅(qū)動(dòng)_客戶例程.zip (96.57 KB, 下載次數(shù): 35)


作者: z741342386    時(shí)間: 2019-10-13 15:12
感謝上傳代碼



歡迎光臨 (http://www.torrancerestoration.com/bbs/) Powered by Discuz! X3.1