class BluetoothInterface: def __init__(self): self.rfcomm = BluetoothSocket(RFCOMM) self.rfcomm.connect(("00:21:13:01:D1:59", 1)) self.rfcomm.settimeout(0.01) def send(self, message): try: self.rfcomm.send(message) except Exception: print_exc() def receive(self): return self.rfcomm.recv(1024)
def connect(addr): global SOCK, CONSOLE, CONNECTED connected = 'Connected: {}' try: SOCK = BluetoothSocket(RFCOMM) SOCK.connect((addr, 1)) SOCK.settimeout(1) except BluetoothError as e: CONSOLE += str(e) + '\n' connected = connected.format('no') SOCK.close() SOCK = None else: CONNECTED = True connected = connected.format('yes') CONSOLE += connected + '\n' return connected
def establishConnectionTo(self, host): # host can also be a name, resolving it is slow though and requires the # device to be visible if not bluetooth.is_valid_address(host): nearby_devices = bluetooth.discover_devices() for bdaddr in nearby_devices: if host == bluetooth.lookup_name(bdaddr): host = bdaddr break if bluetooth.is_valid_address(host): con = BluetoothSocket(bluetooth.RFCOMM) con.settimeout(0.5) # half second to make IO interruptible while True: try: con.connect((host, 1)) # 0 is channel self.bt_connections.append(con) return len(self.bt_connections) - 1 except bluetooth.btcommon.BluetoothError as e: if not self._isTimeOut(e): logger.error("unhandled Bluetooth error: %s", repr(e)) break else: return -1
import os,sys import time from bluetooth import ( BluetoothSocket, RFCOMM, ) from bluetooth.btcommon import BluetoothError rfcomm = BluetoothSocket(RFCOMM) rfcomm.connect(('00:18:E5:03:F5:99', 1)) # Bricogeek #rfcomm.connect(('98:D3:31:F6:1C:51', 1)) # white #rfcomm.connect(('98:D3:31:70:13:AB', 1)) # green rfcomm.settimeout(None) # set blocking True #rfcomm.settimeout(0.0) # set blocking False #rfcomm.settimeout(0.001) print("Connected") data = 'a'*900 data += '\n' period = 0.001 print('len(data', len(data)+1, 'period', period, 'bytes/ms', len(data)/period/1000.0) t1 = time.time() while (True): try: rfcomm.send(data.encode('utf-8'))
class Bridge: serial_worker = None ser_to_bt = None intentional_exit = False in_packet = False packet = bytearray() def __init__(self, log, port=COM_PORT, baud=BAUD, address=PAPERANG_ADDR, uuid=UUID): self.host_port = COM_PORT self.address = address self.uuid = uuid self.host = serial.serial_for_url(COM_PORT, baudrate=baud, timeout=1, do_not_open= True) self.logging = log.logger def connect_host(self): try: self.host.open() except serial.SerialException as e: self.logging.error( 'Could not open serial port {}: {}\n'.format(self.host_port, e)) return False self.logging.info("Host connected.") return True def disconnect_host(self): self.host.close() self.logging.info("Host disconnected.") def scanservices(self): self.logging.info("Searching for services...") service_matches = find_service(uuid=self.uuid, address=self.address) valid_service = list(filter( lambda s: 'protocol' in s and 'name' in s and s[ 'protocol'] == 'RFCOMM' and s['name'] == b'Port\x00', service_matches )) if len(valid_service) == 0: self.logging.error("Cannot find valid services on device with MAC %s." % self.address) return False self.logging.info("Found a valid service on target device.") self.service = valid_service[0] return True def connect_device(self): if not self.scanservices(): self.logging.error('Not found valid service.') return False self.logging.info("Service found. Connecting to \"%s\" on %s..." % (self.service["name"], self.service["host"])) self.sock = BluetoothSocket(RFCOMM) self.sock.connect((self.service["host"], self.service["port"])) self.sock.settimeout(60) self.logging.info("Device connected.") return True def disconnect_device(self): try: self.sock.close() except: pass self.logging.info("Device disconnected.") def redirect_Serial2Bt_thread(self): # start thread to handle redirect host data to device self.ser_to_bt = SerialToBt(self.logging) self.ser_to_bt.socket = self.sock self.serial_worker = serial.threaded.ReaderThread(self.host, self.ser_to_bt) self.serial_worker.start() self.logging.info("Start to redirect host data to device ...") def get_packet(self, data): """Find data enclosed in START/STOP""" for d in data: byte = d.to_bytes(1, 'little') if byte == PKT_START: self.in_packet = True elif byte == PKT_STOP: self.in_packet = False self.logging.info( 'Device2Host Packet: 02{}03'.format(self.packet.hex())) del self.packet[:] elif self.in_packet: self.packet.extend(byte) else: # data that is received outside of packets pass def redirect_Bt2Serial(self): self.logging.info("Start to redirect device data to host ...") device_socket = self.sock self.intentional_exit = False try: # enter network <-> serial loop while not self.intentional_exit: try: data = device_socket.recv(MAX_RECV_LEN) if not data: break # get a bunch of bytes and send them self.logging.debug('Device2Host Data: {}'.format(data.hex())) self.get_packet(data) self.host.write(data) except BluetoothError as msg: self.logging.error('ERROR: {}\n'.format(msg)) # probably got disconnected break except KeyboardInterrupt: self.intentional_exit = True raise def enable(self): self.connect_host() if not self.connect_device(): return self.redirect_Serial2Bt_thread() self.redirect_Bt2Serial() def disable(self): self.intentional_exit = True self.ser_to_bt.socket = None self.disconnect_host() self.serial_worker.stop() self.disconnect_device()
class Paperang: standardKey = 0x35769521 padding_line = 300 max_send_msg_length = 2016 max_recv_msg_length = 1024 service_uuid = "00001101-0000-1000-8000-00805F9B34FB" def __init__(self, address=None): self.address = address self.crckeyset = False self.connected = True if self.connect() else False def connect(self): if self.address is None and not self.scandevices(): return False if not self.scanservices(): return False logging.info("Service found. Connecting to \"%s\" on %s..." % (self.service["name"], self.service["host"])) self.sock = BluetoothSocket(RFCOMM) self.sock.connect((self.service["host"], self.service["port"])) self.sock.settimeout(60) logging.info("Connected.") self.registerCrcKeyToBt() return True def disconnect(self): try: self.sock.close() except: pass logging.info("Disconnected.") def scandevices(self): logging.warning( "Searching for devices...\n" "It may take time, you'd better specify mac address to avoid a scan." ) valid_names = ['MiaoMiaoJi', 'Paperang'] nearby_devices = discover_devices(lookup_names=True) valid_devices = list( filter(lambda d: len(d) == 2 and d[1] in valid_names, nearby_devices)) if len(valid_devices) == 0: logging.error("Cannot find device with name %s." % " or ".join(valid_names)) return False elif len(valid_devices) > 1: logging.warning( "Found multiple valid machines, the first one will be used.\n") logging.warning("\n".join(valid_devices)) else: logging.warning("Found a valid machine with MAC %s and name %s" % (valid_devices[0][0], valid_devices[0][1])) self.address = valid_devices[0][0] return True def scanservices(self): logging.info("Searching for services...") service_matches = find_service(uuid=self.service_uuid, address=self.address) valid_service = list( filter( lambda s: 'protocol' in s and 'name' in s and s['protocol'] == 'RFCOMM' and s['name'] == 'SerialPort', service_matches)) if len(valid_service) == 0: logging.error("Cannot find valid services on device with MAC %s." % self.address) return False logging.info("Found a valid service") self.service = valid_service[0] return True def sendMsgAllPackage(self, msg): # Write data directly to device sent_len = self.sock.send(msg) logging.info("Sending msg with length = %d..." % sent_len) def crc32(self, content): return zlib.crc32(content, self.crcKey if self.crckeyset else self.standardKey) & 0xffffffff def packPerBytes(self, bytes, control_command, i): result = struct.pack('<BBB', 2, control_command, i) result += struct.pack('<H', len(bytes)) result += bytes result += struct.pack('<I', self.crc32(bytes)) result += struct.pack('<B', 3) return result def addBytesToList(self, bytes): length = self.max_send_msg_length result = [bytes[i:i + length] for i in range(0, len(bytes), length)] return result def sendToBt(self, data_bytes, control_command, need_reply=True): bytes_list = self.addBytesToList(data_bytes) for i, bytes in enumerate(bytes_list): tmp = self.packPerBytes(bytes, control_command, i) self.sendMsgAllPackage(tmp) if need_reply: return self.recv() def recv(self): # Here we assume that there is only one received packet. raw_msg = self.sock.recv(self.max_recv_msg_length) parsed = self.resultParser(raw_msg) logging.info("Recv: " + codecs.encode(raw_msg, "hex_codec").decode()) logging.info("Received %d packets: " % len(parsed) + "".join([str(p) for p in parsed])) return raw_msg, parsed def resultParser(self, data): base = 0 res = [] while base < len(data) and data[base] == '\x02': class Info(object): def __str__(self): return "\nControl command: %s(%s)\nPayload length: %d\nPayload(hex): %s" % ( self.command, BtCommandByte.findCommand( self.command), self.payload_length, codecs.encode(self.payload, "hex_codec")) info = Info() _, info.command, _, info.payload_length = struct.unpack( '<BBBH', data[base:base + 5]) info.payload = data[base + 5:base + 5 + info.payload_length] info.crc32 = data[base + 5 + info.payload_length:base + 9 + info.payload_length] base += 10 + info.payload_length res.append(info) return res def registerCrcKeyToBt(self, key=0x6968634 ^ 0x2e696d): logging.info("Setting CRC32 key...") msg = struct.pack('<I', int(key ^ self.standardKey)) self.sendToBt(msg, BtCommandByte.PRT_SET_CRC_KEY) self.crcKey = key self.crckeyset = True logging.info("CRC32 key set.") def sendPaperTypeToBt(self, paperType=0): # My guess: # paperType=0: normal paper # paperType=1: official paper msg = struct.pack('<B', paperType) self.sendToBt(msg, BtCommandByte.PRT_SET_PAPER_TYPE) def sendPowerOffTimeToBt(self, poweroff_time=0): msg = struct.pack('<H', poweroff_time) self.sendToBt(msg, BtCommandByte.PRT_SET_POWER_DOWN_TIME) def sendImageToBt(self, binary_img): self.sendPaperTypeToBt() # msg = struct.pack("<%dc" % len(binary_img), *map(bytes, binary_img)) msg = b"".join( map(lambda x: struct.pack("<c", x.to_bytes(1, byteorder="little")), binary_img)) self.sendToBt(msg, BtCommandByte.PRT_PRINT_DATA, need_reply=False) self.sendFeedLineToBt(self.padding_line) def sendSelfTestToBt(self): msg = struct.pack('<B', 0) self.sendToBt(msg, BtCommandByte.PRT_PRINT_TEST_PAGE) def sendDensityToBt(self, density): msg = struct.pack('<B', density) self.sendToBt(msg, BtCommandByte.PRT_SET_HEAT_DENSITY) def sendFeedLineToBt(self, length): msg = struct.pack('<H', length) self.sendToBt(msg, BtCommandByte.PRT_FEED_LINE) def queryBatteryStatus(self): msg = struct.pack('<B', 1) self.sendToBt(msg, BtCommandByte.PRT_GET_BAT_STATUS) def queryDensity(self): msg = struct.pack('<B', 1) self.sendToBt(msg, BtCommandByte.PRT_GET_HEAT_DENSITY) def sendFeedToHeadLineToBt(self, length): msg = struct.pack('<H', length) self.sendToBt(msg, BtCommandByte.PRT_FEED_TO_HEAD_LINE) def queryPowerOffTime(self): msg = struct.pack('<B', 1) self.sendToBt(msg, BtCommandByte.PRT_GET_POWER_DOWN_TIME) def querySNFromBt(self): msg = struct.pack('<B', 1) self.sendToBt(msg, BtCommandByte.PRT_GET_SN) def queryHardwareInfo(self): msg = struct.pack('<B', 1) self.sendToBt(msg, BtCommandByte.PRT_GET_HW_INFO)
class Wiimote: def __init__(self, addr): self._addr = addr self._inSocket = BluetoothSocket(L2CAP) self._outSocket = BluetoothSocket(L2CAP) self._connected = False def __del__(self): self.disconnect() def _send(self, *data, **kwargs): check_connection = True if 'check_connection' in kwargs: check_connection = kwargs['check_connection'] if check_connection and not self._connected: raise IOError('No wiimote is connected') self._inSocket.send(''.join(map(chr, data))) def disconnect(self): if self._connected: self._inSocket.close() self._outSocket.close() self._connected = False def connect(self, timeout=0): if self._connected: return None self._inSocket.connect((self._addr, 0x13)) self._outSocket.connect((self._addr, 0x11)) self._inSocket.settimeout(timeout) self._outSocket.settimeout(timeout) # TODO give the choice of the mode to the user # Set the mode of the data reporting of the Wiimote with the last byte of this sending # 0x30 : Only buttons (2 bytes) # 0x31 : Buttons and Accelerometer (3 bytes) # 0x32 : Buttons + Extension (8 bytes) # 0x33 : Buttons + Accel + InfraRed sensor (12 bytes) # 0x34 : Buttons + Extension (19 bytes) # 0x35 : Buttons + Accel + Extension (16 bytes) # 0x36 : Buttons + IR sensor (10 bytes) + Extension (9 bytes) # 0x37 : Buttons + Accel + IR sensor (10 bytes) + Extension (6 bytes) # 0x3d : Extension (21 bytes) # 0x3e / 0x3f : Buttons + Accel + IR sensor (36 bytes). Need two reports for a sigle data unit. self._send(0x52, 0x12, 0x00, 0x33, check_connection=False) # Enable the IR camera # Enable IR Camera (Send 0x04 to Output Report 0x13) # Enable IR Camera 2 (Send 0x04 to Output Report 0x1a) # Write 0x08 to register 0xb00030 # Write Sensitivity Block 1 to registers at 0xb00000 # Write Sensitivity Block 2 to registers at 0xb0001a # Write Mode Number to register 0xb00033 # Write 0x08 to register 0xb00030 (again) # Put a sleep of 50ms to avoid a bad configuration of the IR sensor # Sensitivity Block 1 is : 00 00 00 00 00 00 90 00 41 # Sensitivity Block 2 is : 40 00 # The mode number is 1 if there is 10 bytes for the IR. # The mode number is 3 if there is 12 bytes for the IR. # The mode number is 5 if there is 36 bytes for the IR. time.sleep(0.050) self._send(0x52, 0x13, 0x04, check_connection=False) time.sleep(0.050) self._send(0x52, 0x1a, 0x04, check_connection=False) time.sleep(0.050) self._send(0x52, 0x16, 0x04, 0xb0, 0x00, 0x30, 1, 0x08, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, check_connection=False) time.sleep(0.050) self._send(0x52, 0x16, 0x04, 0xb0, 0x00, 0x06, 1, 0x90, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, check_connection=False) time.sleep(0.050) self._send(0x52, 0x16, 0x04, 0xb0, 0x00, 0x08, 1, 0x41, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, check_connection=False) time.sleep(0.050) self._send(0x52, 0x16, 0x04, 0xb0, 0x00, 0x1a, 1, 0x40, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, check_connection=False) time.sleep(0.050) self._send(0x52, 0x16, 0x04, 0xb0, 0x00, 0x33, 1, 0x03, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, check_connection=False) time.sleep(0.050) self._send(0x52, 0x16, 0x04, 0xb0, 0x00, 0x30, 1, 0x08, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, check_connection=False) self._connected = True def vibrate(self, duration=1): self._send(0x52, 0x15, 0x01) time.sleep(duration) self._send(0x52, 0x15, 0x00) def setLed(self, binary): self._send(0x52, 0x11, int(n << 4)) def _getData(self, check_connection=True): if check_connection and not self._connected: raise IOError('No wiimote is connected') data = self._inSocket.recv(19) if len(data) != 19: raise IOError('Impossible to receive all data') return list(map(ord, data)) def _checkButton(self, bit, mask): return self._getData()[bit] & mask != 0 def buttonAPressed(self): return self._checkButton(3, 0x08) def buttonBPressed(self): return self._checkButton(3, 0x04) def buttonUpPressed(self): return self._checkButton(2, 0x08) def buttonDownPressed(self): return self._checkButton(2, 0x04) def buttonLeftPressed(self): return self._checkButton(2, 0x01) def buttonRightPressed(self): return self._checkButton(2, 0x02) def buttonPlusPressed(self): return self._checkButton(2, 0x10) def buttonMinusPressed(self): return self._checkButton(3, 0x10) def buttonHomePressed(self): return self._checkButton(3, 0x80) def buttonOnePressed(self): return self._checkButton(3, 0x02) def buttonTwoPressed(self): return self._checkButton(3, 0x01) # 0x80 means no movement def getAcceleration(self): d = self._getData() ax = d[4] << 2 | d[2] & (0x20 | 0x40) ay = d[5] << 1 | d[3] & 0x20 az = d[6] << 1 | d[3] & 0x40 return (ax, ay, az) def getIRPoints(self): d = self._getData() x1 = d[7] | ((d[9] & (0b00110000)) << 4) y1 = d[8] | ((d[9] & (0b11000000)) << 2) i1 = d[9] & 0b00001111 x2 = d[10] | ((d[12] & (0b00110000)) << 4) y2 = d[11] | ((d[12] & (0b11000000)) << 2) i2 = d[12] & 0b00001111 x3 = d[13] | ((d[15] & (0b00110000)) << 4) y3 = d[14] | ((d[15] & (0b11000000)) << 2) i3 = d[15] & 0b00001111 x4 = d[16] | ((d[18] & (0b00110000)) << 4) y4 = d[17] | ((d[18] & (0b11000000)) << 2) i4 = d[18] & 0b00001111 return [(x1, y2, i1), (x2, y2, i2), (x3, y3, i3), (x4, y4, i4)]
class Proxy(Agent): def on_init(self): self.log = [] self.buffer = b'' self.log_filter = None self.filtered = None self.spinete_pub = self.bind('PUB', alias='spinete', transport='tcp', serializer='raw', addr='127.0.0.1:5000') def setup(self, address, port): self.rfcomm = BluetoothSocket(RFCOMM) self.rfcomm.connect((address, port)) self.rfcomm.settimeout(0.01) self.each(0, 'receive') def filter_next(self, level=None, function=None): self.log_filter = LogFilter(level=level, function=function) def wait_filtered(self, timeout=0.5): t0 = time.time() while True: self.receive() if self.filtered: break if time.time() - t0 > timeout: print('`wait_filtered` timed out!') break result = self.filtered self.filtered = None self.log_filter = None return result def publish(self, log): body = log[-1] if not body.startswith('PUB'): return fields = body.split(',') if fields[1] != 'line': raise NotImplementedError() print(fields) self.send('spinete', pickle.dumps((fields[2], log[0], fields[3]))) def process_received(self, received): self.buffer += received if b'\n' not in self.buffer: return 0 splits = self.buffer.split(b'\n') for message in splits[:-1]: fields = message.split(b',') log = [x.decode('utf-8') for x in fields[:4]] try: log[0] = float(log[0]) except ValueError: pass body = b','.join(fields[4:]) if not body.startswith(b'RAW'): body = body.decode('utf-8') else: raise NotImplementedError() log = tuple(log + [body]) if log[1] == 'ERROR': print(log) if log_matches_filter(log, self.log_filter): self.filtered = log self.log_filter = None self.log.append(log) self.publish(log) self.buffer = splits[-1] return len(splits) - 1 def send_bt(self, message): for retry in range(10): try: self.rfcomm.settimeout(1.) self.rfcomm.send(message) except Exception as error: print_exc() # Wait for the robot ACK t0 = time.time() while (time.time() - t0 < 0.1): received = self.receive() for i in range(received): log = self.log[-1 - i] body = log[-1] if log[1] != 'DEBUG': continue if 'Processing' not in body: continue if body != 'Processing "%s"' % message.strip('\0'): break return True print('Command "%s" unsuccessful!' % message) return False def receive(self): try: self.rfcomm.settimeout(0.01) received = self.rfcomm.recv(1024) return self.process_received(received) except BluetoothError as error: if str(error) != 'timed out': raise return 0 def tail(self, N): return self.log[-N:] def last_log_time(self): if not self.log: return 0 return float(self.log[-1].split(b',')[0]) def get_battery_voltage(self): self.filter_next(function='log_battery_voltage') self.send_bt('battery\0') return float(self.wait_filtered()[-1]) def get_configuration_variables(self): self.filter_next(function='log_configuration_variables') self.send_bt('configuration_variables\0') return json.loads(self.wait_filtered()[-1])
class HID: def __init__(self, bdaddress=None): self.cport = 0x11 # HID's control PSM self.iport = 0x13 # HID' interrupt PSM self.backlog = 1 self.address = "" if bdaddress: self.address = bdaddress # create the HID control socket self.csock = BluetoothSocket(L2CAP) self.csock.bind((self.address, self.cport)) set_l2cap_mtu(self.csock, 64) self.csock.settimeout(2) self.csock.listen(self.backlog) # create the HID interrupt socket self.isock = BluetoothSocket(L2CAP) self.isock.bind((self.address, self.iport)) set_l2cap_mtu(self.isock, 64) self.isock.settimeout(2) self.isock.listen(self.backlog) self.connected = False self.client_csock = None self.caddress = None self.client_isock = None self.iaddress = None def listen(self): try: (self.client_csock, self.caddress) = self.csock.accept() print("Accepted Control connection from %s" % self.caddress[0]) (self.client_isock, self.iaddress) = self.isock.accept() print("Accepted Interrupt connection from %s" % self.iaddress[0]) self.connected = True return True except Exception as _: self.connected = False return False @staticmethod def get_local_address(): hci = BluetoothSocket(HCI) fd = hci.fileno() buf = array.array('B', [0] * 96) ioctl(fd, _bluetooth.HCIGETDEVINFO, buf, 1) data = struct.unpack_from("H8s6B", buf.tostring()) return data[2:8][::-1] def get_control_socket(self): if self.connected: return self.client_csock, self.caddress else: return None def get_interrupt_socket(self): if self.connected: return self.client_isock, self.iaddress else: return None
class Hal(object): # class global, so that the front-end can cleanup on forced termination # popen objects cmds = [] # led blinker led_blink_thread = None led_blink_running = False GYRO_MODES = { 'angle': 'GYRO-ANG', 'rate': 'GYRO-RATE', } LED_COLORS = { 'green': ev3dev.Leds.GREEN + ev3dev.Leds.GREEN, 'red': ev3dev.Leds.RED + ev3dev.Leds.RED, 'orange': ev3dev.Leds.ORANGE + ev3dev.Leds.ORANGE, 'black': ev3dev.Leds.BLACK + ev3dev.Leds.BLACK, } LED_ALL = ev3dev.Leds.LEFT + ev3dev.Leds.RIGHT # usedSensors is unused, the code-generator for lab.openroberta > 1.4 wont # pass it anymore def __init__(self, brickConfiguration, usedSensors=None): self.cfg = brickConfiguration dir = os.path.dirname(__file__) # char size: 6 x 12 -> num-chars: 29.666667 x 10.666667 self.font_s = ImageFont.load(os.path.join(dir, 'ter-u12n_unicode.pil')) # char size: 10 x 18 -> num-chars: 17.800000 x 7.111111 # self.font_s = ImageFont.load(os.path.join(dir, 'ter-u18n_unicode.pil')) self.lcd = ev3dev.Screen() self.led = ev3dev.Leds self.keys = ev3dev.Button() self.sound = ev3dev.Sound (self.font_w, self.font_h) = self.lcd.draw.textsize('X', font=self.font_s) # logger.info('char size: %d x %d -> num-chars: %f x %f', # self.font_w, self.font_h, 178 / self.font_w, 128 / self.font_h) self.timers = {} self.sys_bus = None self.bt_server = None self.bt_connections = [] self.lang = 'de' # factory methods @staticmethod # TODO(ensonic): remove 'side' param, server will stop using it >=3.3.0 # TODO(ensonic): 'regulated' is unused, it is passed to the motor-functions # direcly, consider making all params after port 'kwargs' def makeLargeMotor(port, regulated, direction, side=None): try: m = ev3dev.LargeMotor(port) if direction == 'backward': m.polarity = 'inversed' else: m.polarity = 'normal' except (AttributeError, OSError): logger.info('no large motor connected to port [%s]', port) logger.exception("HW Config error") m = None return m @staticmethod def makeMediumMotor(port, regulated, direction, side=None): try: m = ev3dev.MediumMotor(port) if direction == 'backward': m.polarity = 'inversed' else: m.polarity = 'normal' except (AttributeError, OSError): logger.info('no medium motor connected to port [%s]', port) logger.exception("HW Config error") m = None return m @staticmethod def makeOtherConsumer(port, regulated, direction, side=None): try: lp = ev3dev.LegoPort(port) lp.mode = 'dc-motor' # https://github.com/ev3dev/ev3dev-lang-python/issues/234 # some time is needed to set the permissions for the new attributes time.sleep(0.5) m = ev3dev.DcMotor(address=port) except (AttributeError, OSError): logger.info('no other consumer connected to port [%s]', port) logger.exception("HW Config error") m = None return m @staticmethod def makeColorSensor(port): try: s = ev3dev.ColorSensor(port) except (AttributeError, OSError): logger.info('no color sensor connected to port [%s]', port) s = None return s @staticmethod def makeGyroSensor(port): try: s = ev3dev.GyroSensor(port) except (AttributeError, OSError): logger.info('no gyro sensor connected to port [%s]', port) s = None return s @staticmethod def makeI2cSensor(port): try: s = ev3dev.I2cSensor(port) except (AttributeError, OSError): logger.info('no i2c sensor connected to port [%s]', port) s = None return s @staticmethod def makeInfraredSensor(port): try: s = ev3dev.InfraredSensor(port) except (AttributeError, OSError): logger.info('no infrared sensor connected to port [%s]', port) s = None return s @staticmethod def makeLightSensor(port): try: p = ev3dev.LegoPort(port) p.set_device = 'lego-nxt-light' s = ev3dev.LightSensor(port) except (AttributeError, OSError): logger.info('no light sensor connected to port [%s]', port) s = None return s @staticmethod def makeSoundSensor(port): try: p = ev3dev.LegoPort(port) p.set_device = 'lego-nxt-sound' s = ev3dev.SoundSensor(port) except (AttributeError, OSError): logger.info('no sound sensor connected to port [%s]', port) s = None return s @staticmethod def makeTouchSensor(port): try: s = ev3dev.TouchSensor(port) except (AttributeError, OSError): logger.info('no touch sensor connected to port [%s]', port) s = None return s @staticmethod def makeUltrasonicSensor(port): try: s = ev3dev.UltrasonicSensor(port) except (AttributeError, OSError): logger.info('no ultrasonic sensor connected to port [%s]', port) s = None return s @staticmethod def makeCompassSensor(port): try: s = ev3dev.Sensor(address=port, driver_name='ht-nxt-compass') except (AttributeError, OSError): logger.info('no compass sensor connected to port [%s]', port) s = None return s @staticmethod def makeIRSeekerSensor(port): try: s = ev3dev.Sensor(address=port, driver_name='ht-nxt-ir-seek-v2') except (AttributeError, OSError): logger.info('no ir seeker v2 sensor connected to port [%s]', port) s = None return s @staticmethod def makeHTColorSensorV2(port): try: s = ev3dev.Sensor(address=port, driver_name='ht-nxt-color-v2') except (AttributeError, OSError): logger.info('no hitechnic color sensor v2 connected to port [%s]', port) s = None return s # state def resetState(self): self.clearDisplay() self.stopAllMotors() self.resetAllOutputs() self.resetLED() logger.debug("terminate %d commands", len(Hal.cmds)) for cmd in Hal.cmds: if cmd: logger.debug("terminate command: %s", str(cmd)) cmd.terminate() cmd.wait() # avoid zombie processes Hal.cmds = [] # control def waitFor(self, ms): time.sleep(ms / 1000.0) def busyWait(self): '''Used as interrupptible busy wait.''' time.sleep(0.0) def waitCmd(self, cmd): '''Wait for a command to finish.''' Hal.cmds.append(cmd) # we're not using cmd.wait() since that is not interruptable while cmd.poll() is None: self.busyWait() Hal.cmds.remove(cmd) # lcd def drawText(self, msg, x, y, font=None): font = font or self.font_s self.lcd.draw.text((x * self.font_w, y * self.font_h), msg, font=font) self.lcd.update() def drawPicture(self, picture, x, y): # logger.info('len(picture) = %d', len(picture)) size = (178, 128) # One image is supposed to be 178*128/8 = 2848 bytes # string data is in utf-16 format and padding with extra 0 bytes data = bytes(picture, 'utf-16')[::2] pixels = Image.frombytes('1', size, data, 'raw', '1;IR', 0, 1) self.lcd.image.paste(pixels, (x, y)) self.lcd.update() def clearDisplay(self): self.lcd.clear() self.lcd.update() # led def ledStopAnim(self): if Hal.led_blink_running: Hal.led_blink_running = False Hal.led_blink_thread.join() Hal.led_blink_thread = None def ledOn(self, color, mode): def ledAnim(anim): while Hal.led_blink_running: for step in anim: self.led.set_color(Hal.LED_ALL, step[1]) time.sleep(step[0]) if not Hal.led_blink_running: break self.ledStopAnim() # color: green, red, orange - LED.COLOR.{RED,GREEN,AMBER} # mode: on, flash, double_flash on = Hal.LED_COLORS[color] off = Hal.LED_COLORS['black'] if mode == 'on': self.led.set_color(Hal.LED_ALL, on) elif mode == 'flash': Hal.led_blink_thread = threading.Thread(target=ledAnim, args=([(0.5, on), (0.5, off)], )) Hal.led_blink_running = True Hal.led_blink_thread.start() elif mode == 'double_flash': Hal.led_blink_thread = threading.Thread(target=ledAnim, args=([(0.15, on), (0.15, off), (0.15, on), (0.55, off)], )) Hal.led_blink_running = True Hal.led_blink_thread.start() def ledOff(self): self.led.all_off() self.ledStopAnim() def resetLED(self): self.ledOff() # key def isKeyPressed(self, key): if key in ['any', '*']: return self.keys.any() else: # remap some keys key_aliases = { 'escape': 'backspace', 'back': 'backspace', } if key in key_aliases: key = key_aliases[key] return key in self.keys.buttons_pressed def isKeyPressedAndReleased(self, key): return False # tones def playTone(self, frequency, duration): # this is already handled by the sound api (via beep cmd) # frequency = frequency if frequency >= 100 else 0 self.waitCmd(self.sound.tone(frequency, duration)) def playFile(self, systemSound): # systemSound is a enum for preset beeps: # http://www.lejos.org/ev3/docs/lejos/hardware/Audio.html#systemSound-int- # http://sf.net/p/lejos/ev3/code/ci/master/tree/ev3classes/src/lejos/remote/nxt/RemoteNXTAudio.java#l20 C2 = 523 if systemSound == 0: self.playTone(600, 200) elif systemSound == 1: self.sound.tone([(600, 150, 50), (600, 150, 50)]).wait() elif systemSound == 2: # C major arpeggio self.sound.tone([(C2 * i / 4, 50, 50) for i in range(4, 7)]).wait() elif systemSound == 3: self.sound.tone([(C2 * i / 4, 50, 50) for i in range(7, 4, -1)]).wait() elif systemSound == 4: self.playTone(100, 500) def setVolume(self, volume): self.sound.set_volume(volume) def getVolume(self): return self.sound.get_volume() def setLanguage(self, lang): # lang: 2digit ISO_639-1 code self.lang = lang def sayText(self, text, speed=30, pitch=50): # a: amplitude, 0..200, def=100 # p: pitch, 0..99, def=50 # s: speed, 80..450, def=175 opts = '-a 200 -p %d -s %d -v %s' % ( int(clamp(pitch, 0, 100) * 0.99), # use range 0 - 99 int(clamp(speed, 0, 100) * 2.5 + 100), # use range 100 - 350 self.lang + "+f1") # female voice self.waitCmd(self.sound.speak(text, espeak_opts=opts)) # actors # http://www.ev3dev.org/docs/drivers/tacho-motor-class/ def scaleSpeed(self, m, speed_pct): return int(speed_pct * m.max_speed / 100.0) def rotateRegulatedMotor(self, port, speed_pct, mode, value): # mode: degree, rotations, distance m = self.cfg['actors'][port] speed = self.scaleSpeed(m, clamp(speed_pct, -100, 100)) if mode == 'degree': m.run_to_rel_pos(position_sp=value, speed_sp=speed) while (m.state and 'stalled' not in m.state): self.busyWait() elif mode == 'rotations': value *= m.count_per_rot m.run_to_rel_pos(position_sp=int(value), speed_sp=speed) while (m.state and 'stalled' not in m.state): self.busyWait() def rotateUnregulatedMotor(self, port, speed_pct, mode, value): speed_pct = clamp(speed_pct, -100, 100) m = self.cfg['actors'][port] if mode == 'rotations': value *= m.count_per_rot if speed_pct >= 0: value = m.position + value m.run_direct(duty_cycle_sp=int(speed_pct)) while (m.position < value and 'stalled' not in m.state): self.busyWait() else: value = m.position - value m.run_direct(duty_cycle_sp=int(speed_pct)) while (m.position > value and 'stalled' not in m.state): self.busyWait() m.stop() def turnOnRegulatedMotor(self, port, value): m = self.cfg['actors'][port] m.run_forever(speed_sp=self.scaleSpeed(m, clamp(value, -100, 100))) def turnOnUnregulatedMotor(self, port, value): value = clamp(value, -100, 100) self.cfg['actors'][port].run_direct(duty_cycle_sp=int(value)) def setRegulatedMotorSpeed(self, port, value): m = self.cfg['actors'][port] # https://github.com/rhempel/ev3dev-lang-python/issues/263 # m.speed_sp = self.scaleSpeed(m, clamp(value, -100, 100)) m.run_forever(speed_sp=self.scaleSpeed(m, clamp(value, -100, 100))) def setUnregulatedMotorSpeed(self, port, value): value = clamp(value, -100, 100) self.cfg['actors'][port].duty_cycle_sp = int(value) def getRegulatedMotorSpeed(self, port): m = self.cfg['actors'][port] return m.speed * 100.0 / m.max_speed def getUnregulatedMotorSpeed(self, port): return self.cfg['actors'][port].duty_cycle def stopMotor(self, port, mode='float'): # mode: float, nonfloat # stop_actions: ['brake', 'coast', 'hold'] m = self.cfg['actors'][port] if mode == 'float': m.stop_action = 'coast' elif mode == 'nonfloat': m.stop_action = 'brake' m.stop() def stopMotors(self, left_port, right_port): self.stopMotor(left_port) self.stopMotor(right_port) def stopAllMotors(self): # [m for m in [Motor(port) for port in ['outA', 'outB', 'outC', 'outD']] if m.connected] for file in glob.glob('/sys/class/tacho-motor/motor*/command'): with open(file, 'w') as f: f.write('stop') for file in glob.glob('/sys/class/dc-motor/motor*/command'): with open(file, 'w') as f: f.write('stop') def resetAllOutputs(self): for port in (ev3dev.OUTPUT_A, ev3dev.OUTPUT_B, ev3dev.OUTPUT_C, ev3dev.OUTPUT_D): lp = ev3dev.LegoPort(port) lp.mode = 'auto' def regulatedDrive(self, left_port, right_port, reverse, direction, speed_pct): # direction: forward, backward # reverse: always false for now speed_pct = clamp(speed_pct, -100, 100) ml = self.cfg['actors'][left_port] mr = self.cfg['actors'][right_port] if direction == 'backward': speed_pct = -speed_pct ml.run_forever(speed_sp=self.scaleSpeed(ml, speed_pct)) mr.run_forever(speed_sp=self.scaleSpeed(mr, speed_pct)) def driveDistance(self, left_port, right_port, reverse, direction, speed_pct, distance): # direction: forward, backward # reverse: always false for now speed_pct = clamp(speed_pct, -100, 100) ml = self.cfg['actors'][left_port] mr = self.cfg['actors'][right_port] circ = math.pi * self.cfg['wheel-diameter'] dc = distance / circ if direction == 'backward': dc = -dc # set all attributes ml.stop_action = 'brake' ml.position_sp = int(dc * ml.count_per_rot) ml.speed_sp = self.scaleSpeed(ml, speed_pct) mr.stop_action = 'brake' mr.position_sp = int(dc * mr.count_per_rot) mr.speed_sp = self.scaleSpeed(mr, speed_pct) # start motors ml.run_to_rel_pos() mr.run_to_rel_pos() # logger.debug("driving: %s, %s" % (ml.state, mr.state)) while (ml.state or mr.state): self.busyWait() def rotateDirectionRegulated(self, left_port, right_port, reverse, direction, speed_pct): # direction: left, right # reverse: always false for now speed_pct = clamp(speed_pct, -100, 100) ml = self.cfg['actors'][left_port] mr = self.cfg['actors'][right_port] if direction == 'left': mr.run_forever(speed_sp=self.scaleSpeed(mr, speed_pct)) ml.run_forever(speed_sp=self.scaleSpeed(ml, -speed_pct)) else: ml.run_forever(speed_sp=self.scaleSpeed(ml, speed_pct)) mr.run_forever(speed_sp=self.scaleSpeed(mr, -speed_pct)) def rotateDirectionAngle(self, left_port, right_port, reverse, direction, speed_pct, angle): # direction: left, right # reverse: always false for now speed_pct = clamp(speed_pct, -100, 100) ml = self.cfg['actors'][left_port] mr = self.cfg['actors'][right_port] circ = math.pi * self.cfg['track-width'] distance = angle * circ / 360.0 circ = math.pi * self.cfg['wheel-diameter'] dc = distance / circ logger.debug("doing %lf rotations" % dc) # set all attributes ml.stop_action = 'brake' ml.speed_sp = self.scaleSpeed(ml, speed_pct) mr.stop_action = 'brake' mr.speed_sp = self.scaleSpeed(mr, speed_pct) if direction == 'left': mr.position_sp = int(dc * mr.count_per_rot) ml.position_sp = int(-dc * ml.count_per_rot) else: ml.position_sp = int(dc * ml.count_per_rot) mr.position_sp = int(-dc * mr.count_per_rot) # start motors ml.run_to_rel_pos() mr.run_to_rel_pos() logger.debug("turning: %s, %s" % (ml.state, mr.state)) while (ml.state or mr.state): self.busyWait() def driveInCurve(self, direction, left_port, left_speed_pct, right_port, right_speed_pct, distance=None): # direction: foreward, backward ml = self.cfg['actors'][left_port] mr = self.cfg['actors'][right_port] left_speed_pct = self.scaleSpeed(ml, clamp(left_speed_pct, -100, 100)) right_speed_pct = self.scaleSpeed(mr, clamp(right_speed_pct, -100, 100)) if distance: left_dc = right_dc = 0.0 speed_pct = (left_speed_pct + right_speed_pct) / 2.0 if speed_pct: circ = math.pi * self.cfg['wheel-diameter'] dc = distance / circ left_dc = dc * left_speed_pct / speed_pct right_dc = dc * right_speed_pct / speed_pct # set all attributes ml.stop_action = 'brake' ml.speed_sp = int(left_speed_pct) mr.stop_action = 'brake' mr.speed_sp = int(right_speed_pct) if direction == 'backward': ml.position_sp = int(-left_dc * ml.count_per_rot) mr.position_sp = int(-right_dc * mr.count_per_rot) else: ml.position_sp = int(left_dc * ml.count_per_rot) mr.position_sp = int(right_dc * mr.count_per_rot) # start motors ml.run_to_rel_pos() mr.run_to_rel_pos() while ((ml.state and left_speed_pct) or (mr.state and right_speed_pct)): self.busyWait() else: if direction == 'backward': ml.run_forever(speed_sp=int(-left_speed_pct)) mr.run_forever(speed_sp=int(-right_speed_pct)) else: ml.run_forever(speed_sp=int(left_speed_pct)) mr.run_forever(speed_sp=int(right_speed_pct)) # sensors def scaledValue(self, sensor): return sensor.value() / float(10.0**sensor.decimals) def scaledValues(self, sensor): scale = float(10.0**sensor.decimals) return tuple( [sensor.value(i) / scale for i in range(sensor.num_values)]) # touch sensor def isPressed(self, port): return self.scaledValue(self.cfg['sensors'][port]) # ultrasonic sensor def getUltraSonicSensorDistance(self, port): s = self.cfg['sensors'][port] if s.mode != 'US-DIST-CM': s.mode = 'US-DIST-CM' return self.scaledValue(s) def getUltraSonicSensorPresence(self, port): s = self.cfg['sensors'][port] if s.mode != 'US-LISTEN': s.mode = 'US-LISTEN' return self.scaledValue(s) != 0.0 # gyro # http://www.ev3dev.org/docs/sensors/lego-ev3-gyro-sensor/ def resetGyroSensor(self, port): # change mode to reset for GYRO-ANG and GYRO-G&A s = self.cfg['sensors'][port] s.mode = 'GYRO-RATE' s.mode = 'GYRO-ANG' def getGyroSensorValue(self, port, mode): s = self.cfg['sensors'][port] if s.mode != Hal.GYRO_MODES[mode]: s.mode = Hal.GYRO_MODES[mode] return self.scaledValue(s) # color # http://www.ev3dev.org/docs/sensors/lego-ev3-color-sensor/ def getColorSensorAmbient(self, port): s = self.cfg['sensors'][port] if s.mode != 'COL-AMBIENT': s.mode = 'COL-AMBIENT' return self.scaledValue(s) def getColorSensorColour(self, port): colors = [ 'none', 'black', 'blue', 'green', 'yellow', 'red', 'white', 'brown' ] s = self.cfg['sensors'][port] if s.mode != 'COL-COLOR': s.mode = 'COL-COLOR' return colors[int(self.scaledValue(s))] def getColorSensorRed(self, port): s = self.cfg['sensors'][port] if s.mode != 'COL-REFLECT': s.mode = 'COL-REFLECT' return self.scaledValue(s) def getColorSensorRgb(self, port): s = self.cfg['sensors'][port] if s.mode != 'RGB-RAW': s.mode = 'RGB-RAW' return self.scaledValues(s) # infrared # http://www.ev3dev.org/docs/sensors/lego-ev3-infrared-sensor/ def getInfraredSensorSeek(self, port): s = self.cfg['sensors'][port] if s.mode != 'IR-SEEK': s.mode = 'IR-SEEK' return self.scaledValues(s) def getInfraredSensorDistance(self, port): s = self.cfg['sensors'][port] if s.mode != 'IR-PROX': s.mode = 'IR-PROX' return self.scaledValue(s) # timer def getTimerValue(self, timer): if timer in self.timers: return int((time.time() - self.timers[timer]) * 1000.0) else: self.timers[timer] = time.time() return 0 def resetTimer(self, timer): self.timers[timer] = time.time() # tacho-motor position def resetMotorTacho(self, actorPort): m = self.cfg['actors'][actorPort] m.position = 0 def getMotorTachoValue(self, actorPort, mode): m = self.cfg['actors'][actorPort] tachoCount = m.position if mode == 'degree': return tachoCount * 360.0 / float(m.count_per_rot) elif mode in ['rotation', 'distance']: rotations = float(tachoCount) / float(m.count_per_rot) if mode == 'rotation': return rotations else: distance = round(math.pi * self.cfg['wheel-diameter'] * rotations) return distance else: raise ValueError('incorrect MotorTachoMode: %s' % mode) def getSoundLevel(self, port): # 100 for silent, # 0 for loud s = self.cfg['sensors'][port] if s.mode != 'DB': s.mode = 'DB' return round(-self.scaledValue(s) + 100, 2) # map to 0 silent 100 loud def getHiTecCompassSensorValue(self, port, mode): s = self.cfg['sensors'][port] if s.mode != 'COMPASS': s.mode = 'COMPASS' # ev3dev currently only supports the compass mode value = self.scaledValue(s) if mode == 'angle': return -(((value + 180) % 360) - 180 ) # simulate the angle [-180, 180] mode from ev3lejos else: return value def getHiTecIRSeekerSensorValue(self, port, mode): s = self.cfg['sensors'][port] if s.mode != mode: s.mode = mode value = self.scaledValue(s) # remap from [1 - 9] default 0 to [120, -120] default NaN like ev3lejos return float('nan') if value == 0 else (value - 5) * -30 def getHiTecColorSensorV2Colour(self, port): s = self.cfg['sensors'][port] if s.mode != 'COLOR': s.mode = 'COLOR' value = s.value() return self.mapHiTecColorIdToColor(int(value)) def mapHiTecColorIdToColor(self, id): if id < 0 or id > 17: return 'none' colors = { 0: 'black', 1: 'red', 2: 'blue', 3: 'blue', 4: 'green', 5: 'yellow', 6: 'yellow', 7: 'red', 8: 'red', 9: 'red', 10: 'red', 11: 'white', 12: 'white', 13: 'white', 14: 'white', 15: 'white', 16: 'white', 17: 'white', } return colors[id] def getHiTecColorSensorV2Ambient(self, port): s = self.cfg['sensors'][port] if s.mode != 'PASSIVE': s.mode = 'PASSIVE' value = abs(s.value(0)) / 380 return min(value, 100) def getHiTecColorSensorV2Light(self, port): s = self.cfg['sensors'][port] if s.mode != 'NORM': s.mode = 'NORM' value = self.scaledValues(s)[3] / 2.55 return value def getHiTecColorSensorV2Rgb(self, port): s = self.cfg['sensors'][port] if s.mode != 'NORM': s.mode = 'NORM' value = self.scaledValues(s) value = list(value) del value[0] return value def setHiTecColorSensorV2PowerMainsFrequency(self, port, frequency): s = self.cfg['sensors'][port] s.command = frequency # communication def _isTimeOut(self, e): # BluetoothError seems to be an IOError, which is an OSError # but all they do is: raise BluetoothError (str (e)) return str(e) == "timed out" def establishConnectionTo(self, host): # host can also be a name, resolving it is slow though and requires the # device to be visible if not bluetooth.is_valid_address(host): nearby_devices = bluetooth.discover_devices() for bdaddr in nearby_devices: if host == bluetooth.lookup_name(bdaddr): host = bdaddr break if bluetooth.is_valid_address(host): con = BluetoothSocket(bluetooth.RFCOMM) con.settimeout(0.5) # half second to make IO interruptible while True: try: con.connect((host, 1)) # 0 is channel self.bt_connections.append(con) return len(self.bt_connections) - 1 except bluetooth.btcommon.BluetoothError as e: if not self._isTimeOut(e): logger.error("unhandled Bluetooth error: %s", repr(e)) break else: return -1 def waitForConnection(self): # enable visibility if not self.sys_bus: self.sys_bus = dbus.SystemBus() # do only once (since we turn off the timeout) # alternatively set DiscoverableTimeout = 0 in /etc/bluetooth/main.conf # and run hciconfig hci0 piscan, from robertalab initscript hci0 = self.sys_bus.get_object('org.bluez', '/org/bluez/hci0') props = dbus.Interface(hci0, 'org.freedesktop.DBus.Properties') props.Set('org.bluez.Adapter1', 'DiscoverableTimeout', dbus.UInt32(0)) props.Set('org.bluez.Adapter1', 'Discoverable', True) if not self.bt_server: self.bt_server = BluetoothSocket(bluetooth.RFCOMM) self.bt_server.settimeout( 0.5) # half second to make IO interruptible self.bt_server.bind(("", bluetooth.PORT_ANY)) self.bt_server.listen(1) while True: try: (con, info) = self.bt_server.accept() con.settimeout(0.5) # half second to make IO interruptible self.bt_connections.append(con) return len(self.bt_connections) - 1 except bluetooth.btcommon.BluetoothError as e: if not self._isTimeOut(e): logger.error("unhandled Bluetooth error: %s", repr(e)) break return -1 def readMessage(self, con_ix): message = "NO MESSAGE" if con_ix < len(self.bt_connections) and self.bt_connections[con_ix]: con = self.bt_connections[con_ix] logger.debug('reading msg') while True: # TODO(ensonic): how much do we actually expect # here is the lejos counter part # https://github.com/OpenRoberta/robertalab-ev3lejos/blob/master/ # EV3Runtime/src/main/java/de/fhg/iais/roberta/runtime/ev3/BluetoothComImpl.java#L40..L59 try: message = con.recv(128).decode('utf-8', errors='replace') logger.debug('received msg [%s]' % message) break except bluetooth.btcommon.BluetoothError as e: if not self._isTimeOut(e): logger.error("unhandled Bluetooth error: %s", repr(e)) self.bt_connections[con_ix] = None break return message def sendMessage(self, con_ix, message): if con_ix < len(self.bt_connections) and self.bt_connections[con_ix]: logger.debug('sending msg [%s]' % message) con = self.bt_connections[con_ix] while True: try: con.send(message) logger.debug('sent msg') break except bluetooth.btcommon.BluetoothError as e: if not self._isTimeOut(e): logger.error("unhandled Bluetooth error: %s", repr(e)) self.bt_connections[con_ix] = None break
class Interp(cmd.Cmd): prompt = "balitronics > " def __init__(self, tty=None, baudrate=38400, addr='98:D3:31:70:13:AB', port=1): cmd.Cmd.__init__(self) self.escape = "\x01" # C-a self.quitraw = "\x02" # C-b self.serial_logging = False self.default_in_log_file = "/tmp/eurobotics.in.log" self.default_out_log_file = "/tmp/eurobotics.out.log" self.ser = None if (tty is not None): self.ser = serial.Serial(tty, baudrate=baudrate) else: self.rfcomm = BluetoothSocket(RFCOMM) self.rfcomm.connect((addr, port)) self.rfcomm.settimeout(0.01) def do_quit(self, args): if self.ser is None: self.rfcomm.close() return True def do_log(self, args): """Activate serial logs. log <filename> logs input and output to <filename> log <filein> <fileout> logs input to <filein> and output to <fileout> log logs to /tmp/eurobotics.log or the last used file""" if self.serial_logging: log.error("Already logging to %s and %s" % (self.ser.filein, self.ser.fileout)) else: self.serial_logging = True files = [os.path.expanduser(x) for x in args.split()] if len(files) == 0: files = [self.default_in_log_file, self.default_out_log_file] elif len(files) == 1: self.default_in_log_file = files[0] self.default_out_log_file = None elif len(files) == 2: self.default_in_log_file = files[0] self.default_out_log_file = files[1] else: print("Can't parse arguments") self.ser = SerialLogger(self.ser, *files) log.info("Starting serial logging to %s and %s" % (self.ser.filein, self.ser.fileout)) def do_unlog(self, args): if self.serial_logging: log.info("Stopping serial logging to %s and %s" % (self.ser.filein, self.ser.fileout)) self.ser = self.ser.ser self.serial_logging = False else: log.error("No log to stop") def do_raw_tty(self, args): "Switch to RAW mode" stdin = os.open("/dev/stdin", os.O_RDONLY) stdout = os.open("/dev/stdout", os.O_WRONLY) stdin_termios = termios.tcgetattr(stdin) raw_termios = stdin_termios[:] try: log.info("Switching to RAW mode") # iflag raw_termios[0] &= ~(termios.IGNBRK | termios.BRKINT | termios.PARMRK | termios.ISTRIP | termios.INLCR | termios.IGNCR | termios.ICRNL | termios.IXON) # oflag raw_termios[1] &= ~termios.OPOST # cflag raw_termios[2] &= ~(termios.CSIZE | termios.PARENB) raw_termios[2] |= termios.CS8 # lflag raw_termios[3] &= ~(termios.ECHO | termios.ECHONL | termios.ICANON | termios.ISIG | termios.IEXTEN) termios.tcsetattr(stdin, termios.TCSADRAIN, raw_termios) mode = "normal" while True: ins, outs, errs = select([stdin, self.ser], [], []) for x in ins: if x == stdin: c = os.read(stdin, 1) if mode == "escape": mode == "normal" if c == self.escape: self.ser.write(self.escape) elif c == self.quitraw: return else: self.ser.write(self.escape) self.ser.write(c) else: if c == self.escape: mode = "escape" else: self.ser.write(c) elif x == self.ser: os.write(stdout, self.ser.read(1)) finally: termios.tcsetattr(stdin, termios.TCSADRAIN, stdin_termios) log.info("Back to normal mode") def do_tm_rfcomm(self, args): self.rfcomm.settimeout(1.) self.rfcomm.send("event power off\n\r") time.sleep(0.1) self.rfcomm.send("event tm on\n\r") t1 = t2 = time.time() self.rfcomm.settimeout(0.01) received = self.rfcomm.recv(1) while t2 - t1 < 1: try: self.rfcomm.settimeout(0.01) #received.append(self.rfcomm.recv(1024)) received += self.rfcomm.recv(1024) except BluetoothError as error: if str(error) != 'timed out': raise t2 = time.time() self.rfcomm.send("event tm on\n\r") time.sleep(0.1) print(received) def do_tm_test(self, args): self.rfcomm.settimeout(1.) self.rfcomm.send("\n\revent tm on\n\r") self.rfcomm.settimeout(0.1) #self.rfcomm.settimeout(None) # set blocking True #self.rfcomm.settimeout(0.0) # set blocking False time.sleep(5) t1 = time.time() while time.time() - t1 < 10: try: received = self.rfcomm.recv(4096) print("rx", len(received)) received = 0 #except BluetoothError as error: except: raise #if str(error) != 'timed out': # raise #if str(error) != 'Resource temporarily unavailable': # raise def do_raw_rfcomm(self, args): "Switch to RAW mode" stdin = os.open("/dev/stdin", os.O_RDONLY) stdout = os.open("/dev/stdout", os.O_WRONLY) stdin_termios = termios.tcgetattr(stdin) raw_termios = stdin_termios[:] try: log.info("Switching to RAW mode") # iflag raw_termios[0] &= ~(termios.IGNBRK | termios.BRKINT | termios.PARMRK | termios.ISTRIP | termios.INLCR | termios.IGNCR | termios.ICRNL | termios.IXON) # oflag raw_termios[1] &= ~termios.OPOST # cflag raw_termios[2] &= ~(termios.CSIZE | termios.PARENB) raw_termios[2] |= termios.CS8 # lflag raw_termios[3] &= ~(termios.ECHO | termios.ECHONL | termios.ICANON | termios.ISIG | termios.IEXTEN) termios.tcsetattr(stdin, termios.TCSADRAIN, raw_termios) #i = 0 mode = "normal" while True: #ins,outs,errs=select([stdin,self.ser],[],[]) ins, outs, errs = select([stdin, self.rfcomm], [], []) for x in ins: if x == stdin: c = os.read(stdin, 1) if mode == "escape": mode == "normal" if c == self.escape: #self.ser.write(self.escape) self.rfcomm.settimeout(1.) self.rfcomm.send(self.escape) elif c == self.quitraw: return else: #self.ser.write(self.escape) #self.ser.write(c) self.rfcomm.settimeout(1.) self.rfcomm.send(self.escape) else: if c == self.escape: mode = "escape" else: #time.sleep(0.1) #self.ser.write(c) self.rfcomm.settimeout(1.) self.rfcomm.send(c) #elif x == self.ser: elif x == self.rfcomm: #os.write(stdout,self.ser.read()) #self.rfcomm.settimeout(0.01) #os.write(stdout,self.rfcomm.recv(1022)) try: self.rfcomm.settimeout(0.0) # set blocking False received = self.rfcomm.recv(4096) os.write(stdout, received) except: pass #except BluetoothError as error: # if str(error) != 'timed out': # raise finally: termios.tcsetattr(stdin, termios.TCSADRAIN, stdin_termios) log.info("Back to normal mode") def do_centrifugal(self, args): try: sa, sd, aa, ad = [int(x) for x in shlex.shlex(args)] except: print("args: speed_a, speed_d, acc_a, acc_d") return print(sa, sd, aa, ad) time.sleep(10) self.ser.write("traj_speed angle %d\n" % (sa)) time.sleep(0.1) self.ser.write("traj_speed distance %d\n" % (sd)) time.sleep(0.1) self.ser.write("traj_acc angle %d\n" % (aa)) time.sleep(0.1) self.ser.write("traj_acc distance %d\n" % (ad)) time.sleep(0.1) self.ser.write("goto da_rel 800 180\n") time.sleep(3) self.ser.flushInput() self.ser.write("position show\n") time.sleep(1) print(self.ser.read()) def do_traj_acc(self, args): try: name, acc = [x for x in shlex.shlex(args)] except: print("args: cs_name acc") return acc = int(acc) self.ser.flushInput() self.ser.write("quadramp %s %d %d 0 0\n" % (name, acc, acc)) time.sleep(1) print(self.ser.read()) def do_traj_speed(self, args): try: name, speed = [x for x in shlex.shlex(args)] except: print("args: cs_name speed") return speed = int(speed) self.ser.flushInput() self.ser.write("traj_speed %s %d\n" % (name, speed)) time.sleep(1) print(self.ser.read()) def do_hc05_stress(self, args): #self.ser.flushInput() #self.ser.flushInput() for i in range(1000): for j in range(9): self.rfcomm.settimeout(1.) self.rfcomm.send(str(j)) #print(str(i)+'\n')) data = '' for i in range(1000): for j in range(9): data += self.rfcomm.recv(1) for i in range(1000): for j in range(9): print(data[i]) def do_tune(self, args): try: name, tlog, cons, gain_p, gain_i, gain_d = [ x for x in shlex.shlex(args) ] except: print("args: cs_name, time_ms, consigne, gain_p, gain_i, gain_d") return # Test parameters tlog = float(tlog) / 1000.0 cons = int(cons) gain_p = int(gain_p) gain_i = int(gain_i) gain_d = int(gain_d) #print(name, cons, gain_p, gain_i, gain_d) # Set position, gains, set tm on and goto consign self.ser.flushInput() self.ser.write("position set 1500 1000 0\n") time.sleep(0.1) if name == "d": self.ser.write("gain distance %d %d %d\n" % (gain_p, gain_i, gain_d)) time.sleep(0.1) self.ser.write("echo off\n") #time.sleep(2) #print(self.ser.read()) print("goto d_rel %d\n" % (cons)) self.ser.write("event tm on\n") self.ser.flushInput() time.sleep(0.1) self.ser.write("goto d_rel %d\n" % (cons)) elif name == "a": self.ser.write("gain angle %d %d %d\n" % (gain_p, gain_i, gain_d)) time.sleep(0.1) self.ser.write("echo off\n") #time.sleep(2) #print(self.ser.read()) print("goto d_rel %d\n" % (cons)) self.ser.write("event tm on\n") self.ser.flushInput() time.sleep(0.1) self.ser.write("goto a_rel %d\n" % (cons)) else: print("unknow cs name") return # Telemetry reading tm_data = '' t1 = time.time() self.ser.flushInput() while True: tm_data += self.ser.read() # Break after test time t2 = time.time() if tlog and (t2 - t1) >= tlog: tlog = 0 self.ser.write("\n\r") time.sleep(0.1) self.ser.write("event tm off\n") self.ser.write("echo on\n") time.sleep(2) self.ser.flushInput() time.sleep(2) self.ser.flushInput() break # Telemetry parser i = 0 time_ms = np.zeros(0) cons = f_cons = err = feedback = out = np.zeros(0) v_cons = v_feedback = np.zeros(1) a_cons = a_feedback = a_cons = a_feedback = np.zeros(2) TM_HEAD_BYTE_0 = '\xFB' TM_HEAD_BYTE_1 = '\xBF' TM_TAIL_BYTE_0 = '\xED' TM_SIZE = 48 TM_STRUCT = 'IiiiiiiiiiiBB' head = TM_HEAD_BYTE_0 + TM_HEAD_BYTE_1 tm_packets = tm_data.split(head) #print(tm_packets)) for data in tm_packets: if data[-1] == TM_TAIL_BYTE_0 and len(data) == (TM_SIZE - 2): tm_data = unpack(TM_STRUCT, data) #print(tm_data, type(tm_data))) time_ms = np.append(time_ms, tm_data[0]) #cons = np.append(cons, tm_data[1]) #f_cons = np.append(f_cons, tm_data[2]) #err = np.append(err, tm_data[3]) #feedback = np.append(feedback, tm_data[4]) #out = np.append(out, tm_data[5]) cons = np.append(cons, tm_data[6]) f_cons = np.append(f_cons, tm_data[7]) err = np.append(err, tm_data[8]) feedback = np.append(feedback, tm_data[9]) out = np.append(out, tm_data[10]) if i > 0: v_cons = np.append(v_cons, (f_cons[i] - f_cons[i - 1]) / (time_ms[i] - time_ms[i - 1])) v_feedback = np.append(v_feedback, (feedback[i] - feedback[i - 1]) / (time_ms[i] - time_ms[i - 1])) if i > 1: a_cons = np.append(a_cons, (v_cons[i] - v_cons[i - 1]) / (time_ms[i] - time_ms[i - 1])) a_feedback = np.append( a_feedback, (v_feedback[i] - v_feedback[i - 1]) / (time_ms[i] - time_ms[i - 1])) i += 1 # Telemetry stuff """ # Telemetry parsing t1 = time.time() while True: # Break after test time t2 = time.time() if tlog and (t2 - t1) >= tlog: tlog = 0 self.ser.write("event tm off\n") break # read log data time.sleep(TS/10000.0) line = self.ser.readline() tm_data = struct.struct('Iiiiiiiiiiic') print(line) #m = re.match("(-?\+?\d+).(-?\+?\d+): \((-?\+?\d+),(-?\+?\d+),(-?\+?\d+)\) " # "%s cons= (-?\+?\d+) fcons= (-?\+?\d+) err= (-?\+?\d+) " # "in= (-?\+?\d+) out= (-?\+?\d+)"%(name), line) m = re.match("%s (\d+) (-?\+?\d+) (-?\+?\d+) (-?\+?\d+) (-?\+?\d+) (-?\+?\d+)"%(name), line) # data logging if m: #print(line) #print(m.groups()) t = np.append(t, i*TS) time_ms = np.append(time_ms, int(m.groups()[0])) cons = np.append(cons, int(m.groups()[1])) f_cons = np.append(f_cons, int(m.groups()[2])) err = np.append(err, int(m.groups()[3])) feedback = np.append(feedback, int(m.groups()[4])) out = np.append(out, int(m.groups()[5])) if i>0: v_cons = np.append(v_cons, (f_cons[i] - f_cons[i-1])/(time_ms[i]-time_ms[i-1])) v_feedback = np.append(v_feedback, (feedback[i] - feedback[i-1])/(time_ms[i]-time_ms[i-1])) if i>1: a_cons = np.append(a_cons, (v_cons[i] - v_cons[i-1])/(time_ms[i]-time_ms[i-1])) a_feedback = np.append(a_feedback, (v_feedback[i] - v_feedback[i-1])/(time_ms[i]-time_ms[i-1])) i += 1 continue # trajectory end m = re.match("returned", line) if m: print(line.rstrip()) """ time_ms = time_ms - time_ms[0] plt.figure(1) plt.subplot(311) plt.plot(time_ms, v_cons, '.-', label="consigna") plt.plot(time_ms, v_feedback, '.-', label="feedback") plt.ylabel('v (pulsos/Ts)') plt.grid(True) plt.legend() plt.title('%s kp=%s, ki=%d, kd=%d' % (name, gain_p, gain_i, gain_d)) plt.subplot(312) plt.plot(time_ms, a_cons, '.-', label="consigna") plt.plot(time_ms, a_feedback, '.-', label="feedback") plt.ylabel('a (pulsos/Ts^2)') plt.grid(True) plt.legend() plt.subplot(313) plt.plot(time_ms, out, '.-') plt.xlabel('t (ms)') plt.ylabel('u (cuentas)') plt.grid(True) plt.figure(2) plt.subplot(311) plt.plot(time_ms, f_cons - feedback[0], '.-', label="consigna") plt.plot(time_ms, feedback - feedback[0], '.-', label="feedback") plt.ylabel('posicion (pulsos)') plt.grid(True) plt.legend() plt.title('%s kp=%s, ki=%d, kd=%d' % (name, gain_p, gain_i, gain_d)) plt.subplot(312) plt.plot(time_ms, err, '.-') plt.xlabel('t (ms)') plt.ylabel('error (pulsos)') plt.grid(True) plt.subplot(313) plt.plot(time_ms, out, '.-') plt.xlabel('t (ms)') plt.ylabel('u (cuentas)') plt.grid(True) plt.show()
class Printer: standardKey = 0x35769521 padding_line = 300 max_send_msg_length = 1536 max_recv_msg_length = 1024 service_uuid = '00001101-0000-1000-8000-00805F9B34FB' def __init__(self, address=None): self.address = address self.crckeyset = False self.connected = True if self.connect() else False def init_converter(self, path): convert_img(path) def connect(self): if self.address is None and not self.scandevices(): return False if not self.scanservices(): return False logging.info('Service found: connecting to %s on %s...' % (self.service['name'], self.service['host'])) self.sock = BluetoothSocket() self.sock.connect((self.service['host'], self.service['port'])) self.sock.settimeout(60) logging.info('Connected.') self.registerCrcKeyToBt() return True def disconnect(self): try: self.sock.close() except: pass logging.info('Disconnected.') def scandevices(self): logging.warning('Searching for devices...') valid_names = ['MiaoMiaoJi', 'Paperang', 'Paperang_P2S'] nearby_devices = discover_devices(lookup_names=True) valid_devices = list( filter(lambda d: len(d) == 2 and d[1] in valid_names, nearby_devices)) if len(valid_devices) == 0: logging.error('Cannot find device with name %s.' % ' or '.join(valid_names)) return False elif len(valid_devices) > 1: logging.warning( 'Found multiple valid machines, the first one will be used.\n') logging.warning('\n'.join(valid_devices)) else: logging.warning('Found a valid machine with MAC %s and name %s' % (valid_devices[0][0], valid_devices[0][1])) self.address = valid_devices[0][0] return True def scanservices(self): logging.info('Searching for services...') service_matches = find_service(uuid=self.service_uuid, address=self.address) valid_service = list( filter( lambda s: 'protocol' in s and 'name' in s and s[ 'protocol'] == 'RFCOMM' and (s['name'] == 'SerialPort' or s['name'] == 'Port'), service_matches)) if len(valid_service) == 0: logging.error('Cannot find valid services on device with MAC %s.' % self.address) return False logging.info('Found a valid service') self.service = valid_service[0] return True def sendMsgAllPackage(self, msg): sent_len = self.sock.send(msg) logging.info('Sending msg with length = %d...' % sent_len) def crc32(self, content): return zlib.crc32(content, self.crcKey if self.crckeyset else self.standardKey) & 0xffffffff def packPerBytes(self, bytes, control_command, i): result = struct.pack('<BBB', 2, control_command, i) result += struct.pack('<H', len(bytes)) result += bytes result += struct.pack('<I', self.crc32(bytes)) result += struct.pack('<B', 3) return result def addBytesToList(self, bytes): length = self.max_send_msg_length result = [bytes[i:(i + length)] for i in range(0, len(bytes), length)] return result def sendToBt(self, data_bytes, control_command, need_reply=True): bytes_list = self.addBytesToList(data_bytes) for i, bytes in enumerate(bytes_list): tmp = self.packPerBytes(bytes, control_command, i) self.sendMsgAllPackage(tmp) if need_reply: return self.recv() def recv(self): raw_msg = self.sock.recv(self.max_recv_msg_length) parsed = self.resultParser(raw_msg) logging.info('Recv: ' + codecs.encode(raw_msg, 'hex_codec').decode()) logging.info('Received %d packets: ' % len(parsed) + ''.join([str(p) for p in parsed])) return raw_msg, parsed def resultParser(self, data): base = 0 res = [] while base < len(data) and data[base] == '\x02': class Info(object): def __str__(self): return '\nControl command: %s(%s)\nPayload length: %d\nPayload(hex): %s' % ( self.command, BtCommandByte.findCommand( self.command), self.payload_length, codecs.encode(self.payload, 'hex_codec')) info = Info() _, info.command, _, info.payload_length = struct.unpack( '<BBBH', data[base:(base + 5)]) info.payload = data[base + 5:base + 5 + info.payload_length] info.crc32 = data[base + 5 + info.payload_length:base + 9 + info.payload_length] base += 10 + info.payload_length res.append(info) return res def registerCrcKeyToBt(self, key=0x6968634 ^ 0x2e696d): logging.info('Setting CRC32 key...') msg = struct.pack('<I', int(key ^ self.standardKey)) self.sendToBt(msg, BtCommandByte.PRT_SET_CRC_KEY) self.crcKey = key self.crckeyset = True logging.info('CRC32 key set.') def sendPaperTypeToBt(self, paperType=0): msg = struct.pack('<B', paperType) self.sendToBt(msg, BtCommandByte.PRT_SET_PAPER_TYPE) def sendPowerOffTimeToBt(self, poweroff_time=0): msg = struct.pack('<H', poweroff_time) self.sendToBt(msg, BtCommandByte.PRT_SET_POWER_DOWN_TIME) def print(self, path): self.sendImageToBt(path) def sendImageToBt(self, path): binary_img = convert_img(path) self.sendPaperTypeToBt() msg = b''.join( map(lambda i: struct.pack('<c', i.to_bytes(1, byteorder='little')), binary_img)) self.sendToBt(msg, BtCommandByte.PRT_PRINT_DATA, need_reply=False) self.sendFeedLineToBt(self.padding_line) def sendSelfTestToBt(self): msg = struct.pack('<B', 0) self.sendToBt(msg, BtCommandByte.PRT_PRINT_TEST_PAGE) def sendDensityToBt(self, density): msg = struct.pack('<B', density) self.sendToBt(msg, BtCommandByte.PRT_SET_HEAT_DENSITY) def sendFeedLineToBt(self, length): msg = struct.pack('<H', length) self.sendToBt(msg, BtCommandByte.PRT_FEED_LINE) def queryBatteryStatus(self): msg = struct.pack('<B', 1) self.sendToBt(msg, BtCommandByte.PRT_GET_BAT_STATUS) def queryDensity(self): msg = struct.pack('<B', 1) self.sendToBt(msg, BtCommandByte.PRT_GET_HEAT_DENSITY) def sendFeedToHeadLineToBt(self, length): msg = struct.pack('<H', length) self.sendToBt(msg, BtCommandByte.PRT_FEED_TO_HEAD_LINE) def queryPowerOffTime(self): msg = struct.pack('<B', 1) self.sendToBt(msg, BtCommandByte.PRT_GET_POWER_DOWN_TIME) def querySNFromBt(self): msg = struct.pack('<B', 1) self.sendToBt(msg, BtCommandByte.PRT_GET_SN) def queryHardwareInfo(self): msg = struct.pack('<B', 1) self.sendToBt(msg, BtCommandByte.PRT_GET_HW_INFO)
class Wiimote: def __init__(self, addr): self._addr = addr self._inSocket = BluetoothSocket(L2CAP) self._outSocket = BluetoothSocket(L2CAP) self._connected = False def __del__(self): self.disconnect() def _send(self, *data, **kwargs): check_connection = True if "check_connection" in kwargs: check_connection = kwargs["check_connection"] if check_connection and not self._connected: raise IOError("No wiimote is connected") self._inSocket.send("".join(map(chr, data))) def disconnect(self): if self._connected: self._inSocket.close() self._outSocket.close() self._connected = False def connect(self, timeout=0): if self._connected: return None self._inSocket.connect((self._addr, 0x13)) self._outSocket.connect((self._addr, 0x11)) self._inSocket.settimeout(timeout) self._outSocket.settimeout(timeout) # TODO give the choice of the mode to the user # Set the mode of the data reporting of the Wiimote with the last byte of this sending # 0x30 : Only buttons (2 bytes) # 0x31 : Buttons and Accelerometer (3 bytes) # 0x32 : Buttons + Extension (8 bytes) # 0x33 : Buttons + Accel + InfraRed sensor (12 bytes) # 0x34 : Buttons + Extension (19 bytes) # 0x35 : Buttons + Accel + Extension (16 bytes) # 0x36 : Buttons + IR sensor (10 bytes) + Extension (9 bytes) # 0x37 : Buttons + Accel + IR sensor (10 bytes) + Extension (6 bytes) # 0x3d : Extension (21 bytes) # 0x3e / 0x3f : Buttons + Accel + IR sensor (36 bytes). Need two reports for a sigle data unit. self._send(0x52, 0x12, 0x00, 0x33, check_connection=False) # Enable the IR camera # Enable IR Camera (Send 0x04 to Output Report 0x13) # Enable IR Camera 2 (Send 0x04 to Output Report 0x1a) # Write 0x08 to register 0xb00030 # Write Sensitivity Block 1 to registers at 0xb00000 # Write Sensitivity Block 2 to registers at 0xb0001a # Write Mode Number to register 0xb00033 # Write 0x08 to register 0xb00030 (again) # Put a sleep of 50ms to avoid a bad configuration of the IR sensor # Sensitivity Block 1 is : 00 00 00 00 00 00 90 00 41 # Sensitivity Block 2 is : 40 00 # The mode number is 1 if there is 10 bytes for the IR. # The mode number is 3 if there is 12 bytes for the IR. # The mode number is 5 if there is 36 bytes for the IR. time.sleep(0.050) self._send(0x52, 0x13, 0x04, check_connection=False) time.sleep(0.050) self._send(0x52, 0x1A, 0x04, check_connection=False) time.sleep(0.050) self._send( 0x52, 0x16, 0x04, 0xB0, 0x00, 0x30, 1, 0x08, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, check_connection=False, ) time.sleep(0.050) self._send( 0x52, 0x16, 0x04, 0xB0, 0x00, 0x06, 1, 0x90, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, check_connection=False, ) time.sleep(0.050) self._send( 0x52, 0x16, 0x04, 0xB0, 0x00, 0x08, 1, 0x41, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, check_connection=False, ) time.sleep(0.050) self._send( 0x52, 0x16, 0x04, 0xB0, 0x00, 0x1A, 1, 0x40, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, check_connection=False, ) time.sleep(0.050) self._send( 0x52, 0x16, 0x04, 0xB0, 0x00, 0x33, 1, 0x03, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, check_connection=False, ) time.sleep(0.050) self._send( 0x52, 0x16, 0x04, 0xB0, 0x00, 0x30, 1, 0x08, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, check_connection=False, ) self._connected = True def vibrate(self, duration=1): self._send(0x52, 0x15, 0x01) time.sleep(duration) self._send(0x52, 0x15, 0x00) def setLed(self, binary): self._send(0x52, 0x11, int(n << 4)) def _getData(self, check_connection=True): if check_connection and not self._connected: raise IOError("No wiimote is connected") data = self._inSocket.recv(19) if len(data) != 19: raise IOError("Impossible to receive all data") return list(map(ord, data)) def _checkButton(self, bit, mask): return self._getData()[bit] & mask != 0 def buttonAPressed(self): return self._checkButton(3, 0x08) def buttonBPressed(self): return self._checkButton(3, 0x04) def buttonUpPressed(self): return self._checkButton(2, 0x08) def buttonDownPressed(self): return self._checkButton(2, 0x04) def buttonLeftPressed(self): return self._checkButton(2, 0x01) def buttonRightPressed(self): return self._checkButton(2, 0x02) def buttonPlusPressed(self): return self._checkButton(2, 0x10) def buttonMinusPressed(self): return self._checkButton(3, 0x10) def buttonHomePressed(self): return self._checkButton(3, 0x80) def buttonOnePressed(self): return self._checkButton(3, 0x02) def buttonTwoPressed(self): return self._checkButton(3, 0x01) # 0x80 means no movement def getAcceleration(self): d = self._getData() ax = d[4] << 2 | d[2] & (0x20 | 0x40) ay = d[5] << 1 | d[3] & 0x20 az = d[6] << 1 | d[3] & 0x40 return (ax, ay, az) def getIRPoints(self): d = self._getData() x1 = d[7] | ((d[9] & (0b00110000)) << 4) y1 = d[8] | ((d[9] & (0b11000000)) << 2) i1 = d[9] & 0b00001111 x2 = d[10] | ((d[12] & (0b00110000)) << 4) y2 = d[11] | ((d[12] & (0b11000000)) << 2) i2 = d[12] & 0b00001111 x3 = d[13] | ((d[15] & (0b00110000)) << 4) y3 = d[14] | ((d[15] & (0b11000000)) << 2) i3 = d[15] & 0b00001111 x4 = d[16] | ((d[18] & (0b00110000)) << 4) y4 = d[17] | ((d[18] & (0b11000000)) << 2) i4 = d[18] & 0b00001111 return [(x1, y2, i1), (x2, y2, i2), (x3, y3, i3), (x4, y4, i4)]
while time.time() - t1 < 60: try: rfcomm.settimeout(None) received = rfcomm.recv(2**16) print(time.time() -t2, len(received)) t2 = time.time() #time.sleep(0.001) except BluetoothError as error: if str(error) == 'timed out': print('time out') else: raise return if __name__ == '__main__': #tm_enable() #time.sleep(5) #tm_read() #rfcomm.close() rfcomm = BluetoothSocket(RFCOMM) #rfcomm.connect(('98:D3:31:F6:1C:51', 1)) # white rfcomm.connect(('98:D3:31:70:13:AB', 1)) # green rfcomm.settimeout(0.01) while (True): data = rfcomm.recv(2**16) print(data)
class NodeBluetoothClient(): ''' Tremium Node side bluetooth client which connects to the Tremium Hub ''' def __init__(self, config_file_path): ''' Parameters ------ config_file_path (str) : path to the hub configuration file ''' super().__init__() # loading configurations self.config_manager = NodeConfigurationManager(config_file_path) log_file_path = os.path.join( self.config_manager.config_data["node-file-transfer-dir"], self.config_manager.config_data["bluetooth-client-log-name"]) # setting up logging logger = logging.getLogger() logger.setLevel(logging.INFO) log_handler = logging.handlers.WatchedFileHandler(log_file_path) log_handler.setFormatter( logging.Formatter('%(name)s - %(levelname)s - %(message)s')) logger.addHandler(log_handler) # defining connection to server self.server_s = None # connecting to local cache try: self.cache = NodeCacheModel(config_file_path) except Exception as e: time_str = datetime.datetime.fromtimestamp( time.time()).strftime('%Y-%m-%d_%H-%M-%S') logging.error( "{0} - NodeBluetoothClient failed to connect to cache {1}". format(time_str, e)) raise def __del__(self): if self.server_s is not None: self.server_s.close() def _connect_to_server(self): ''' Establishes a connection with the Tremium Hub Bluetooth server ''' bluetooth_port = self.config_manager.config_data["bluetooth-port"] try: # creating a new socket self.server_s = BluetoothSocket() self.server_s.bind( (self.config_manager. config_data["bluetooth-adapter-mac-client"], bluetooth_port)) # connecting to the hub time.sleep(0.25) self.server_s.connect( (self.config_manager. config_data["bluetooth-adapter-mac-server"], bluetooth_port)) self.server_s.settimeout( self.config_manager.config_data["bluetooth-comm-timeout"]) time.sleep(0.25) # handling server connection failure except Exception as e: self.server_s.close() time_str = datetime.datetime.fromtimestamp( time.time()).strftime('%Y-%m-%d_%H-%M-%S') logging.error( "{0} - NodeBluetoothClient failed to connect to server : {1}". format(time_str, e)) raise def _check_available_updates(self, node_id=None): ''' Returns list of available update images from the Hub Parameters ---------- node_id (str) : node id to give hub for update checking ''' update_image_names = [] if node_id is None: node_id = self.config_manager.config_data["node-id"] try: # pulling list of update image names self._connect_to_server() self.server_s.send( bytes("CHECK_AVAILABLE_UPDATES {}".format(node_id), 'UTF-8')) response_str = self.server_s.recv( self.config_manager.config_data["bluetooth-message-max-size"] ).decode("utf-8") update_image_names = response_str.split(",") # if there are no updates if update_image_names == [' ']: update_image_names = [] # logging completion time_str = datetime.datetime.fromtimestamp( time.time()).strftime('%Y-%m-%d_%H-%M-%S') logging.info("{0} - NodeBluetoothClient successfully checked available updates : {1}".\ format(time_str, str(update_image_names))) except Exception as e: time_str = datetime.datetime.fromtimestamp( time.time()).strftime('%Y-%m-%d_%H-%M-%S') logging.error( "{0} - NodeBluetoothClient failed to check Hub for updates : {1}" .format(time_str, e)) self.server_s.close() return update_image_names def _get_update_file(self, update_file): ''' Pulls the specified udate file from the Hub Parameters ---------- update_file (str) : name of update file to fetch ''' try: # downloading file from hub self._connect_to_server() self.server_s.send( bytes("GET_UPDATE {}".format(update_file), 'UTF-8')) self._download_file(update_file) # logging completion time_str = datetime.datetime.fromtimestamp( time.time()).strftime('%Y-%m-%d_%H-%M-%S') logging.info( "{0} - NodeBluetoothClient successfully pulled update file ({1}) from Hub\ ".format(time_str, update_file)) except Exception as e: time_str = datetime.datetime.fromtimestamp( time.time()).strftime('%Y-%m-%d_%H-%M-%S') logging.error( "{0} - NodeBluetoothClient failed to pull update from Hub : {1}" .format(time_str, e)) self.server_s.close() def _download_file(self, file_name): ''' Creates the specified file and writes the incoming Hub server data in it. ** assumes that the connection with the hub is already established ** no error handling ** does not close the existing connection (even if exception is thrown) Parameters ---------- file_name (str) : name of the output file ''' update_file_path = os.path.join( self.config_manager.config_data["node-image-archive-dir"], file_name) try: # writing incoming data to file with open(update_file_path, "wb") as archive_file_h: file_data = self.server_s.recv( self.config_manager. config_data["bluetooth-message-max-size"]) while file_data: archive_file_h.write(file_data) file_data = self.server_s.recv( self.config_manager. config_data["bluetooth-message-max-size"]) # logging completion time_str = datetime.datetime.fromtimestamp( time.time()).strftime('%Y-%m-%d_%H-%M-%S') logging.info( "{0} - NodeBluetoothClient successfully downloaded file ({1}) (socket timeout) from Hub\ ".format(time_str, file_name)) # consider time out as : (no more available data) # this is the worst way of checking download is complete except BluetoothError: pass def _upload_file(self, file_name): ''' Sends the specified file (from the node transfer folder) to the Hub ** lets exceptions bubble up Parameters ---------- file_name (str) : name of upload file (must be in transfer folder) ''' upload_file_path = os.path.join( self.config_manager.config_data["node-file-transfer-dir"], file_name) try: self._connect_to_server() self.server_s.send( bytes("STORE_FILE {}".format(file_name), 'UTF-8')) # uploading specified file to the hub with open(upload_file_path, "rb") as image_file_h: data = image_file_h.read( self.config_manager. config_data["bluetooth-message-max-size"]) while data: self.server_s.send(data) data = image_file_h.read( self.config_manager. config_data["bluetooth-message-max-size"]) self.server_s.close() # logging completion time_str = datetime.datetime.fromtimestamp( time.time()).strftime('%Y-%m-%d_%H-%M-%S') logging.info( "{0} - NodeBluetoothClient successfully uploaded file ({1}) to Hub\ ".format(time_str, file_name)) except: self.server_s.close() raise def _transfer_data_files(self): ''' Transfers the contents of the data-transfer folder to the hub 1) copy the contents of the extracted data file (sensor data) to a temp file 2) create a new extracted data file (blank) for new data extraction (sensor data) 3) transfer/delete all data/log files to the Tremium Hub ''' transfer_files = [] transfer_dir = self.config_manager.config_data[ "node-file-transfer-dir"] transfer_file_name = self.config_manager.config_data[ "node-extracted-data-file"] data_file_max_size = self.config_manager.config_data[ "node-data-file-max-size"] archived_data_pattern_segs = self.config_manager.config_data[ "node-archived-data-file"].split(".") # when the main data file is big enough transfer the contents to an other file data_file_path = os.path.join(transfer_dir, transfer_file_name) if os.path.isfile(data_file_path): if os.stat(data_file_path).st_size > data_file_max_size: # waiting for data file availability and locking it while not self.cache.data_file_available(): time.sleep(0.1) self.cache.lock_data_file() # renaming the filled / main data file time_str = datetime.datetime.fromtimestamp( time.time()).strftime('%Y-%m-%d_%H-%M-%S') archive_file_name = archived_data_pattern_segs[ 0] + "-{}".format( time_str) + "." + archived_data_pattern_segs[1] archive_file_path = os.path.join(transfer_dir, archive_file_name) os.rename(data_file_path, archive_file_path) # creating new main data file open(data_file_path, "w").close() # unlocking the data file self.cache.unlock_data_file() # collecting all (archived / ready for transfer) data files + log files for element in os.listdir(transfer_dir): element_path = os.path.join(transfer_dir, element) if os.path.isfile(element_path): is_log_file = element.endswith(".log") is_archived_data = re.search(archived_data_pattern_segs[0], element) is not None is_full = os.stat(element_path).st_size > data_file_max_size if (is_log_file or is_archived_data) and is_full: transfer_files.append((element, element_path)) try: # uploading transfer files to the Hub and deleting them from local storage for file_info in transfer_files: self._upload_file(file_info[0]) os.remove(file_info[1]) except Exception as e: time_str = datetime.datetime.fromtimestamp( time.time()).strftime('%Y-%m-%d_%H-%M-%S') logging.error( "{0} - NodeBluetoothClient failed while transfering data files : {1}" .format(time_str, e)) raise # return the names of files that were sent return [file_info[0] for file_info in transfer_files] def launch_maintenance(self): ''' Launches the hub - node maintenance sequence - transfers/purges data files (acquisition and logs) - fetches available updates - adds necessary entries in the image update file ''' update_entries = [] archive_dir = self.config_manager.config_data["node-image-archive-dir"] time_stp_pattern = self.config_manager.config_data[ "image-archive-pattern"] docker_registry_prefix = self.config_manager.config_data[ "docker_registry_prefix"] try: # transfering data/log files to the hub self._transfer_data_files() # pulling available updates from the hub update_files = self._check_available_updates() for update_file in update_files: # getting old image to be updated, if any old_image_file = get_matching_image(update_file, self.config_manager) if old_image_file is not None: # downloading update image from the Hub self._get_update_file(update_file) # deleting old image archive files (.tar and .tar.gz) old_image_path = os.path.join(archive_dir, old_image_file) try: os.remove(old_image_path) except: pass # adding update file entry old_image_time_stp = re.search(time_stp_pattern, old_image_file).group(3) old_image_reg_path = docker_registry_prefix + old_image_file.split( old_image_time_stp)[0][:-1] update_image_time_stp = re.search(time_stp_pattern, update_file).group(3) update_image_reg_path = docker_registry_prefix + update_file.split( update_image_time_stp)[0][:-1] update_entries.append(old_image_reg_path + " " + update_file + " " + update_image_reg_path + "\n") # if updates were pulled from the hub if len(update_entries) > 0: # halting the data collection self.cache.stop_data_collection() # logging the update entries time_str = datetime.datetime.fromtimestamp( time.time()).strftime('%Y-%m-%d_%H-%M-%S') logging.info("{0} - NodeBluetoothClient writting out update entries : {1}".\ format(time_str, str(update_entries))) # writing out the update entries with open( self.config_manager. config_data["node-image-update-file"], "w") as update_file_h: for entry in update_entries: update_file_h.write(entry) update_file_h.write("End") # logging maintenance success time_str = datetime.datetime.fromtimestamp( time.time()).strftime('%Y-%m-%d_%H-%M-%S') logging.info( "{0} - Node Bluetooth client successfully performed maintenance" .format(time_str)) except Exception as e: time_str = datetime.datetime.fromtimestamp( time.time()).strftime('%Y-%m-%d_%H-%M-%S') logging.error("{0} - Node Bluetooth client failed : {1}".format( time_str, e))