實驗名稱
使用 Raspberry Pi 操作伺服馬達機械手臂相關文章: Raspberry Pi2: 用 Python 透過 I2C 控制 16 頻道 PWM 模組 (PCA9685)
實驗目的
在高度工業化與資訊化的現代，機械人與機械手臂在各方面的應用越來越受到重視，並且在物聯網、大數據所組成的工業4.0的未來世界裡，將扮演一個極度重要的角色。以往研究控制機械人與機械手臂控制系統是一門高成本且高技術門檻的領域，非相關人員的一般民眾在自行開發與研究上難以入門。但是隨著科技的發展，SoC 嵌入式系統的性能越來越高，價格越來越低，使得進入該領域的門檻開始慢慢降低，而使得一般民眾開始可以在車庫裡發展該方面的應用。
在 Raspberry Pi 的 Linux 環境底下，撰寫程式控制伺服馬達組成的機械手臂。並使用市面上的遊戲機搖桿來操作機械手臂。 藉此研究整個訊號控制流程與程式結構設計，建立相關研究的基礎。
使用材料及設備
六個伺服馬達組成的機械手臂一組。
安裝好 Raspbian 作業系統的 Raspberry Pi 板子(Pi B+ 或 Pi2都可)。
PCA9685 模組，有I2C介面的 16 channel PWM 控制模組。
無線遊戲機搖桿模組。
Arduino + 無線遊戲機搖桿訊號接收模組:。
原理介紹
整個實驗是以 Raspberry Pi 為中心，透過 I2C介面操作 PCA9685 來同時控制六顆伺服馬達。而 PS2 無線模組則是利用現成的 Library 搭載在 Arudino 子板之上，並且透過 UART 介面將 PS2 搖桿的訊號傳送到 Raspberry Pi 系統之上。這裡我們使用 Python 程式語言來快速開發整個架構。對整個機械手臂做手動操作模式上的設計與研究。
所以整個實驗我們必須了解伺服馬達的控制原理、I2C 控制 PCA9685 的方式、整個控制模式的程式架構、和 Arduino + PS2 無線模組的結合。
PCA9685 控制伺服馬達
先來看一下，我們用的伺服馬達大至上長這樣子:
市面上可以輕易買到的伺服馬達，其控制方式很單純，就是輸入一個 PWM 訊號給它，然後藉由調整 PWM 訊號的脈衝寬度 來控制馬達轉軸應該轉到哪個角度並且固定維持在該角度。
這邊順便說明一下 PWM 訊號: 就是固定週期反覆發出的方形脈衝波，電壓隨時間的變化如下圖:
主要的特性參數大致上有兩個，一個是脈衝寬度，一個是週期時間: 每發脈衝的間隔時間。伺服馬達會接收一個固定週期約 20ms ( 頻率50Hz ) 的週期性方形脈衝波 (50Hz PWM)，並且會依照輸入的PWM訊號的脈衝寬度大小來調整轉軸應該轉到幾度的位置。一般來說 1.5 ms 的 PWM 訊號脈衝寬度大都是該馬達轉軸的中心位置，1.0 和 2.0 ms 的脈衝寬度對應到馬達轉軸順逆時針轉動的極限位置 ( -90 度和 +90 度)。 但是通常來說都會有許些的誤差，對於控制角度上有精確要求的話，就要想辦法報做校正的動作了。
一般的伺服馬達的接頭大都是一個三個端點的接頭如下所示:
最上面的端點是由PWM訊號產生裝置(在這裡是PCA9685)輸入 PWM 訊號給馬達用的，要注意的是馬達的主要 Power 來源並非由這裡來，這裡就只是單純的給控制訊號。最下面的端點是接地，而夾在中間的是馬達的電源輸入，看馬達的規格要小心輸入的電壓和吃電流的大小。一般來說馬達吃的電流都不小，而且在馬達運轉的時候會產生大量的雜訊，所以這個 5V 端點並不會是從 Raspberry Pi 板上直接拉出來，而是另外接一個可提供適當功率的 DC 直流電源。
I2C 控制 PCA9685
這個實驗之中至少需要 6 個 channel 的 PWM 輸出來控制伺服機械手臂， 但 Pi 的硬體 PWM 輸出數目不足。而利用軟體控制 GPIO 所實作出來的 PWM 訊號並不穩定，使用在機械手臂上的伺服馬達時，可能會讓整個手臂會不時的抖抖抖...。一般來說不會想要一支會自行抖抖抖的機械手臂。
因應這個原因才會採用外接 PWM 控制器 PCA9685 ，可以同時設定16 個 channel 各自的脈衝寬度，所以使用在伺服馬達的控制上正好合適。 特別是使用 I2C 通訊介面，使得Raspberry Pi 可以輕易地將其整合進來整個控制系統之中。
當然也要考慮到這樣的控制方案是否在反應速度上是否足夠。已知道控制伺服馬達的 PWM 訊號是 50 Hz ，也就是說伺服馬達對訊號的反應時間最快也就 20 ms。而Raspberry Pi 上的 I2C 訊號傳送速度據說預設是 100 kbits/sec，粗估傳送一個 byte 約只需要 0.01 ms，而和 PCA9685 進行一次操作設定也就最多 32 byte 的來回傳送。換句話說，Pi 和 PCA9685 一次指令的來回會在 1 ms 以下，故在反應速度上來說來得及。
有關 PCA9685 的控制方式可以參考文章:
Raspberry Pi2: 用 Python 透過 I2C 控制 16 頻道 PWM 模組 (PCA9685)
Arduino 模組
一開始最基礎的控制模組方案，可以直接用 Arduino 的兩個類比輸入讀取兩個香菇頭類比搖桿，再用四個數位輸入讀取四個按鈕。然後透過UART介面把控制資訊送給Raspberry Pi。如此一來可以學習到最基礎的控制器模組製作，但是到了後來考慮到操作上的舒適度與完整度之後，我們在Arduino系統上架上了搖桿模組。
Arduino + PS2 搖桿無線模組
在操作機械手臂的方式來說，為了研究各種場合、模擬人手動作、測試穩定性...等等方面的考量，能引入人性化手動操作方式是一個很重要的考量。
所以，我們使用目前應該是世界上最多人使用且最受歡迎的人機手動操控介面 -- PlayStation 2 遊樂器手把 -- (PS2 joystick)。該操作介面上有兩個類比 xy 軸類比控制器、一個數位方向控制盤 (十字鈕)、八顆按鈕，並有優秀的人體工學操作介面廣為世人所熟悉，操作起來樂趣無窮。加上價格便宜、使用方便; 比起自行去買齊兩個Arduino的類比香菇頭八個按鈕還要便宜。因為在 Arduino 上有現成的 Library 可以直接去驅動它，所以這裡我們用 Arduino 去讀取搖桿的指令，並且透過 UART 介面傳送自行設計的封包到 Pi 上面。
USB 搖桿模組
但是後來發現，我們買的PS2 無線模組上面有附 USB 介面，下圖為訊息接受模組，上面有一個 USB mini 接孔。
插上在 Pi 的 USB port 之後，Linux 系統會自動認出是一個標準的 USB Joystick 模組並自動掛載，然後就可以透過檔案系統直接讀取搖桿傳過來的訊息。而網路上已經有人寫好用 Python 來和 USB Joystick 溝通的範例程式，可以參考一下。下面是範例程式所在的 Github:
https://gist.github.com/rdb/8864666
一般來說，會再把這份Code包裝成一個 class 以利使用。這樣一來，整個硬體系統架設上少了外接Arduino之後，整個系統變得非常的簡潔有力。
程式架構
如下圖所示，整個程式架構就是利用兩個物件: arm 物件控制機械手臂，joystick 物件讀取搖桿資訊。而主程式 main 則用來進行連結與整個系統的運作。
機械手臂控制物件 arm，裡面包含了一個 pwm 物件負責透過 I2C 和 PCA9685 做溝通以此控制所有的伺服馬達。設計上，整個機械手臂定義了六個馬達的名稱分別為: "stage", "shoulder", "elbow", "wrist", "writs_twist", "finger"如下圖所示。在 arm 物件上會設計一組 API 使控制六組伺服馬達時的程式能更為直接簡單。而對於透過 I2C 介面來操作 PCA9685 的部份，則交由另一個物件來處理。
PWM 控制物件，主要負責用 I2C 和 PCA9685 IC 做溝通，和 I2C 介面的溝通一般是使用 package: python-smbus 所提供的 API 來達成。要注意的是，使用該 API 在 Raspbian 的環境底下要先載入 Linux kernel module : i2c-dev 才能使用。
Joystick 物件，負責接收搖桿的控制訊息。這邊一共使用了三種硬體設置: Arduino，Arduino + PS2 joystick，USB Joystick。
主程式--操作模式。其主要的功能就是從 Joystick 物件讀取指令，然後根據指令來操作機械手臂控制物件 。因為需要控制軸數只有六軸，又是結構上又類似於人的手臂，所以在程式上的手動操作模式就變得相對單純。最簡單可以用兩個類比香菇頭控制元件對應四個馬達"stage", "shoulder", "elbow", "writs"，然後四顆按鈕對應"wrist_twister"和"finger"的正反轉。但在實際操作過一陣子之後就會發現不太直覺與人性化，而需要其他風格的操作模式。所以在主程這邊要做的事情其實是--提供不同的操作模式。
如果有玩過像是飛行模擬的遊戲的話，應該都會知道遊戲裡面會提供一些設定給玩家操做不同風格的飛機操控。像是可以選擇類比搖桿往下推，飛機是會拉升機身的動作還是下壓機身的動作。類比搖桿的左右方向，是讓機身做出左右翻滾還是轉向的動作。
不同的控制模式對應不同的應用場合，而在這個實驗之中，我們會在主程式實作兩個動作控制模式。第一種，一個控制元件對應一個馬達，是最簡單的控制模式。第二種則是有某種程度上的連動，控制上會比較直覺。而之後也可以針對目的開始實作各種自動控制模式，或再加裝某些Sensor來做回饋而達到一些自動修正行為。
程式碼簡介
主迴圈程式碼如下:
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| #!/usr/bin/env python | |
| from multiprocessing import Process, Queue | |
| from time import sleep | |
| import signal, os | |
| from PCA9685 import PCA9685 | |
| from Joystick_usb import joystick_usb | |
| from Arm import Arm | |
| from LCM import LCM1602 | |
| pwm = '' | |
| system_stop = False | |
| ## signal handler | |
| def sigint_handler(signum, frame): | |
| global system_stop | |
| system_stop = True | |
| ## main program START | |
| if __name__ == '__main__': | |
| ########################## | |
| ## Program initialization | |
| ########################## | |
| signal.signal(signal.SIGINT, sigint_handler) | |
| ## START PCA9685 module | |
| pwm = PCA9685() | |
| pwm.reset() | |
| pwm.wakeup() | |
| ## setup the mech arm pwm channel | |
| motor_set_dic = { | |
| "stage": 15, | |
| "shoulder": 14, | |
| "elbow": 13, | |
| "wrist": 12, | |
| "wrist_twist" : 11, | |
| "finger": 10, | |
| } | |
| arm = Arm(pwm, motor_set_dic) | |
| ## setup the Joystick | |
| joystick = joystick_usb() | |
| ## setup the LCM module to print the motor position | |
| mylcd = LCM1602(debug_mode = False) | |
| ######################## | |
| ## Program Main Loop | |
| ######################## | |
| while(not system_stop): | |
| motor_pos = [] | |
| # this delay time control the response time of the arm | |
| sleep(0.02) | |
| if(arm.control_mode == 0): | |
| arm.moveByJoystickPS2_mode0(joystick.read()) | |
| elif(arm.control_mode == 1): | |
| arm.moveByJoystickPS2_mode1(joystick.read()) | |
| # send motor position information to LCM module | |
| for i in range(6): | |
| motor_pos.append(arm.pwm.getValChOff(15-i)) | |
| mylcd.the_queue.put(motor_pos) | |
| ###################################### | |
| # Program STOP when accept ctrl+c | |
| ###################################### | |
| joystick.close() | |
| mylcd.close() | |
------------------------
主迴圈程式碼的目的在於整合所有的物件，所以結構上來說相對簡單。程式的執行大約分為三個階段: 一開始的準備、主要的無窮迴圈、程式結束後的收尾。
為了使整個程式的流程易於維護，我們把整個系統分為joystick、arm、mylcd 三個主要的物件來分別負責控制。
joystick 物件要負責接受搖桿傳來的訊號，並轉換為我們自行定義的指令格式。主程式之中主要只會使用 joystick 物件的 read 成員函式，來讀取自定義指令。所以在實驗中不管搖桿的訊號傳輸介面為 UART 或 USB，面對傳輸介面實作好I/O的部份，而面向主程式實作好相同的成員函式，就無需再修改主程式。USB 介面和 UART 介面的 Joystick 類別部份的程式碼放在github空間之上。
arm 物件會實作兩個成員函式 move_mode0() 和 move_mode1() 提供兩種不同的操作模式，傳入自定義的指令後會透過 PCA9685 PWM 控制器來操作六軸機械手臂。兩種操作模式的差別在於，是否將肘部和腕部的伺服馬達做特定的連動關係。這樣子的連動可以在操作上得到"手前臂往前伸或打開" 感覺，而達到更為直觀的操作。而未來也可以更進一步的擴充更為抽像複雜的動作模式。因為 arm 物件會使用到 PCA9685 物件，所以程式的一開始就宣告了一個 pwm 物件並將其設定為 arm 物件的內部參數。之後所有針對伺服馬達的操作就都在 arm 物件進行。
在機械人的應用，必須考慮系統獨立操作的情形。在這樣的情況之下，系統本身必須有可以馬上顯示重要訊息的子系統。因此在硬體上我們使用教學子板的 LCM 連接埠來外接一個LCM1602的顯示模組。在主程式中也宣告了一個 LCM1602 物件透過 GPIO 控制該顯示模組，讓整個 Pi 的六軸機械手臂系統在脫離電腦連線時的獨立操作情形下，也能顯示內部重要的資訊。而在這邊我們則是直接顯示六個伺服馬達相對應的 PCA9685 的設定值。
在各個子系統都使用物件類別的方式實作完之後，主程式的結構就可以簡化很多，只需要整合各各子系統來達成目的。因此一開始宣告物件: pwm、arm、mylcd、joystick。然後整合 pwm 物件到 arm 物件之中，這樣 arm 物件就可以接收指令來控制六軸機械手臂。mylcd 物件顯示系統內部資訊。joystick 物件監視 USB 搖桿傳送過來的訊號並轉換成自訂的指令格式轉輸給 arm 物件。而主程式的無窮迴圈運用 sleep 函式做簡易的時程控制。
在實作完手動模式之後，未來也可以實作一些自動化的功能以模擬工廠生產所需要的動作流程。下面執行結果的影片之中，展示了手動操作模式和一組簡易模擬機械手臂將貨物由 A 位置移到 B 位置的動作流程。
相關文章:
Raspberry Pi2: 用 Python 透過 I2C 控制 16 頻道 PWM 模組 (PCA9685)
執行結果
沒有留言 :
張貼留言