def test_temperature():
_temperature = None
_fan = None
_log = Logger('temp-test', Level.INFO)
try:
# scan I2C bus
_log.info('scanning I²C address bus...')
scanner = I2CScanner(_log.level)
_addresses = scanner.get_int_addresses()
# load configuration
_loader = ConfigLoader(Level.INFO)
filename = 'config.yaml'
_config = _loader.configure(filename)
# create Fan if available
_fan_config = _config['ros'].get('fan')
_fan_i2c_address = _fan_config.get('i2c_address')
fan_available = (_fan_i2c_address in _addresses)
if fan_available:
_fan = Fan(_config, Level.INFO)
_log.info('fan is available at I²C address of 0x{:02X}.'.format(
_fan_i2c_address))
else:
_fan = None
_log.info(
'fan is not available at I²C address of 0x{:02X}.'.format(
_fan_i2c_address))
_message_factory = MessageFactory(Level.INFO)
_message_bus = MessageBus(Level.DEBUG)
_clock = Clock(_config, _message_bus, _message_factory, Level.WARN)
_temperature = Temperature(_config, _clock, _fan, Level.INFO)
_temperature.enable()
_counter = itertools.count()
_clock.enable()
while True:
_count = next(_counter)
# _temperature.display_temperature()
# _value = _temperature.get_cpu_temperature()
# _is_warning_temp = _temperature.is_warning_temperature()
# _is_max_temp = _temperature.is_max_temperature()
# if _is_warning_temp:
# _log.warning('[{:04d}] loop; is warning temp? {}; is max temp? {}'.format(_count, _is_warning_temp, _is_max_temp))
# else:
# _log.info('[{:04d}] loop; is warning temp? {}; is max temp? {}'.format(_count, _is_warning_temp, _is_max_temp))
time.sleep(1.0)
except KeyboardInterrupt:
_log.info(Fore.YELLOW + 'exited via Ctrl-C' + Style.RESET_ALL)
except Exception:
_log.error(Fore.RED + Style.BRIGHT +
'error getting RPI temperature: {}'.format(
traceback.format_exc()) + Style.RESET_ALL)
finally:
if _temperature:
_temperature.close()
if _fan:
_fan.disable()
def main():
signal.signal(signal.SIGINT, signal_handler)
_log = Logger("scanner", Level.INFO)
_log.info(Fore.CYAN + Style.BRIGHT + ' INFO : starting test...')
_log.info(Fore.YELLOW + Style.BRIGHT + ' INFO : Press Ctrl+C to exit.')
try:
_i2c_scanner = I2CScanner(Level.WARN)
_addresses = _i2c_scanner.getAddresses()
hexAddresses = _i2c_scanner.getHexAddresses()
_addrDict = dict(
list(map(lambda x, y: (x, y), _addresses, hexAddresses)))
for i in range(len(_addresses)):
_log.debug(Fore.BLACK + Style.DIM +
'found device at address: {}'.format(hexAddresses[i]))
vl53l1x_available = (0x29 in _addresses)
ultraborg_available = (0x36 in _addresses)
if not vl53l1x_available:
raise OSError('VL53L1X hardware dependency not available.')
elif not ultraborg_available:
raise OSError('UltraBorg hardware dependency not available.')
_log.info('starting scan...')
# _player = Player(Level.INFO)
_loader = ConfigLoader(Level.INFO)
filename = 'config.yaml'
_config = _loader.configure(filename)
_lidar = Lidar(_config, Level.INFO)
_lidar.enable()
values = _lidar.scan()
_angle_at_min = values[0]
_min_mm = values[1]
_angle_at_max = values[2]
_max_mm = values[3]
_log.info(
Fore.CYAN + Style.BRIGHT +
'min. distance at {:>5.2f}°:\t{}mm'.format(_angle_at_min, _min_mm))
_log.info(
Fore.CYAN + Style.BRIGHT +
'max. distance at {:>5.2f}°:\t{}mm'.format(_angle_at_max, _max_mm))
time.sleep(1.0)
_lidar.close()
_log.info(Fore.CYAN + Style.BRIGHT + 'test complete.')
except Exception:
_log.info(traceback.format_exc())
sys.exit(1)
def main():
_log = Logger("cat-scan test", Level.INFO)
_log.info('starting test...')
_log.info(Fore.YELLOW + Style.BRIGHT + ' INFO : Press Ctrl+C to exit.')
try:
_i2c_scanner = I2CScanner(Level.WARN)
_addresses = _i2c_scanner.getAddresses()
hexAddresses = _i2c_scanner.getHexAddresses()
_addrDict = dict(list(map(lambda x, y:(x,y), _addresses, hexAddresses)))
for i in range(len(_addresses)):
_log.debug(Fore.BLACK + Style.DIM + 'found device at address: {}'.format(hexAddresses[i]) + Style.RESET_ALL)
ultraborg_available = ( 0x36 in _addresses )
if not ultraborg_available:
raise OSError('UltraBorg hardware dependency not available.')
_log.info('starting cat scan...')
_loader = ConfigLoader(Level.INFO)
filename = 'config.yaml'
_config = _loader.configure(filename)
_queue = MockMessageQueue(Level.INFO)
_catscan = CatScan(_config, _queue, Level.INFO)
_catscan.enable()
_catscan.set_mode(True)
_count = 0
while not _queue.is_complete():
if _count % 5 == 0:
_log.info('waiting for cat... ' + Fore.BLACK + Style.DIM + '({:d} sec)'.format(_count))
time.sleep(1.0)
_count += 1
time.sleep(9.0)
_catscan.close()
_log.info('test complete.')
except KeyboardInterrupt:
if _catscan:
_catscan.close()
except Exception:
_log.info(traceback.format_exc())
finally:
if _catscan:
_catscan.close()
def main():
signal.signal(signal.SIGINT, signal_handler)
print('follower test :' + Fore.CYAN + Style.BRIGHT + ' INFO : starting test...' + Style.RESET_ALL)
print('follower test :' + Fore.YELLOW + Style.BRIGHT + ' INFO : Press Ctrl+C to exit.' + Style.RESET_ALL)
_log = Logger("follower", Level.INFO)
try:
_i2c_scanner = I2CScanner(Level.WARN)
_addresses = _i2c_scanner.getAddresses()
hexAddresses = _i2c_scanner.getHexAddresses()
_addrDict = dict(list(map(lambda x, y:(x,y), _addresses, hexAddresses)))
for i in range(len(_addresses)):
_log.debug(Fore.BLACK + Style.DIM + 'found device at address: {}'.format(hexAddresses[i]) + Style.RESET_ALL)
vl53l1x_available = ( 0x29 in _addresses )
ultraborg_available = ( 0x36 in _addresses )
if not vl53l1x_available:
raise OSError('VL53L1X hardware dependency not available.')
elif not ultraborg_available:
raise OSError('UltraBorg hardware dependency not available.')
_log.info('starting wall follower...')
_loader = ConfigLoader(Level.INFO)
filename = 'config.yaml'
_config = _loader.configure(filename)
_follower = WallFollower(_config, Level.INFO)
_follower.enable()
_follower.set_position(Orientation.PORT)
time.sleep(1.5)
_follower.set_position(Orientation.STBD)
time.sleep(1.5)
# for i in range(10):
# mm = _follower.scan()
# time.sleep(1.0)
_follower.close()
print('follower test :' + Fore.CYAN + Style.BRIGHT + ' INFO : test complete.' + Style.RESET_ALL)
except Exception:
_log.info(traceback.format_exc())
sys.exit(1)
def main():
signal.signal(signal.SIGINT, signal_handler)
print('uscanner_test :' + Fore.CYAN + Style.BRIGHT +
' INFO : starting test...' + Style.RESET_ALL)
print('uscanner_test :' + Fore.YELLOW + Style.BRIGHT +
' INFO : Press Ctrl+C to exit.' + Style.RESET_ALL)
_log = Logger("uscanner", Level.INFO)
try:
_scanner = I2CScanner(Level.WARN)
_int_addresses = _scanner.get_int_addresses()
_hex_addresses = _scanner.get_hex_addresses()
_addrDict = dict(
list(map(lambda x, y: (x, y), _int_addresses, _hex_addresses)))
for i in range(len(_int_addresses)):
_log.debug(
Fore.BLACK + Style.DIM +
'found device at address: {}'.format(_hex_addresses[i]) +
Style.RESET_ALL)
ultraborg_available = (0x36 in _int_addresses)
if ultraborg_available:
_log.info('starting ultrasonic scan...')
_loader = ConfigLoader(Level.INFO)
filename = 'config.yaml'
_config = _loader.configure(filename)
_scanner = UltrasonicScanner(_config, Level.INFO)
_scanner.enable()
values = _scanner.scan()
time.sleep(1.0)
_scanner.close()
else:
_log.error('required hardware dependencies not available.')
print('uscanner_test :' + Fore.CYAN + Style.BRIGHT +
' INFO : test complete.' + Style.RESET_ALL)
except Exception:
_log.info(traceback.format_exc())
sys.exit(1)
def test_bno08x(loop=False):
_log = Logger("bno085-test", Level.INFO)
_i2c_scanner = I2CScanner(Level.WARN)
if not _i2c_scanner.has_address([0x4a]):
_log.warning('test ignored: no bno085 found.')
return
_log.info('starting test...')
_loader = ConfigLoader(Level.INFO)
filename = 'config.yaml'
_config = _loader.configure(filename)
_message_factory = MessageFactory(Level.INFO)
_queue = MessageQueue(_message_factory, Level.INFO)
_bno = BNO08x(_config, _queue, Level.INFO)
# begin ............
_log.info('starting BNO08x read loop...')
_count = 0
_errors = 0
_amin = list(_bno.magneto())
_amax = list(_bno.magneto())
while _count < 10 if not loop else True:
_count += 1
mag = _bno.magneto()
if mag is not None:
heading = Convert.heading_from_magnetometer(_amin, _amax, mag)
_log.info('\tMag: {:05.2f} {:05.2f} {:05.2f} '.format(mag[0], mag[1], mag[2]) \
+ Fore.YELLOW + '\tHeading: {:d}°'.format(heading))
else:
_log.info('null result')
# result = _bno.read()
# if result != None:
# _log.info('[{:>3d}] result: {:>5.2f} | {:>5.2f} | {:>5.2f} |\t'.format(_count, result[0], result[1], result[2]) + Fore.BLACK + '{:d} errors.'.format(_errors))
# else:
# _errors += 1
# _log.warning('[{:>3d}] result: NA'.format(_count))
time.sleep(0.25)
def __init__(self, ros, level):
self._log = Logger("import", level)
if ros is None:
raise ValueError('null ros argument.')
self._ros = ros
try:
scanner = I2CScanner(Level.WARN)
self._addresses = scanner.getAddresses()
hexAddresses = scanner.getHexAddresses()
self._addrDict = dict(
list(map(lambda x, y: (x, y), self._addresses, hexAddresses)))
for i in range(len(self._addresses)):
self._log.debug(
Fore.BLACK + Style.DIM +
'found device at address: {}'.format(hexAddresses[i]) +
Style.RESET_ALL)
except Exception:
self._log.info(traceback.format_exc())
sys.exit(1)
def __init__(self, level):
global enabled
self._log = Logger("rgbmatrix", level)
_i2c_scanner = I2CScanner(Level.WARN)
_addresses = _i2c_scanner.get_int_addresses()
self._rgbmatrix5x5_PORT = RGBMatrix5x5(
address=0x77) if (0x77 in _addresses) else None
# self._rgbmatrix5x5_PORT = RGBMatrix5x5(address=0x77)
if self._rgbmatrix5x5_PORT:
self._log.info('port rgbmatrix at 0x77.')
self._rgbmatrix5x5_PORT.set_brightness(0.8)
self._rgbmatrix5x5_PORT.set_clear_on_exit()
else:
self._log.info('no port rgbmatrix found.')
self._rgbmatrix5x5_STBD = RGBMatrix5x5(address=0x74)
self._log.info('starboard rgbmatrix at 0x74.')
self._rgbmatrix5x5_STBD.set_brightness(0.8)
self._rgbmatrix5x5_STBD.set_clear_on_exit()
self._height = self._rgbmatrix5x5_STBD.height
self._width = self._rgbmatrix5x5_STBD.width
self._log.info('rgbmatrix width,height: {},{}'.format(
self._width, self._height))
self._thread_PORT = None
self._thread_STBD = None
self._color = Color.RED # used by _solid
enabled = False
self._closing = False
self._closed = False
self._display_type = DisplayType.DARK # default
# color used by wipe display
self._wipe_color = Color.WHITE # default
# used by _cpu:
self._max_value = 0.0 # TEMP
self._buf = numpy.zeros(
(self._rgbmatrix5x5_STBD._width, self._rgbmatrix5x5_STBD._height))
self._colors = [
Color.GREEN, Color.YELLOW_GREEN, Color.YELLOW, Color.ORANGE,
Color.RED
]
self._log.info('ready.')
def __init__(self, level):
self._log = Logger("matrices", level)
_i2c_scanner = I2CScanner(Level.DEBUG)
self._enabled = True
if _i2c_scanner.has_address([0x77]): # port
self._port_matrix = Matrix(Orientation.PORT, Level.INFO)
self._log.info('port-side matrix available.')
else:
self._port_matrix = None
self._log.warning('no port-side matrix available.')
if _i2c_scanner.has_address([0x75]): # starboard
self._stbd_matrix = Matrix(Orientation.STBD, Level.INFO)
self._log.info('starboard-side matrix available.')
else:
self._stbd_matrix = None
self._log.warning('no starboard-side matrix available.')
if not self._port_matrix and not self._stbd_matrix:
self._enabled = False
self._log.warning('no matrix displays available.')
self._log.info('ready.')
def test_rot_encoder():
_log = Logger("rot-test", Level.INFO)
_i2c_scanner = I2CScanner(Level.WARN)
if not _i2c_scanner.has_address([0x16]):
_log.warning('test ignored: no rotary encoder found.')
return
_rot = None
try:
# read YAML configuration
_loader = ConfigLoader(Level.INFO)
filename = 'config.yaml'
_config = _loader.configure(filename)
_rot = RotaryEncoder(_config, 0x0F, Level.INFO)
_count = 0
_updates = 0
_update_led = True
_last_value = 0
_rate = Rate(20)
_log.info(Fore.WHITE + Style.BRIGHT +
'waiting for rotary encoder to make 10 ticks...')
while _updates < 10:
# _value = _rot.update() # original method
_value = _rot.read(_update_led) # improved method
if _value != _last_value:
_log.info(Style.BRIGHT + 'returned value: {:d}'.format(_value))
_updates += 1
_last_value = _value
_count += 1
if _count % 33 == 0:
_log.info(Fore.BLACK + Style.BRIGHT + 'waiting…')
_rate.wait()
finally:
if _rot:
_log.info('resetting rotary encoder...')
_rot.reset()
def test_imu():
_log = Logger("imu-test", Level.INFO)
_i2c_scanner = I2CScanner(Level.WARN)
_addresses = _i2c_scanner.get_int_addresses()
_rgbmatrix = RgbMatrix(Level.INFO) if (0x74 in _addresses) else None
if _rgbmatrix:
_rgbmatrix.set_display_type(DisplayType.SOLID)
_rgbmatrix.enable()
try:
_loader = ConfigLoader(Level.INFO)
filename = 'config.yaml'
_config = _loader.configure(filename)
_cardinal = Cardinal.get_heading_from_degrees(0)
_imu = IMU(_config, Level.INFO)
while True:
_heading = _imu.read_heading()[0]
if _rgbmatrix:
_cardinal = Cardinal.get_heading_from_degrees(_heading)
_color = Cardinal.get_color_for_direction(_cardinal)
_rgbmatrix.set_color(_color)
_log.info(Fore.YELLOW + 'heading: {:05.2f}°;\tcardinal: {}'.format(
_heading, _cardinal.display) + Style.RESET_ALL)
time.sleep(2.0)
except KeyboardInterrupt:
_log.info('Ctrl-C caught: exiting...')
except Exception as e:
_log.error('error closing: {}\n{}'.format(e, traceback.format_exc()))
finally:
if _rgbmatrix:
_rgbmatrix.set_color(Color.BLACK)
_rgbmatrix.disable()
_log.info('complete.')
def __init__(self, config, clock, enable_mock=False, level=Level.INFO):
self._log = Logger("motor-conf", level)
if config is None:
raise ValueError('null configuration argument.')
if clock is None:
raise ValueError('null clock argument.')
self._config = config
# Import the ThunderBorg library, then configure and return the Motors.
self._log.info('configure thunderborg & motors...')
try:
_i2c_scanner = I2CScanner(Level.WARN)
_has_thunderborg = _i2c_scanner.has_address([THUNDERBORG_ADDRESS])
if _has_thunderborg:
self._log.info('importing ThunderBorg...')
import lib.ThunderBorg3 as ThunderBorg
self._log.info('successfully imported ThunderBorg.')
else:
self._log.info('importing mock ThunderBorg...')
import mock.thunderborg as ThunderBorg
self._log.info('successfully imported mock ThunderBorg.')
TB = ThunderBorg.ThunderBorg(Level.INFO) # create a new ThunderBorg object
TB.Init() # set the board up (checks the board is connected)
self._log.info('successfully instantiated ThunderBorg.')
if not TB.foundChip:
boards = ThunderBorg.ScanForThunderBorg()
if len(boards) == 0:
self._log.error('No ThunderBorg found, check you are attached :)')
else:
self._log.error('No ThunderBorg at address %02X, but we did find boards:' % (TB.i2cAddress))
for board in boards:
self._log.info(' %02X (%d)' % (board, board))
self._log.error('If you need to change the I²C address change the setup line so it is correct, e.g. TB.i2cAddress = 0x{}'.format(boards[0]))
raise Exception('unable to instantiate ThunderBorg [1].')
# sys.exit(1)
TB.SetLedShowBattery(True)
# initialise ThunderBorg ...........................
self._log.debug('getting battery reading...')
# get battery voltage to determine max motor power
# could be: Makita 12V or 18V power tool battery, or 12V line supply
voltage_in = TB.GetBatteryReading()
if voltage_in is None:
raise OSError('cannot continue: cannot read battery voltage.')
self._log.info('voltage in: {:>5.2f}V'.format(voltage_in))
# voltage_in = 20.5
# maximum motor voltage
voltage_out = 9.0
self._log.info('voltage out: {:>5.2f}V'.format(voltage_out))
if voltage_in < voltage_out:
raise OSError('cannot continue: battery voltage too low ({:>5.2f}V).'.format(voltage_in))
# Setup the power limits
if voltage_out > voltage_in:
_max_power_ratio = 1.0
else:
_max_power_ratio = voltage_out / float(voltage_in)
# convert float to ratio format
self._log.info('battery level: {:>5.2f}V; motor voltage: {:>5.2f}V; maximum power ratio: {}'.format(voltage_in, voltage_out, \
str(Fraction(_max_power_ratio).limit_denominator(max_denominator=20)).replace('/',':')))
except OSError as e:
if enable_mock:
self._log.info('using mock ThunderBorg.')
import mock.thunderborg as ThunderBorg
else:
self._log.error('unable to import ThunderBorg: {}'.format(e))
traceback.print_exc(file=sys.stdout)
raise Exception('unable to instantiate ThunderBorg [2].')
except Exception as e:
if enable_mock:
self._log.info('using mock ThunderBorg.')
import mock.thunderborg as ThunderBorg
else:
self._log.error('unable to import ThunderBorg: {}'.format(e))
traceback.print_exc(file=sys.stdout)
raise Exception('unable to instantiate ThunderBorg [2].')
# sys.exit(1)
# now import motors
from lib.motors import Motors
try:
self._log.info('getting raspberry pi...')
self._log.info('configuring motors...')
self._motors = Motors(self._config, clock, TB, Level.INFO)
self._motors.get_motor(Orientation.PORT).set_max_power_ratio(_max_power_ratio)
self._motors.get_motor(Orientation.STBD).set_max_power_ratio(_max_power_ratio)
except OSError as oe:
self._log.error('failed to configure motors: {}'.format(oe))
self._motors = None
# sys.stderr = DevNull()
raise Exception('unable to instantiate ThunderBorg [3].')
# sys.exit(1)
self._log.info('ready.')
def _configure(self):
'''
Configures ROS based on both KR01 standard hardware as well as
optional features, the latter based on devices showing up (by
address) on the I²C bus. Optional devices are only enabled at
startup time via registration of their feature availability.
'''
scanner = I2CScanner(Level.WARN)
self._addresses = scanner.getAddresses()
hexAddresses = scanner.getHexAddresses()
self._addrDict = dict(
list(map(lambda x, y: (x, y), self._addresses, hexAddresses)))
for i in range(len(self._addresses)):
self._log.debug(
Fore.BLACK + Style.DIM +
'found device at address: {}'.format(hexAddresses[i]) +
Style.RESET_ALL)
self._log.info('configure default features...')
# standard devices ...........................................
self._log.info('configuring integrated front sensors...')
self._ifs = IntegratedFrontSensor(self._config, self._queue,
Level.INFO)
self._log.info('configure ht0740 switch...')
self._switch = Switch(Level.INFO)
# since we're using the HT0740 Switch, turn it on to enable sensors that rely upon its power
# self._switch.enable()
self._switch.on()
self._log.info('configuring button...')
self._button = Button(self._config, self.get_message_queue(),
self._mutex)
self._log.info('configure battery check...')
_battery_check = BatteryCheck(self._config, self.get_message_queue(),
Level.INFO)
self.add_feature(_battery_check)
self._log.info('configure CPU temperature check...')
_temperature_check = Temperature(self._config, self._queue, Level.INFO)
self.add_feature(_temperature_check)
ultraborg_available = (0x36 in self._addresses)
if ultraborg_available:
self._log.debug(Fore.CYAN + Style.BRIGHT +
'-- UltraBorg available at 0x36.' +
Style.RESET_ALL)
else:
self._log.debug(Fore.RED + Style.BRIGHT +
'-- no UltraBorg available at 0x36.' +
Style.RESET_ALL)
self._set_feature_available('ultraborg', ultraborg_available)
thunderborg_available = (0x15 in self._addresses)
if thunderborg_available: # then configure motors
self._log.debug(Fore.CYAN + Style.BRIGHT +
'-- ThunderBorg available at 0x15' +
Style.RESET_ALL)
_motor_configurer = MotorConfigurer(self, self._config, Level.INFO)
self._motors = _motor_configurer.configure()
else:
self._log.debug(Fore.RED + Style.BRIGHT +
'-- no ThunderBorg available at 0x15.' +
Style.RESET_ALL)
self._set_feature_available('thunderborg', thunderborg_available)
# optional devices ...........................................
# the 5x5 RGB Matrix is at 0x74 for port, 0x77 for starboard
rgbmatrix5x5_stbd_available = (0x74 in self._addresses)
if rgbmatrix5x5_stbd_available:
self._log.debug(Fore.CYAN + Style.BRIGHT +
'-- RGB Matrix available at 0x74.' +
Style.RESET_ALL)
else:
self._log.debug(Fore.RED + Style.BRIGHT +
'-- no RGB Matrix available at 0x74.' +
Style.RESET_ALL)
self._set_feature_available('rgbmatrix5x5_stbd',
rgbmatrix5x5_stbd_available)
rgbmatrix5x5_port_available = (0x77 in self._addresses)
if rgbmatrix5x5_port_available:
self._log.debug(Fore.CYAN + Style.BRIGHT +
'-- RGB Matrix available at 0x77.' +
Style.RESET_ALL)
else:
self._log.debug(Fore.RED + Style.BRIGHT +
'-- no RGB Matrix available at 0x77.' +
Style.RESET_ALL)
self._set_feature_available('rgbmatrix5x5_port',
rgbmatrix5x5_port_available)
if rgbmatrix5x5_stbd_available or rgbmatrix5x5_port_available:
self._log.info('configure rgbmatrix...')
self._rgbmatrix = RgbMatrix(Level.INFO)
self.add_feature(self._rgbmatrix) # FIXME this is added twice
# ............................................
# the 11x7 LED matrix is at 0x75 for starboard, 0x77 for port. The latter
# conflicts with the RGB LED matrix, so both cannot be used simultaneously.
matrix11x7_stbd_available = (0x75 in self._addresses)
if matrix11x7_stbd_available:
self._log.debug(Fore.CYAN + Style.BRIGHT +
'-- 11x7 Matrix LEDs available at 0x75.' +
Style.RESET_ALL)
else:
self._log.debug(Fore.RED + Style.BRIGHT +
'-- no 11x7 Matrix LEDs available at 0x75.' +
Style.RESET_ALL)
self._set_feature_available('matrix11x7_stbd',
matrix11x7_stbd_available)
# device availability ........................................
bno055_available = (0x28 in self._addresses)
if bno055_available:
self._log.info('configuring BNO055 9DoF sensor...')
self._bno055 = BNO055(self._config, self.get_message_queue(),
Level.INFO)
else:
self._log.debug(Fore.RED + Style.BRIGHT +
'no BNO055 orientation sensor available at 0x28.' +
Style.RESET_ALL)
self._set_feature_available('bno055', bno055_available)
# NOTE: the default address for the ICM20948 is 0x68, but this conflicts with the PiJuice
icm20948_available = (0x69 in self._addresses)
if icm20948_available:
self._log.debug(Fore.CYAN + Style.BRIGHT +
'ICM20948 available at 0x69.' + Style.RESET_ALL)
else:
self._log.debug(Fore.RED + Style.BRIGHT +
'no ICM20948 available at 0x69.' + Style.RESET_ALL)
self._set_feature_available('icm20948', icm20948_available)
lsm303d_available = (0x1D in self._addresses)
if lsm303d_available:
self._log.debug(Fore.CYAN + Style.BRIGHT +
'LSM303D available at 0x1D.' + Style.RESET_ALL)
else:
self._log.debug(Fore.RED + Style.BRIGHT +
'no LSM303D available at 0x1D.' + Style.RESET_ALL)
self._set_feature_available('lsm303d', lsm303d_available)
vl53l1x_available = (0x29 in self._addresses)
if vl53l1x_available:
self._log.debug(Fore.CYAN + Style.BRIGHT +
'VL53L1X available at 0x29.' + Style.RESET_ALL)
else:
self._log.debug(Fore.RED + Style.BRIGHT +
'no VL53L1X available at 0x29.' + Style.RESET_ALL)
self._set_feature_available('vl53l1x', vl53l1x_available)
as7262_available = (0x49 in self._addresses)
if as7262_available:
self._log.debug(Fore.CYAN + Style.BRIGHT +
'-- AS7262 Spectrometer available at 0x49.' +
Style.RESET_ALL)
else:
self._log.debug(Fore.RED + Style.BRIGHT +
'-- no AS7262 Spectrometer available at 0x49.' +
Style.RESET_ALL)
self._set_feature_available('as7262', as7262_available)
pijuice_available = (0x68 in self._addresses)
if pijuice_available:
self._log.debug(Fore.CYAN + Style.BRIGHT +
'PiJuice hat available at 0x68.' + Style.RESET_ALL)
else:
self._log.debug(Fore.RED + Style.BRIGHT +
'no PiJuice hat available at 0x68.' +
Style.RESET_ALL)
self._set_feature_available('pijuice', pijuice_available)
self._log.info('configured.')
def __init__(self, config, level):
super().__init__()
global video_width, video_height, annotation_title
self._log = Logger('video', level)
if config is None:
raise ValueError("no configuration provided.")
self._config = config
_config = self._config['ros'].get('video')
self._enable_streaming = _config.get('enable_streaming')
self._port = _config.get('port')
self._lux_threshold = _config.get('lux_threshold')
self._counter = itertools.count()
self._server = None
# camera configuration
self._ctrl_lights = _config.get('ctrl_lights')
self._width = _config.get('width')
self._height = _config.get('height')
self._resolution = (self._width, self._height)
self._framerate = _config.get('framerate')
self._convert_mp4 = _config.get('convert_mp4')
self._remove_h264 = _config.get('remove_h264')
self._quality = _config.get('quality')
self._annotate = _config.get('annotate')
self._title = _config.get('title')
self._basename = _config.get('basename')
self._dirname = _config.get('dirname')
# set globals
video_width = self._width
video_height = self._height
annotation_title = self._title
self._filename = None
self._thread = None
self._killer = None
# scan I2c bus for devices
_i2c_scanner = I2CScanner(Level.DEBUG)
if _i2c_scanner.has_address([0x23]):
self._lux = Lux(Level.INFO)
self._log.info(
'LTR-559 lux sensor found: camera adjustment active.')
else:
self._lux = None
self._log.warning(
'no LTR-559 lux sensor found: camera adjustment inactive.')
# lighting configuration
self._port_light = None
self._stbd_light = None
if self._ctrl_lights:
if _i2c_scanner.has_address([0x77]): # port
self._port_light = Matrix(Orientation.PORT, Level.INFO)
self._log.info('port-side camera lighting available.')
else:
self._log.warning('no port-side camera lighting available.')
if _i2c_scanner.has_address([0x75]): # starboard
self._stbd_light = Matrix(Orientation.STBD, Level.INFO)
self._log.info('starboard-side camera lighting available.')
else:
self._log.warning(
'no starboard-side camera lighting available.')
else:
self._log.info('lighting control is disabled.')
if self._enable_streaming:
self._log.info('ready: streaming on port {:d}'.format(self._port))
else:
self._log.info('ready: save to file only, no streaming.')
def configure(self, arguments):
'''
Provided with a set of configuration arguments, configures ROS based on
both KD01/KR01 standard hardware as well as optional features, the
latter based on devices showing up (by address) on the I²C bus. Optional
devices are only enabled at startup time via registration of their feature
availability.
'''
self._log.heading('configuration', 'configuring ros...',
'[1/2]' if arguments.start else '[1/1]')
self._log.info('application log level: {}'.format(
self._log.level.name))
# configuration from command line arguments
self._using_mocks = False
self._permit_mocks = arguments.mock
self._enable_camera = arguments.camera # TODO
# read YAML configuration
_loader = ConfigLoader(self._log.level)
_config_file = arguments.config_file if arguments.config_file is not None else 'config.yaml'
self._config = _loader.configure(_config_file)
# scan I2C bus
self._log.info('scanning I²C address bus...')
scanner = I2CScanner(self._log.level)
self._addresses = scanner.get_int_addresses()
_hex_addresses = scanner.get_hex_addresses()
self._addrDict = dict(
list(map(lambda x, y: (x, y), self._addresses, _hex_addresses)))
# for i in range(len(self._addresses)):
for _address in self._addresses:
_device_name = self.get_device_for_address(_address)
self._log.info('found device at I²C address 0x{:02X}: {}'.format(
_address, _device_name) + Style.RESET_ALL)
# TODO look up address and make assumption about what the device is
# establish basic subsumption components
self._log.info('configure application messaging...')
self._message_factory = MessageFactory(self._log.level)
self._message_bus = MessageBus(self._log.level)
self._log.info('configuring system clock...')
self._clock = Clock(self._config, self._message_bus,
self._message_factory, Level.WARN)
self.add_feature(self._clock)
# standard devices ...........................................
self._log.info('configure default features...')
self._log.info('configure CPU temperature check...')
_temperature_check = Temperature(self._config, self._clock,
self._log.level)
if _temperature_check.get_cpu_temperature() > 0:
self.add_feature(_temperature_check)
else:
self._log.warning('no support for CPU temperature.')
motors_enabled = not arguments.no_motors and (0x15 in self._addresses)
if motors_enabled: # then configure motors
self._log.debug(Fore.CYAN + Style.BRIGHT +
'-- ThunderBorg available at 0x15' +
Style.RESET_ALL)
_motor_configurer = MotorConfigurer(self._config, self._clock,
self._log.level)
self._motors = _motor_configurer.get_motors()
self.add_feature(self._motors)
self._set_feature_available('motors', motors_enabled)
elif self._permit_mocks:
self._using_mocks = True
self._log.debug(Fore.RED + Style.BRIGHT +
'-- no ThunderBorg available, using mocks.' +
Style.RESET_ALL)
from mock.motor_configurer import MockMotorConfigurer
_motor_configurer = MockMotorConfigurer(self._config, self._clock,
self._log.level)
self._motors = _motor_configurer.get_motors()
self.add_feature(self._motors)
self._set_feature_available('motors', motors_enabled)
ifs_available = (0x0E in self._addresses)
if ifs_available:
self._log.info('configuring integrated front sensor...')
self._ifs = IntegratedFrontSensor(self._config, self._clock,
self._log.level)
self.add_feature(self._ifs)
elif self._permit_mocks:
self._using_mocks = True
self._log.info(
'integrated front sensor not available; loading mock sensor.')
from mock.ifs import MockIntegratedFrontSensor
self._ifs = MockIntegratedFrontSensor(self._message_bus,
exit_on_complete=False,
level=self._log.level)
self._message_bus.set_ifs(self._ifs)
self.add_feature(self._ifs)
else:
self._ifs = None
self._log.warning('no integrated front sensor available.')
# ultraborg_available = ( 0x36 in self._addresses )
# if ultraborg_available:
# self._log.debug(Fore.CYAN + Style.BRIGHT + '-- UltraBorg available at 0x36.' + Style.RESET_ALL)
# else:
# self._log.debug(Fore.RED + Style.BRIGHT + '-- no UltraBorg available at 0x36.' + Style.RESET_ALL)
# self._set_feature_available('ultraborg', ultraborg_available)
# # optional devices ...........................................
self._log.info('configure optional features...')
self._gamepad_enabled = arguments.gamepad and self._config['ros'].get(
'gamepad').get('enabled')
# # the 5x5 RGB Matrix is at 0x74 for port, 0x77 for starboard
# rgbmatrix5x5_stbd_available = ( 0x74 in self._addresses )
# if rgbmatrix5x5_stbd_available:
# self._log.debug(Fore.CYAN + Style.BRIGHT + '-- RGB Matrix available at 0x74.' + Style.RESET_ALL)
# else:
# self._log.debug(Fore.RED + Style.BRIGHT + '-- no RGB Matrix available at 0x74.' + Style.RESET_ALL)
# self._set_feature_available('rgbmatrix5x5_stbd', rgbmatrix5x5_stbd_available)
# rgbmatrix5x5_port_available = ( 0x77 in self._addresses )
# if rgbmatrix5x5_port_available:
# self._log.debug(Fore.CYAN + Style.BRIGHT + '-- RGB Matrix available at 0x77.' + Style.RESET_ALL)
# else:
# self._log.debug(Fore.RED + Style.BRIGHT + '-- no RGB Matrix available at 0x77.' + Style.RESET_ALL)
# self._set_feature_available('rgbmatrix5x5_port', rgbmatrix5x5_port_available)
# if rgbmatrix5x5_stbd_available or rgbmatrix5x5_port_available:
# self._log.info('configure rgbmatrix...')
# self._rgbmatrix = RgbMatrix(Level.INFO)
# self.add_feature(self._rgbmatrix) # FIXME this is added twice
# # ............................................
# # the 11x7 LED matrix is at 0x75 for starboard, 0x77 for port. The latter
# # conflicts with the RGB LED matrix, so both cannot be used simultaneously.
# matrix11x7_stbd_available = ( 0x75 in self._addresses )
# if matrix11x7_stbd_available:
# self._log.debug(Fore.CYAN + Style.BRIGHT + '-- 11x7 Matrix LEDs available at 0x75.' + Style.RESET_ALL)
# else:
# self._log.debug(Fore.RED + Style.BRIGHT + '-- no 11x7 Matrix LEDs available at 0x75.' + Style.RESET_ALL)
# self._set_feature_available('matrix11x7_stbd', matrix11x7_stbd_available)
# # device availability ........................................
# bno055_available = ( 0x28 in self._addresses )
# if bno055_available:
# self._log.info('configuring BNO055 9DoF sensor...')
# self._bno055 = BNO055(self._config, self.get_message_queue(), Level.INFO)
# else:
# self._log.debug(Fore.RED + Style.BRIGHT + 'no BNO055 orientation sensor available at 0x28.' + Style.RESET_ALL)
# self._set_feature_available('bno055', bno055_available)
# # NOTE: the default address for the ICM20948 is 0x68, but this conflicts with the PiJuice
# icm20948_available = ( 0x69 in self._addresses )
# if icm20948_available:
# self._log.debug(Fore.CYAN + Style.BRIGHT + 'ICM20948 available at 0x69.' + Style.RESET_ALL)
# else:
# self._log.debug(Fore.RED + Style.BRIGHT + 'no ICM20948 available at 0x69.' + Style.RESET_ALL)
# self._set_feature_available('icm20948', icm20948_available)
# lsm303d_available = ( 0x1D in self._addresses )
# if lsm303d_available:
# self._log.debug(Fore.CYAN + Style.BRIGHT + 'LSM303D available at 0x1D.' + Style.RESET_ALL)
# else:
# self._log.debug(Fore.RED + Style.BRIGHT + 'no LSM303D available at 0x1D.' + Style.RESET_ALL)
# self._set_feature_available('lsm303d', lsm303d_available)
#
# vl53l1x_available = ( 0x29 in self._addresses )
# if vl53l1x_available:
# self._log.debug(Fore.CYAN + Style.BRIGHT + 'VL53L1X available at 0x29.' + Style.RESET_ALL)
# else:
# self._log.debug(Fore.RED + Style.BRIGHT + 'no VL53L1X available at 0x29.' + Style.RESET_ALL)
# self._set_feature_available('vl53l1x', vl53l1x_available)
# as7262_available = ( 0x49 in self._addresses )
# if as7262_available:
# self._log.debug(Fore.CYAN + Style.BRIGHT + '-- AS7262 Spectrometer available at 0x49.' + Style.RESET_ALL)
# else:
# self._log.debug(Fore.RED + Style.BRIGHT + '-- no AS7262 Spectrometer available at 0x49.' + Style.RESET_ALL)
# self._set_feature_available('as7262', as7262_available)
# pijuice_available = ( 0x68 in self._addresses )
# if pijuice_available:
# self._log.debug(Fore.CYAN + Style.BRIGHT + 'PiJuice hat available at 0x68.' + Style.RESET_ALL)
# else:
# self._log.debug(Fore.RED + Style.BRIGHT + 'no PiJuice hat available at 0x68.' + Style.RESET_ALL)
# self._set_feature_available('pijuice', pijuice_available)
self._log.info(Fore.YELLOW + 'configure subsumption support...')
# configure the MessageQueue, Controller and Arbitrator
self._log.info('configuring message queue...')
self._queue = MessageQueue(self._message_bus, self._log.level)
self._message_bus.add_handler(Message, self._queue.handle)
self._log.info('configuring controller...')
self._controller = Controller(self._config, self._ifs, self._motors,
self._callback_shutdown, self._log.level)
self._log.info('configuring arbitrator...')
self._arbitrator = Arbitrator(self._config, self._queue,
self._controller, self._log.level)
self._log.info('configured.')
def test_matrix():
_port_light = None
_stbd_light = None
_log = Logger('matrix-test', Level.INFO)
try:
_log.info(Fore.CYAN + 'start matrix test...')
_i2c_scanner = I2CScanner(Level.DEBUG)
if _i2c_scanner.has_address([0x77]): # port
_port_light = Matrix(Orientation.PORT, Level.INFO)
_log.info('port-side matrix available.')
else:
_log.warning('no port-side matrix available.')
if _i2c_scanner.has_address([0x75]): # starboard
_stbd_light = Matrix(Orientation.STBD, Level.INFO)
_log.info('starboard-side matrix available.')
else:
_log.warning('no starboard-side matrix available.')
if not _port_light and not _stbd_light:
_log.warning('skipping test: no matrix displays available.')
return
if _port_light:
_port_light.text('Y', False, False)
if _stbd_light:
_stbd_light.text('N', False, False)
time.sleep(2)
if _port_light:
_port_light.disable()
_port_light.clear()
if _stbd_light:
_stbd_light.disable()
_stbd_light.clear()
time.sleep(1)
_log.info('matrix on...')
if _port_light:
_port_light.light()
if _stbd_light:
_stbd_light.light()
time.sleep(2)
if _port_light:
_port_light.clear()
if _stbd_light:
_stbd_light.clear()
time.sleep(1)
_log.info('matrix gradient...')
for x in range(3):
for i in range(1, 8):
_log.info('matrix at {:d}'.format(i))
if _port_light:
_port_light.gradient(i)
if _stbd_light:
_stbd_light.gradient(i)
time.sleep(0.01)
if _port_light:
_port_light.clear()
if _stbd_light:
_stbd_light.clear()
time.sleep(0.1)
except KeyboardInterrupt:
_log.info(Fore.MAGENTA + 'Ctrl-C caught: interrupted.')
finally:
_log.info('closing matrix test...')
if _port_light:
_port_light.clear()
if _stbd_light:
_stbd_light.clear()
def test_motors():
global _port_motor, _stbd_motor, action_A, action_B
# read YAML configuration
_loader = ConfigLoader(Level.INFO)
filename = 'config.yaml'
_config = _loader.configure(filename)
_log.info('creating message factory...')
_message_factory = MessageFactory(Level.INFO)
_log.info('creating message bus...')
_message_bus = MessageBus(Level.WARN)
_log.info('creating clock...')
_clock = Clock(_config, _message_bus, _message_factory, Level.WARN)
_motor_configurer = MotorConfigurer(_config, _clock, Level.INFO)
_motors = _motor_configurer.get_motors()
_motors.enable()
_i2c_scanner = I2CScanner(Level.WARN)
if ENABLE_MOTH:
if _i2c_scanner.has_address([0x18]):
_rgbmatrix = RgbMatrix(Level.WARN)
# _rgbmatrix.set_display_type(DisplayType.SOLID)
_moth = Moth(_config, None, Level.WARN)
else:
_log.warning('cannot enable moth: no IO Expander found.')
_moth = None
_pin_A = 16
_button_16 = Button(_pin_A, callback_method_A, Level.INFO)
_log.info(
Style.BRIGHT +
'press button A (connected to pin {:d}) to toggle or initiate action.'.
format(_pin_A))
_pin_B = 24
_button_24 = Button(_pin_B, callback_method_B, Level.INFO)
_log.info(Style.BRIGHT +
'press button B connected to pin {:d}) to exit.'.format(_pin_B))
_log.info('starting motors...')
_port_motor = _motors.get_motor(Orientation.PORT)
_stbd_motor = _motors.get_motor(Orientation.STBD)
if ENABLE_PORT:
_port_motor.enable()
if ENABLE_STBD:
_stbd_motor.enable()
_rot_ctrl = RotaryControl(_config, 0, 50, 2, Level.WARN) # min, max, step
_rate = Rate(5)
_step_limit = 2312
_velocity_stbd = 0.0
_velocity_port = 0.0
_start_time = dt.now()
_moth_port = 1.0
_moth_stbd = 1.0
_moth_offset = 0.6
_moth_bias = [0.0, 0.0]
_moth_fudge = 0.7
try:
action_A = True # if not using buttons at all set to True
action_B = True
while INFINITE or action_B or (_port_motor.steps < _step_limit
and _stbd_motor.steps < _step_limit):
if action_A:
action_A = False # trigger once
while action_B:
if USE_ROTARY_CONTROL:
_target_velocity = _rot_ctrl.read()
else:
_target_velocity = 30.0
# _power = _target_velocity / 100.0
_power = Motor.velocity_to_power(_target_velocity)
if ENABLE_MOTH and _moth:
_moth_bias = _moth.get_bias()
# _log.info(Fore.WHITE + Style.BRIGHT + 'port: {:5.2f}; stbd: {:5.2f}'.format(_moth_port, _moth_stbd))
# _rgbmatrix.show_hue(_moth_hue, Orientation.BOTH)
_orientation = _moth.get_orientation()
if _orientation is Orientation.PORT:
_moth_port = _moth_bias[0] * _moth_fudge
_moth_stbd = 1.0
_rgbmatrix.show_color(Color.BLACK,
Orientation.STBD)
_rgbmatrix.show_color(Color.RED, Orientation.PORT)
elif _orientation is Orientation.STBD:
_moth_port = 1.0
_moth_stbd = _moth_bias[1] * _moth_fudge
_rgbmatrix.show_color(Color.BLACK,
Orientation.PORT)
_rgbmatrix.show_color(Color.GREEN,
Orientation.STBD)
else:
_moth_port = 1.0
_moth_stbd = 1.0
_rgbmatrix.show_color(Color.BLACK,
Orientation.PORT)
_rgbmatrix.show_color(Color.BLACK,
Orientation.STBD)
if ENABLE_STBD:
_stbd_motor.set_motor_power(_stbd_rotate * _power *
_moth_stbd)
_velocity_stbd = _stbd_motor.velocity
if ENABLE_PORT:
_port_motor.set_motor_power(_port_rotate * _power *
_moth_port)
_velocity_port = _port_motor.velocity
_log.info(Fore.YELLOW + 'power: {:5.2f}; bias: '.format(_power) \
+ Fore.RED + ' {:5.2f} '.format(_moth_bias[0]) + Fore.GREEN + '{:5.2f};'.format(_moth_bias[1]) \
+ Fore.BLACK + ' target velocity: {:5.2f};'.format(_target_velocity) \
+ Fore.CYAN + ' velocity: ' \
+ Fore.RED + ' {:5.2f} '.format(_velocity_port) + Fore.GREEN + ' {:5.2f}'.format(_velocity_stbd))
# _log.info(Fore.RED + 'power {:5.2f}/{:5.2f}; {:>4d} steps; \t'.format(_stbd_motor.get_current_power_level(), _power, _port_motor.steps) \
# + Fore.GREEN + 'power {:5.2f}/{:5.2f}; {:>4d} steps.'.format(_port_motor.get_current_power_level(), _power, _stbd_motor.steps))
_rate.wait()
action_B = True # reentry into B loop, waiting for A
_log.info('waiting for A button press...')
time.sleep(1.0)
# end wait loop ....................................................
if ENABLE_PORT:
_log.info('port motor: {:d} steps.'.format(_port_motor.steps))
if ENABLE_STBD:
_log.info('stbd motor: {:d} steps.'.format(_stbd_motor.steps))
except KeyboardInterrupt:
_log.info('Ctrl-C caught; exiting...')
except Exception as e:
_log.error('error: {}'.format(e))
finally:
close_motors(_log)
_elapsed_ms = round((dt.now() - _start_time).total_seconds() * 1000.0)
_log.info(Fore.YELLOW + 'complete: elapsed: {:d}ms'.format(_elapsed_ms))
def __init__(self, level):
super().__init__()
_loader = ConfigLoader(Level.INFO)
filename = 'config.yaml'
_config = _loader.configure(filename)
self._log = Logger("gamepad-demo", level)
self._log.heading('gamepad-demo', 'Configuring Gamepad...', None)
self._config = _config['ros'].get('gamepad_demo')
self._enable_ifs = self._config.get('enable_ifs')
self._enable_compass = self._config.get('enable_compass')
self._enable_indicator = self._config.get('enable_indicator')
self._message_factory = MessageFactory(level)
self._motors = Motors(_config, None, Level.INFO)
# self._motor_controller = SimpleMotorController(self._motors, Level.INFO)
self._pid_motor_ctrl = PIDMotorController(_config, self._motors,
Level.INFO)
# i2c scanner, let's us know if certain devices are available
_i2c_scanner = I2CScanner(Level.WARN)
_addresses = _i2c_scanner.get_int_addresses()
ltr559_available = (0x23 in _addresses)
'''
Availability of displays:
The 5x5 RGB Matrix is at 0x74 for port, 0x77 for starboard.
The 11x7 LED matrix is at 0x75 for starboard, 0x77 for port. The latter
conflicts with the RGB LED matrix, so both cannot be used simultaneously.
We check for either the 0x74 address to see if RGB Matrix displays are
used, OR for 0x75 to assume a pair of 11x7 Matrix displays are being used.
'''
# rgbmatrix5x5_stbd_available = ( 0x74 in _addresses ) # not used yet
# matrix11x7_stbd_available = ( 0x75 in _addresses ) # used as camera lighting
matrix11x7_stbd_available = False
# self._blob = BlobSensor(_config, self._motors, Level.INFO)
self._blob = None
self._lux = Lux(Level.INFO) if ltr559_available else None
self._video = None
# self._video = Video(_config, self._lux, matrix11x7_stbd_available, Level.INFO)
self._message_bus = MessageBus(Level.INFO)
# in this application the gamepad controller is the message queue
# self._queue = MessageQueue(self._message_factory, Level.INFO)
self._clock = Clock(_config, self._message_bus, self._message_factory,
Level.INFO)
# attempt to find the gamepad
self._gamepad = Gamepad(_config, self._message_bus,
self._message_factory, Level.INFO)
# if self._enable_indicator:
# self._indicator = Indicator(Level.INFO)
# if self._enable_compass:
# self._compass = Compass(_config, self._queue, self._indicator, Level.INFO)
# self._video.set_compass(self._compass)
_enable_battery_check = False
if _enable_battery_check:
self._log.info('starting battery check thread...')
self._battery_check = BatteryCheck(_config, self._queue,
self._message_factory,
Level.INFO)
else:
self._battery_check = None
if self._enable_ifs:
self._log.info('integrated front sensor enabled.')
self._ifs = IntegratedFrontSensor(_config, self._clock,
self._message_bus,
self._message_factory,
Level.INFO)
# add indicator as message consumer
if self._enable_indicator:
self._queue.add_consumer(self._indicator)
else:
self._ifs = None
self._log.info('integrated front sensor disabled.')
# self._ctrl = GamepadController(_config, self._queue, self._pid_motor_ctrl, self._ifs, self._video, self._blob, matrix11x7_stbd_available, Level.INFO, self._close_demo_callback)
self._ctrl = GamepadController(_config, self._message_bus,
self._pid_motor_ctrl, self._ifs,
self._video, self._blob,
matrix11x7_stbd_available, Level.INFO,
self._close_demo_callback)
self._message_bus.add_handler(Message, self._ctrl.handle_message)
self._enabled = False
self._log.info('connecting gamepad...')
self._gamepad.connect()
self._log.info('ready.')