[好朋友專題分享]自製Raspberry Pi 2 兩輪自平衡車

本篇文章感謝 CAVEDU 好朋友 Kevin 授權轉貼，好東西就是要讓更多人知道，也請大家點擊原文到 Kevin 的部落格留言發問喔！程式碼 Github 請點我 這台機器人 3/29 會出現在 CAVEDU Day喔，快點報名來看看吧~

0. 前言

本文將介紹如何利用Raspberry Pi自行製作一台兩輪自平衡車(segway)。先聲明一下, 這方面我只是初學者, 自動控制與工程數學理論都還給老師很久了，寫這篇純粹是為了拋磚引玉, 希望有經驗的高手們也能分享經驗.

1. 硬體需求

• Raspberry Pi + 外殼 + WiFi網卡。我用的是Pi 2, 請自行先燒錄並安裝 Raspbian 作業系統來開機。WiFi網卡是edimax的, 安裝方法不另贅述
• DC motors + 輪胎 + 車架 + 壓克力板 + 銅柱 我從這裡買的，我當時買到的馬達規格是減速比1:34. 我想1:21應該更好。其它的馬達如果轉速夠快扭力夠大應該也可以用, 有經驗的朋友們請幫忙分享心得。
• L293D IC 馬達驅動IC 我從這裡買的
• MPU6050六軸感測器 我從這裡買的。順便也買到焊接針腳, 這樣就可以將感測器固定到麵包板上了.

2. 機構組裝

• 上面是我買的那一包車體零件開箱照
• 依照說明安裝車體, 包括L架, 馬達, 輪胎, 最後把壓克力板還有銅柱都裝上去。請參考這裡的組裝圖
• 裝完以後的樣子

(其實拍完上面那一張照片，下一分鐘我的Pi就摔到地面, 透明壓克力盒就摔破了...) 這裡要說明一下, 車體零件內附的銅柱是 3cm的, 如果您的Pi外殼超過3cm 會塞不進去, 可以考慮換銅柱. 或是更換Pi的外殼。我是考量原廠外殼太重，卡榫又容易摔斷，所以後來換了一款薄型類似名片盒的外殼。又輕又耐摔。最上面那一片壓克力板是準備要來放麵包板的。

4. 安裝線路

開始插麵包板吧 這裡簡單說明一下電路，MPU6050模組是使用I2C介面，有四根線需要接上Raspberry Pi 的 GPIO腳位，分別是: Vcc --> RPi GPIO header pin#1 (3.3V), SDA --> RPi GPIO header pin#3 (SDA) SCL --> RPI GPIO header pin#5 (SCL) GND --> RPi GPIO header pin#6 (GND) 關於馬達的部分，我使用常見的L293D 驅動IC。 GPIO header pin#11, pin#13, pin#15 控制左輪, 接到L293D的pin#2, pin#7, pin#1 GPIO header pin#16, pin#18, pin#22 控制右輪, 接到L293D的pin#15, pin#10, pin#9 L293D的pin#3, pin#6 接左輪馬達 L293D的pin#14, pin#11 接右輪馬達 Vs 接 12V DC Vss 接 5V DC (Vs & Vss 這兩隻腳不要搞混) 還有就是 L293D 需要有夠高的電壓來源才能驅動馬達，我的做法是買一個 12V DC 1A 變壓器，再配一個轉接頭再接上麵包板，這樣才能供應 L293D 的 Vs (pin#8)。 還要注意一點就是，請把車體前後的接線整理固定好，因為後面做平衡調校測試的時候一定會摔車好多次，前後的線路就是首當其衝, 最好都用束線帶綁好。全部都固定完畢的樣子 後面我們會講到用軟體測試上面的接線是否正確.

5. 軟體部份

這裡我們要搞定I2C, 並且使用wiringPi函式庫。它有基本GPIO功能又支援I2C 讀取MPU6050資料，還有SoftPWM 功能可以控制DC motors轉速，真是一舉數得呀!! 安裝步驟如下 先安裝i2c driver. $ sudo apt-get install libi2c-dev 設定下次開機要啟動 i2c driver, $ sudo vi /etc/modules 增加以下兩行到裡面, 儲存. i2c-bcm2708 i2c-dev 有一個blacklist檔案要檢查 $ sudo vi /etc/modprobe.d/raspi-blacklist.conf 確認裡面沒有以下兩行, 如果有出現, 請在前面都加上 #符號 (也就是把他comment掉), 儲存. #blacklist spi-bcm2708 #blacklist i2c-bcm2708 然後Pi要重新開機 $ sudo bash; sync;sync;reboot 開機回來後, 要確認i2c driver kernel module 有正常啟動 $ lsmod |grep i2c i2c_dev                 6027  0 i2c_bcm2708             4990  0 安裝 git $ sudo apt-get install git-core 下載並安裝 wiringPi $ cd $ git clone git://git.drogon.net/wiringPi $ cd wiringPi $ sudo ./build

6. 測試MPU6050

先安裝測試工具 $ sudo apt-get install i2c-tools 執行這個工具測試 i2c bus上是否有看到MPU6050. $ sudo i2cdetect -y 0 (for a Revision 1 board) or $ sudo i2cdetect -y 1 (for a Revision 2 board) 如果看到以下輸出... 注意那個"68", 就表示已經正常抓到MPU6050了. 0  1  2  3  4  5  6  7  8  9  a  b  c  d  e  f 00:          -- -- -- -- -- -- -- -- -- -- -- -- -- 10: -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- 20: -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- 30: -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- 40: -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- 50: -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- 60: -- -- -- -- -- -- -- -- 68 -- -- -- -- -- -- -- 70: -- -- -- -- -- -- -- -- 再來我們要下載測試程式嘗試抓取加速度計(accelerometer) & 陀螺儀(gyroscope meter)數據. 下載測試程式, 並編譯 $ cd $ git clone https://github.com/wennycooper/mpu6050_in_c.git $ cd mpu6050_in_c $ cat README.md $ gcc -o mpu6050_accl ./mpu6050_accl.c  -lwiringPi -lpthread -lm $ gcc -o mpu6050_gyro ./mpu6050_gyro.c  -lwiringPi -lpthread -lm 現在來看看加速度的數據, 請執行以下程式: $ sudo ./mpu6050_accl 應該要看到X, Y, Z 加速度數據, 如下: My acclX_scaled: 0.140625 My acclY_scaled: -0.031006 My acclZ_scaled: 0.994141 My X rotation: -1.768799 My Y rotation: -8.047429 其中 acclX, acclY, acclZ 是三軸加速度數據 (單位是g, 沒錯! 就是物理課本上面的重力加速度 g=9.8m/s^2) My X/Y rotation 是換算出來的角度 (單位是 degree). 請試著將車體前傾或後仰, 看看數據的變化. 如果您安裝MPU6050 的方向正確的話, 車體前傾或後仰應該會看到 Y rotation 介於 -90度 ~ 90度. 再來看看陀螺儀的數據, 請執行以下程式: $ sudo ./mpu6050_gyro 應該要看到X, Y, Z軸的角速度, 單位是 degree/s My gyroZ_scaled: -1.954198 My gyroX_scaled: -4.312977 My gyroY_scaled: 0.458015 My gyroZ_scaled: 0.366412 My gyroX_scaled: -4.053435 My gyroY_scaled: 0.427481 My gyroZ_scaled: -0.160305 請嘗試著觀察轉動瞬間與數據的變化, 就會理解陀螺儀數據了。這裡說明一下，單純用上面那個加速度計不就可以推算出傾斜角度不是嗎? 為何需要用到陀螺儀數據呢？主要是因為加速度數據很容易浮動, 後來有人發明了使用陀螺儀數據再補上加速度數據的方法, 這樣就能算出比較穩定可靠的傾斜角度。這套方法叫做 "complementary filter". 詳細說明請看文末的參考文件.

7. 測試DC motors

現在我們用 wiringPi 寫一個程式測試 DC motors. $ cd $ git clone https://github.com/wennycooper/dcMotor.git $ cd dcMotor $ cat README.md $ gcc -o dcMotor0 dcMotor0.c -lwiringPi -lpthread 準備要讓馬達轉囉!! 請確認12V電源有正常插上, 並把車體用手拿起來, 執行程式!! $ sudo ./dcMotor0 如何?? 正常的話兩個馬達應該正轉5秒鐘, 再反轉5秒鐘, 最後停止. 馬達如果沒有轉, 表示前面步驟執行有錯, 請回頭去檢查. 如果有轉, 請確認兩個馬達都有轉同樣方向, 如果反向, 請把一個馬達接腳兩根交換, 應該可以改善. 還要注意一點, 您的兩個馬達都有轉得很 "夠快" 嗎?? 如果轉得很慢, 比方說每秒鐘大約才2轉, 很有可能是L293D 的部分接線有問題, 請回頭再確認!! 轉速太慢可能會造成車體傾斜時馬達來不及將車體導正, 這樣就無法平衡了.

8. 最後整合

令人興奮的時刻要來了!! 我們要把上面所有的努力全部整合到一起!! 請下載並編譯最後這套兩輪自平衡車控制程式. $ cd $ git clone https://github.com/wennycooper/mySegway.git $ cd mySegway $ gcc -o mySegway ./mySegway.c ./motors.c  -lwiringPi -lpthread -lm 這程式會根據測到的數據, 努力保持車體平衡. 如果傾斜角度過大(>60度), 會自動讓馬達停止.

9. 執行

由於每一台車體平衡角度不一樣, 本程式開始執行的時候是假設車體已平衡, 並取出當下角度作為基準點, 再努力透過馬達運作努力保持角度。這意思是說: "程式啟動瞬間請保持車體平衡" 有一點點難度，我的做法是左手放車子到地面，扶著它盡量保持車體平衡，然後平衡瞬間右手敲鍵盤執行程式。以下是結果錄影 https://www.youtube.com/watch?v=QyjIQknHPFw https://www.youtube.com/watch?v=GkUGnLWh41k https://www.youtube.com/watch?v=KOAsOaqeauU 如果不成功, 可能原因如下: 1. 馬達驅動方向和感測器方向相反, 可能需要改一下motors.c speed 的方向 2. 如果馬達有往正確的方向轉, 但還是傾倒, 可能是角度太大, 馬達轉速不夠或扭力不夠. 3. 執行啟動的時候沒有保持平衡, 請再執行一次 4. 不一樣的車體, 高度, 重心都不一樣, 可能需要改 PID參數 5. 其他我也不知道的原因 最後祝福各位也能夠成功!!

參考文件

• http://blog.bitify.co.uk/2013/11/interfacing-raspberry-pi-and-mpu-6050.html
• http://blog.bitify.co.uk/2013/11/reading-data-from-mpu-6050-on-raspberry.html
• https://projects.drogon.net/raspberry-pi/wiringpi/software-pwm-library/
• http://robotrabbit.blogspot.tw/2012/07/pid.html
• http://www.bajdi.com/building-a-self-balancing-bot/