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的外殼.
我是考量原廠外殼太重, 卡榫又容易摔斷, 所以後來換了一款薄型類似名片盒的外殼. 又輕又耐摔. 最上面那一片壓克力板是準備要來放麵包板的.
3. 安裝線路
開始插麵包版吧
這裡簡單說明一下電路,
MPU6050模組是使用I2C介面, 有四根線需要接上Raspberry Pi, 分別是:
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).
還要注意一點就是, 請把車體前後的接線整理固定好, 因為後面做平衡調校測試的時候一定會摔車好多次, 前後的線路就是首當其衝, 最好都用束線帶綁好.
全部都固定完畢的樣子
後面我們會講到用軟體測試上面的接線是否正確.
4. 軟體部份
這裡我們要搞定I2C, 並且使用wiringPi函式庫.
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
再確定raspi-config
$ sudo raspi-config
進入Advanced Options -> I2C, 把它 enabled
然後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
5. 測試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". 詳細說明請看參考文件.
6. 測試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 的部分接線有問題, 請回頭再確認!!
轉速太慢可能會造成車體傾斜時馬達來不及將車體導正, 這樣就無法平衡了.
7. 最後整合
令人興奮的時刻要來了!! 我們要把上面所有的努力全部整合到一起!!
請下載並編譯最後這套兩輪自平衡車控制程式.
$ cd
$ git clone https://github.com/wennycooper/mySegway.git
$ cd mySegway
$ gcc -o mySegway ./mySegway.c ./motors.c -lwiringPi -lpthread -lm
這程式會根據測到的數據, 努力保持車體平衡.
如果傾斜角度過大(>60度), 會自動讓馬達停止.
8. 執行
由於每一台車體安裝後重心位置都不一樣, 所以平衡角度也不一樣.
我在程式碼 mySegway.c 裡面有一個變數 angle_offset 就是這個用途.
這個變數如果沒有調整適當, 車子會偏向某一個方向最後傾倒.
請試幾次, 找到適合您的車子的angle_offset.
還有就是, 由於馬達扭力與轉速的限制, 這台車無法在傾斜角度過大的狀況平衡回來.
所以在 "程式啟動瞬間請盡量保持車體平衡". 這有一點點難度, 我的做法是左手放車子到地面, 扶著它盡量保持車體水平, 然後瞬間右手敲鍵盤執行程式.
以下是結果錄影.
https://www.youtube.com/watch?v=QyjIQknHPFw
VIDEO
https://www.youtube.com/watch?v=GkUGnLWh41k
VIDEO
https://www.youtube.com/watch?v=KOAsOaqeauU
VIDEO
如果不成功, 可能原因如下:
1. 馬達驅動方向和感測器方向相反, 可能需要改一下motors.c speed 的方向
2. 如果馬達有往正確的方向轉, 但還是傾倒, 可能是角度太大, 馬達轉速不夠或扭力不夠.
3. 執行啟動的時候沒有保持平衡, 請再執行一次
4. 不一樣的車體, 高度, 重心都不一樣, 可能需要改 PID參數
5. 其他我也不知道的原因
最後祝福各位也能夠成功!!
9. 參考文件
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/