Ejemplo n.º 1
0
        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))
Ejemplo n.º 2
0
	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)
Ejemplo n.º 3
0
        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))
Ejemplo n.º 4
0
        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))
Ejemplo n.º 5
0
 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
Ejemplo n.º 6
0
	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
Ejemplo n.º 7
0
 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()
Ejemplo n.º 8
0
    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)
Ejemplo n.º 9
0
    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
Ejemplo n.º 10
0
 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)
Ejemplo n.º 11
0
 def led_unpack(self, strData):
     """"""
     l = common.unpack(strData)[4:]
     self.leds = l
     print(("    LEDs: " + str(self.leds)))
Ejemplo n.º 12
0
 def buttons_unpack(self, strData):
     """"""
     return common.unpack(strData)
Ejemplo n.º 13
0
    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)
Ejemplo n.º 14
0
	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
Ejemplo n.º 15
0
    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)
Ejemplo n.º 16
0
	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