python控制电机_如何用Python控制LEGO Wedo 2.0的电机

乐高官方提供的编程工具都是图形化的，这篇文章分享下如何用Python来连接控制LEGO Wedo 2.0中提供的蓝牙主控设备和电机。

学习资源

网上有两篇很有用的文章：

使用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

使用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。

源码