標(biāo)題: 兩輪自平衡小車資料 [打印本頁(yè)]
作者: KXZSSB    時(shí)間: 2017-8-8 10:52
標(biāo)題: 兩輪自平衡小車資料



所有資料51hei提供下載:
兩輪自平衡小車資料.rar (8.27 MB, 下載次數(shù): 13)



部分源程序如下：
  1. #include "sys.h"
  2. #include "usart.h"               
  3. //#include "delay.h"       
  4. // 編碼器：100線；電機(jī)減速比：150
  5. #include "ofme_led.h"
  6. #include "ofme_filter.h"
  7. #include "ofme_iic.h"
  8. #include "ofme_iic_dev.h"
  9. #include "ofme_pwm.h"
  10. #include "ofme_pid.h"
  11. #include "ofme_encoder.h"
  12. #include "ofme_io.h"
  13. #include "ofme_time.h"
  14. #include "ofme_ir_nec.h"

  16. #define PI (3.14159265)
  17. // 度數(shù)表示的角速度*1000
  18. #define MDPS (70)
  19. // 弧度表示的角速度
  20. #define RADPS ((float)MDPS*PI/180000)
  21. // 每個(gè)查詢周期改變的角度
  22. #define RADPT (RADPS/(-100))


  25. // 平衡的角度范圍；+-60度(由于角度計(jì)算采用一階展開(kāi)，實(shí)際值約為46度)
  26. #define ANGLE_RANGE_MAX (60*PI/180)
  27. #define ANGLE_RANGE_MIN (-60*PI/180)

  29. // 全局變量
  30. pid_s sPID;                                        // PID控制參數(shù)結(jié)構(gòu)體
  31. float radian_filted=0;                // 濾波后的弧度
  32. accelerometer_s acc;                // 加速度結(jié)構(gòu)體，包含3維變量
  33. gyroscope_s gyr;                        // 角速度結(jié)構(gòu)體，包含3維變量
  34. int speed=0, distance=0;        // 小車移動(dòng)的速度，距離
  35. int tick_flag = 0;                        // 定時(shí)中斷標(biāo)志
  36. int pwm_speed = 0;                        // 電機(jī)pwm控制的偏置值，兩個(gè)電機(jī)的大小、正負(fù)相同，使小車以一定的速度前進(jìn)
  37. int pwm_turn = 0;                        // 電機(jī)pwm控制的差異值，兩個(gè)電機(jī)的大小相同，正負(fù)相反，使小車左、右轉(zhuǎn)向
  38. float angle_balance = 0;        // 小車的平衡角度。由于小車重心的偏移，小車的平衡角度不一定是radian_filted為零的時(shí)候


  41. // 定時(shí)器周期中斷，10ms
  42. void sys_tick_proc(void)
  43. {
  44.         static unsigned int i = 0;

  46.         tick_flag++;

  48.         i++;
  49.         if(i>=100) i=0;

  51.         if(i==0)                   // 綠燈的閃爍周期為1秒
  52.         {
  53.                 LED1_OFF();
  54.         }
  55.         else if(i==50)
  56.         {
  57.                 LED1_ON();
  58.         }
  59. }

  61. void control_proc(void)
  62. {
  63.         int i = ir_key_proc(); // 將紅外接收到的按鍵值，轉(zhuǎn)換為小車控制的相應(yīng)按鍵值。

  65.         switch(i)
  66.         {
  67.                 case KEY_TURN_LEFT:
  68.                         if(pwm_turn<500) pwm_turn += 50;
  69.                         break;
  70.                 case KEY_TURN_RIGHT:
  71.                         if(pwm_turn>-500) pwm_turn -= 50;
  72.                         break;
  73.                 case KEY_TURN_STOP:
  74.                         pwm_turn = 0;
  75.                         distance = 0;
  76.                         pwm_speed = 0;
  77.                         break;
  78.                 case KEY_SPEED_UP:
  79.                         if(pwm_speed<500) pwm_speed+=100;
  80.                         break;
  81.                 case KEY_SPEED_DOWN:
  82.                         if(pwm_speed>-500) pwm_speed-=100;
  83.                         break;
  84.                 case KEY_SPEED_0:
  85.                         pwm_speed = 0;
  86.                         break;
  87.                 case KEY_SPEED_F1:
  88.                         pwm_speed = 150;
  89.                         break;
  90.                 case KEY_SPEED_F2:
  91.                         pwm_speed = 300;
  92.                         break;
  93.                 case KEY_SPEED_F3:
  94.                         pwm_speed = 450;
  95.                         break;
  96.                 case KEY_SPEED_F4:
  97.                         pwm_speed = 600;
  98.                         break;
  99.                 case KEY_SPEED_F5:
  100.                         pwm_speed = 750;
  101.                         break;
  102.                 case KEY_SPEED_F6:
  103.                         pwm_speed = 900;
  104.                         break;
  105.                 case KEY_SPEED_B1:
  106.                         pwm_speed = -150;
  107.                 case KEY_SPEED_B2:
  108.                         pwm_speed = -300;
  109.                 case KEY_SPEED_B3:
  110.                         pwm_speed = -450;
  111.                         break;
  112.                 default:
  113.                         break;
  114.         }

  116.         pwm_turn *= 0.9;        // pwm_turn的值以0.9的比例衰減，使小車在接收到一個(gè)轉(zhuǎn)向信號(hào)后只轉(zhuǎn)動(dòng)一定的時(shí)間后停止轉(zhuǎn)動(dòng)。


  119.         speed = speed*0.7 +0.3*(encoder_read());        // 定周期（10ms）讀取編碼器數(shù)值得到實(shí)時(shí)速度，再對(duì)速度進(jìn)行平滑濾波
  120.         if(speed!=0)
  121.         {
  122.                 printf("speed: %d, dst: %d, pwm: %d \r\n", speed,distance,(int)(speed*6+distance*0.1));
  123.         }



  127.         encoder_write(0);                                                        // 編碼器值重新設(shè)為0

  129.         distance += speed;                                                        // 對(duì)速度進(jìn)行積分，得到移動(dòng)距離

  131.         if(distance>6000) distance = 6000;                        // 減少小車懸空、空轉(zhuǎn)對(duì)控制的影響
  132.         else if(distance<-6000) distance = -6000;

  134. }

  136. void balance_proc(void)
  137. {
  138.         static unsigned int err_cnt=0;

  140. //        float tFloat;
  141.         int pwm_balance;
  142. //        static float angle;
  143. //        float angle_t;
  144.         float radian, radian_pt;          // 當(dāng)前弧度及弧度的微分(角速度，角度值用弧度表示）

  146.         adxl345_read(&acc);                        // 讀取當(dāng)前加速度。由于傳感器按照的位置原因，傳感器的值在函數(shù)內(nèi)部經(jīng)過(guò)處理，變?yōu)樾≤嚨奶摂M坐標(biāo)系。
  147.         l3g4200d_read(&gyr);                // 讀取當(dāng)前角速度。同樣經(jīng)過(guò)坐標(biāo)系變換。


  150. // 此段程序用于傳感器出錯(cuò)時(shí)停止小車
  151.         err_cnt = err_cnt*115>>7;        // err_cnt以0.9的比例系數(shù)衰減(115>>7的值約為0.9，避免浮點(diǎn)數(shù)，提高速度)
  152.         if(acc.flag != 0x0F || gyr.flag != 0x0F)   // 讀取的角度、角速度值有誤�？赡苁请姶鸥蓴_、iic線太長(zhǎng)等導(dǎo)致出錯(cuò)。
  153.         {
  154.                 LED0_ON();                // 亮紅燈
  155.                 err_cnt +=100;        // 等比數(shù)列，比例系數(shù)0.9(115>>7)，常數(shù)項(xiàng)100；根據(jù)公式，連續(xù)10項(xiàng)的和約為657
  156.                 if(err_cnt>657) goto err;        // 當(dāng)連續(xù)發(fā)生約10次（約0.1秒）錯(cuò)誤則超過(guò)657而溢出。
  157.         }


  160. // 此段程序用于倒立或失重時(shí)停止小車
  161.         if(acc.z<=0)
  162.         {
  163.                 goto err;
  164.         }


  167. // 小車的虛擬x軸方向?yàn)樾≤嚽斑M(jìn)方向，虛擬y軸為小車左邊，虛擬z軸為小車上升方向。
  168. // 前傾角度為負(fù)，后傾角度為正。
  169.         // 通過(guò)計(jì)算加速度分量，得到小車傾斜弧度（未濾波）
  170.         radian = (float)(acc.x)/acc.z;        //  一階展開(kāi)：Q =f(x)=x-x^3/3+x^5/5+...+(-1)^k*x^(2k+1)/(2k+1)+...
  171.         // 通過(guò)角速度傳感器，得到小車的角速度（單位為 弧度/秒）
  172.         radian_pt = gyr.y*RADPT;
  173.         radian_filted = ofme_filter(radian_filted, radian, radian_pt);                // 互補(bǔ)濾波得到小車的傾斜角度

  175. // 此段程序用于小車傾斜角度過(guò)大時(shí)，停止小車
  176.         if(radian_filted> ANGLE_RANGE_MAX || radian_filted<ANGLE_RANGE_MIN)
  177.         {
  178.                 goto err;
  179.         }

  181. // 通過(guò)PID計(jì)算，得到保持小車角度為零所需要的電機(jī)pwm輸出
  182.         pwm_balance = pid_proc(&sPID, radian_filted, radian_pt);
  183.         //        printf("%d\r\n",speed);
  184. // 通過(guò)小車移動(dòng)速度與移動(dòng)距離，調(diào)整小車平衡所需的pwm輸出
  185.         pwm_balance += (speed*6+distance*0.1);

  187. // 在pwm_balance的基礎(chǔ)上，加上速度分量與轉(zhuǎn)動(dòng)分量，調(diào)整小車兩個(gè)電機(jī)的轉(zhuǎn)速。
  188.         pwm_control(pwm_balance+pwm_speed+pwm_turn, pwm_balance+pwm_speed-pwm_turn);

  190. // 如果pwm超出有效值，紅燈亮。用于調(diào)試，了解系統(tǒng)狀態(tài)。
  191.         if(pwm_balance>=1000||pwm_balance<=-1000) LED1_ON();
  192.         LED0_OFF();
  193.         return;
  194. err:
  195.         puts("balance error.\r\n");
  196.         pwm_control(0, 0);                                   // 關(guān)閉電機(jī)
  197.         return;
  198. }


  201. int main(void)
  202. {
  203. //        int i=0, t;
  204. //        int pwm;
  205. //        float radian, radian_pt;

  207.           Stm32_Clock_Init(9);//系統(tǒng)時(shí)鐘設(shè)置
  208.         uart_init(72,57600); //串口1初始化   
  209.         hw_tick_start();   // 定時(shí)器周期性中斷，用于提供系統(tǒng)脈搏
  210. ////////////////////////////////////////////////////////////////////////////////
  211.         led_init();
  212.         pwm_init();
  213.         iic_init();
  214.         adxl345_init(&acc);
  215.         l3g4200d_init(&gyr);
  216.         hw_ir_init();
  217.         encoder_init();
  218.         while(0)
  219.         {
  220.                 if(tick_flag>100)
  221.                 {
  222.                         tick_flag = 0;
  223.                         printf("count: %d\r\n",encoder_read());

  225.                 }
  226.         }



  230. ////////////////////////////////////////////////////////////////////////////////
  231. //        pid_init(&sPID, 4500,0,-300);6000--350        ;8000--350;11000--350;
  232. //        pid_init(&sPID, 6000,0,-35000);
  233.         pid_init(&sPID, 7500,0,-35000);          // 調(diào)節(jié)PID參數(shù)，后3個(gè)形參分別為：比例系數(shù)P，積分系數(shù)I，微分系數(shù)D
  234.         sPID.target = -3.5*PI/180;

  236.         radian_filted = 0;
  237.         adxl345_init(&acc);
  238.         l3g4200d_init(&gyr);
  239.         while(1)
  240.         {
  241.                 if(tick_flag)
  242.                 {
  243.                         tick_flag = 0;
  244.                         balance_proc();        // 調(diào)節(jié)小車，保持平衡
  245.                         control_proc();        // 根據(jù)遙控接收到的數(shù)據(jù)，調(diào)整電機(jī)輸出參數(shù)
  246.                 }
  247.         }
  248. }

復(fù)制代碼










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