def handleData(p):
if (p.cls, p.cmd) != (4, 5): return
c, attr, typ = common.unpack('BHB', p.payload[:4])
pay = p.payload[5:]
if attr == 0x27:
vals = common.unpack('8HB', pay)
emg = vals[:8]
moving = vals[8]
self.onEmg(emg, moving)
elif attr == 0x1c:
vals = common.unpack('10h', pay)
quat = vals[:4]
acc = vals[4:7]
gyro = vals[7:10]
self.onImu(quat, acc, gyro)
elif attr == 0x23:
typ, val, xdir, _, _, _ = common.unpack('6B', pay)
if typ == 1:
self.onArm(Arm(val), XDirection(xdir))
elif typ == 2:
self.onArm(Arm.unknown, XDirection.unknown)
elif typ == 3:
self.onPose(Pose(val))
else:
print('data with unknown attr: %02X %s' % (attr, p))
def decode(cls, data):
"""Decodes a local map structure from data and returns it."""
(width, height), data = unpack('II', data)
northwest, data = unpack('ff', data)
northeast, data = unpack('ff', data)
southwest, data = unpack('ff', data)
return cls(width, height, northwest, northeast, southwest, data)
def handle_data(p):
if (p.cls, p.cmd) != (4, 5):
return
c, attr, typ = unpack('BHB', p.payload[:4])
pay = p.payload[5:]
if attr == 0x27:
vals = unpack('8HB', pay)
# not entirely sure what the last byte is, but it's a bitmask
# that seems to indicate which sensors think they're being
# moved around or something
emg = vals[:8]
moving = vals[8]
self.on_emg(emg, moving)
elif attr == 0x1c:
vals = unpack('10h', pay)
quat = vals[:4]
acc = vals[4:7]
gyro = vals[7:10]
self.on_imu(quat, acc, gyro)
elif attr == 0x23:
typ, val, xdir, _, _, _ = unpack('6B', pay)
if typ == 1: # on arm
self.on_arm(Arm(val), XDirection(xdir))
elif typ == 2: # removed from arm
self.on_arm(Arm.UNKNOWN, XDirection.UNKNOWN)
elif typ == 3: # pose
self.on_pose(Pose(val))
else:
print('data with unknown attr: %02X %s' % (attr, p))
def handle_data(p):
if (p.cls, p.cmd) != (4, 5):
return
c, attr, typ = unpack('BHB', p.payload[:4])
pay = p.payload[5:]
if attr == 0x27:
vals = unpack('8HB', pay)
# not entirely sure what the last byte is,
# but it's a bitmask that seems to indicate which
# sensors think they're being moved around or something
emg = vals[:8]
moving = vals[8]
self.on_emg(emg, moving)
elif attr == 0x1c:
vals = unpack('10h', pay)
quat = vals[:4]
acc = vals[4:7]
gyro = vals[7:10]
self.on_imu(quat, acc, gyro)
elif attr == 0x23:
if len(pay) == 6:
try:
typ, val, xdir, _, _, _ = unpack('6B', pay)
except Exception as e:
print("Got exception: " + str(e) + "\nContinuing...")
return
elif len(pay) == 3:
try:
typ, val, xdir = unpack('3B', pay)
except Exception as e:
print("Got exception: " + str(e) + "\nContinuing...")
return
h = Header()
h.stamp = rospy.Time.now()
if typ == 1: # on arm
self.on_arm(MyoArm(arm=val, xdir=xdir))
elif typ == 2: # removed from arm
self.on_arm(MyoArm(MyoArm.UNKNOWN, MyoArm.UNKNOWN))
elif typ == 3: # pose
if val == 255:
pose = MyoPose(h, 0)
else:
pose = MyoPose(h, val + 1)
self.on_pose(pose)
else:
print('data with unknown attr: %02X %s' % (attr, p))
def mode_unpack(self, strData):
""""""
cont = common.unpack(strData[0])
mode = common.tonum(strData[1])
print((" Mode: " + hex(mode)))
print((" Continuous: " + str(cont[2])))
return cont
def decode(cls, value_type, data):
"""Decode value from data according to value_type, return (value, remaining data).
Note the decoded value for objects is (added, deleted) where added is a list of (key, value_id)
and deleted is just a list of value_id."""
if value_type in cls.TYPE_MAP:
value, data = unpack(cls.TYPE_MAP[value_type], data)
elif value_type == ValueType.STRING:
value, data = parse_string(data)
elif value_type == ValueType.ARRAY:
# array is uint16 length, elements are uint32 ids
length, data = unpack('H', data)
value, data = unpack(length * 'I', data, as_tuple=True)
elif value_type == ValueType.OBJECT:
# object is two parts:
# added: (uint16 length, elements are (uint32 id, string key))
# removed: uint16, uint32 id
length, data = unpack('H', data)
added = {}
for x in range(length):
id, data = unpack('I', data)
key, data = parse_string(data)
added[key] = id
length, data = unpack('H', data)
removed, data = unpack(length * 'I', data, as_tuple=True)
value = added, removed
else:
raise ValueError("Unknown value type {!r}".format(value_type))
return value, data
def handle_read(self):
self.read_buffer += self.recv(self.BUFFER_SIZE)
data, self.read_buffer = common.unpack(self.read_buffer)
if data is not None:
try:
self.handler(data)
except Exception as e:
print '[microserver] Client receive error:', e
traceback.print_exc()
def __init__(self, fp, address):
self.address = address
self.fp = fp
fp.seek(address)
children_count, = unpack('!L', fp.read(4))
word_count, = unpack('!L', fp.read(4))
self.word_count, = unpack('!L', fp.read(4))
self.addresses = {}
for i in range(children_count):
way, address = unpack('!BL', fp.read(5))
self.addresses[way] = address
self.words = []
for i in range(word_count):
word = Word()
word.read(fp)
self.words.append(word)
def read_ways(self, fp):
""" Reads ways from file.
"""
ways_count = unpack('!L', fp.read(4))[0]
ways = {}
for i in range(ways_count):
letter = unpack('!4s', fp.read(4))[0].decode(
'utf-8').replace('\0', '')
way = []
part = unpack('!B', fp.read(1))[0]
while part & 0x80:
way.append(part & 0x7f)
part = unpack('!B', fp.read(1))[0]
way.append(part)
ways[letter] = way
return ways
def handle(self, kind, data):
""""""
op = ord(data[0])
payload = data[1:]
arrows = "<<<"
if kind == INPUT_PREFIX:
arrows = ">>>"
name = "0x" + common.tohex(data[0])
if op in di:
name = name + " (" + di.get(op) + ")"
print((arrows + " " + name + ": " + common.tohex(data[1:])))
self.wm.rumble(common.unpack(payload[0])[0])
if op == 0x11:
self.wm.led_unpack(payload)
elif op == 0x12:
self.wm.mode_unpack(payload)
elif op == 0x16:
self.wm.write(payload)
def led_unpack(self, strData):
""""""
l = common.unpack(strData)[4:]
self.leds = l
print((" LEDs: " + str(self.leds)))
def buttons_unpack(self, strData):
""""""
return common.unpack(strData)
def connect(self):
self.bt.endScan()
self.bt.disconnect(0)
self.bt.disconnect(1)
self.bt.disconnect(2)
print('scanning MyoArmband')
self.bt.discover()
while True:
p = self.bt.recvPacket()
print('scan response MyoArmBand:', p)
if p.payload.endswith(
b'\x06\x42\x48\x12\x4A\x7F\x2C\x48\x47\xB9\xDE\x04\xA9\x01\x00\x06\xD5'
):
addr = list(multiord(p.payload[2:8]))
break
self.bt.endScan()
connPkt = self.bt.connect(addr)
self.conn = multiord(connPkt.payload)[-1]
self.bt.waitEvent(3, 0)
fw = self.readAttr(0x17)
_, _, _, _, v0, v1, v2, v3 = common.unpack('BHBBHHHH', fw.payload)
print('firmware version MyoArmband: %d.%d.%d.%d' % (v0, v1, v2, v3))
self.old = (v0 == 0)
if self.old:
self.writeAttr(0x19, b'\x01\x02\x00\x00')
self.writeAttr(0x2f, b'\x01\x00')
self.writeAttr(0x2c, b'\x01\x00')
self.writeAttr(0x32, b'\x01\x00')
self.writeAttr(0x35, b'\x01\x00')
self.writeAttr(0x28, b'\x01\x00')
self.writeAttr(0x1d, b'\x01\x00')
C = 1000
emgHz = 50
emgSmooth = 100
imuHz = 50
self.writeAttr(
0x19,
common.pack('BBBBHBBBBB', 2, 9, 2, 1, C, emgSmooth, C // emgHz,
imuHz, 0, 0))
else:
name = self.readAttr(0x03)
print('device name MyoArmband: %s' % name.payload)
self.writeAttr(0x1d, b'\x01\x00')
self.writeAttr(0x24, b'\x02\x00')
# self.write_attr(0x19, b'\x01\x03\x00\x01\x01')
self.startRaw()
def handleData(p):
if (p.cls, p.cmd) != (4, 5): return
c, attr, typ = common.unpack('BHB', p.payload[:4])
pay = p.payload[5:]
if attr == 0x27:
vals = common.unpack('8HB', pay)
emg = vals[:8]
moving = vals[8]
self.onEmg(emg, moving)
elif attr == 0x1c:
vals = common.unpack('10h', pay)
quat = vals[:4]
acc = vals[4:7]
gyro = vals[7:10]
self.onImu(quat, acc, gyro)
elif attr == 0x23:
typ, val, xdir, _, _, _ = common.unpack('6B', pay)
if typ == 1:
self.onArm(Arm(val), XDirection(xdir))
elif typ == 2:
self.onArm(Arm.unknown, XDirection.unknown)
elif typ == 3:
self.onPose(Pose(val))
else:
print('data with unknown attr: %02X %s' % (attr, p))
self.bt.addHandler(handleData)
def decode(cls, data):
"""Decode message from data and return (message_type, payload, remaining data),
or raise Incomplete if more data is needed to complete a message."""
(length, message_type), data = unpack('IB', data)
payload, data = eat(data, length)
return message_type, payload, data
def connect(self):
# stop everything from before
self.bt.end_scan()
self.bt.disconnect(0)
self.bt.disconnect(1)
self.bt.disconnect(2)
# start scanning
print('scanning...')
self.bt.discover()
while True:
p = self.bt.recv_packet()
print('scan response:', p)
if p.payload.endswith(b'\x06\x42\x48\x12\x4A\x7F\x2C\x48\x47\xB9\xDE\x04\xA9\x01\x00\x06\xD5'):
addr = list(multiord(p.payload[2:8]))
break
self.bt.end_scan()
# connect and wait for status event
conn_pkt = self.bt.connect(addr)
self.conn = multiord(conn_pkt.payload)[-1]
self.bt.wait_event(3, 0)
# get firmware version
fw = self.read_attr(0x17)
_, _, _, _, v0, v1, v2, v3 = unpack('BHBBHHHH', fw.payload)
print('firmware version: %d.%d.%d.%d' % (v0, v1, v2, v3))
self.old = (v0 == 0)
if self.old:
# don't know what these do
# Myo Connect sends them, though we get data
# fine without them
self.write_attr(0x19, b'\x01\x02\x00\x00')
self.write_attr(0x2f, b'\x01\x00')
self.write_attr(0x2c, b'\x01\x00')
self.write_attr(0x32, b'\x01\x00')
self.write_attr(0x35, b'\x01\x00')
# enable EMG data
self.write_attr(0x28, b'\x01\x00')
# enable IMU data
self.write_attr(0x1d, b'\x01\x00')
# Sampling rate of the underlying EMG sensor, capped to 1000.
# If it's less than 1000, emg_hz is correct. If it is greater,
# the actual framerate starts dropping inversely. Also, if this
# is much less than 1000, EMG data becomes slower to respond to
# changes. In conclusion, 1000 is probably a good value.
C = 1000
emg_hz = 50
# strength of low-pass filtering of EMG data
emg_smooth = 100
imu_hz = 50
# send sensor parameters, or we don't get any data
self.write_attr(0x19, pack('BBBBHBBBBB', 2, 9, 2, 1, C,
emg_smooth, C // emg_hz, imu_hz, 0, 0))
else:
name = self.read_attr(0x03)
print('device name: %s' % name.payload)
# enable IMU data
self.write_attr(0x1d, b'\x01\x00')
# enable on/off arm notifications
self.write_attr(0x24, b'\x02\x00')
self.start_raw()
# add data handlers
def handle_data(p):
if (p.cls, p.cmd) != (4, 5):
return
c, attr, typ = unpack('BHB', p.payload[:4])
pay = p.payload[5:]
if attr == 0x27:
vals = unpack('8HB', pay)
# not entirely sure what the last byte is,
# but it's a bitmask that seems to indicate which
# sensors think they're being moved around or something
emg = vals[:8]
moving = vals[8]
self.on_emg(emg, moving)
elif attr == 0x1c:
vals = unpack('10h', pay)
quat = vals[:4]
acc = vals[4:7]
gyro = vals[7:10]
self.on_imu(quat, acc, gyro)
elif attr == 0x23:
if len(pay) == 6:
try:
typ, val, xdir, _, _, _ = unpack('6B', pay)
except Exception as e:
print("Got exception: " + str(e) + "\nContinuing...")
return
elif len(pay) == 3:
try:
typ, val, xdir = unpack('3B', pay)
except Exception as e:
print("Got exception: " + str(e) + "\nContinuing...")
return
if typ == 1: # on arm
self.on_arm(MyoArm(arm=val, xdir=xdir))
elif typ == 2: # removed from arm
self.on_arm(MyoArm(MyoArm.UNKNOWN, MyoArm.UNKNOWN))
elif typ == 3: # gesture
if val == 255:
gesture = MyoGesture(0)
else:
gesture = MyoGesture(val + 1)
self.on_gesture(gesture)
else:
print('data with unknown attr: %02X %s' % (attr, p))
self.bt.add_handler(handle_data)
def decode(self, data):
"""Decode a DATA_UPDATE message, yielding (id, value_type, value) updates."""
while data:
(value_type, id), data = unpack('BI', data)
value, data = PipValue.decode(value_type, data)
yield id, value_type, value
