[AI, Python, 深蹲, squat] Python 蹲的門面

 Python 蹲的門面

Ted Lee的土饅頭（To Mentor）工坊

Line：ted2016.kpvs
Email：Lct4246@gmail.com
FB：http://gg.gg/TedLeeFB/

Blog：http://gg.gg/TedLeeBlog/

Jan. 15, 2023

[1]

在筆者的先前拙著《用變數傾印法玩 Python 蹲》（https://reurl.cc/LXk1qx）一文中，我們成功地在陳會安老師研發的 fChart 中的 Thonny IDE 測試了深蹲偵測及計次範例。本文將進一步地甞試以 fChart 內建的 Tinker 套件 [2] 為之再加上一套使用者介面（user interface，UI）來梳妝打扮一番。

新增功能

在本文中，我們將為《用變數傾印法玩 Python 蹲》範例擴充下列兩個新功能：

1.  改用攝影機即時取得的影像（而非事先預錄的影片）。
2. 增設深蹲達標關卡（為了便於測試，暫訂為深蹲三下）。

 

UI 佈局（layout）

在圖 1 上方，我們設計了一個 3×2 大小的畫面，第一行顯示專案名稱（Python 蹲）與版次（V1.0），第二行顯示由攝影機取得的即時影像與相關訊息，第三行為留白及作者姓名（Ted Lee）。圖 1 下方為 Tinker 刻出來的畫面。

 

圖 1：UI 設計圖

在圖 2 中，我們先建立最外層的視窗 window 元件（component），再置入 frmTitle、frmVideo 和 frmTeam 三個框架元件（Frame）。緊接著，在 frmTitle 中擺放 lblTitle 和 lblVersion、在 frmVideo 中擺放 lblVideo 和 lblResult、在 frmTeam 中擺放 lblSpace 和 lblTeam。所有元件皆以 pack 佈局方式安排。詳如如圖 2 所示。

圖 2：UI 元件佈局

程式邏輯

我們將程式執行的流程畫成圖 3 的流程圖。此外，程式採模組化程式設計（modularized programming）的方法，將獨立的功能以下列三個副程式來達成：

1. detectPose()：偵測姿勢。
2. updateUI()：即時更新影像標籤 lblVideo。
3. cleanUp()：程式結束，釋放已配置資源（allocated resources）。

圖 3：本文的流程圖

本文的執行結果請參考 https://youtu.be/Gm6o7m-QH6Y。完整程式碼詳列如下（讀者亦可由 https://reurl.cc/rZ7aob 下載）。

import cv2

import numpy as np

from PIL import ImageFont, ImageDraw, Image

    

from tkinter import *

from PIL import Image, ImageTk

from PoseModule import PoseDetector

dir = 0 

count = 0

def updateUI():

    global img, lblVideo

    

    cv2image = cv2.cvtColor(img, cv2.COLOR_BGR2RGBA)

    img = Image.fromarray(cv2image)

    imgtk = ImageTk.PhotoImage(image = img)    

    lblVideo.imgtk = imgtk

    lblVideo.config(image = imgtk)

    lblVideo.after(100, detectPose)

    

    if count > 3:

       lblResult.config(text = '你已達標！')   

    

def detectPose():

    global cap, detector, dir, count, img

    success, img = cap.read()

    if success:

        img = cv2.resize(img, (640, 480))

        h, w, c = img.shape

        pose, img = detector.findPose(img, draw=True)

        

        if pose:

            lmList = pose["lmList"]

            angle, img = detector.findAngle(lmList[24], lmList[26], lmList[28], img)

            bar = np.interp(angle, (85, 180), (w//2-100, w//2+100))

            cv2.rectangle(img, (w//2-100, 50), (int(bar), 100), (0, 255, 0), cv2.FILLED)

            if angle <= 150:                

                if dir == 0:

                    count = count + 0.5

                    dir = 1

                    

            if angle >= 170: 

                if dir == 1:

                    count = count + 0.5

                    dir = 0

                    

            msg = str(int(count))        

            cv2.putText(img, msg, (100, 150), cv2.FONT_HERSHEY_SIMPLEX, 5, (255, 255, 255), 20)

            

        cv2.imshow("Pose", img)

        updateUI()

def cleanUp():

    window.destroy()

    cap.release()

    cv2.destroyAllWindows()

cap = cv2.VideoCapture(0) #####開啟攝影機

detector = PoseDetector()

#開 UI ---------------------------------------------------------------------------------------------------------   

#widow

window = Tk()

window.title('Python 蹲')

#window.geometry('1024x768')

window.attributes('-fullscreen', True)

#frame

frmTitle = Frame(window)

frmTitle.pack(side = TOP, fill = BOTH, expand = YES)

frmVideo = Frame(window)

frmVideo.pack(side = TOP, fill = BOTH, expand = YES)   

frmTeam = Frame(window)

frmTeam.pack(side = BOTTOM, fill = BOTH, expand = YES)

#Title

lblTitle = Label(frmTitle, text = "Python 蹲", fg = '#005E5E', bg = '#666666', font = ('標楷體', 70), borderwidth = 0, highlightthickness = 0, relief = "sunken") 

lblTitle.pack(side = LEFT, fill = BOTH, expand = YES)

lblVersion = Label(frmTitle, text = "V1.0", fg = '#005E5E', bg = '#666666', font = ('標楷體', 20), borderwidth = 0, highlightthickness = 0, relief = "sunken") 

lblVersion.pack(side = RIGHT, fill = BOTH, expand = YES)

#Video

lblVideo = Label(frmVideo, bg = '#005E5E', borderwidth = 0, highlightthickness = 0, relief = "ridge")

lblVideo.pack(side = LEFT, fill = BOTH, expand = YES)

lblResult = Label(frmVideo, text = "", fg = 'white', bg = '#005E5E', font = ('標楷體', 40), borderwidth = 0, highlightthickness = 0, relief = "sunken")

lblResult.pack(side = RIGHT, fill = BOTH, expand = YES)

#Team

lblSpace = Label(frmTeam, text = "", fg = '#005E5E', bg = '#666666', font = ('標楷體', 20), borderwidth = 0, highlightthickness = 0, relief = "sunken") 

lblSpace.pack(side = LEFT, fill = BOTH, expand = YES)

lblTeam = Label(frmTeam, text = "Ted Lee", fg = '#005E5E', bg = '#666666', font = ('標楷體', 20), borderwidth = 0, highlightthickness = 0, relief = "sunken") 

lblTeam.pack(side = RIGHT, fill = BOTH, expand = YES)

#姿勢識別 ---------------------------------------------------------------------------------------------------

detectPose()

    

window.bind('<Escape>', lambda e: cleanUp())

mainloop()

進階問題

1. 建置一「分數雲端排行榜」來記錄玩家的深蹲分數。
2. 修改程式，使得深蹲次數達標後出現是否「重新訓練」詢問。

---

[1] 六種授權條款。

[2] 請參考拙著《AI 成績查詢系統》（https://reurl.cc/aaWlKl）一文。

[AI, Python, 深蹲, squat] 用變數傾印法玩 Python 蹲

用變數傾印法玩 Python 蹲

Ted Lee的土饅頭（To Mentor）工坊

Line：ted2016.kpvs
Email：Lct4246@gmail.com
FB：http://gg.gg/TedLeeFB/

Blog：http://gg.gg/TedLeeBlog/

Jan. 14, 2023

[1]

本文將一門深入陳會安老師在 https://reurl.cc/mZ1mD1 分享的《看圖學Python人工智慧程式設計》（https://reurl.cc/rZW79r）一書中的範例。此範例使用了 Google  MediaPipe（https://reurl.cc/nZg8mX）跨平台機器學習解決方案，讓我們得以使用短短的 4x 行 Python 程式碼就能實現深蹲（squat）姿勢的判別並能計次。

會安老師說：「只要懂得程式語言三大結構（循序、選擇與重複），再了解一點 Python 程式語言的語法，就能開始玩 AI了！」讀者們，Python 蹲、Python 蹲，Python 蹲完換你們蹲了喔！

開發環境建置

本文使用會安老師開發的 fChart 流程圖轉語法中的 Thonny IDE [2]，它已裝妥了 CVZone 與 Tinker 套件。

下載　英文版「fChartThonny6_cvzone_en.zip」可由 https://reurl.cc/gQ59Vb 下載。此外，深蹲範例程式的原始碼「Squat.zip」可由 https://reurl.cc/x1qzE5 下載。

解壓縮　將上述的兩支 .zip 檔解壓縮至磁機的「根」目錄（root directory）下（例如：c:\），如圖 1 所示。

 

圖 1：將「fChartThonny6_cvzone_en.zip」和「Squat.zip」解壓縮至 c:\

Thonny　雙撃「C:\fChartThonny6_cvzone_en\WinPython」路徑下的「thonny.vbs」以開啟 Thonny（圖 2）。

圖 2：開啟 Thonny IDE

Squat.py　在視窗左手邊的 Files 檔案管理中切換到「C:\Squat」後再雙撃「Squat.py」（圖 3）以載入原始碼（source code）。

圖 3：在 Thonny 中開啟 Squat.py

程式碼追蹤

Squat.py 援用了「PoseModule.py」中的「PoseDetector」類別（class），並載入「squat3_clip.mp4」預錄的影像檔做為測試資料。 在 4x 行的程式碼中，最核心（core）的指令為第 17 行定義於的 PoseDetector 類別的 findAngle() 方法（method）！讀者們可先學習呼叫它的方法和需傳入的參數列（parameter list）。有需要再鑽進該類別去深究它是如何從影像資料中計算出深蹲的夾角的。

本小節將使用「變數傾印（dump）」暴力法（紮實的蹲馬步）將範例程式中插入「print(變數名)」，將用到變數一一印出來觀察其內容，藉以了解程式的設計邏輯。我們以

L行號 變數名稱

方式來標示查看程式碼的第幾行變數值。

L10 img　在第 10 和第 11 行中插入 print(img) 指令，我們可以看到變數 img 的第一筆內容如下：

[[[  0  44  37]

  [  2  50  43]

  [  7  63  54]

  ...

  [ 20  48  78]

  [ 19  47  77]

  [ 18  46  76]]

 [[  0  44  37]

  [  2  50  43]

  [  7  63  54]

  ...

  [ 18  46  76]

  [ 17  45  75]

  [ 16  44  74]]

 [[  0  45  37]

  [  0  51  43]

  [  4  62  53]

  ...

  [ 15  43  73]

  [ 12  40  70]

  [ 11  39  69]]

 ...

 [[121 115 110]

  [123 117 112]

  [127 121 116]

  ...

  [ 19  40  71]

  [ 22  43  74]

  [ 23  44  75]]

 [[144 138 133]

  [146 140 135]

  [152 146 141]

  ...

  [ 23  44  75]

  [ 23  44  75]

  [ 23  44  75]]

 [[153 147 142]

  [152 146 141]

  [152 146 141]

  ...

  [ 23  44  75]

  [ 21  42  73]

  [ 19  40  71]]]

圖 4：變數傾印法

L12 img　

[[[  1  47  40]

  [ 10  66  57]

  [ 12  70  61]

  ...

  [ 16  44  74]

  [ 20  48  78]

  [ 18  46  76]]

 [[  0  48  40]

  [  8  66  57]

  [  1  56  47]

  ...

  [ 16  44  74]

  [ 16  44  74]

  [ 13  41  71]]

 [[  1  50  42]

  [  8  66  57]

  [  5  61  52]

  ...

  [ 15  43  73]

  [ 13  41  71]

  [  9  37  67]]

 ...

 [[151 145 140]

  [143 137 132]

  [133 127 122]

  ...

  [ 15  36  67]

  [ 12  33  64]

  [ 15  36  67]]

 [[128 122 117]

  [135 129 124]

  [145 139 134]

  ...

  [ 17  38  69]

  [ 19  40  71]

  [ 23  44  75]]

 [[151 145 140]

  [153 147 142]

  [156 150 145]

  ...

  [ 22  43  74]

  [ 23  44  75]

  [ 21  42  73]]]

L13 h, w, c　「480 640 3」 color channel, 彩色圖片是3

L14 detector.findPose(img, draw=True)　

({'lmList': [[331, 126], [334, 121], [336, 121], [338, 121], [328, 121], [326, 121], [324, 121], [339, 124], [321, 124], [335, 133], [328, 132], [355, 164], [306, 163], [357, 213], [302, 210], [371, 198], [289, 200], [376, 194], [283, 193], [375, 190], [281, 190], [372, 191], [284, 192], [344, 253], [315, 254], [358, 328], [301, 332], [368, 399], [291, 400], [365, 407], [294, 410], [374, 422], [283, 423]], 'bbox': (282, 97, 97, 337), 'center': (330, 265)}, array([[[ 1, 47, 40],

[ 10, 66, 57],

[ 12, 70, 61],

...,

[ 16, 44, 74],

[ 20, 48, 78],

[ 18, 46, 76]],

[[ 0, 48, 40],

[ 8, 66, 57],

[ 1, 56, 47],

...,

[ 16, 44, 74],

[ 16, 44, 74],

[ 13, 41, 71]],

[[ 1, 50, 42],

[ 8, 66, 57],

[ 5, 61, 52],

...,

[ 15, 43, 73],

[ 13, 41, 71],

[ 9, 37, 67]],

...,

[[151, 145, 140],

[143, 137, 132],

[133, 127, 122],

...,

[ 15, 36, 67],

[ 12, 33, 64],

[ 15, 36, 67]],

[[128, 122, 117],

[135, 129, 124],

[145, 139, 134],

...,

[ 17, 38, 69],

[ 19, 40, 71],

[ 23, 44, 75]],

[[151, 145, 140],

[153, 147, 142],

[156, 150, 145],

...,

[ 22, 43, 74],

[ 23, 44, 75],

[ 21, 42, 73]]], dtype=uint8))

L14 pose　

{'lmList': [[331, 126], [334, 121], [336, 121], [338, 121], [328, 121], [326, 121], [324, 121], [339, 124], [322, 124], [335, 133], [328, 132], [355, 163], [306, 163], [357, 215], [299, 214], [371, 199], [289, 199], [376, 194], [283, 191], [376, 191], [281, 188], [374, 192], [283, 189], [344, 254], [316, 255], [358, 328], [301, 332], [368, 400], [291, 400], [365, 409], [294, 410], [374, 422], [283, 423]], 'bbox': (282, 97, 97, 337), 'center': (330, 265)}

L14 img　

[[[ 1 47 40]

[ 10 66 57]

[ 12 70 61]

...

[ 16 44 74]

[ 20 48 78]

[ 18 46 76]]

[[ 0 48 40]

[ 8 66 57]

[ 1 56 47]

...

[ 16 44 74]

[ 16 44 74]

[ 13 41 71]]

[[ 1 50 42]

[ 8 66 57]

[ 5 61 52]

...

[ 15 43 73]

[ 13 41 71]

[ 9 37 67]]

...

[[151 145 140]

[143 137 132]

[133 127 122]

...

[ 15 36 67]

[ 12 33 64]

[ 15 36 67]]

[[128 122 117]

[135 129 124]

[145 139 134]

...

[ 17 38 69]

[ 19 40 71]

[ 23 44 75]]

[[151 145 140]

[153 147 142]

[156 150 145]

...

[ 22 43 74]

[ 23 44 75]

[ 21 42 73]]]

L16 lmList　

[[331, 126], [334, 121], [336, 121], [338, 121], [328, 121], [326, 121], [324, 121], [339, 124], [322, 124], [335, 133], [328, 132], [355, 163], [306, 163], [357, 215], [299, 214], [371, 199], [289, 199], [376, 194], [283, 191], [376, 191], [281, 188], [374, 192], [283, 189], [344, 254], [316, 255], [358, 328], [301, 332], [368, 400], [291, 400], [365, 409], [294, 410], [374, 422], [283, 423]]

L17 detector.findAngle(lmList[24], lmList[26], lmList[28], img)　

[ 15, 36, 67],

[ 12, 33, 64],

[ 15, 36, 67]],

[[128, 121, 118],

[135, 128, 125],

[145, 138, 135],

...,

[ 17, 38, 69],

[ 19, 40, 71],

[ 23, 44, 75]],

[[151, 144, 141],

[153, 146, 143],

[156, 149, 146],

...,

[ 22, 43, 74],

[ 23, 44, 75],

[ 21, 42, 73]]], dtype=uint8))

(178.20385684750428, array([[[ 1, 47, 40],

[ 10, 66, 57],

[ 12, 70, 61],

...,

[ 16, 44, 74],

[ 20, 48, 78],

[ 18, 46, 76]],

[[ 0, 48, 40],

[ 8, 66, 57],

[ 1, 56, 47],

...,

[ 16, 44, 74],

[ 16, 44, 74],

[ 13, 41, 71]],

[[ 1, 50, 42],

[ 8, 66, 57],

[ 5, 61, 52],

...,

[ 15, 43, 73],

[ 13, 41, 71],

[ 9, 37, 67]],

...,

[[151, 144, 141],

[143, 136, 133],

[133, 126, 123],

...,

[ 15, 36, 67],

[ 12, 33, 64],

[ 15, 36, 67]],

[[128, 121, 118],

[135, 128, 125],

[145, 138, 135],

...,

[ 17, 38, 69],

[ 19, 40, 71],

[ 23, 44, 75]],

[[151, 144, 141],

[153, 146, 143],

[156, 149, 146],

...,

[ 22, 43, 74],

[ 23, 44, 75],

[ 21, 42, 73]]], dtype=uint8))

L17 angle　177.34243016028947

L17 img　

  [ 15  36  67]

  [ 12  33  64]

  [ 15  36  67]]

 [[128 122 117]

  [135 129 124]

  [145 139 134]

  ...

  [ 17  38  69]

  [ 19  40  71]

  [ 23  44  75]]

 [[151 145 140]

  [153 147 142]

  [156 150 145]

  ...

  [ 22  43  74]

  [ 23  44  75]

  [ 21  42  73]]]

[[[  1  47  40]

  [ 10  66  57]

  [ 12  70  61]

  ...

  [ 16  44  74]

  [ 20  48  78]

  [ 18  46  76]]

 [[  0  48  40]

  [  8  66  57]

  [  1  56  47]

  ...

  [ 16  44  74]

  [ 16  44  74]

  [ 13  41  71]]

 [[  1  50  42]

  [  8  66  57]

  [  5  61  52]

  ...

  [ 15  43  73]

  [ 13  41  71]

  [  9  37  67]]

 ...

 [[151 144 141]

  [143 136 133]

  [133 126 123]

  ...

  [ 15  36  67]

  [ 12  33  64]

  [ 15  36  67]]

 [[128 121 118]

  [135 128 125]

  [145 138 135]

  ...

  [ 17  38  69]

  [ 19  40  71]

  [ 23  44  75]]

 [[151 144 141]

  [153 146 143]

  [156 149 146]

  ...

  [ 22  43  74]

  [ 23  44  75]

  [ 21  42  73]]]

原理探討

從上小節一步一腳印地以土法煉綱的方式將程式中的每一個存放資料的變數印在 Thonny 的「互動環境（Shell）」中一一檢視。我們試圖以達文西的解剖術，一刀刀的拆解出基元後再拼湊出藏在程式碼背後的密碼──流程圖（flowchart）。

本範例以右大腿和小腿間因直立／深蹲所造成的夾角（angle）變化來判斷動作的完成與否。角度計算函式 findAngle() （定義於 PoseModule.py 中）以「髖骨─膝蓋─腳踝」三點的座標計算之（圖 5）：

蹲下， £ 105 

直立， ³ 170

圖 5：深蹲動作的角度值範例判斷

另一方面，深蹲的判別及其次數計算方式為（圖 6）：

dir = 0，站直，count <- count + 0.5（蹲半下）

dir = 1，蹲下，count <- count + 0.5（蹲半下）

其中，「<-」表示指定（assign），意即將符號右邊的值指定給左邊（將計次變數 count 加上蹲半下 0.5 後指定到 count 之中）。

圖 6：深蹲動作的判別及計次

因此，我們可以整理出程式的執行流程如圖 7 所示。

圖 7：本文的流程圖

進階問題

1. 將「夾角」的偵測擴充到四足，讓程式可以判斷出四手腳的動作。
2. 找一個運動的肢體變化過程，例如國標舞舞步，將深蹲改寫為「國標舞 AI 教練」。
3. 請模仿我們先前拙著《AI 成績查詢系統》（https://reurl.cc/YdzLAX》一文，使用 fChart 內建的 Tinker 視窗設計工具包（GUI toolkit），替本文訂製一個使用者界面。
4. 參考拙著《Python玩AI，你也可以 – 從CVZone入門吧！》（https://reurl.cc/AyZGkE），將測試資料置換成由攝影機餵入的即時（real-time）影像資料。

---

[1] 六種授權條款。

[2] https://reurl.cc/gQ59Vb 中的「fChart_CVZone工具」連結。

[科技宝, Cagebot, 8787, AI, Arduino] 國廠祥儀科技寶智慧手臂 3/3：電控程式設篇

 國廠祥儀科技寶智慧手臂 3/3：

電控程式設計篇

Ted Lee的土饅頭（To Mentor）工坊

Line：ted2016.kpvs
Email：Lct4246@gmail.com
FB：http://gg.gg/TedLeeFB/

Blog：http://gg.gg/TedLeeBlog/

Nov. 13, 2022

[1]

  [2][3]

本系列第三篇文章延續了第一篇《國廠祥儀科技寶智慧手臂 1/3：機構設計篇（https://reurl.cc/Wq1aWZ）》的解析，本文將進一步地著墨在控制板（Arduino Nano）、PCA9685 與 8787 的電控程式設計上（圖 1 最右方塊）。

圖 1：機電整合專案三大設計

完整的電控程式

我們先來看看將控制板、PCA9685 與 8787 三者整合後的完整程式（https://reurl.cc/585AlM）（註：祥儀的 RD 是以 Performio（https://www.performio.com）這套 IDE 來開發智慧手臂的測試程式的，所以，它的主程式為 main.cpp。我們打開這隻程式細看，setup()、loop() 依舊是 Arduino IDE 的 C/ C++ 語法。因此，只要將智慧手臂的函式庫（https://reurl.cc/58paLz）複製到Arduino IDE 預設的函式庫存放路徑 C:\Program Files (x86)\Arduino\libraries 或 C:\Users\<user>\Documents\Arduino\libraries 之下即可順利編譯之。）：

黃色指令為用來預測 8787 移動路徑的卡爾曼濾波器（Kalman filter）的程式碼；

紫色指令為 PCA9685 伺服馬達擴充板的控制程式碼；

綠色指令為 威盛 8787 AI 視覺感測器的控制程式碼。

#include <Arduino.h>

#include <Wire.h>

#include <SoftwareSerial.h>

#include "PCA9685.h"

#include "SimpleKalmanFilter.h"

#include "Pixetto.h"

#define sevo1_pin 0

#define servo1_angle_min -90

#define servo1_angle_max 90

#define servo1_angle_init 0

#define sevo2_pin 1

#define servo2_angle_min -90

#define servo2_angle_max 90

#define servo2_angle_init 0

#define sevo3_pin 2

#define servo3_angle_min -90

#define servo3_angle_max 90

#define servo3_angle_init 0

#define sevo4_pin 3

#define servo4_angle_min -90

#define servo4_angle_max 90

#define servo4_angle_init 0

#define rxPin 11

#define txPin 9

Pixetto ss(rxPin, txPin);

PCA9685 driver;

// PCA9685 輸出 = 12 位 = 4096 步

// 20ms 的 2.5% = 0.5ms ; 20ms 的 12.5% = 2.5ms

// 4096 的 2.5% = 102 步；4096 的 12.5% = 512 步

PCA9685_ServoEval pwmServo1; // (0deg, 90deg, 180deg)

PCA9685_ServoEval pwmServo2; // (0deg, 90deg, 180deg)

PCA9685_ServoEval pwmServo3; // (0deg, 90deg, 180deg)

PCA9685_ServoEval pwmServo4; // (0deg, 90deg, 180deg)

SimpleKalmanFilter KalmanFilter_X(1, 1, 0.01);

SimpleKalmanFilter KalmanFilter_Y(1, 1, 0.01);

int servo1Pos, servo2Pos, servo3Pos; // 當前角度

int KalmanX, KalmanY;

void setup() {

  Wire.begin(); // Wire must be started first

  driver.resetDevices(); // Software resets all PCA9685 devices on Wire line

  driver.init(); // Address pins A5-A0 set to B000000

  driver.setPWMFreqServo(); // Set frequency to 50Hz

  driver.setChannelPWM(0, pwmServo1.pwmForAngle(servo1_angle_init));

  delay(10);

  driver.setChannelPWM(1, pwmServo2.pwmForAngle(servo2_angle_init));

  delay(10);

  driver.setChannelPWM(2, pwmServo3.pwmForAngle(servo3_angle_init));

  delay(10);

  driver.setChannelPWM(3, pwmServo4.pwmForAngle(servo4_angle_init));

  delay(10);

  ss.begin();

  ss.enableFunc(Pixetto::FUNC_COLOR_DETECTION);

  delay(10);

  Serial.begin(9600);

  

  servo1Pos = servo1_angle_init+90;

  servo2Pos = servo2_angle_init+90;

  servo3Pos = servo3_angle_init+90;

}

void loop() {

  if (ss.isDetected()) {

    Serial.print("x:");

    Serial.println(ss.getPosX());

    Serial.print("y:");

    Serial.println(ss.getPosY());

    Serial.print("w:");

    Serial.println(ss.getWidth());

    Serial.println("");

    KalmanX = KalmanFilter_X.updateEstimate(ss.getPosX());

    KalmanY = KalmanFilter_Y.updateEstimate(ss.getPosY());

    if (ss.getFuncID() == Pixetto::FUNC_COLOR_DETECTION) {

      if (ss.getTypeID() == Pixetto::COLOR_RED) {

        if (KalmanX >= 50) {

          servo1Pos -= 5;

          if (servo1Pos >= servo1_angle_max+90)

            servo1Pos = 180;

          driver.setChannelPWM(0, pwmServo1.pwmForAngle(servo1Pos-90));

        }

        else if (KalmanX <= 25) {

          servo1Pos += 5;

          if (servo1Pos <= servo1_angle_min+90)

            servo1Pos = 0;

          driver.setChannelPWM(0, pwmServo1.pwmForAngle(servo1Pos-90));

        }

        else if (KalmanY <= 17) {

          servo2Pos += 5;

          if (servo2Pos >= servo2_angle_max+70)

            servo2Pos = 160;

          driver.setChannelPWM(1, pwmServo2.pwmForAngle(servo2Pos-90));

        }

        else if (KalmanY >= 60) {

          servo2Pos -= 5;

          if (servo2Pos <= servo2_angle_min+90)

            servo2Pos = 0;

          driver.setChannelPWM(1, pwmServo2.pwmForAngle(servo2Pos-90));

        }

        if (servo2Pos <= 65) {

          servo3Pos = servo2Pos + 25;

          if (servo3Pos <= servo3_angle_min+90)

            servo3Pos = 0;

          driver.setChannelPWM(2, pwmServo3.pwmForAngle(servo3Pos-90));

        }

        else if (servo2Pos >= 85) {

          servo3Pos = servo2Pos + 10;

          if (servo3Pos >= servo3_angle_max+90)

            servo3Pos = 180;

          driver.setChannelPWM(2, pwmServo3.pwmForAngle(servo3Pos-90));

        }

        else {

          servo3Pos = servo3_angle_init;

          driver.setChannelPWM(2, pwmServo3.pwmForAngle(servo3Pos));

        }

      }

    }

  }

}

如圖 2 所示，電控程式是用來控制硬體運作的指令集合，因此，我們先要想像智慧手臂要如何動作，然後將之對應到伺服馬達群的一連串控制劇本（script）。例如，我們想要讓智慧手臂追蹤到紅球就伸爪去夾住它，將這段劇本翻譯成伺服馬達控制語言則變成：左右轉動伺服馬達 1 去追紅球，水平定位到之後再移動伺服馬達 3 重直定位到紅球蹤跡。接著，調整伺服馬達 2 調整紅球和 8787 遠近的深度（depth）。三軸鎖定紅球的位置後即可張爪夾球。

圖 2：電控程式學習拆解

圖 2 中最右邊的淺藍色方塊表示現今流行的伺服馬達控制程式界面概可分為積木程式、C/ C++ 和 MicroPython 的語法程式。

關於智慧手臂如何自動追蹤紅球，在 Notepad++ 文字編輯器中，我們將上述的程式碼打開，就能很清楚的看到圖 2 中紅框內所框選的即為實作的程式碼，它判斷的數據如圖 3 上半部圖示。此外，在追蹤移動時，為了讓 8787 永遠保持置中，圖 2 中的綠框處即為實作的程式碼，它的判斷數據如圖 3 的最下方圖示。

圖 2：自動追蹤的程式片斷

圖 3：自動追蹤紅球時 8787 的移動位置調整

電控程式之主控板及開發環境建置

智慧手臂的控制板是 Arduino Nano 的相容板，裝妥 CH340 驅動程式、從裝置管理員確認它被指派到的 COM 埠編號後，我們就可以使用 Arduino IDE （我們使用 V1.8.9（https://reurl.cc/06Xq3Y）版本，祥儀 RD 建議以 V1.8.X 版為佳──經我們測試，最新的 V2.0.x 版改幅度不小，不建議讀者們採用）來開啟祥儀研發的範例程式。其中，Board 設定為 Arduino Nano、Processor 設定為 ATmega32p (Old Bootloader)、Port 設定為裝置管理員中列出 CH34 的 COM 埠編號。

萬事皆備，我們就可以將 Arduino 界最著名的 Blink 範例程式（讓板載在 D13 腳位上的 SMD LED 閃爍）上傳到控制板來測試我們的開發環境是否全然準備妥當，如圖 4 所示。我們將 Blink 的程式碼摘列如下：

void setup() {

  // initialize digital pin LED_BUILTIN as an output.

  pinMode(LED_BUILTIN, OUTPUT);

}

void loop() {

  digitalWrite(LED_BUILTIN, HIGH);   // turn the LED on (HIGH is the voltage level)

  delay(1000);                       // wait for a second

  digitalWrite(LED_BUILTIN, LOW);    // turn the LED off by making the voltage LOW

  delay(1000);                       // wait for a second

}

圖 4：控制板在 Arduino IDE V.1.8.9 的設定

電控程式之 PCA9685

為了能準確的控制每一顆伺服馬達，我們應該要先弄清楚一顆伺服馬達是如何以指令程式達成控制的。從 Github 上取得 PCA9685 的控制程式碼 pwmtest.ino 後（https://reurl.cc/06XNMo），我們將之調整成「單顆」伺服馬達的測試程式如下：

#include <Wire.h>

#include <Adafruit_PWMServoDriver.h>

Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver();

#define SERVOMIN  150

#define SERVOMAX  600

#define USMIN  600

#define USMAX  2400

#define SERVO_FREQ 50

uint8_t servonum = 0;

void setup() {

  Serial.begin(9600);

  Serial.println("8 channel Servo test!");

  pwm.begin();

 

  pwm.setOscillatorFrequency(27000000);

  pwm.setPWMFreq(SERVO_FREQ);

  delay(10);

}

void setServoPulse(uint8_t n, double pulse) {

  double pulselength;

  

  pulselength = 1000000;

  pulselength /= SERVO_FREQ;

  Serial.print(pulselength); Serial.println(" us per period"); 

  pulselength /= 4096;

  Serial.print(pulselength); Serial.println(" us per bit"); 

  pulse *= 1000000;

  pulse /= pulselength;

  Serial.println(pulse);

  pwm.setPWM(n, 0, pulse);

}

void loop() {

  Serial.println(servonum);

  for (uint16_t pulselen = SERVOMIN; pulselen < SERVOMAX; pulselen++) {

    pwm.setPWM(servonum, 0, pulselen);

  }

  delay(500);

  for (uint16_t pulselen = SERVOMAX; pulselen > SERVOMIN; pulselen--) {

    pwm.setPWM(servonum, 0, pulselen);

  }

  delay(500);

  for (uint16_t microsec = USMIN; microsec < USMAX; microsec++) {

    pwm.writeMicroseconds(servonum, microsec);

  }

  delay(500);

  for (uint16_t microsec = USMAX; microsec > USMIN; microsec--) {

    pwm.writeMicroseconds(servonum, microsec);

  }

  delay(500);

  if (servonum > 7) servonum = 0; // Testing the first 8 servo channels

}

現在，我們已經學會了單顆伺服馬達了，接下來就可以輕鬆地把像圖 2 提及的智慧手臂動作劇本轉換成對應的四顆伺服馬達控制指令了，讀者們可以自行挑戰看看！

此外，卡爾曼濾波器的測試程式可從此 https://reurl.cc/X5V05M 下載測試並從中了解這個數學模型的使用方式。

電控程式之 8787

首先，在 Pixtto 公用程式（Pixetto Utility）中設定使用 8787 內建的顏色偵測（color detection） AI 模型，如圖 5 所示。

圖 5：在 Pixetto 公用程式中指定顏色偵測的內建 AI 功能

緊接著，因為智慧手臂的 Github（https://reurl.cc/58paLz）中已含有 8787 V1.5.1 版的 Arduino 函式庫，所以，在 Arduino IDE 中可以使用下列的語法架構如來設計：

if (ss.isDetected()) {

    if (ss.getFuncID() == Pixetto::FUNC_COLOR_DETECTION) {

        if (ss.getTypeID() == Pixetto::COLOR_RED) {

        //...

    }

}

}

關於 8787 內建的 AI 模型詳列如下，有興趣的讀者們可詳參 https://reurl.cc/qZN7Mp 這隻 C 語言的標頭檔（header file）：

enum EFunc

{

FUNC_COLOR_DETECTION = 1,

FUNC_COLOR_CODE_DETECTION = 2,

FUNC_SHAPE_DETECTION = 3,

FUNC_SPHERE_DETECTION = 4,

FUNC_TEMPLATE_MATCHING = 6,

FUNC_KEYPOINTS = 8,

FUNC_NEURAL_NETWORK = 9,

FUNC_APRILTAG = 10,

FUNC_FACE_DETECTION = 11,

FUNC_TRAFFIC_SIGN_DETECTION = 12,

FUNC_HANDWRITTEN_DIGITS_DETECTION = 13,

FUNC_HANDWRITTEN_LETTERS_DETECTION = 14,

FUNC_CLOUD_DETECTION = 15,

FUNC_LANES_DETECTION = 16,

FUNC_EQUATION_DETECTION = 17,

FUNC_SIMPLE_CLASSIFIER = 18,

FUNC_VOICE_COMMAND = 19

};