如何用Python控制LEGO Wedo 2.0的電機

樂高官方提供的編程工具都是圖形化的，這篇文章分享下如何用Python來鏈接控制LEGO Wedo 2.0中提供的藍牙主控設備和電機。

學習資源

網上有兩篇頗有用的文章：

• Controlling a WeDo 2.0 motor
• WeDo 2.0 – reverse engineering

使用pygattlib能夠經過Python和藍牙低功耗設備通信。這個庫只支持Linux。

命令行工具和開發環境

經過測試發現，Windows的Linux子系統和VMWare虛擬機是不支持的。我最終選了樹莓派作開發環境。 使用系統自帶的命令行工具能夠直接測試藍牙鏈接。 掃描設備獲取地址：

sudo hcitool -i hci0 lescan
LE Scan ...
98:07:2D:DD:98:56 (unknown)
98:07:2D:DD:98:56 LPF2 Smart Hub

鏈接設備：

gatttool -I
[                 ][LE]> connect <ble address>

使用Python建立界面控制程序

安裝依賴庫以及gattlib:

sudo apt-get update
sudo apt-get install libbluetooth-dev bluez bluez-hcidump libboost-python-dev libboost-thread-dev libglib2.0-dev
sudo pip install gattlib

用Tkinter建立界面：

import Tkinter as tk
from gattlib import DiscoveryService
from gattlib import GATTRequester
from time import sleep
 
def run():
    global button_run
    button_run.after(DELAY, motor_run)
 
def stop():
    global button_stop
    button_stop.after(DELAY, motor_stop)
 
def connect():
    global button_disconnect
    button_disconnect.after(DELAY, smart_hub_connect)
 
def disconnect():
    global button_disconnect
    button_disconnect.after(DELAY, smart_hub_disconnect)
 
def up():
    global button_up
    button_up.after(DELAY, motor_up)
 
def down():
    global button_down
    button_down.after(DELAY, motor_down)
 
 
root = tk.Tk()
root.title("Lego Wedo 2.0 Motor Control")
 
label = tk.Label(root, fg="dark green", text='N/A')
label.pack()
 
button_connect = tk.Button(root, text='Connect Smart Hub', width=BUTTON_WIDTH, command=connect)
button_connect.pack()
 
button_disconnect = tk.Button(root, text='Disconnect Smart Hub', width=BUTTON_WIDTH, command=disconnect, state='disabled')
button_disconnect.pack()
 
button_run = tk.Button(root, text='Run motor', width=BUTTON_WIDTH, command=run, state='disabled')
button_run.pack()
 
button_up = tk.Button(root, text='Speed up', width=BUTTON_WIDTH, command=up, state='disabled')
button_up.pack()
 
button_down = tk.Button(root, text='Speed down', width=BUTTON_WIDTH, command=down, state='disabled')
button_down.pack()
 
button_stop = tk.Button(root, text='Stop motor', width=BUTTON_WIDTH, command=stop, state='disabled')
button_stop.pack()
 
root.mainloop()

使用DiscoveryService和GATTRequester查找鏈接設備：

def smart_hub_connect():
    service = DiscoveryService("hci0")
    devices = service.discover(2)
 
    for address, name in devices.items():
        if name != '' and 'Smart Hub' in name:
            label['text'] = address
 
            global button_run, button_stop, button_disconnect, req
            button_connect['state'] = 'disabled'
            button_run['state'] = 'normal'
            button_stop['state'] = 'normal'
            button_disconnect['state'] = 'normal'
            button_up['state'] = 'normal'
            button_down['state'] = 'normal'
 
            req = GATTRequester(address, True, "hci0")
            break

運行電機：

def motor_run():
    global req
    if req != None:
        req.write_by_handle(0x3d, str(bytearray([0x01, 0x01, 0x01, 0x64]))
)

第一個字節表明端口號。主控設備上有兩個端口，選擇對應的端口號。

經過gatttool也能夠查看端口變化：

[98:07:2D:DD:98:56][LE]> char-read-hnd 0015                                                                     
Characteristic value/descriptor: 01 01 00 01 01 00 00 00 01 00 00 00                                                    
[98:07:2D:DD:98:56][LE]> char-read-hnd 0015                                                                             
Characteristic value/descriptor: 02 01 01 01 01 00 00 00 01 00 00 00

最後一個字節表明速度。能夠經過代碼動態調節：

MAX_SPEED = 100
MIN_SPEED = 1
SPEED_CHANGE = 4
 
current_speed = 100
req = None
 
def motor_up():
    global req, current_speed
    if req != None:
        if current_speed == MAX_SPEED:
            return
 
        current_speed += SPEED_CHANGE
        req.write_by_handle(HANDLE, str(bytearray([0x01, 0x01, 0x01, current_speed])))
        sleep(WEDO_DELAY)
 
def motor_down():
    global req, current_speed
    if req != None:
        if current_speed == MIN_SPEED:
            return
 
        current_speed -= SPEED_CHANGE
        req.write_by_handle(HANDLE, str(bytearray([0x01, 0x01, 0x01, current_speed])))
        sleep(WEDO_DELAY)

運行程序的時候，若是沒有root權限，會出現錯誤。

運行界面程序須要用gksudo而不是sudo。

在Windows中能夠經過SmarTTY來顯示遠程的GUI。

源碼

https://github.com/yushulx/lego-wedo-motor-control