67 lines
1.9 KiB
Python
67 lines
1.9 KiB
Python
#!python2-32
|
|
from struct import unpack
|
|
from py9b.link.base import LinkOpenException, LinkTimeoutException
|
|
#from py9b.link.tcp import TCPLink
|
|
#from py9b.link.ble import BLELink
|
|
from py9b.link.serial import SerialLink
|
|
from py9b.transport.base import BaseTransport as BT
|
|
from py9b.transport.packet import BasePacket as PKT
|
|
from py9b.transport.xiaomi import XiaomiTransport
|
|
from py9b.transport.ninebot import NinebotTransport
|
|
|
|
link = SerialLink()
|
|
#link = TCPLink()
|
|
#link = BLELink()
|
|
|
|
with link:
|
|
print "Scanning..."
|
|
ports = link.scan()
|
|
print ports
|
|
|
|
#tran = XiaomiTransport(link)
|
|
tran = NinebotTransport(link)
|
|
|
|
#link.open(("127.0.0.1", 6000))
|
|
link.open(ports[0][1])
|
|
print "Connected"
|
|
|
|
last_esc_64 = ""
|
|
last_esc_65 = ""
|
|
last_ble_64 = ""
|
|
try:
|
|
while True:
|
|
try:
|
|
rsp = tran.recv()
|
|
if not rsp:
|
|
continue
|
|
if rsp.src==BT.HOST and rsp.dst==BT.ESC and rsp.cmd in (0x64, 0x65):
|
|
if len(rsp.data)==5:
|
|
if rsp.data==last_esc_65:
|
|
continue
|
|
ll, throttle, brake, u2, u3 = unpack("<BBBBB", rsp.data)
|
|
print "BLE->ESC: TH: %02X, BR: %02X, %02X %02X" % (throttle, brake, u2, u3)
|
|
last_esc_65 = rsp.data
|
|
continue
|
|
elif len(rsp.data)==7:
|
|
if rsp.data==last_esc_64:
|
|
continue
|
|
ll, throttle, brake, u2, u3, ver = unpack("<BBBBBH", rsp.data)
|
|
print "BLE->ESC: TH: %02X, BR: %02X, %02X %02X, VER: %04X" % (throttle, brake, u2, u3, ver)
|
|
last_esc_64 = rsp.data
|
|
continue
|
|
elif rsp.src==BT.HOST and rsp.dst==BT.BLE and rsp.cmd==0x64:
|
|
if len(rsp.data)==4:
|
|
if rsp.data==last_ble_64:
|
|
continue
|
|
u0, u1, u2, u3 = unpack("<BBBB", rsp.data)
|
|
print "ESC->BLE: %02X %02X %02X %02X" % (u0, u1, u2, u3)
|
|
last_ble_64 = rsp.data
|
|
continue
|
|
print rsp
|
|
except LinkTimeoutException:
|
|
pass
|
|
except KeyboardInterrupt:
|
|
pass
|
|
|
|
link.close()
|