[好朋友專題分享]自製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. 執行

由於每一台車體平衡角度不一樣, 本程式開始執行的時候是假設車體已平衡, 並取出當下角度作為基準點, 再努力透過馬達運作努力保持角度。這意思是說: “程式啟動瞬間請保持車體平衡”

有一點點難度，我的做法是左手放車子到地面，扶著它盡量保持車體平衡，然後平衡瞬間右手敲鍵盤執行程式。以下是結果錄影

如果不成功, 可能原因如下:

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/