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如下：
留言
張貼留言