def __init__(self, connector):
self.connection = connector
self.arm = md.arm.UNKNOWN
self.pose = md.pose.REST
self.x_direction = md.x_direction.ELBOW
self.synced = False
self.startq = Quaternion(0, 0, 0, 1)
self.napq = Quaternion(0, 0, 0, 1)
self.imu = md.IMU()
self.emg = md.EMG()
def handleNotification(self, cHandle, data):
try:
handle = md.handle(cHandle)
except:
raise Exception('Unknown data handle +' % str(cHandle))
if handle == handle.CLASSIFIER:
# sometimes gets the poses mixed up, if this happens, try wearing it in a different orientation.
data = struct.unpack('>6b', data)
try:
ev_type = md.classifierEvent(data[0])
except:
raise Exception('Unknown classifier event: ' + str(data[0]))
if ev_type == ev_type.POSE:
self.myo.pose = md.pose(data[1])
if self.myo.pose == md.pose.UNSYNC:
self.myo.synced = False
self.myo.arm = md.arm(-1)
self.myo.pose = md.pose(-1)
self.myo.x_direction = md.x_direction(-1)
self.myo.startq = Quaternion(0, 0, 0, 1)
else:
self.myo.napq = self.myo.imu.quat.copy()
self.on_pose(self.myo)
else:
if ev_type == ev_type.SYNC:
self.myo.synced = True
# rewrite handles
self.myo.arm = md.arm(data[1])
self.myo.x_direction = md.x_direction(data[2])
self.myo.startq = self.myo.imu.quat.copy()
self.on_sync(self.myo)
elif ev_type == ev_type.UNSYNC:
self.myo.synced = False
self.myo.arm = md.arm(-1)
self.myo.x_direction = md.x_direction(-1)
self.myo.pose = md.pose(-1)
self.myo.startq = Quaternion(0, 0, 0, 1)
self.on_unsync(self.myo)
elif ev_type == ev_type.UNLOCK:
self.on_unlock(self.myo)
elif ev_type == ev_type.LOCK:
self.on_lock(self.myo)
elif ev_type == ev_type.SYNCFAIL:
self.myo.synced = False
self.on_sync_failed(self.myo)
elif ev_type == ev_type.WARMUP:
self.on_warmup(self.myo)
elif handle == handle.IMU:
self.myo.imu = md.IMU(data)
self.on_imu(self.myo)
elif handle == handle.EMG:
self.myo.emg = md.EMG(data)
self.on_emg(self.myo)
else:
logging.error("Unknown data handle %s" % cHandle)
def info(self):
out = dict()
services = self.getServices()
for i in services:
s = binascii.b2a_hex(i.uuid.binVal).decode('utf-8')[4:8]
num = int(s, base=16)
sname = md.services.get(num, s)
if sname in ('1801', '0004', '0006'): # unknown
continue
print(str(sname))
ch = i.getCharacteristics()
dat = dict()
for c in ch:
s = binascii.b2a_hex(c.uuid.binVal).decode('utf-8')[4:8]
num = int(s, base=16)
name = md.services.get(num, hex(num))
if 'EmgData' in name:
logging.info('\t%s' % (name))
dat.update({name: ''})
continue
if name in ('0x602', '0x104', 'Command', '0x2a05'):
logging.info('\t%s' % (name))
dat.update({name: ''})
continue
if c.supportsRead():
b = bytearray(c.read())
try:
if name in ('Info1', 'Info2'):
b = list(b)
logging.info('\t%s: %s' % (name, b))
dat.update({name: b})
continue
elif name == 'FirmwareVersion':
b = md.firmware(b)
logging.info('\t%s: %s' % (name, b))
dat.update({name: b})
continue
elif name == 'HardwareInfo':
b = md.hardwareInfo(b)
logging.info('\t%s: %s' % (name, b))
dat.update({name: b})
continue
elif name == 'BatteryLevel':
b = b[0]
logging.info('\t%s: %s' % (name, b))
dat.update({name: int(b)})
continue
else:
logging.info('\t%s: %s' % (name, b))
dat.update({name: str(b)})
except Exception as e:
logging.info('\t%s: %s' % (name, list(b)))
dat.update({name: list(b)})
else:
try:
b = bytearray(c.read())
if name in ('0x104', 'ClassifierEvent'):
b = list(b)
logging.info('\t%s: %s' % (name, b))
dat.update({name: b})
continue
if name == 'IMUData':
b = md.IMU(b)
logging.info('\t%s: %s' % (name, b))
dat.update({name: b})
continue
if name == 'MotionEvent':
b = md.motionEvent(b)
logging.info('\t%s: %s' % (name, b))
dat.update({name: b})
continue
logging.info('\t%s: %s' % (name, b))
dat.update({name: list(b)})
except:
logging.info('\t%s: %s' % (name, c.props))
dat.update({name: c})
out.update({sname: dat})
return out