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))
