Python可以這樣玩(25)：MeArm 自動控制

用搖桿來控制機器手臂，是 MeArm 機器手臂的套件，可以讓我們知道自行組裝的機器手臂是否正常運作，也順便練習一下操作技巧。

由於我是從事工廠自動化產線設計的工作，所以機器手臂對我而言，應該是讓它能夠加入生產的工作，如果要達到這個目的，我們就可以使用 Python 程式來控制機器手臂。

首先，請上傳 Standard Firmata 到開發板，所有的線路都可以保留，雖然我們用不到搖桿，也不用拆掉，因為哪天想玩搖桿夾娃娃的時候，只要上傳上一個單元的 Arduino 程式即可。

角度初始化

在Python 程式的最前面，請複製以下的程式碼，前面的單元都已經介紹過，相信大家都很熟悉：

from pyfirmata import Arduino

from pyfirmata import SERVO

from time import sleep

# Setting up the Arduino board

port = 'COM3'

board = Arduino(port)

# Need to give some time to pyFirmata and Arduino to synchronize

sleep(5)

sleep 五秒鐘是為了讓電腦與開發板能夠同步，接下來就是定義 PIN 腳位以及初始的角度：

# 設定底座，左邊，右邊，鉗爪的 pin

pinBottom, pinLeft, pinRight, pinHand = 9, 10, 11, 6

bAngle, lAngle, rAngle, hAngle = 90, 90, 90, 0

請注意，四個PIN腳一定要支援PWM，當然，如果上一個單元您是按部就班做完的，這裡就沒有爭議，然後定義四個伺服馬達的角度。底座、左側(上下)、右側(前後)都定義成90度，鉗爪定義成 0度，也就是張開。

在我的實際的程式中，我將 bAngle 定義成 75度，這是因為我在安裝的時候，把伺服馬達卡上底做的時候稍微震動，所以角度跑掉了一些，就會發現90度的時候有點歪，其實不需要拆掉重新校正再重新安裝，在程式中修改一點角度就可以。

接下來我寫一個簡單的函數setServoAngle(pin, angle)，去控制特定伺服馬達的角度，然後分別把四個伺服馬達物件定義出來，再轉到初始位置，程式如下：

# Custom angle to set Servo motor angle

def setServoAngle(pin, angle):

    board.digital[pin].write(angle)

    sleep(0.015)

# 初始化並設定初始角度

board.digital[pinBottom].mode = SERVO

setServoAngle(pinBottom, bAngle)

board.digital[pinLeft].mode = SERVO

setServoAngle(pinLeft, lAngle)

board.digital[pinRight].mode = SERVO

setServoAngle(pinRight, rAngle)

board.digital[pinHand].mode = SERVO

setServoAngle(pinHand, hAngle)

這裡有個地方要注意，在定義了一個伺服馬達之後，隨後要馬上設定初始角度，逐一做完。不可以先把四個伺服馬達物件定義好之後，再逐一設定初始角度，這樣的話手臂會出現怪異的動作。(這是實驗結果)

機器手臂的Hello World!

按照慣例，我們要寫 Hello World!，當然，機器手臂不會說話，也不會寫字，我是用幾個動作讓它跟世界Say Hello。

我就讓它搖頭一次、抬頭三次、前後兩次、最後再磕頭兩次。

在做這些動作之前，我利用四個變數curBtmAngel, curLftAngle, curRhtAngle, curHndAngle來記憶四個伺服馬達的目前角度，這是為了讓機器手臂的動作可以連續，後面的角度會接著目前的角度做，當然，一開始的角度是由四個初始角度bAngle, lAngle, rAngle, hAngle來的。

整個搖頭的動作，就是讓底做的伺服馬達轉向右邊60度、再轉到左邊60度(若是由右到左，就等於向左轉120度)、然後回正(再向右轉60度)。程式碼如下：

curBtmAngel,curLftAngle,curRhtAngle,curHndAngle =   

bAngle,lAngle,rAngle,hAngle

print('搖頭一次:')

for s in range(1):

    angle = 60

    for i in range(angle):

        setServoAngle(pinBottom, curBtmAngel-i)

    curBtmAngel = curBtmAngel - angle

    angle = 120

    for i in range(angle):

        setServoAngle(pinBottom, curBtmAngel+i)

    curBtmAngel = curBtmAngel + angle

    angle = 60

    for i in range(angle):

        setServoAngle(pinBottom, curBtmAngel-i)

    curBtmAngel = curBtmAngel - angle

print('底盤角度',curBtmAngel)

我為了要讓伺服馬達很平順地轉動，所以我使用了 for 迴圈，讓它一度一度的轉，這樣看起來比較有機器手臂的感覺，如果真的夾娃娃的時候才不會掉下來。轉動完畢之後，再修正新的現行角度變數。另外，我前後使用了兩個 print() 函數把目前的執行狀態顯示出來，方便查看。

如果您夠細心，一定會發現明明只有搖頭一次，為何我在最外層還用了一個for 迴圈，而且該迴圈根本只執行一次。這是為了方便大家修改，如果想改成搖頭三次，只要把 range(1) 改成 range(3) 即可。

接下來就是抬頭三次，跟上面的程式碼很像，在這裡我並沒有用很複雜的函數去包裝這些動作，而是平舖直述，主要目的是為了可讀性，程式碼如下：

print('抬頭三次:')

for s in range(3):

    for i in range(angle):

        setServoAngle(pinLeft, curLftAngle+i)

    curLftAngle = curLftAngle + angle

    for i in range(angle):

        setServoAngle(pinLeft, curLftAngle-i)

    curLftAngle = curLftAngle - angle

print('高低角度',curLftAngle)

前後兩次也差不多，其實我的目的就是讓機器手臂的三個關節分別活動一下：

print('前後兩次:')

for s in range(2):

    angle = 60

    for i in range(angle):

        setServoAngle(pinRight, curRhtAngle-i)

    curRhtAngle = curRhtAngle - angle

    angle = 120

    for i in range(angle):

        setServoAngle(pinRight, curRhtAngle+i)

    curRhtAngle = curRhtAngle + angle

    angle = 60

    for i in range(angle):

        setServoAngle(pinRight, curRhtAngle-i)

    curRhtAngle = curRhtAngle - angle

print('前後角度',curRhtAngle)

最後一個動作，磕頭兩次就不太一樣了，這次必須一次動到兩個關節，這個動作的目的是讓讀者知道如何同時運動多個關節，程式碼如下：

print('磕頭兩次:')

angle = 60

for s in range(2):

    for i in range(angle):

        setServoAngle(pinLeft, curLftAngle-i)

        setServoAngle(pinRight, curRhtAngle+i)

    curLftAngle = curLftAngle - angle

    curRhtAngle = curRhtAngle + angle

    for i in range(angle):

        setServoAngle(pinLeft, curLftAngle+i)

        setServoAngle(pinRight, curRhtAngle-i)

    curLftAngle = curLftAngle + angle

    curRhtAngle = curRhtAngle - angle

print('磕頭角度',curLftAngle,curRhtAngle)

上面同時運動兩個關節的寫法有一個條件，就是移動的角度必須相同，這裡的例子是60度，才能放再同一個迴圈裡面，如果角度不同，那麼就要自行調整寫法了。可以加入if 指令另行設計。

這幾個動作做完之後，手臂又會回到初始位置，我是故意這樣設計的，方便後續的動作。

夾取物品

打完招呼之後，就是進入重頭戲，我們想像一個場景，在自動化產線上面，需要用一個機器手臂把半成品從一條產線移動到另一條產線上，所以我們就要模擬 MeArm 從右邊把一個歐治鼻空瓶子(沒有打廣告的意思，我不小心找到的空瓶，其他東西太重)夾起來，然後轉到左邊，在輕輕放下。

為了讓機器手臂的動作可以成為一個循環，所以接下來我讓機器手臂在從左邊把歐治鼻夾起來，放回右邊，這樣就可以一直循環下去。

當我們要將右邊的瓶子夾起，對機器手臂而言，其實就包含了轉、下、去、夾、上、回，這幾個動作，要在左邊放下，又必須有轉、去、下、開、回、上，這些動作。其中 “上下”、 “去回”、 “開夾” 這些都是相對應的動作，必須先弄清楚。

完整的程式碼如下，可以接在 Hello World! 的程式碼後面，我讓它夾兩次並且形成一個迴圈，我直接在程式中用#紅色說明：

print('夾兩次:')

print('右轉,curBtmAngel=',curBtmAngel)

angle = 45   #右轉 45度

for i in range(angle):

    setServoAngle(pinBottom, curBtmAngel-i)

curBtmAngel = curBtmAngel - angle

sleep(0.5)

print('下,curBtmAngel=',curBtmAngel)

angle = 40   #這個角度剛好會到瓶子頸部，測試的結果

for i in range(angle):

    setServoAngle(pinLeft, curLftAngle-i)

curLftAngle = curLftAngle - angle

sleep(0.5)

for s in range(3):

    #從這裡開始進入迴圈，可以自行修改上面的3次

    print('去,curLftAngle=',curLftAngle)

    angle = 40    #這個角度剛好會到瓶子頸部，我讓下、去的角度相同

    for i in range(angle):

        setServoAngle(pinRight, curRhtAngle+i)

    curRhtAngle = curRhtAngle + angle

    sleep(0.5)

    print('夾,curRhtAngle=',curRhtAngle)

    angle = 45    #這個角度可以夾緊

    for i in range(angle):

        setServoAngle(pinHand, curHndAngle+i)

    curHndAngle = curHndAngle + angle

    sleep(0.5)

    print('上,curHndAngle=',curHndAngle)

    angle = 40

    for i in range(angle):

        setServoAngle(pinLeft, curLftAngle+i)

    curLftAngle = curLftAngle + angle

    sleep(0.5)

    print('回,curLftAngle=',curLftAngle)

    angle = 40

    for i in range(angle):

        setServoAngle(pinRight, curRhtAngle-i)

    curRhtAngle = curRhtAngle - angle

    sleep(0.5)

    print('大左轉,curRhtAngle=',curRhtAngle)

    angle = 90

    for i in range(angle):

        setServoAngle(pinBottom, curBtmAngel+i)

    curBtmAngel = curBtmAngel + angle

    sleep(0.5)

    print('去,curBtmAngel=',curBtmAngel)

    angle = 40 - 4 #原本40度，我故意減少4度造成回拉效果

    for i in range(angle):

        setServoAngle(pinRight, curRhtAngle+i)

    curRhtAngle = curRhtAngle + angle

    sleep(0.5)

    print('下,curRhtAngle=',curRhtAngle)

    angle = 40

    for i in range(angle):

        setServoAngle(pinLeft, curLftAngle-i)

    curLftAngle = curLftAngle - angle

    sleep(0.5)

    print('開,curLftAngle=',curLftAngle)

    angle = 45

    for i in range(angle):

        setServoAngle(pinHand, curHndAngle-i)

    curHndAngle = curHndAngle - angle

    sleep(0.5)

    print('回,curHndAngle=',curHndAngle)

    angle = 40 - 4 #去的動左少4度，這裡需要回正也要少4度

    for i in range(angle):

        setServoAngle(pinRight, curRhtAngle-i)

    curRhtAngle = curRhtAngle - angle

    sleep(0.5)

    print('去,curLftAngle=',curLftAngle)

    angle = 40

    for i in range(angle):

        setServoAngle(pinRight, curRhtAngle+i)

    curRhtAngle = curRhtAngle + angle

    sleep(0.5)

    print('夾,curRhtAngle=',curRhtAngle)

    angle = 45

    for i in range(angle):

        setServoAngle(pinHand, curHndAngle+i)

    curHndAngle = curHndAngle + angle

    sleep(0.5)

    print('上,curHndAngle=',curHndAngle)

    angle = 40

    for i in range(angle):

        setServoAngle(pinLeft, curLftAngle+i)

    curLftAngle = curLftAngle + angle

    sleep(0.5)

    print('回,curLftAngle=',curLftAngle)

    angle = 40

    for i in range(angle):

        setServoAngle(pinRight, curRhtAngle-i)

    curRhtAngle = curRhtAngle - angle

    sleep(0.5)

    print('大右轉,curRhtAngle=',curRhtAngle)

    angle = 90

    for i in range(angle):

        setServoAngle(pinBottom, curBtmAngel-i)

    curBtmAngel = curBtmAngel - angle

    sleep(0.5)

    print('去,curBtmAngel=',curBtmAngel)

    angle = 40 - 4 # 回拉，同上

    for i in range(angle):

        setServoAngle(pinRight, curRhtAngle+i)

    curRhtAngle = curRhtAngle + angle

    sleep(0.5)

    print('下,curRhtAngle=',curRhtAngle)

    angle = 40

    for i in range(angle):

        setServoAngle(pinLeft, curLftAngle-i)

    curLftAngle = curLftAngle - angle

    sleep(0.5)

    print('開,curLftAngle=',curLftAngle)

    angle = 45

    for i in range(angle):

        setServoAngle(pinHand, curHndAngle-i)

    curHndAngle = curHndAngle - angle

    sleep(0.5)

    print('回,curHndAngle=',curHndAngle)

    angle = 40 - 4 # 回正

    for i in range(angle):

        setServoAngle(pinRight, curRhtAngle-i)

    curRhtAngle = curRhtAngle - angle

    sleep(0.5)

#迴圈結束，準備回到原始位置

print('上,curHndAngle=',curHndAngle)

angle = 40

for i in range(angle):

    setServoAngle(pinLeft, curLftAngle+i)

curLftAngle = curLftAngle + angle

sleep(0.5)

print('左轉,curLftAngle=',curLftAngle)

angle = 45

for i in range(angle):

    setServoAngle(pinBottom, curBtmAngel+i)

curBtmAngel = curBtmAngel + angle

sleep(0.5)

board.exit()

在程式碼裡面只有一個地方比較奇怪，就是我做了一個回拉的動作，那是因為我們的機器手臂很陽春，並沒有辦法做到像專業的機器手臂那樣的精確，所以如果我用 40度把東西夾起來，也用40度把東西放下的時候，位置會偏掉，下次要夾的時候就會夾不到。因此我夾起的時候用 40度，而要放下的時候，我故意上它把東西放前面一點(減4度)，這樣下次夾到的機率會比較高。

當然，在一個地方少移動4度回拉之後，它的相對動作就必須也少移動4度，才能回正。這樣，增加了夾到東西的機率，也保證整個迴圈的動作做完能夠回到正確的位置。

(下面是影片，可以看到夾取的動作)

最後我再列出完整的程式碼，理論上複製貼上就可以執行：

from pyfirmata import Arduino

from pyfirmata import SERVO

from time import sleep

# Setting up the Arduino board

port = 'COM3'

board = Arduino(port)

# Need to give some time to pyFirmata and Arduino to synchronize

sleep(5)

# 設定底座，左邊，右邊，鉗爪的 pin

pinBottom, pinLeft, pinRight, pinHand = 9, 10, 11, 6

bAngle, lAngle, rAngle, hAngle = 75, 90, 90, 0    #75原本是90，目的是校正

# Custom angle to set Servo motor angle

def setServoAngle(pin, angle):

    board.digital[pin].write(angle)

    sleep(0.025)

# 初始化並設定初始角度

board.digital[pinBottom].mode = SERVO

setServoAngle(pinBottom, bAngle)

board.digital[pinLeft].mode = SERVO

setServoAngle(pinLeft, lAngle)

board.digital[pinRight].mode = SERVO

setServoAngle(pinRight, rAngle)

board.digital[pinHand].mode = SERVO

setServoAngle(pinHand, hAngle)

curBtmAngel,curLftAngle,curRhtAngle,curHndAngle = bAngle,lAngle,rAngle,hAngle

print('夾兩次:')

print('右轉,curBtmAngel=',curBtmAngel)

angle = 45

for i in range(angle):

    setServoAngle(pinBottom, curBtmAngel-i)

curBtmAngel = curBtmAngel - angle

sleep(0.5)

print('下,curBtmAngel=',curBtmAngel)

angle = 40

for i in range(angle):

    setServoAngle(pinLeft, curLftAngle-i)

curLftAngle = curLftAngle - angle

sleep(0.5)

for s in range(3):

    print('去,curLftAngle=',curLftAngle)

    angle = 40

    for i in range(angle):

        setServoAngle(pinRight, curRhtAngle+i)

    curRhtAngle = curRhtAngle + angle

    sleep(0.5)

    print('夾,curRhtAngle=',curRhtAngle)

    angle = 45

    for i in range(angle):

        setServoAngle(pinHand, curHndAngle+i)

    curHndAngle = curHndAngle + angle

    sleep(0.5)

    print('上,curHndAngle=',curHndAngle)

    angle = 40

    for i in range(angle):

        setServoAngle(pinLeft, curLftAngle+i)

    curLftAngle = curLftAngle + angle

    sleep(0.5)

    print('回,curLftAngle=',curLftAngle)

    angle = 40

    for i in range(angle):

        setServoAngle(pinRight, curRhtAngle-i)

    curRhtAngle = curRhtAngle - angle

    sleep(0.5)

    print('大左轉,curRhtAngle=',curRhtAngle)

    angle = 90

    for i in range(angle):

        setServoAngle(pinBottom, curBtmAngel+i)

    curBtmAngel = curBtmAngel + angle

    sleep(0.5)

    print('去,curBtmAngel=',curBtmAngel)

    angle = 40 - 4 #回拉

    for i in range(angle):

        setServoAngle(pinRight, curRhtAngle+i)

    curRhtAngle = curRhtAngle + angle

    sleep(0.5)

    print('下,curRhtAngle=',curRhtAngle)

    angle = 40

    for i in range(angle):

        setServoAngle(pinLeft, curLftAngle-i)

    curLftAngle = curLftAngle - angle

    sleep(0.5)

    print('開,curLftAngle=',curLftAngle)

    angle = 45

    for i in range(angle):

        setServoAngle(pinHand, curHndAngle-i)

    curHndAngle = curHndAngle - angle

    sleep(0.5)

    print('回,curHndAngle=',curHndAngle)

    angle = 40 - 4 #回正

    for i in range(angle):

        setServoAngle(pinRight, curRhtAngle-i)

    curRhtAngle = curRhtAngle - angle

    sleep(0.5)

    print('去,curLftAngle=',curLftAngle)

    angle = 40

    for i in range(angle):

        setServoAngle(pinRight, curRhtAngle+i)

    curRhtAngle = curRhtAngle + angle

    sleep(0.5)

    print('夾,curRhtAngle=',curRhtAngle)

    angle = 45

    for i in range(angle):

        setServoAngle(pinHand, curHndAngle+i)

    curHndAngle = curHndAngle + angle

    sleep(0.5)

    print('上,curHndAngle=',curHndAngle)

    angle = 40

    for i in range(angle):

        setServoAngle(pinLeft, curLftAngle+i)

    curLftAngle = curLftAngle + angle

    sleep(0.5)

    print('回,curLftAngle=',curLftAngle)

    angle = 40

    for i in range(angle):

        setServoAngle(pinRight, curRhtAngle-i)

    curRhtAngle = curRhtAngle - angle

    sleep(0.5)

    print('大右轉,curRhtAngle=',curRhtAngle)

    angle = 90

    for i in range(angle):

        setServoAngle(pinBottom, curBtmAngel-i)

    curBtmAngel = curBtmAngel - angle

    sleep(0.5)

    print('去,curBtmAngel=',curBtmAngel)

    angle = 40 - 4 # 回拉

    for i in range(angle):

        setServoAngle(pinRight, curRhtAngle+i)

    curRhtAngle = curRhtAngle + angle

    sleep(0.5)

    print('下,curRhtAngle=',curRhtAngle)

    angle = 40

    for i in range(angle):

        setServoAngle(pinLeft, curLftAngle-i)

    curLftAngle = curLftAngle - angle

    sleep(0.5)

    print('開,curLftAngle=',curLftAngle)

    angle = 45

    for i in range(angle):

        setServoAngle(pinHand, curHndAngle-i)

    curHndAngle = curHndAngle - angle

    sleep(0.5)

    print('回,curHndAngle=',curHndAngle)

    angle = 40 - 4 # 回正

    for i in range(angle):

        setServoAngle(pinRight, curRhtAngle-i)

    curRhtAngle = curRhtAngle - angle

    sleep(0.5)

print('上,curHndAngle=',curHndAngle)

angle = 40

for i in range(angle):

    setServoAngle(pinLeft, curLftAngle+i)

curLftAngle = curLftAngle + angle

sleep(0.5)

print('左轉,curLftAngle=',curLftAngle)

angle = 45

for i in range(angle):

    setServoAngle(pinBottom, curBtmAngel+i)

curBtmAngel = curBtmAngel + angle

sleep(0.5)

board.exit()

玩到這裡，您就可以藉由修改上面的程式碼，試著拿不同的東西來夾，在整個夾取的過程裡面，您一定會發現整個機器手臂在左右轉的時候感覺不是很順，這也沒辦法，因為底盤的伺服馬達承受整個手臂的重量，小小的伺服馬達又不是很夠力，只能接受了。

當然，您還可以在搖桿上面做改變，例如使用藍芽模組來連接，就可以用手機來控制機器手臂。或者，加上 Raspberry Pi 模組讓 Python在上面執行來操作機器手臂，這樣就可以脫離電腦了。幾個單元之後，我們就會進入 Raspberry Pi 的領域了。

影片DEMO如下：