標(biāo)題: [寧?kù)o清幽的平衡小車(chē)進(jìn)程帖] [打印本頁(yè)]
作者: 寧?kù)o清幽    時(shí)間: 2016-2-4 11:52
標(biāo)題: [寧?kù)o清幽的平衡小車(chē)進(jìn)程帖]
本帖最后由 寧?kù)o清幽 于 2016-2-5 11:24 編輯

組裝曬圖
來(lái)幾發(fā)小車(chē)照片，曬一曬。
小車(chē)主控板正面照片




小車(chē)主控板反面照片


主控板給我的感覺(jué)：板子質(zhì)量杠杠的，焊工也是棒棒噠，沒(méi)有什么質(zhì)量問(wèn)題。


小車(chē)底盤(pán)反面照片



全身照片


感覺(jué)小車(chē)帥帥噠。
基礎(chǔ)實(shí)驗(yàn)
  
1.      Keil工程模板的建立

2.      LED指示燈實(shí)驗(yàn)

這兩個(gè)實(shí)驗(yàn)就放一塊了，Keil工程模板的建立在圖中了，不知道這樣算不算完成，好多文件還沒(méi)有寫(xiě)呢，做以后的實(shí)驗(yàn)再補(bǔ)充。

LED指示燈實(shí)驗(yàn)主函數(shù)代碼在下圖就不再貼出來(lái)了



all_header.h的代碼如下：
  1. #ifndef __all_head_
  2. #define __all_head_

  4. #include "STC15W4Kxx.h"
  5. #include <intrins.h>
  6. #include "Delay.h"

  8. #endif
復(fù)制代碼


Delay.h的代碼和all_header.h的代碼格式一樣，也不貼啦。
還是貼一下吧。
  1. #ifndef __Delay_
  2. #define __Dealy_
  3. #include <intrins.h>
  4. void Delay300ms();                //@20.000MHz
  5. #endif
復(fù)制代碼
在貼一下Delay.c的代碼吧！
  1. #include "Delay.h"
  2. void Delay300ms()                //@20.000MHz
  3. {
  4.         unsigned char i, j, k;
  5.         _nop_();
  6.         _nop_();
  7.         i = 23;
  8.         j = 205;
  9.         k = 120;
  10.         do
  11.         {
  12.                 do
  13.                 {
  14.                         while (--k);
  15.                 } while (--j);
  16.         } while (--i);
  17. }
復(fù)制代碼
最后來(lái)張效果圖

打死我，我也不會(huì)說(shuō)這個(gè)程序里面控制的兩個(gè)LED，但是只有一個(gè)亮。這個(gè)問(wèn)題實(shí)在是無(wú)關(guān)緊要啦

3.      UART串口通訊實(shí)驗(yàn)

還是先上個(gè)效果圖


寫(xiě)好程序編譯，在左側(cè)選好單片機(jī)型號(hào)，串口號(hào)，用的是內(nèi)部時(shí)鐘，填好IRC頻率，為了方便通信嘛，就選的11.0592M。
在右側(cè)最下面的藍(lán)色框中，選好串口好，波特率，無(wú)校驗(yàn)位，停止位1位。上面兩個(gè)藍(lán)色框就是效果圖了
對(duì)了，還得用安卓數(shù)據(jù)線把小車(chē)和電腦連起來(lái)，裝上CP2102的驅(qū)動(dòng)。
再上代碼
主函數(shù)main代碼：
  1. #include "all_header.h"

  3. void main()
  4. {        
  5.         Uart1Init();
  6.         EA=1;
  7.                 while(1);
  8. }
  9. void UART1() interrupt 4 using 1
  10. {
  11.         unsigned char date;
  12.         if (RI)
  13.         {        
  14.                 RI=0;
  15.                 date=SBUF;
  16.                 UART1SendByte(date);
  17.         }
  18.         if (TI)
  19.         {
  20.                 TI=0;
  21.         }
  22. }
復(fù)制代碼

all_header.h頭文件代碼：
  1. #ifndef __all_head_
  2. #define __all_head_


  5. #include "STC15W4Kxx.h"
  6. #include <intrins.h>
  7. //#include "Delay.h"
  8. #include "UART1.h"

  10. #endif
復(fù)制代碼
UART1.h代碼：
  1. #ifndef _UART_H_  
  2. #define _UART_H_  

  4. #include "STC15W4Kxx.h"

  6. void Uart1Init(void);
  7. void UART1SendByte(unsigned char TxD1);

  9. #endif  
復(fù)制代碼
UART1.c代碼：
  1. #include "UART1.h"

  3. void Uart1Init(void)                //115200bps@11.0592MHz
  4. {
  5.         SCON = 0x50;                //8位數(shù)據(jù),可變波特率
  6.         AUXR |= 0x01;                //串口1選擇定時(shí)器2為波特率發(fā)生器
  7.         AUXR |= 0x04;                //定時(shí)器2時(shí)鐘為Fosc,即1T
  8.         T2L = 0xE8;                //設(shè)定定時(shí)初值
  9.         T2H = 0xFF;                //設(shè)定定時(shí)初值
  10.         AUXR |= 0x10;                //啟動(dòng)定時(shí)器2
  11. }


  14. void UART1SendByte(unsigned char TxD1)  
  15. {   
  16.     SBUF=TxD1;  
  17.     while(TI == 0);
  18.     TI=0;   
  19. }
復(fù)制代碼

串口通信實(shí)驗(yàn)就算是完成了
作者: 喵大    時(shí)間: 2016-2-5 18:05
666
作者: 寧?kù)o清幽    時(shí)間: 2016-2-5 22:12
4.      PWM驅(qū)動(dòng)電機(jī)實(shí)驗(yàn)
還是先上效果圖打算傳視頻來(lái)著，不過(guò)不會(huì)傳


先是逆時(shí)針加速再減速，后是順時(shí)針加速再減速。
是時(shí)候上代碼了
main.c:
  1. #include "all_header.h"
  2. //20MHZ
  3. void main()
  4. {        int i,temp=0;
  5.         PWMInit();
  6.         P2M0=0x02;      //芯片上電后所有與PWM相關(guān)的IO口均為高阻態(tài)
  7.         P2M1=0x00;        //將PWM2和PWM3IO 設(shè)置為強(qiáng)上拉（推挽）
  8.         P3M0=0x80;        //其余的P2P3口為弱上拉
  9.         P3M1=0x00;
  10.                 while(1)
  11.                 {        for(i=0;i<100;i++)//逆時(shí)針加速
  12.                         {
  13.                         temp+=200;
  14.                         PWM2T1=20000-temp;
  15.                         Delay25ms();
  16.                         }
  17.                         for(i=0;i<100;i++)//逆時(shí)針減速
  18.                         {
  19.                         temp-=200;
  20.                         PWM2T1=20000-temp;
  21.                         Delay25ms();
  22.                         }//上面的兩個(gè)循環(huán)執(zhí)行完后，PWM2T1=20000就不用再重新賦值了
  23.                         for(i=0;i<100;i++)//順時(shí)針加速
  24.                         {
  25.                         temp+=200;
  26.                         PWM3T1=20000-temp;
  27.                         Delay25ms();
  28.                         }
  29.                         for(i=0;i<100;i++)//順時(shí)針減速
  30.                         {
  31.                         temp-=200;
  32.                         PWM3T1=20000-temp;
  33.                         Delay25ms();
  34.                         }
  35.                 }
  36. }
復(fù)制代碼
pwm.c:
  1. #include "PWM.h"
  2. void PWMInit()
  3. {
  4.         P_SW2|=0x80;                                    //使能訪問(wèn)XSFR
  5.         PWMCFG = 0x00;          //配置PWM初始化電平為低電平
  6.         PWMCKS = 0x00;          //選擇PWM的時(shí)鐘為Fosc/(0+1)=20M
  7.         PWMC = 20001;                //設(shè)置PWM周期 PWM頻率為 PWM時(shí)鐘頻率(上行代碼)20M/20001=1k
  8.        
  9.         PWM2T1 = 20000;                //設(shè)置PWM2第一次反轉(zhuǎn)的PWM計(jì)數(shù)
  10.         PWM2T2 = 20001;                //設(shè)置PWM2第二次反轉(zhuǎn)的PWM計(jì)數(shù)

  12.         PWM3T1 = 20000;                 
  13.         PWM3T2 = 20001;
  14.        
  15.         PWM2CR=0x00;                 //輸出管腳為P3.7 無(wú)PWM中斷，無(wú)PWM反轉(zhuǎn)中斷
  16.         PWM3CR=0x00;                //輸出管腳為P2.1 無(wú)PWM中斷，無(wú)PWM反轉(zhuǎn)中斷
  17.         PWMCR |= 0x03;                //使能PWM2 PWM3輸出
  18.         PWMCR |= 0x80;          //使能PWM博興發(fā)生器，PWM計(jì)數(shù)器開(kāi)始計(jì)數(shù)
復(fù)制代碼
就貼出這兩個(gè)主要的代碼吧！全部代碼打包！
PWM驅(qū)動(dòng)電機(jī)實(shí)驗(yàn).zip (41 KB, 下載次數(shù): 12)

作者: 寧?kù)o清幽    時(shí)間: 2016-2-6 00:20
bjyxyc 發(fā)表于 2016-2-6 00:03
用什么型號(hào)的芯片呢

IPA15w4k61s4
作者: 寧?kù)o清幽    時(shí)間: 2016-2-21 16:36
5.      編碼器數(shù)據(jù)采集實(shí)驗(yàn)

先上一張速度圖像(用的wps表格畫(huà)的)



這是在2.5秒內(nèi)加速到最大，延遲300毫秒，再在2.5秒內(nèi)減速的圖像，一共兩個(gè)這樣的過(guò)程。
如何采集速度：
電機(jī)上有霍爾編碼器：電機(jī)轉(zhuǎn)一圈回輸出11個(gè)左右的脈沖（脈沖個(gè)數(shù)有編碼器決定），通過(guò)配置單片機(jī)的定時(shí)器3和定時(shí)器4，使其對(duì)外部脈沖計(jì)數(shù)，配置定時(shí)器1使其每隔8毫秒讀出脈沖來(lái)，再通過(guò)串口發(fā)給電腦，這樣就采集到了速度。
配置定時(shí)器1的代碼：
  1. void Timer1Init(void)                //8毫秒@20.000MHz
  2. {
  3.         AUXR &= 0xBF;                //定時(shí)器時(shí)鐘12T模式
  4.         TMOD &= 0x0F;                //設(shè)置定時(shí)器模式
  5.         TL1 = 0xEB;                //設(shè)置定時(shí)初值
  6.         TH1 = 0xCB;                //設(shè)置定時(shí)初值
  7.         TF1 = 0;                //清除TF1標(biāo)志
  8.         TR1 = 1;                //定時(shí)器1開(kāi)始計(jì)時(shí)
  9.         ET1 = 1;                //允許定時(shí)器1中斷

  11. }
復(fù)制代碼
配置定時(shí)器3、4的代碼：
  1. void Timer_3_4_Init(void)
  2. {

  4.   T4T3M |= 0xCC;        //允許定時(shí)器3、4運(yùn)行對(duì)外部脈沖計(jì)數(shù)
  5.   IE2 &= 0x1F;        //進(jìn)制定時(shí)器3、4產(chǎn)生中斷
  6. }
復(fù)制代碼

主函數(shù)MAIN和定時(shí)器1中斷代碼：
  1. #include "all_header.h"
  2. //20M晶振
  3. int num=0;
  4. void main()
  5. {        int i,temp=0;
  6.         PWMInit();        //PWM初始化
  7.         Uart1Init();        //串口1初始化
  8.         Timer1Init();        //定時(shí)器1初始化
  9.         Timer_3_4_Init();//定時(shí)器3 4初始化
  10.         P2M0=0x02;      //芯片上電后所有與PWM相關(guān)的IO口均為高阻態(tài)
  11.         P2M1=0x00;        //將PWM2和PWM3所在的IO口設(shè)置為強(qiáng)上拉（推挽）
  12.         P3M0=0x80;        //取余的P2P3口為弱上拉（準(zhǔn)雙向扣）
  13.         P3M1=0x00;
  14.         EA=1;                //允許總中斷
  15.         T3L=0;
  16.         T3H=0;
  17.                 while(1)
  18.                 {        for(i=0;i<100;i++)//正傳加速
  19.                         {
  20.                         temp+=200;
  21.                         PWM2T1=20000-temp;
  22.                         Delay25ms();
  23.                         }
  24.                         Delay300ms();//延遲300毫秒
  25.                         for(i=0;i<100;i++)//正傳減速
  26.                         {
  27.                         temp-=200;
  28.                         PWM2T1=20000-temp;
  29.                         Delay25ms();
  30.                         }
  31.                 }
  32. }

  34. void Timer1_Int() interrupt 3
  35. {
  36.         num=(T3H<<8)+T3L; //獲取脈沖個(gè)數(shù)
  37.         T4T3M&=        0x77;        //關(guān)閉定時(shí)器3、4
  38.         T3H=T3L=0;        //脈沖個(gè)數(shù)清零
  39.         T4T3M |= 0xCC;        //將定時(shí)器3、4做計(jì)數(shù)用并開(kāi)啟定時(shí)器3、4
  40.         UART1SendByte(num);//發(fā)送num低8位（8毫秒內(nèi)最大在17左右，num的低8位計(jì)數(shù)就夠）
  41. }
復(fù)制代碼

實(shí)驗(yàn)所有代碼： 編碼器數(shù)據(jù)采集.zip (45.88 KB, 下載次數(shù): 12)


作者: 寧?kù)o清幽    時(shí)間: 2016-2-21 23:23
6.傳感器數(shù)據(jù)采集實(shí)驗(yàn)
還是先上一張圖

上圖是我找了一個(gè)認(rèn)為平衡時(shí)，陀螺儀輸出的原始數(shù)據(jù)，可以大致認(rèn)為Y軸加速度的零點(diǎn)漂移大約為-390，X軸角速度零點(diǎn)漂移大約為-17。
如何得到的這些數(shù)據(jù)：
      MPU6050是一個(gè)可以測(cè)加速度和角速度的6軸傳感器（里面還有一個(gè)溫度傳感器），單片機(jī)通過(guò)IIC協(xié)議和MPU6050通信，配置MPU6050，定時(shí)通信讀出數(shù)據(jù)，將這些數(shù)據(jù)通過(guò)串口1發(fā)送給電腦。
下面是配置MPU6050的代碼：
  1. void InitMPU6050()
  2. {
  3.         Single_WriteI2C(PWR_MGMT_1, 0x00);        //正常啟用
  4.         Single_WriteI2C(SMPLRT_DIV, 0x07);        //采樣率 = 陀螺儀的輸出率 / (1 + SMPLRT_DIV)
  5.         Single_WriteI2C(CONFIG, 0x04);                //Accelerometer:21hz 8.5ms ; Gyroscope:20hz 8.3ms
  6.         Single_WriteI2C(GYRO_CONFIG, 0x18);        //+-2000°/s
  7.         Single_WriteI2C(ACCEL_CONFIG, 0x00);        //+-2g不自檢
  8. }
復(fù)制代碼
讀取數(shù)據(jù)的代碼：
  1. int GetData(unsigned char REG_Address)
  2. {
  3.         unsigned int H;
  4.         unsigned char L;
  5.         H=Single_ReadI2C(REG_Address);
  6.         L=Single_ReadI2C(REG_Address+1);
  7.         return (H<<8)+L;   //和成數(shù)據(jù)
  8. }
復(fù)制代碼
MPU6050常用寄存器：
  1. //****************************************
  2. //定義MPU6050內(nèi)部寄存器地址
  3. //****************************************

  5. #define        SMPLRT_DIV                0x19        //陀螺儀采樣率0x07(125Hz)
  6. #define        CONFIG                        0x1A        //低通濾波頻率
  7. #define        GYRO_CONFIG                0x1B        //陀螺儀自檢及測(cè)量范圍
  8. #define        ACCEL_CONFIG        0x1C        //加速度自檢測(cè)量范圍
  9. #define        ACCEL_XOUT_H        0x3B
  10. #define        ACCEL_XOUT_L        0x3C
  11. #define        ACCEL_YOUT_H        0x3D
  12. #define        ACCEL_YOUT_L        0x3E
  13. #define        ACCEL_ZOUT_H        0x3F
  14. #define        ACCEL_ZOUT_L        0x40
  15. #define        TEMP_OUT_H                0x41
  16. #define        TEMP_OUT_L                0x42
  17. #define        GYRO_XOUT_H                0x43
  18. #define        GYRO_XOUT_L                0x44       
  19. #define        GYRO_YOUT_H                0x45
  20. #define        GYRO_YOUT_L                0x46
  21. #define        GYRO_ZOUT_H                0x47
  22. #define        GYRO_ZOUT_L                0x48
  23. #define        PWR_MGMT_1                0x6B        //電源管理 0x00正常啟用
復(fù)制代碼

IIC的代碼就不貼啦，最后打包。
主函數(shù)MAIN代碼
  1. #include "all_header.h"
  2. //20M晶振
  3. int ACCEL,GYRO;
  4. void main()
  5. {       
  6. //設(shè)置所有IO口為弱上拉
  7.         P1M0=0x00;
  8.         P1M1=0x00;
  9.         P2M0=0x00;
  10.         P2M1=0x00;
  11.         P3M0=0x00;
  12.         P3M1=0x00;
  13.         P4M0=0x00;
  14.         P4M1=0x00;
  15.         P5M0=0x00;
  16.         P5M1=0x00;
  17.         Uart1Init();        //串口1初始化
  18.         Timer1Init();        //定時(shí)器1初始化8ms中斷

  20.         InitMPU6050();        //初始化MPU6050
  21.         Delay300ms();
  22.                 EA=1;        開(kāi)總中斷
  23.                 while(1)
  24.                 {       
  25.                 }
  26. }

  28. void Timer1_Int() interrupt 3        //定時(shí)器18ms中斷
  29. {
  30.        
  31.         ACCEL=GetData(ACCEL_YOUT_H);        //讀取Y軸加速度
  32.         GYRO=GetData(GYRO_XOUT_H);        //讀取X軸角速度
  33.         UART1SendByte(ACCEL);                //發(fā)送加速度低八位
  34.         UART1SendByte(ACCEL>>8);        //發(fā)送加速度高八位
  35.         UART1SendByte(GYRO);
  36.         UART1SendByte(GYRO>>8);
  37. }
復(fù)制代碼
本實(shí)驗(yàn)代碼 傳感器數(shù)據(jù)采集.zip (53.16 KB, 下載次數(shù): 9)


作者: moyuqilin    時(shí)間: 2016-2-23 20:36
你串口助手很好啊，是什么？
作者: 寧?kù)o清幽    時(shí)間: 2016-2-23 22:31
moyuqilin 發(fā)表于 2016-2-23 20:36
你串口助手很好啊，是什么？

為了看數(shù)據(jù)方便，就動(dòng)手寫(xiě)了這么個(gè)不能再簡(jiǎn)陋的助手。
作者: 寧?kù)o清幽    時(shí)間: 2016-2-24 17:09
7.      無(wú)線通訊實(shí)驗(yàn)（藍(lán)牙）沒(méi)有nRF24l0（不會(huì)玩，以后得鼓搗鼓搗），而且小車(chē)上有藍(lán)牙，就用藍(lán)牙啦
先上圖


左邊用的是手機(jī)藍(lán)牙串口助手，發(fā)送的 hello world，右邊在電腦串口助手接收到的。一共用了單片機(jī)上的2個(gè)串口，串口2和藍(lán)牙相連接，串口1和電腦連接。在電腦上選好串口號(hào)和波特率校驗(yàn)位，停止位，燒寫(xiě)上程序就可以通信啦。
數(shù)據(jù)流向：手機(jī)藍(lán)牙-->藍(lán)牙模塊-->單片機(jī)串口2-->串口1-->電腦串口助手。感覺(jué)實(shí)際上還是串口通信，藍(lán)牙默認(rèn)的波特率是115200，名稱是HMSoft，喵大應(yīng)該是提前設(shè)置好了，通過(guò)命令可以更改藍(lán)牙的一些參數(shù)。
下面是代碼：
串口1 、2初始化代碼：

  2. void Uart1Init(void)                //115200bps@20.000MHz
  3. {
  4.         SCON = 0x50;
  5.         AUXR |= 0x01;
  6.         AUXR |= 0x04;       
  7.         T2L = 0xD5;
  8.         T2H = 0xFF;
  9.         AUXR |= 0x10;
  10.         ES=1;
  11. }
  12. void Uart2Init(void)                //115200bps@20.000MHz
  13. {
  14.         S2CON = 0x50;                //8位數(shù)據(jù),可變波特率
  15.         AUXR |= 0x04;                //定時(shí)器2時(shí)鐘為Fosc,即1T
  16.         T2L = 0xD5;                //設(shè)定定時(shí)初值
  17.         T2H = 0xFF;                //設(shè)定定時(shí)初值
  18.         AUXR |= 0x10;                //啟動(dòng)定時(shí)器2
  19.         IE2 |=0x01;                /允許定時(shí)器2中斷
  20. }
復(fù)制代碼

串口1發(fā)送數(shù)據(jù)代碼：
  1. void UART1SendByte(unsigned char TxD1)  
  2. {   
  3.     SBUF=TxD1;  
  4.     while(TI == 0);//等待發(fā)送完成
  5.     TI=0;   
  6. }
復(fù)制代碼

Main代碼：
  1. #include "all_header.h"
  2. //20MH
  3. void main()
  4. {               

  6.         //P13 I/O口設(shè)置為弱上拉
  7.         P1M0=0x00;
  8.         P1M1=0x00;
  9.         P3M0=0x00;
  10.         P3M1=0x00;
  11.         Uart1Init();
  12.         Uart2Init();
  13.         EA=1;
  14.                 while(1);
  15. }
  16. void  Interrupt_Uart2(void) interrupt  8 using 1         //125hz
  17. {
  18.         if(S2CON & 0x01)
  19.         {
  20.                 S2CON&=~0x01;
  21.                 UART1SendByte(S2BUF);
  22.         }
  23. }
復(fù)制代碼
 無(wú)線通訊實(shí)驗(yàn)就算完成啦




閃亮.gif (667.5 KB, 下載次數(shù): 282)

閃亮.gif

長(zhǎng)亮.gif (311.88 KB, 下載次數(shù): 254)

長(zhǎng)亮.gif
作者: 寧?kù)o清幽    時(shí)間: 2016-2-26 01:10
進(jìn)階實(shí)驗(yàn)：

1.     多傳感器數(shù)據(jù)融合



看了喵大和網(wǎng)上的資料，用互補(bǔ)濾波進(jìn)行數(shù)據(jù)融合就可以滿足，但要調(diào)節(jié)好比例權(quán)重（不知道這樣說(shuō)有沒(méi)有為題），我用的也是互補(bǔ)濾波。
下面就是我所觀察到的實(shí)驗(yàn)結(jié)果：（紅色線為融合后的數(shù)據(jù)，黑色線為由加速度算出的角度，縱軸的單位就是度，一個(gè)像素代表1度，我沒(méi)有軟件中寫(xiě)坐標(biāo)）

圖1：
圖1是加速度比例權(quán)重（此圖為0.1）比較大的圖像，其代碼為CarAngle=0.9*(CarAngle+GyroSpeed*0.008)+0.1*AccelAngle;
小車(chē)稍微有點(diǎn)震動(dòng)，加速度就會(huì)變化，但是車(chē)體的角度并沒(méi)有變化，微小的晃動(dòng)是噪聲，所以由加速度算出來(lái)的角度也會(huì)有很多噪聲，加速度權(quán)重過(guò)大，就會(huì)摻雜更多的噪聲，不能反應(yīng)真實(shí)的車(chē)體角度。
圖2是車(chē)體直立突然到向一個(gè)方向，待一會(huì)，然后直立，再反相倒的車(chē)體角度的圖像。圖中的那些突起的黑線，是轉(zhuǎn)小車(chē)時(shí)突然停止產(chǎn)生的，車(chē)體的角度并沒(méi)有那樣的突變（先變大在減�。�(chē)體角度而只是增大然后突然不變，在由于加速度比例權(quán)重過(guò)大，可以看到融合后的圖像（紅色線）也不能真實(shí)反應(yīng)車(chē)體的角度。
圖2：
圖2是加速度比例權(quán)重適當(dāng)（此圖為0.005）的數(shù)據(jù)融合圖像。其代碼為CarAngle=0.995*(CarAngle+GyroSpeed*0.008)+0.005*AccelAngle;

可以看到在那些黑線突起地方，紅線也平滑的變化，能夠反應(yīng)真實(shí)的車(chē)體角度變化。
圖3：
圖3是車(chē)體一開(kāi)始就有一定的傾斜角的情況下供電的圖像（剛開(kāi)始時(shí)黑色線（也就是由加速度算出的角度）能夠反應(yīng)車(chē)體的真是角度），進(jìn)行數(shù)據(jù)融合之前沒(méi)有給車(chē)體角度賦初值（就是先先將由加速度算出的角度賦給車(chē)體角度變量）就會(huì)出現(xiàn)這樣的情況。在代碼中有賦初值的注釋。
圖4：
圖4是進(jìn)行數(shù)據(jù)融合之前，給車(chē)體角度賦初值得圖像，在剛開(kāi)始的圖像部分，黑色線被紅色先覆蓋了。這樣無(wú)論小車(chē)在什么角度開(kāi)機(jī)，總能夠確保一開(kāi)機(jī)融合后的角度都是正確的。
下面是MAIN代碼：
  1. #include "all_header.h"
  2. //20MHz
  3. int AccelData;
  4. int GyrolData;
  5. float AccelAngle;
  6. float GyroSpeed;
  7. float CarAngle;
  8. float GyroAngle;
  9. int temp;
  10. void main()
  11. {       
  12.         //設(shè)置所有IO口為準(zhǔn)雙向口
  13.         P0M0=0x00;
  14.         P0M1=0x00;
  15.         P1M0=0x00;
  16.         P1M1=0x00;
  17.         P2M0=0x00;
  18.         P2M1=0x00;
  19.         P3M0=0x00;
  20.         P3M1=0x00;
  21.         P4M0=0x00;
  22.         P4M1=0x00;
  23.         P5M0=0x00;
  24.         P5M1=0x00;
  25.         InitMPU6050();//MPU6050初始化
  26.         Delay300ms();
  27.         Uart1Init();//串口1初始化
  28.         AccelData=GetData(ACCEL_YOUT_H);
  29.         AccelAngle=(float)(AccelData-370)/16384.0f;
  30.         //I在此先將由加速度算出的角度給CarAngle，
  31.         //來(lái)確保一開(kāi)機(jī)，融合后的角度就是正確的。
  32.         CarAngle=AccelAngle*57.2957795f;
  33.         Timer1Init();//定時(shí)器1初始化
  34.         Delay300ms();
  35.         EA=1;
  36.                 while(1);
  37. }
  38. void  Interrupt_Timer1(void) interrupt  3         //125hz
  39. {
  40.         AccelData=GetData(ACCEL_YOUT_H);        //獲取Y軸加速度
  41.         GyrolData=GetData(GYRO_XOUT_H);                //獲取X軸角速度
  42.         AccelAngle=(float)(AccelData-370)/16384.0f;//出去零點(diǎn)偏移，就是你得到角度（單位是弧度）
  43.         AccelAngle=AccelAngle*57.2957795f;// 180/3.1415926535898弧度轉(zhuǎn)換為度
  44.         temp=(int)AccelAngle;                //強(qiáng)制轉(zhuǎn)換為整形量
  45.         UART1SendByte(temp);                //發(fā)低八位
  46.         UART1SendByte(temp>>8);        //發(fā)高八位
  47.         GyroSpeed=(float)(GyrolData-(-17))/16.4f;
  48.         CarAngle=0.995*(CarAngle+GyroSpeed*0.008)+0.005*AccelAngle;//互補(bǔ)濾波
  49.         temp=(int)CarAngle;
  50.         UART1SendByte(temp);
  51.         UART1SendByte(temp>>8);
  52. }
復(fù)制代碼

實(shí)驗(yàn)代碼： 數(shù)據(jù)融合.zip (51.21 KB, 下載次數(shù): 7)
作者: 喵大    時(shí)間: 2016-2-27 17:47
寧?kù)o清幽 發(fā)表于 2016-2-26 01:10
進(jìn)階實(shí)驗(yàn)：

1.     多傳感器數(shù)據(jù)融合

非常好。繼續(xù)加油。把小車(chē)都學(xué)透~
作者: 騰飛的龍    時(shí)間: 2016-3-8 10:37
真是高手，佩服啊，長(zhǎng)見(jiàn)識(shí)啦，謝謝分享
作者: 寧?kù)o清幽    時(shí)間: 2016-3-9 21:35
騰飛的龍 發(fā)表于 2016-3-8 10:37
真是高手，佩服啊，長(zhǎng)見(jiàn)識(shí)啦，謝謝分享

啥高手，參數(shù)調(diào)不好，上面的都是跟著喵大學(xué)的！
作者: 寧?kù)o清幽    時(shí)間: 2016-3-24 12:55
進(jìn)階實(shí)驗(yàn)

2.     PID調(diào)試

關(guān)于小車(chē)PID的調(diào)試，也在網(wǎng)上找了好多資料，也看了喵大的角度環(huán)視頻，調(diào)試經(jīng)驗(yàn)有從網(wǎng)上學(xué)的也有些自己簡(jiǎn)單的體會(huì)。
原來(lái)沒(méi)接觸的時(shí)候也是一頭霧水（畢竟不是學(xué)自控的孩子），后來(lái)在網(wǎng)上搜了一些資料，對(duì)這些基本概念也有了大概的了解，下面說(shuō)一下我對(duì)PID的了解吧！1）我對(duì)PID的理解：PID分增量式和位置式，增量式的計(jì)算結(jié)果是在上一次的控制量基礎(chǔ)上需要增加的量，或減少的量；位置式的每一次計(jì)算都是一個(gè)新的控制量的大小，而不是增加量，也不知道這說(shuō)對(duì)不對(duì)（表達(dá)能力有限）。喵大的貌似是位置式，我的代碼也是參考喵大的。PID控制一定要有反饋，要不然何談控制（有點(diǎn)像廢話）。PID控制就是比例P、積分I、微分控制D，這些運(yùn)算是對(duì)于偏差來(lái)說(shuō)的。    （1）比例運(yùn)算：對(duì)偏差進(jìn)行比例運(yùn)算，就是偏差乘上一個(gè)系數(shù)，把這個(gè)偏差放大或縮小，對(duì)于平衡小車(chē)角度環(huán)來(lái)說(shuō)，偏差角度=目標(biāo)角度-當(dāng)前角度，目標(biāo)角度就是平衡時(shí)候的角度，就是0嘍，比例控制量=偏差*比例系數(shù)，偏差大了，就是小車(chē)傾斜程度大了，我們給小車(chē)的電機(jī)大點(diǎn)電壓，使小車(chē)加速運(yùn)動(dòng)，來(lái)減小偏差，從而使小車(chē)趨向平衡，同理，偏差小，給電機(jī)的電壓小點(diǎn)，這就是比例控制，比例參數(shù)過(guò)小，小車(chē)“沒(méi)勁”，不能抵消自身重力，太大的話，大幅度搖擺太厲害。
    （2）積分運(yùn)算：對(duì)偏差進(jìn)行積分，就是把偏差進(jìn)行累加，偏差和=偏差和+偏差*積分系數(shù)；只要出現(xiàn)偏差，通過(guò)累加就會(huì)使偏差和變大，我們給電機(jī)的電壓就要大，來(lái)使小車(chē)趨向平衡，直到偏差和為零的時(shí)候，積分這一部分對(duì)電壓控制沒(méi)有貢獻(xiàn)。相對(duì)于比例參數(shù)來(lái)說(shuō)，積分系數(shù)是很小的，但積分系數(shù)過(guò)小累加和變化不明顯，控制也就不明顯，過(guò)大，即出現(xiàn)了一個(gè)很小的誤差，通過(guò)積分系數(shù)放大了好多，累加和也會(huì)變得很大，使系統(tǒng)不穩(wěn)定。
    （3）微分控制：就是偏差對(duì)時(shí)間求導(dǎo)，得到的是偏差的變化率。對(duì)于小車(chē)的角度環(huán)來(lái)說(shuō)偏差d=C-f(t)；C就是我們目標(biāo)角度，是一個(gè)恒定值，一般是0，f(t)是當(dāng)前的角度值，偏差d對(duì)時(shí)間求導(dǎo)=常數(shù)C對(duì)時(shí)間求導(dǎo)+f(t)對(duì)時(shí)間求導(dǎo)，常數(shù)的導(dǎo)數(shù)是0，所以偏差d對(duì)時(shí)間求導(dǎo)相當(dāng)于角度對(duì)時(shí)間求導(dǎo)，得到的是角速度，角速度可以直接從陀螺儀上面獲取，就不用我們運(yùn)算了，不過(guò)我們要注意角速度的正負(fù)號(hào)。角速度的正負(fù)號(hào)代表小車(chē)像前還是向后倒下了，可以自己規(guī)定。角速度的絕對(duì)值代表小車(chē)倒下的速度，小車(chē)倒下的速度越快，我們就要給小車(chē)電機(jī)更大的電壓，來(lái)抵消小車(chē)倒下，微分控制起到加快調(diào)節(jié)的作用。微分控制量=角速度*微分系數(shù)，我們把角速度乘以一個(gè)系數(shù)（也就是微分系數(shù)）就得到微分控制部分的控制量的大小了。微分系數(shù)太小也不能抵消小車(chē)的倒下，太大了會(huì)使小車(chē)小幅度高頻震蕩，容易燒驅(qū)動(dòng)。
以上只是我的簡(jiǎn)單理解，有什么不對(duì)或者不全的地方還請(qǐng)大家指出，共同進(jìn)步。
2）調(diào)試
關(guān)于平衡車(chē)的控制，用到了兩個(gè)環(huán)的控制，分別是角度環(huán)和速度環(huán)，角度環(huán)用PD，速度環(huán)用PI，在只有角度環(huán)的時(shí)候我也用過(guò)PID，以可以達(dá)到平衡，但是由于沒(méi)有速度環(huán)，小車(chē)不一定跑到哪了。
寫(xiě)一下主要的公式：

    角度環(huán)：角度偏差d_Angle=SetAngle-CarAngle;     AngleCtrlOut=d_Angle*P_Angle+w*D_Angle;    w是角速度，在這之前還要對(duì)小車(chē)的角度進(jìn)行濾波，前面的實(shí)驗(yàn)有過(guò)介紹，AngleCtrlOut是角度環(huán)的PWM輸出。
    速度環(huán)：速度偏差d_Speed=SetSpeed-CarSeed;  Car_I=Car_I+d_Speed*I_Speed;    SpeedCtrlOut=d_Speed*P_Speed+CarI;  SpeedCtrlOut是速度環(huán)的PWM輸出。這些系數(shù)與單片機(jī)PWM的最大量有關(guān)系，比如單片機(jī)PWM的最大量是10000；在調(diào)角度環(huán)的時(shí)候一般是先讓微分系數(shù)D_Angle=0，先只調(diào)比例系數(shù)，這樣AngleCtrlOut=d_Angle*P_Angl;當(dāng)偏差角度 d_Angle=10度，比例系數(shù)P_Angle=10，AngleCtrlOut才=100，小車(chē)PWM輸出占PWM最大量的1%，理論上小車(chē)電機(jī)的轉(zhuǎn)速為最快轉(zhuǎn)速的1%（實(shí)際上達(dá)不到，也有可能死區(qū)電壓都沒(méi)過(guò)），這樣小車(chē)沒(méi)有足夠的加速度來(lái)克服重力使車(chē)達(dá)到平衡。比例系數(shù)P_Angle還需要調(diào)大。但是當(dāng)單片機(jī)PWM的最大量是256的時(shí)候，偏差角度 d_Angle=10度，比例系數(shù)P_Angle=10的時(shí)候，AngleCtrlOut還是=100，約占PWM最大量的39%，這個(gè)系數(shù)可能還靠點(diǎn)譜，所以說(shuō)這些系數(shù)PWM最大量是有關(guān)系的。
    調(diào)試這些系數(shù)的過(guò)程：先調(diào)角度環(huán)，再速度環(huán)。關(guān)于反饋類型說(shuō)的文鄒鄒點(diǎn)：角度環(huán)是負(fù)反饋，速度環(huán)是正反饋。說(shuō)的通俗點(diǎn)：角度環(huán)中，偏差角度變大時(shí)，小車(chē)要控制偏差角度減小，就是讓車(chē)輪朝著倒下的方向加速轉(zhuǎn)。速度環(huán)中，速度變大，我們得給小車(chē)更快的速度，讓小車(chē)追上自己，保持平衡。在調(diào)試的時(shí)候，可以分別調(diào)一下兩個(gè)環(huán)，注意參數(shù)的正負(fù)，保證每個(gè)環(huán)，都是使車(chē)輪朝向小車(chē)傾斜的方向轉(zhuǎn)就行了，在只有速度環(huán)時(shí)，手動(dòng)轉(zhuǎn)一個(gè)車(chē)輪，另一個(gè)車(chē)輪會(huì)慢慢轉(zhuǎn)起來(lái)，最終兩個(gè)車(chē)輪越轉(zhuǎn)越快。
    調(diào)角度環(huán)時(shí)，一般是先讓微分系數(shù)D_Angle=0，先只調(diào)比例系數(shù)，調(diào)到能使小車(chē)站起來(lái)，在平衡位置來(lái)回的擺動(dòng)，再稍微增加比例系數(shù)，加入微分系數(shù)D_Angle，微分系數(shù)D_Angle從小到大。調(diào)到小車(chē)能夠比較穩(wěn)定的平衡，這時(shí)候，你輕推小車(chē)，小車(chē)會(huì)朝著一個(gè)方向加速跑過(guò)去，直至倒下。這個(gè)也是正常的，當(dāng)然小車(chē)能接近平衡跑的越遠(yuǎn)說(shuō)明參數(shù)越好，加入速度環(huán)可以解決這個(gè)問(wèn)題，如果不加速度環(huán)，在角度環(huán)里面加入對(duì)偏差的積分（累加偏差），也能使小車(chē)平衡。只是對(duì)小車(chē)的速度沒(méi)法控制。
    調(diào)速度環(huán)時(shí)，一般是先讓積分系數(shù)I_Speed=0，先只調(diào)比例系數(shù)，由0開(kāi)始增加，調(diào)到你推一下小車(chē)，小車(chē)能有足夠的加速度來(lái)使小車(chē)保持平衡，這時(shí)小車(chē)新的平衡位置不再原地了，感覺(jué)小車(chē)能夠很好的保持平衡之后，這時(shí)再加速度積分，加入速度積分后，小車(chē)會(huì)趨向于原地平衡
    我的理解就是這些，有什么不對(duì)或者不全的還請(qǐng)大家指出來(lái)，一塊討論共同學(xué)習(xí)哈。我自己感覺(jué)對(duì)于車(chē)速控制的理解還不夠深，對(duì)于兩個(gè)車(chē)輪速度分別控制還有待加強(qiáng)。
    挺感謝喵大的這個(gè)平臺(tái)的，給我提供了學(xué)習(xí)平衡車(chē)基礎(chǔ)，有機(jī)會(huì)讓我這樣的平衡車(chē)入門(mén)級(jí)玩家能從頭到尾深刻了解平衡車(chē)的原理。給這個(gè)網(wǎng)站平臺(tái)點(diǎn)個(gè)贊，給喵大的開(kāi)源精神點(diǎn)個(gè)贊，給喵大的熱心腸點(diǎn)個(gè)贊。
    本想弄點(diǎn)圖片的，電池，又讓我弄過(guò)放了，廢了，平衡充接電池的時(shí)候，接反了一下，燈不亮了，平衡沖也出問(wèn)題了。唉我勒個(gè)去，我也是被自己整無(wú)語(yǔ)了。在下個(gè)實(shí)驗(yàn)中在上圖片吧。

作者: 寧?kù)o清幽    時(shí)間: 2016-4-16 23:09
進(jìn)階實(shí)驗(yàn)
3.     實(shí)現(xiàn)平衡小車(chē)直立時(shí)處于靜止?fàn)顟B(tài)
下面兩個(gè)圖片是小車(chē)靜止時(shí)受到干擾后小車(chē)運(yùn)動(dòng)的圖片

第一張圖的速度環(huán)是用的PI，第張圖用的是PD，可以看到第一張圖中：用手推車(chē)，車(chē)會(huì)回到原來(lái)的位置；第二張圖中：用手推車(chē)則不會(huì)會(huì)到原來(lái)的位置。下面是代碼：

  1. Speed_Least=(float)(g_iLeftMotorPluseSigma+g_iRightMotorPluseSigma)/2.0;//獲取速度

  3. g_iRightMotorPluseSigma=g_iLeftMotorPluseSigma=0;//速度清零
  4. g_fCarSpeed=Speed_Least*0.75+g_fCarSpeed*0.25;//速度濾波
  5. fDelta = g_iCarSpeedSet;//此處全局整形目標(biāo)車(chē)速 g_iCarSpeedSet為零
  6. fDelta -= g_fCarSpeed;//得到速度偏差
  7. fP = fDelta*g_fcSpeed_P;//速度偏差乘上比例系數(shù)


復(fù)制代碼
上面代碼是兩個(gè)車(chē)速度環(huán)的公共代碼。

下面是第一個(gè)圖片中PI速度環(huán)的代碼：

  1. fI = fDelta*g_fcSpeed_I;速度偏差乘上積分系數(shù)
  2. g_fCarPosition += fI; //偏差累加，在開(kāi)始初始化的時(shí)候g_fCarPosition是為零的
  3. /****************加入積分限幅*****************/
  4. if(g_fCarPosition>20000)g_fCarPosition=20000;
  5. if(g_fCarPosition<-20000)g_fCarPosition=-20000;
  6. /*********************************************/
  7. g_fSpeedCtrlOut = fP+g_fCarPosition;//速度環(huán)控制量輸出
復(fù)制代碼

下面是第二個(gè)圖片中PD速度環(huán)的代碼：

  1. fD = (fDelta-g_fcSpeedDerta)*g_fcSpeed_D;//得到速度的變化率
  2. g_fSpeedCtrlOut = fP+fD;<span style="line-height: 1.5;">//速度環(huán)控制量輸出</span>
復(fù)制代碼

所有代碼： 所有代碼.zip (144.52 KB, 下載次數(shù): 36)
附件里的代碼跟喵大的差不多，我是照著喵大的代碼敲的，不過(guò)我的這個(gè)有些地方比較難看。我剛開(kāi)始看喵大的代碼的時(shí)候，看著也頭大，后來(lái)我就先看程序名，看看這個(gè)程序的流程；看個(gè)大概了，之后再了解程序的細(xì)節(jié)，死扣每一句，之后慢慢理解了小車(chē)。

由衷感謝喵大的這個(gè)平臺(tái)，讓我學(xué)到了平衡小車(chē)的知識(shí)，感謝，感謝，感謝。


靜止1.gif (3.48 MB, 下載次數(shù): 180)

靜止1

靜止1
作者: hlkhlkh    時(shí)間: 2016-4-21 10:47
厲害,大神,請(qǐng)接下我的膝蓋你做了轉(zhuǎn)向沒(méi)?
作者: 撿垃圾的    時(shí)間: 2016-5-4 12:43
樓主能否給我一份具體的資料啊  ？
作者: 478738975    時(shí)間: 2016-11-2 22:11
不錯(cuò)�。。。≈档脤W(xué)習(xí)！
作者: 你所知道的回憶    時(shí)間: 2017-5-14 17:31
值得學(xué)習(xí)學(xué)習(xí)，我也在做小車(chē)，希望最后能夠成功




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