def test_message():
_log = Logger('message-test', Level.INFO)
try:
_log.info('start message test...')
_message_bus = MessageBus(Level.INFO)
_message_factory = MessageFactory(_message_bus, Level.INFO)
_subscriber1 = Subscriber('behaviour', Fore.YELLOW, _message_bus,
Level.INFO)
_message_bus.register_subscriber(_subscriber1)
_subscriber2 = Subscriber('infrared', Fore.MAGENTA, _message_bus,
Level.INFO)
_message_bus.register_subscriber(_subscriber2)
_subscriber3 = Subscriber('bumper', Fore.GREEN, _message_bus,
Level.INFO)
_message_bus.register_subscriber(_subscriber3)
_message_bus.print_publishers()
_message_bus.print_subscribers()
# ................................
_message = _message_factory.get_message(Event.BRAKE, True)
_log.info('created message {}; event: {}'.format(
_message.name, _message.event.name))
_message.acknowledge(_subscriber1)
_subscriber1.print_message_info('sub1 info for message:', _message,
None)
except Exception as e:
_log.error('error: {}'.format(e))
finally:
_log.info('complete.')
def test_ifs():
'''
Test the functionality of Integrated Front Sensor.
'''
_log = Logger("test-ifs", Level.INFO)
# read YAML configuration
_loader = ConfigLoader(Level.INFO)
filename = 'config.yaml'
_config = _loader.configure(filename)
_message_factory = MessageFactory(Level.INFO)
_message_bus = MessageBus(Level.INFO)
_log.info('creating clock...')
_clock = Clock(_config, _message_bus, _message_factory, Level.WARN)
_ifs = IntegratedFrontSensor(_config, _clock, Level.INFO)
# establish queue to receive messages
_queue = MockMessageQueue(Level.INFO)
_message_bus.add_handler(Message, _queue.add)
_ifs.enable()
_clock.enable()
while not _queue.all_triggered:
_queue.waiting_for_message()
time.sleep(0.5)
_ifs.disable()
assert _queue.count > 0
_log.info('test complete.' + Style.RESET_ALL)
def main(argv):
try:
_log = Logger('brake', Level.INFO)
_log.info('configuring...')
# read YAML configuration
_loader = ConfigLoader(Level.WARN)
_config = _loader.configure()
_log.info('creating message factory...')
_message_factory = MessageFactory(Level.INFO)
_log.info('creating message bus...')
_message_bus = MessageBus(Level.INFO)
_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()
_log.info('braking...')
_motors.brake()
_log.info('done.')
except KeyboardInterrupt:
_log.error('Ctrl-C caught in main: exiting...')
finally:
if _motors:
_motors.stop()
_motors.close()
_log.info('complete.')
def main():
try:
# read YAML configuration
_loader = ConfigLoader(Level.INFO)
filename = 'config.yaml'
_config = _loader.configure(filename)
_message_factory = MessageFactory(Level.INFO)
_queue = MessageQueue(_message_factory, Level.INFO)
_rate = Rate(5) # 20Hz
_nxp9dof = NXP9DoF(_config, _queue, Level.INFO)
_nxp = NXP(_nxp9dof, Level.INFO)
# _nxp.enable()
# _nxp9dof.enable()
_nxp.ahrs2(True)
while True:
# _nxp.heading(20)
# _nxp.ahrs(10)
# _nxp.imu(10)
_nxp.ahrs2(False)
_rate.wait()
except KeyboardInterrupt:
print(Fore.RED + 'Ctrl-C caught; exiting...' + Style.RESET_ALL)
def main():
try:
_log = Logger("test", Level.INFO)
_log.info(Fore.BLUE + 'configuring...')
_loader = ConfigLoader(Level.INFO)
filename = 'config.yaml'
_config = _loader.configure(filename)
_message_factory = MessageFactory(Level.INFO)
_message_bus = MessageBus(Level.INFO)
_clock = Clock(_config, _message_bus, _message_factory, Level.WARN)
_clock.enable()
_ifs = IntegratedFrontSensor(_config, _clock, Level.INFO)
_ifs.enable()
_handler = MessageHandler(Level.INFO)
_message_bus.add_handler(Message, _handler.handle)
try:
_log.info(Fore.BLUE + 'starting loop... type Ctrl-C to exit.')
_rate = Rate(1)
# while True:
for i in range(360):
_rate.wait()
_log.info(Fore.BLUE + 'exited loop.')
except KeyboardInterrupt:
print(Fore.RED + 'Ctrl-C caught; exiting...' + Style.RESET_ALL)
except Exception as e:
print(Fore.RED + Style.BRIGHT + 'error: {}'.format(e))
traceback.print_exc(file=sys.stdout)
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 __init__(self, message_bus, exit_on_complete=True, level=Level.INFO):
super().__init__()
self._log = Logger("mock-ifs", level)
self._message_factory = MessageFactory(Level.INFO)
self._message_bus = message_bus
# self._message_bus = MockMessageBus(self, Level.INFO)
self.exit_on_complete = exit_on_complete
# self._rate = Rate(10)
self._thread = None
self._enabled = False
self._verbose = True
self._suppress = False
self._closed = False
self._counter = itertools.count()
# .....
self._triggered_ir_port_side = self._triggered_ir_port = self._triggered_ir_cntr = self._triggered_ir_stbd = \
self._triggered_ir_stbd_side = self._triggered_bmp_port = self._triggered_bmp_cntr = self._triggered_bmp_stbd = 0
self._limit = 3
self._fmt = '{0:>9}'
self._log.info('ready.')
def test_clock():
global tock_count
_log = Logger('clock-test', Level.INFO)
_loader = ConfigLoader(Level.INFO)
filename = 'config.yaml'
_config = _loader.configure(filename)
tock_count = 0
_message_factory = MessageFactory(Level.INFO)
_message_bus = MockMessageBus(Level.INFO)
_clock = Clock(_config, _message_bus, _message_factory, Level.INFO)
_message_bus.set_clock(_clock)
_clock.enable()
_log.info('ready; begin test.')
_loops = 3
while INFINITE or tock_count < _loops:
time.sleep(1.0)
_log.info('test complete.')
def main(argv):
_log = Logger("main", Level.INFO)
try:
_log.info(Fore.BLUE + 'configuring objects...')
_loop_freq_hz = 10
_ticker = Ticker(_loop_freq_hz, Level.INFO)
_message_factory = MessageFactory(Level.INFO)
_message_bus = MessageBus()
# _publisher = Publisher(_message_bus)
_publisher = MyPublisher(_message_factory, _message_bus)
# _publisher.enable()
_publish = _publisher.publish(10)
_log.info(Fore.BLUE + 'generating subscribers...')
_subscribers = []
_subscriptions = []
for x in range(10):
_subscriber = MySubscriber('s{}'.format(x), _ticker, _message_bus)
_subscribers.append(_subscriber)
_subscriptions.append(_subscriber.subscribe())
_ticker.enable()
loop = asyncio.get_event_loop()
_log.info(Fore.BLUE + 'starting loop...')
loop.run_until_complete(asyncio.gather(_publish, *_subscriptions))
_log.info(Fore.BLUE + 'closing {} subscribers...'.format(len(_subscribers)))
for subscriber in _subscribers:
_log.info(Fore.BLUE + 'subscriber {} has {:d} messages remaining in queue: {}'.format(subscriber.name, subscriber.queue_length(), _subscriber.print_queue_contents()))
_log.info(Fore.BLUE + 'loop complete.')
except KeyboardInterrupt:
_log.info('caught Ctrl-C; exiting...')
except Exception:
_log.error('error processing message bus: {}'.format(traceback.format_exc()))
finally:
_log.info('exit.')
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 main():
signal.signal(signal.SIGINT, signal_handler)
_log = Logger("orient_test", Level.INFO)
_log.info('start...')
_message_factory = MessageFactory(Level.INFO)
_queue = MessageQueue(_message_factory, Level.INFO)
try:
i2c = busio.I2C(board.SCL, board.SDA)
lsm9ds1 = adafruit_lsm9ds1.LSM9DS1_I2C(i2c)
success = 0
_log.info(Fore.RED + Style.BRIGHT +
'to calibrate, wave robot in air until it beeps...')
while True:
# read magnetometer...
mag_x, mag_y, mag_z = lsm9ds1.magnetic
accel_x, accel_y, accel_z = lsm9ds1.acceleration
degrees = convert_to_degrees(mag_x, mag_y, mag_z)
direction = Cardinal.get_heading_from_degrees(degrees)
lsm9ds1_heading, filtered_lsm9ds1_heading, roll, pitch, yaw = convert_to_degrees_with_accelerometer(
mag_x, mag_y, mag_z, accel_x, accel_y, accel_z)
_log.info(Fore.BLACK + 'LSM9DS1 heading: {:0.3f}°;\tdirection: {} (no accel);'.format(degrees, direction) + Fore.BLACK + Style.DIM \
+ ' magnetometer (gauss):\t({0:0.3f}, {1:0.3f}, {2:0.3f})'.format(mag_x, mag_y, mag_z))
_log.info(Fore.BLUE + 'LSM9DS1 heading: {:>5.2f}°;\traw heading: {:>5.2f}°; (with accel)'.format(filtered_lsm9ds1_heading, lsm9ds1_heading) + Fore.BLACK + Style.DIM \
+ '\tpitch: {:>5.2f}; roll: {:>5.2f}; yaw: {:>5.2f}.'.format(pitch, roll, yaw) + Style.RESET_ALL)
print(Fore.BLACK + '.' + Style.RESET_ALL)
time.sleep(0.5)
# .......................................
except KeyboardInterrupt:
_log.info(Fore.RED + Style.BRIGHT +
'Ctrl-C caught: keyboard interrupt.')
_log.info('done.')
sys.exit(0)
def main():
_log = Logger("mot-det test", Level.INFO)
_log.info('starting test...')
_log.info(Fore.YELLOW + Style.BRIGHT + ' INFO : Press Ctrl+C to exit.')
_motion_detector = None
try:
_loader = ConfigLoader(Level.INFO)
filename = 'config.yaml'
_config = _loader.configure(filename)
_message_factory = MessageFactory(Level.INFO)
_message_bus = MockMessageBus(Level.INFO)
_motion_detector = MotionDetector(_config, _message_factory,
_message_bus, Level.INFO)
_log.info('starting motion detector...')
_motion_detector.enable()
_counter = itertools.count()
while not _message_bus.is_complete():
if next(_counter) % 5 == 0:
_log.info('waiting for motion... ')
time.sleep(1.0)
time.sleep(5.0)
_motion_detector.close()
_log.info('test complete.')
except KeyboardInterrupt:
if _motion_detector:
_motion_detector.close()
except Exception:
_log.info(traceback.format_exc())
finally:
if _motion_detector:
_motion_detector.close()
def test_battery_check():
_log = Logger("batcheck test", Level.INFO)
_log.heading('battery check', 'Starting test...', '[1/3]')
_log.info(Fore.RED + 'Press Ctrl+C to exit.')
# read YAML configuration
_loader = ConfigLoader(Level.INFO)
filename = 'config.yaml'
_config = _loader.configure(filename)
_log.heading('battery check', 'Creating objects...', '[2/3]')
_log.info('creating message factory...')
_message_factory = MessageFactory(Level.INFO)
_log.info('creating message queue...')
_queue = MessageQueue(_message_factory, Level.INFO)
# _consumer = MockConsumer()
# _queue.add_consumer(_consumer)
_log.info('creating message bus...')
_message_bus = MessageBus(Level.INFO)
_log.info('creating clock...')
_clock = Clock(_config, _message_bus, _message_factory, Level.INFO)
_log.info('creating battery check...')
_battery_check = BatteryCheck(_config, _clock, _queue, _message_factory,
Level.INFO)
_battery_check.enable()
# _battery_check.set_enable_messaging(True)
_log.heading('battery check', 'Enabling battery check...', '[3/3]')
_clock.enable()
while _battery_check.count < 40:
time.sleep(0.5)
_battery_check.close()
# _log.info('consumer received {:d} messages.'.format(_consumer.count))
_log.info('complete.')
def main(argv):
try:
# read YAML configuration
_loader = ConfigLoader(Level.INFO)
filename = 'config.yaml'
_config = _loader.configure(filename)
_message_factory = MessageFactory(Level.INFO)
_queue = MessageQueue(_message_factory, Level.INFO)
_msg_handler = MessageHandler(Level.INFO)
# _tick_handler = TickHandler(_ifs, Level.INFO)
# create message bus...
_message_bus = MessageBus(Level.INFO)
_message_bus.add_handler(Message, _msg_handler.add)
# _message_bus.add_handler(Tick, _tick_handler.tick)
# _message_bus.add_handler(Tock, _tick_handler.tock)
# _message_bus.add_handler(Tick, _tick_handler.add)
# _message_bus.add_consumer(_ifs)
_clock = Clock(_config, _message_bus, _message_factory, Level.INFO)
_ifs = IntegratedFrontSensor(_config, _queue, _clock, _message_bus,
_message_factory, Level.INFO)
_clock.enable()
_ifs.enable()
while True:
time.sleep(1.0)
except KeyboardInterrupt:
print(Fore.CYAN + Style.BRIGHT + 'caught Ctrl-C; exiting...')
except Exception:
print(Fore.RED + Style.BRIGHT +
'error starting ros: {}'.format(traceback.format_exc()) +
Style.RESET_ALL)
class MockMessageQueue(object):
def __init__(self, level=Level.INFO):
self._log = Logger('flask.wrapper', level)
self._message_factory = MessageFactory(None, level)
self._log.info('ready.')
# ......................................................
def respond(self, event):
'''
Responds to the Event by wrapping it in Message and adding it to the backing queue.
This is only used by FlaskWrapper.
'''
self._log.info('RESPOND to event {}.'.format(event.name))
_message = self._message_factory.get_message(event, None)
self.add(_message)
return jsonify([{
'id': _message.message_id
}, {
'event': event.name
}, {
'priority': event.priority
}])
# ..........................................................................
def add(self, message):
'''
Add a new Message to the queue, then additionally to any consumers.
'''
# if self._queue.full():
# try:
# _dumped = self._queue.get()
# self._log.debug('dumping old message eid#{}/msg#{}: {}'.format(_dumped.eid, _dumped.number, _dumped.description))
# except Empty:
# pass
# self._queue.put(message);
self._log.info('added message id {} with event: {}'.format(
message.message_id, message.event.description))
def test_bno08x():
_log = Logger('fusion', Level.INFO)
_loader = ConfigLoader(Level.INFO)
filename = 'config.yaml'
_config = _loader.configure(filename)
_message_factory = MessageFactory(Level.INFO)
_queue = MessageQueue(_message_factory, Level.INFO)
_bno08x = BNO08x(_config, _queue, Level.INFO)
_bno055 = BNO055(_config, _queue, Level.INFO)
while True:
_bno08x_heading = _bno08x.read()
_mag_degrees = _bno08x_heading[
0] if _bno08x_heading is not None else 0.0
_bno055_result = _bno055.read()
_calibration = _bno055_result[0]
_bno055_heading = _bno055_result[1]
if _calibration.calibrated:
_mag_diff = _bno055_heading - _mag_degrees
_log.info(Fore.BLUE + Style.BRIGHT +
'heading: {:>6.2f}°\t(bno085)\t'.format(_mag_degrees) +
Style.DIM + 'diff: {:>5.2f}°;\ttrim: {:>5.2f}°'.format(
_mag_diff, _bno08x.heading_trim))
_log.info(Fore.CYAN + Style.BRIGHT +
'heading: {:>6.2f}°\t(bno055)'.format(_bno055_heading))
_cardinal = Cardinal.get_heading_from_degrees(_bno055_heading)
_log.info(Fore.WHITE + Style.BRIGHT +
'cardinal: \t{}\n'.format(_cardinal.display.lower()))
# _log.info('')
# _log.info(Fore.BLACK + '.' + Style.RESET_ALL)
time.sleep(2.0)
def main():
_log = Logger("main", Level.INFO)
_log.info(Fore.BLUE + 'configuring pub-sub test...')
# read YAML configuration
_loader = ConfigLoader(Level.INFO)
filename = 'config.yaml'
_config = _loader.configure(filename)
_loop_freq_hz = 10
_ticker = Ticker(_loop_freq_hz, Level.INFO)
_message_bus = MessageBus(Level.INFO)
_message_factory = MessageFactory(_message_bus, Level.INFO)
_publisher1 = IfsPublisher('A', _message_bus, _message_factory)
# _publisher1 = Publisher('A', _message_bus, _message_factory)
_message_bus.register_publisher(_publisher1)
# _publisher2 = Publisher('B', _message_bus, _message_factory, Level.INFO)
# _message_bus.register_publisher(_publisher2)
# ROAM is commonly accepted by all subscribers
# _subscriber1 = Subscriber('behaviour', Fore.YELLOW, _message_bus, Level.INFO)
# _subscriber1.events = [ Event.ROAM, Event.SNIFF ] # reacts to ROAM and SNIFF
# _message_bus.register_subscriber(_subscriber1)
_subscriber2 = Subscriber('infrared', Fore.MAGENTA, _message_bus,
Level.INFO)
_subscriber2.events = [
Event.INFRARED_PORT, Event.INFRARED_CNTR, Event.INFRARED_STBD
] # reacts to IR
_subscriber2.add_event(Event.ROAM)
_message_bus.register_subscriber(_subscriber2)
# _subscriber3 = Subscriber('bumper', Fore.GREEN, _message_bus, Level.INFO)
# _subscriber3.events = [ Event.SNIFF, Event.BUMPER_PORT, Event.BUMPER_CNTR, Event.BUMPER_STBD ] # reacts to bumpers
# _subscriber3.add_event(Event.ROAM)
# _message_bus.register_subscriber(_subscriber3)
_motors = None
# add motor controller, reacts to STOP, HALT, BRAKE, INCREASE_SPEED and DECREASE_SPEED
# _motor_configurer = MotorConfigurer(_config, _ticker, _message_bus, enable_mock=True, level=Level.INFO)
# _motors = _motor_configurer.get_motors()
# _motors.add_event(Event.ROAM)
# _message_bus.register_subscriber(_motors)
_message_bus.print_publishers()
_message_bus.print_subscribers()
# sys.exit(0)
try:
if _motors:
_motors.enable()
_message_bus.enable()
except KeyboardInterrupt:
_log.info('publish-subscribe interrupted')
except Exception as e:
_log.error('error in pub-sub: {} / {}'.format(e,
traceback.print_stack()))
finally:
_message_bus.close()
_log.info('successfully shutdown the message bus service.')
def __init__(self, config, queue, clock, message_bus, message_factory,
level):
if config is None:
raise ValueError('no configuration provided.')
self._log = Logger("ifs", level)
self._log.info('configuring integrated front sensor...')
_cruise_config = config['ros'].get('cruise_behaviour')
self._cruising_velocity = _cruise_config.get('cruising_velocity')
self._config = config['ros'].get('integrated_front_sensor')
self._clock = clock
self._clock.add_consumer(self)
self._message_bus = message_bus
# _queue = queue
# _queue.add_consumer(self)
self._device_id = self._config.get(
'device_id'
) # i2c hex address of slave device, must match Arduino's SLAVE_I2C_ADDRESS
self._channel = self._config.get('channel')
self._ignore_duplicates = self._config.get('ignore_duplicates')
self._tick_modulo = self._config.get('tick_modulo')
_max_workers = self._config.get('max_workers')
self._log.info('tick modulo: {:d}'.format(self._tick_modulo))
# event thresholds:
self._callback_cntr_min_trigger = self._config.get(
'callback_center_minimum_trigger')
self._callback_side_min_trigger = self._config.get(
'callback_side_minimum_trigger')
self._callback_min_trigger = self._config.get(
'callback_minimum_trigger')
self._port_side_trigger_distance = self._config.get(
'port_side_trigger_distance')
self._port_trigger_distance = self._config.get('port_trigger_distance')
self._center_trigger_distance = self._config.get(
'center_trigger_distance')
self._stbd_trigger_distance = self._config.get('stbd_trigger_distance')
self._stbd_side_trigger_distance = self._config.get(
'stbd_side_trigger_distance')
self._log.info('event thresholds: \t' \
+Fore.RED + ' port side={:>5.2f}; port={:>5.2f};'.format(self._port_side_trigger_distance, self._port_trigger_distance) \
+Fore.BLUE + ' center={:>5.2f};'.format(self._center_trigger_distance) \
+Fore.GREEN + ' stbd={:>5.2f}; stbd side={:>5.2f}'.format(self._stbd_trigger_distance, self._stbd_side_trigger_distance))
# hardware pin assignments
self._port_side_ir_pin = self._config.get('port_side_ir_pin')
self._port_ir_pin = self._config.get('port_ir_pin')
self._center_ir_pin = self._config.get('center_ir_pin')
self._stbd_ir_pin = self._config.get('stbd_ir_pin')
self._stbd_side_ir_pin = self._config.get('stbd_side_ir_pin')
self._log.info('infrared pin assignments:\t' \
+Fore.RED + ' port side={:d}; port={:d};'.format(self._port_side_ir_pin, self._port_ir_pin) \
+Fore.BLUE + ' center={:d};'.format(self._center_ir_pin) \
+Fore.GREEN + ' stbd={:d}; stbd side={:d}'.format(self._stbd_ir_pin, self._stbd_side_ir_pin))
self._port_bmp_pin = self._config.get('port_bmp_pin')
self._center_bmp_pin = self._config.get('center_bmp_pin')
self._stbd_bmp_pin = self._config.get('stbd_bmp_pin')
self._log.info('bumper pin assignments:\t' \
+Fore.RED + ' port={:d};'.format(self._port_bmp_pin) \
+Fore.BLUE + ' center={:d};'.format(self._center_bmp_pin) \
+Fore.GREEN + ' stbd={:d}'.format(self._stbd_bmp_pin))
if message_factory:
self._message_factory = message_factory
else:
self._message_factory = MessageFactory(level)
# self._executor = ProcessPoolExecutor(max_workers=_max_workers)
self._log.info(
'creating thread pool executor with maximum of {:d} workers.'.
format(_max_workers))
self._executor = ThreadPoolExecutor(max_workers=_max_workers,
thread_name_prefix='ifs')
# config IO Expander
self._ioe = IoExpander(config, Level.INFO)
# calculating means for IR sensors
self._pot = Potentiometer(config, Level.INFO)
_queue_limit = 2 # larger number means it takes longer to change
self._deque_port_side = Deque([], maxlen=_queue_limit)
self._deque_port = Deque([], maxlen=_queue_limit)
self._deque_cntr = Deque([], maxlen=_queue_limit)
self._deque_stbd = Deque([], maxlen=_queue_limit)
self._deque_stbd_side = Deque([], maxlen=_queue_limit)
# ...
self._last_event = None
self._last_value = None
self._enabled = False
self._suppressed = False
self._closed = False
self._log.info(Fore.MAGENTA + 'ready.')
class IntegratedFrontSensor():
'''
IntegratedFrontSensor: communicates with the integrated front bumpers and
infrared sensors, receiving messages from the IO Expander board or I²C
Arduino slave, sending the messages with its events onto the message bus.
This listens to the Clock and at a frequency of ( TICK % tick_modulo )
calls the _poll() method.
Parameters:
:param config: the YAML based application configuration
:param queue: the message queue receiving activation notifications
:param clock: the clock providing the polling loop trigger
:param message_bus: the message bus to send event messages
:param message_factory: optional MessageFactory
:param level: the logging Level
'''
# ..........................................................................
def __init__(self, config, queue, clock, message_bus, message_factory,
level):
if config is None:
raise ValueError('no configuration provided.')
self._log = Logger("ifs", level)
self._log.info('configuring integrated front sensor...')
_cruise_config = config['ros'].get('cruise_behaviour')
self._cruising_velocity = _cruise_config.get('cruising_velocity')
self._config = config['ros'].get('integrated_front_sensor')
self._clock = clock
self._clock.add_consumer(self)
self._message_bus = message_bus
# _queue = queue
# _queue.add_consumer(self)
self._device_id = self._config.get(
'device_id'
) # i2c hex address of slave device, must match Arduino's SLAVE_I2C_ADDRESS
self._channel = self._config.get('channel')
self._ignore_duplicates = self._config.get('ignore_duplicates')
self._tick_modulo = self._config.get('tick_modulo')
_max_workers = self._config.get('max_workers')
self._log.info('tick modulo: {:d}'.format(self._tick_modulo))
# event thresholds:
self._callback_cntr_min_trigger = self._config.get(
'callback_center_minimum_trigger')
self._callback_side_min_trigger = self._config.get(
'callback_side_minimum_trigger')
self._callback_min_trigger = self._config.get(
'callback_minimum_trigger')
self._port_side_trigger_distance = self._config.get(
'port_side_trigger_distance')
self._port_trigger_distance = self._config.get('port_trigger_distance')
self._center_trigger_distance = self._config.get(
'center_trigger_distance')
self._stbd_trigger_distance = self._config.get('stbd_trigger_distance')
self._stbd_side_trigger_distance = self._config.get(
'stbd_side_trigger_distance')
self._log.info('event thresholds: \t' \
+Fore.RED + ' port side={:>5.2f}; port={:>5.2f};'.format(self._port_side_trigger_distance, self._port_trigger_distance) \
+Fore.BLUE + ' center={:>5.2f};'.format(self._center_trigger_distance) \
+Fore.GREEN + ' stbd={:>5.2f}; stbd side={:>5.2f}'.format(self._stbd_trigger_distance, self._stbd_side_trigger_distance))
# hardware pin assignments
self._port_side_ir_pin = self._config.get('port_side_ir_pin')
self._port_ir_pin = self._config.get('port_ir_pin')
self._center_ir_pin = self._config.get('center_ir_pin')
self._stbd_ir_pin = self._config.get('stbd_ir_pin')
self._stbd_side_ir_pin = self._config.get('stbd_side_ir_pin')
self._log.info('infrared pin assignments:\t' \
+Fore.RED + ' port side={:d}; port={:d};'.format(self._port_side_ir_pin, self._port_ir_pin) \
+Fore.BLUE + ' center={:d};'.format(self._center_ir_pin) \
+Fore.GREEN + ' stbd={:d}; stbd side={:d}'.format(self._stbd_ir_pin, self._stbd_side_ir_pin))
self._port_bmp_pin = self._config.get('port_bmp_pin')
self._center_bmp_pin = self._config.get('center_bmp_pin')
self._stbd_bmp_pin = self._config.get('stbd_bmp_pin')
self._log.info('bumper pin assignments:\t' \
+Fore.RED + ' port={:d};'.format(self._port_bmp_pin) \
+Fore.BLUE + ' center={:d};'.format(self._center_bmp_pin) \
+Fore.GREEN + ' stbd={:d}'.format(self._stbd_bmp_pin))
if message_factory:
self._message_factory = message_factory
else:
self._message_factory = MessageFactory(level)
# self._executor = ProcessPoolExecutor(max_workers=_max_workers)
self._log.info(
'creating thread pool executor with maximum of {:d} workers.'.
format(_max_workers))
self._executor = ThreadPoolExecutor(max_workers=_max_workers,
thread_name_prefix='ifs')
# config IO Expander
self._ioe = IoExpander(config, Level.INFO)
# calculating means for IR sensors
self._pot = Potentiometer(config, Level.INFO)
_queue_limit = 2 # larger number means it takes longer to change
self._deque_port_side = Deque([], maxlen=_queue_limit)
self._deque_port = Deque([], maxlen=_queue_limit)
self._deque_cntr = Deque([], maxlen=_queue_limit)
self._deque_stbd = Deque([], maxlen=_queue_limit)
self._deque_stbd_side = Deque([], maxlen=_queue_limit)
# ...
self._last_event = None
self._last_value = None
self._enabled = False
self._suppressed = False
self._closed = False
self._log.info(Fore.MAGENTA + 'ready.')
# ......................................................
def add(self, message):
'''
Reacts to every nth (modulo) TICK message, submitting a _poll Thread
from the thread pool executor, which polls the various sensors and
sending callbacks for each.
Note that if the loop frequency is set this method is disabled.
'''
if self._enabled and message.event is Event.CLOCK_TICK:
if (message.value % self._tick_modulo) == 0:
_future = self._executor.submit(self._poll, message.value,
lambda: self.enabled)
# ......................................................
def _poll(self, count, f_is_enabled):
'''
Poll the various infrared and bumper sensors, executing callbacks for each.
In tests this typically takes 173ms from ItsyBitsy, 85ms from the Pimoroni IO Expander.
We add a messsage for the bumpers immediately (rather than use a callback) after reading
the sensors since their response must be as fast as possible.
'''
if not f_is_enabled():
self._log.warning('[{:04d}] poll not enabled'.format(count))
return
self._log.info(Fore.BLACK + '[{:04d}] ifs poll start.'.format(count))
_current_thread = threading.current_thread()
_current_thread.name = 'poll'
_start_time = dt.datetime.now()
# pin 10: digital bumper sensor ........................................
_port_bmp_data = self.get_input_for_event_type(Event.BUMPER_PORT)
_cntr_bmp_data = self.get_input_for_event_type(Event.BUMPER_CNTR)
_stbd_bmp_data = self.get_input_for_event_type(Event.BUMPER_STBD)
_port_side_ir_data = self.get_input_for_event_type(
Event.INFRARED_PORT_SIDE)
_port_ir_data = self.get_input_for_event_type(Event.INFRARED_PORT)
_cntr_ir_data = self.get_input_for_event_type(Event.INFRARED_CNTR)
_stbd_ir_data = self.get_input_for_event_type(Event.INFRARED_STBD)
_stbd_side_ir_data = self.get_input_for_event_type(
Event.INFRARED_STBD_SIDE)
# self._callback(Event.BUMPER_CNTR, _cntr_bmp_data)
if _cntr_bmp_data == 1:
_message = self._message_factory.get_message(
Event.BUMPER_CNTR, _cntr_bmp_data)
self._log.info(Fore.BLUE + Style.BRIGHT +
'adding new message eid#{} for BUMPER_CNTR event.'.
format(_message.eid))
self._message_bus.handle(_message)
# pin 9: digital bumper sensor .........................................
# self._callback(Event.BUMPER_PORT, _port_bmp_data)
if _port_bmp_data == 1:
_message = self._message_factory.get_message(
Event.BUMPER_PORT, _port_bmp_data)
self._log.info(Fore.BLUE + Style.BRIGHT +
'adding new message eid#{} for BUMPER_PORT event.'.
format(_message.eid))
self._message_bus.handle(_message)
# pin 11: digital bumper sensor ........................................
# self._callback(Event.BUMPER_STBD, _stbd_bmp_data)
if _stbd_bmp_data == 1:
_message = self._message_factory.get_message(
Event.BUMPER_STBD, _stbd_bmp_data)
self._log.info(Fore.BLUE + Style.BRIGHT +
'adding new message eid#{} for BUMPER_STBD event.'.
format(_message.eid))
self._message_bus.handle(_message)
# port side analog infrared sensor .....................................
if _port_side_ir_data > self._callback_side_min_trigger:
# _message = self._message_factory.get_message(Event.INFRARED_PORT_SIDE, _port_side_ir_data)
# self._log.info(Fore.RED + Style.BRIGHT + 'adding new message eid#{} for INFRARED_PORT_SIDE event.'.format(_message.eid))
# self._message_bus.handle(_message)
self._log.info(Fore.RED + '[{:04d}] ANALOG IR ({:d}): \t'.format(count, 1) + (Fore.RED if (_port_side_ir_data > 100.0) else Fore.YELLOW) \
+ Style.BRIGHT + '{:d}'.format(_port_side_ir_data) + Style.DIM + '\t(analog value 0-255)')
self._callback(Event.INFRARED_PORT_SIDE, _port_side_ir_data)
# port analog infrared sensor ..........................................
if _port_ir_data > self._callback_min_trigger:
# _message = self._message_factory.get_message(Event.INFRARED_PORT, _port_ir_data)
# self._log.info(Fore.RED + Style.BRIGHT + 'adding new message eid#{} for INFRARED_PORT event.'.format(_message.eid))
# self._message_bus.handle(_message)
self._log.info('[{:04d}] ANALOG IR ({:d}): \t'.format(count, 2) + (Fore.RED if (_port_ir_data > 100.0) else Fore.YELLOW) \
+ Style.BRIGHT + '{:d}'.format(_port_ir_data) + Style.DIM + '\t(analog value 0-255)')
self._callback(Event.INFRARED_PORT, _port_ir_data)
# center analog infrared sensor ........................................
if _cntr_ir_data > self._callback_cntr_min_trigger:
# _message = self._message_factory.get_message(Event.INFRARED_CNTR, _cntr_ir_data)
# self._log.info(Fore.BLUE + Style.BRIGHT + 'adding new message eid#{} for INFRARED_CNTR event.'.format(_message.eid))
# self._message_bus.handle(_message)
self._log.info(Fore.BLUE + '[{:04d}] ANALOG IR ({:d}): \t'.format(count, 3) + (Fore.RED if (_cntr_ir_data > 100.0) else Fore.YELLOW) \
+ Style.BRIGHT + '{:d}'.format(_cntr_ir_data) + Style.DIM + '\t(analog value 0-255)')
self._callback(Event.INFRARED_CNTR, _cntr_ir_data)
# starboard analog infrared sensor .....................................
if _stbd_ir_data > self._callback_min_trigger:
# _message = self._message_factory.get_message(Event.INFRARED_STBD, _stbd_ir_data)
# self._log.info(Fore.GREEN + Style.BRIGHT + 'adding new message eid#{} for INFRARED_STBD event.'.format(_message.eid))
# self._message_bus.handle(_message)
self._log.info('[{:04d}] ANALOG IR ({:d}): \t'.format(count, 4) + (Fore.RED if (_stbd_ir_data > 100.0) else Fore.YELLOW) \
+ Style.BRIGHT + '{:d}'.format(_stbd_ir_data) + Style.DIM + '\t(analog value 0-255)')
self._callback(Event.INFRARED_STBD, _stbd_ir_data)
# starboard side analog infrared sensor ................................
if _stbd_side_ir_data > self._callback_side_min_trigger:
# _message = self._message_factory.get_message(Event.INFRARED_STBD_SIDE, _stbd_side_ir_data)
# self._log.info(Fore.GREEN + Style.BRIGHT + 'adding new message eid#{} for INFRARED_STBD_SIDE event.'.format(_message.eid))
# self._message_bus.handle(_message)
self._log.info(
'[{:04d}] ANALOG IR ({:d}): \t'.format(count, 5) +
(Fore.RED if (_stbd_side_ir_data > 100.0) else Fore.YELLOW) +
Style.BRIGHT + '{:d}'.format(_stbd_side_ir_data) + Style.DIM +
'\t(analog value 0-255)')
self._callback(Event.INFRARED_STBD_SIDE, _stbd_side_ir_data)
_delta = dt.datetime.now() - _start_time
_elapsed_ms = int(_delta.total_seconds() * 1000)
self._log.info(Fore.BLACK +
'[{:04d}] poll end; elapsed processing time: {:d}ms'.
format(count, _elapsed_ms))
# return True
# ..........................................................................
def _get_mean_distance(self, orientation, value):
'''
Returns the mean of values collected in the queue for the specified IR sensor.
'''
if value == None or value == 0:
return None
if orientation is Orientation.PORT_SIDE:
_deque = self._deque_port_side
elif orientation is Orientation.PORT:
_deque = self._deque_port
elif orientation is Orientation.CNTR:
_deque = self._deque_cntr
elif orientation is Orientation.STBD:
_deque = self._deque_stbd
elif orientation is Orientation.STBD_SIDE:
_deque = self._deque_stbd_side
else:
raise ValueError('unsupported orientation.')
_deque.append(value)
_n = 0
_mean = 0.0
for x in _deque:
_n += 1
_mean += (x - _mean) / _n
if _n < 1:
return float('nan')
else:
return _mean
# ..........................................................................
def _convert_to_distance(self, value):
'''
Converts the value returned by the IR sensor to a distance in centimeters.
Distance Calculation ---------------
This is reading the distance from a 3 volt Sharp GP2Y0A60SZLF infrared
sensor to a piece of white A4 printer paper in a low ambient light room.
The sensor output is not linear, but its accuracy is not critical. If
the target is too close to the sensor the values are not valid. According
to spec 10cm is the minimum distance, but we get relative variability up
until about 5cm. Values over 150 clearly indicate the robot is less than
10cm from the target.
0cm = unreliable
5cm = 226.5
7.5cm = 197.0
10cm = 151.0
20cm = 92.0
30cm = 69.9
40cm = 59.2
50cm = 52.0
60cm = 46.0
70cm = 41.8
80cm = 38.2
90cm = 35.8
100cm = 34.0
110cm = 32.9
120cm = 31.7
130cm = 30.7 *
140cm = 30.7 *
150cm = 29.4 *
* Maximum range on IR is about 130cm, after which there is diminishing
stability/variability, i.e., it's hard to determine if we're dealing
with a level of system noise rather than data. Different runs produce
different results, with values between 28 - 31 on a range of any more
than 130cm.
See: http://ediy.com.my/blog/item/92-sharp-gp2y0a21-ir-distance-sensors
'''
if value == None or value == 0:
return None
self._use_pot = False
# if self._use_pot:
# _pot_value = self._pot.get_scaled_value()
# _EXPONENT = _pot_value
# else:
# _pot_value = 0.0
_EXPONENT = 1.33
_NUMERATOR = 1000.0
_distance = pow(_NUMERATOR / value, _EXPONENT) # 900
# self._log.debug(Fore.BLACK + Style.NORMAL + 'value: {:>5.2f}; pot value: {:>5.2f}; distance: {:>5.2f}cm'.format(value, _pot_value, _distance))
return _distance
# ..........................................................................
def _callback(self, event, value):
'''
This is the callback method from the I²C Master, whose events are
being returned from the source, e.g., Arduino or IO Expander board.
'''
if not self._enabled or self._suppressed:
self._log.info(Fore.BLACK + Style.DIM +
'SUPPRESSED callback: event {}; value: {:d}'.format(
event.name, value))
return
try:
# self._log.debug(Fore.BLACK + 'callback: event {}; value: {:d}'.format(event.name, value))
_event = None
_value = value
# bumpers ..................................................................................
if event == Event.BUMPER_PORT:
if value == 1:
_event = Event.BUMPER_PORT
_value = 1
elif event == Event.BUMPER_CNTR:
if value == 1:
_event = Event.BUMPER_CNTR
_value = 1
elif event == Event.BUMPER_STBD:
if value == 1:
_event = Event.BUMPER_STBD
_value = 1
# ..........................................................................................
# For IR sensors we rewrite the value with a dynamic mean distance (cm),
# setting the event type only if the value is less than a distance threshold.
elif event == Event.INFRARED_PORT_SIDE:
_value = self._get_mean_distance(
Orientation.PORT_SIDE, self._convert_to_distance(value))
self._log.info(
Fore.RED + Style.BRIGHT +
'mean distance: {:5.2f}cm;\tPORT SIDE'.format(_value))
if _value != None and _value < self._port_side_trigger_distance:
_fire_message(event, _value)
# _event = event
# else:
# self._log.info(Fore.RED + 'mean distance: {:5.2f}cm;\tPORT SIDE'.format(_value))
elif event == Event.INFRARED_PORT:
_value = self._get_mean_distance(
Orientation.PORT, self._convert_to_distance(value))
self._log.info(
Fore.RED + Style.BRIGHT +
'mean distance: {:5.2f}cm;\tPORT'.format(_value))
if _value != None and _value < self._port_trigger_distance:
_fire_message(event, _value)
# _event = event
# else:
# self._log.info(Fore.RED + 'mean distance: {:5.2f}cm;\tPORT'.format(_value))
elif event == Event.INFRARED_CNTR:
_value = self._get_mean_distance(
Orientation.CNTR, self._convert_to_distance(value))
self._log.info(
Fore.BLUE + Style.BRIGHT +
'mean distance: {:5.2f}cm;\tCNTR'.format(_value))
if _value != None and _value < self._center_trigger_distance:
_fire_message(event, _value)
# _event = event
# else:
# self._log.info(Fore.BLUE + 'mean distance: {:5.2f}cm;\tCNTR'.format(_value))
elif event == Event.INFRARED_STBD:
_value = self._get_mean_distance(
Orientation.STBD, self._convert_to_distance(value))
self._log.info(
Fore.GREEN + Style.BRIGHT +
'mean distance: {:5.2f}cm;\tSTBD'.format(_value))
if _value != None and _value < self._stbd_trigger_distance:
_fire_message(event, _value)
# _event = event
# else:
# self._log.info(Fore.GREEN + 'mean distance: {:5.2f}cm;\tSTBD'.format(_value))
elif event == Event.INFRARED_STBD_SIDE:
_value = self._get_mean_distance(
Orientation.STBD_SIDE, self._convert_to_distance(value))
self._log.info(
Fore.GREEN + Style.BRIGHT +
'mean distance: {:5.2f}cm;\tSTBD SIDE'.format(_value))
if _value != None and _value < self._stbd_side_trigger_distance:
_fire_message(event, _value)
# _event = event
# else:
# self._log.info(Fore.GREEN + 'mean distance: {:5.2f}cm;\tSTBD SIDE'.format(_value))
# if _event is not None and _value is not None:
# # if not self._ignore_duplicates or ( _event != self._last_event and _value != self._last_value ):
# _message = self._message_factory.get_message(_event, _value)
# self._log.info(Fore.WHITE + Style.BRIGHT + 'adding new message eid#{} for event {}'.format(_message.eid, _event.description))
# self._message_bus.handle(_message)
# # else:
# # self._log.warning(Fore.CYAN + Style.NORMAL + 'ignoring message for event {}'.format(_event.description))
# self._last_event = _event
# self._last_value = _value
# else:
# self._log.info(Fore.WHITE + Style.BRIGHT + 'NOT ADDING message eid#{} for event {}'.format(_message.eid, _event.description))
except Exception as e:
self._log.error('callback error: {}\n{}'.format(
e, traceback.format_exc()))
# ..........................................................................
def _fire_message(self, event, value):
self._log.info(
Fore.YELLOW + Style.BRIGHT +
'fire message with event {} and value {}'.format(event, value))
if event is not None and value is not None:
_message = self._message_factory.get_message(event, value)
self._log.info(Fore.WHITE + Style.BRIGHT +
'adding new message eid#{} for event {}'.format(
_message.eid, _event.description))
self._message_bus.handle(_message)
else:
self._log.info(Fore.RED + Style.BRIGHT +
'ignoring message with event {} and value {}'.
format(event, value))
# ..........................................................................
def get_input_for_event_type(self, event):
'''
Return the current value of the pin corresponding to the Event type.
'''
if event is Event.INFRARED_PORT_SIDE:
return self._ioe.get_port_side_ir_value()
elif event is Event.INFRARED_PORT:
return self._ioe.get_port_ir_value()
elif event is Event.INFRARED_CNTR:
return self._ioe.get_center_ir_value()
elif event is Event.INFRARED_STBD:
return self._ioe.get_stbd_ir_value()
elif event is Event.INFRARED_STBD_SIDE:
return self._ioe.get_stbd_side_ir_value()
elif event is Event.BUMPER_PORT:
return self._ioe.get_port_bmp_value()
elif event is Event.BUMPER_CNTR:
return self._ioe.get_center_bmp_value()
elif event is Event.BUMPER_STBD:
return self._ioe.get_stbd_bmp_value()
else:
raise Exception('unexpected event type.')
# ..........................................................................
def suppress(self, state):
self._log.info('suppress {}.'.format(state))
self._suppressed = state
# ..........................................................................
@property
def enabled(self):
return self._enabled
# ..........................................................................
def enable(self):
if not self._enabled:
if not self._closed:
self._log.info('enabled.')
self._enabled = True
else:
self._log.warning('cannot enable: already closed.')
else:
self._log.warning('already enabled.')
# ..........................................................................
def disable(self):
if self._enabled:
self._enabled = False
self._log.info(Fore.YELLOW +
'shutting down thread pool executor...')
self._executor.shutdown(
wait=False) # python 3.9: , cancel_futures=True)
self._log.info(
Fore.YELLOW +
'disabled: thread pool executor has been shut down.')
else:
self._log.debug('already disabled.')
# ..........................................................................
def close(self):
'''
Permanently close and disable the integrated front sensor.
'''
if not self._closed:
self.disable()
self._closed = True
self._log.info('closed.')
else:
self._log.warning('already closed.')
# ..........................................................................
@staticmethod
def clamp(n, minn, maxn):
return max(min(maxn, n), minn)
# ..........................................................................
@staticmethod
def remap(x, in_min, in_max, out_min, out_max):
return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min
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.')
class MockIntegratedFrontSensor(object):
'''
A mock IFS.
'''
def __init__(self, message_bus, exit_on_complete=True, level=Level.INFO):
super().__init__()
self._log = Logger("mock-ifs", level)
self._message_factory = MessageFactory(Level.INFO)
self._message_bus = message_bus
# self._message_bus = MockMessageBus(self, Level.INFO)
self.exit_on_complete = exit_on_complete
# self._rate = Rate(10)
self._thread = None
self._enabled = False
self._verbose = True
self._suppress = False
self._closed = False
self._counter = itertools.count()
# .....
self._triggered_ir_port_side = self._triggered_ir_port = self._triggered_ir_cntr = self._triggered_ir_stbd = \
self._triggered_ir_stbd_side = self._triggered_bmp_port = self._triggered_bmp_cntr = self._triggered_bmp_stbd = 0
self._limit = 3
self._fmt = '{0:>9}'
self._log.info('ready.')
# ..........................................................................
def name(self):
return 'MockIntegratedFrontSensor'
# ..........................................................................
def suppress(self, mode):
'''
Enable or disable capturing characters. Upon starting the loop the
suppress flag is set False, but can be enabled or disabled as
necessary without halting the thread.
'''
self._suppress = mode
# ..........................................................................
def _start_loop(self, f_is_enabled):
self._log.info('start loop:\t' + Fore.YELLOW + 'type the \"' + Fore.RED + 'Delete' + Fore.YELLOW \
+ '\" or \"' + Fore.RED + 'q' + Fore.YELLOW + '\" key to exit sensor loop, the \"' \
+ Fore.RED + 'h' + Fore.YELLOW + '\" key for help.')
print('\n')
while f_is_enabled():
self.sense()
self._rate.wait()
self._log.info(Fore.YELLOW + 'exit loop.')
# ..........................................................................
def sense(self):
'''
See if any sensor (key) has been activated.
'''
if self._suppress: # then just sleep during the loop
time.sleep(0.2)
else:
_count = next(self._counter)
self._log.info('[{:03d}]'.format(_count))
ch = readchar.readchar()
och = ord(ch)
if och == 91 or och == 113 or och == 127: # 'q' or delete
self.disable()
return
elif och == 104: # 'h' for help
self.print_keymap()
return
elif och == 109: # 'j' to toggle messages sent to IFS
self._verbose = not self._verbose
self._log.info(
Fore.YELLOW +
'setting verbose mode to: {}'.format(self._verbose))
_event = self.get_event_for_char(och)
if _event is not None:
# self._log.info('firing message for event {}'.format(event))
# _message = self._message_factory.get_message(event, True)
# self._message_bus.publish(_message)
# await asyncio.sleep(0.1)
self.fire_message(_event)
self._log.info('[{:03d}] "{}" ({}) pressed; event: {}'.format(
_count, ch, och, _event))
if self.exit_on_complete and self.all_triggered:
self._log.info('[{:03d}] COMPLETE.'.format(_count))
self.disable()
return
elif self._verbose:
self.waiting_for_message()
else:
self._log.info(
'[{:03d}] unmapped key "{}" ({}) pressed.'.format(
_count, ch, och))
# ..........................................................................
# async def fire_message(self, event):
def fire_message(self, event):
self._log.info('firing message for event {}'.format(event))
_message = self._message_factory.get_message(event, True)
self._message_bus.publish(_message)
# await asyncio.sleep(0.1)
# message handling .........................................................
# ......................................................
def process_message(self, message):
'''
Processes the message, keeping count and providing a display of status.
'''
_event = message.event
if _event is Event.BUMPER_PORT:
self._print_event(Fore.RED, _event, message.value)
if self._triggered_bmp_port < self._limit:
self._triggered_bmp_port += 1
elif _event is Event.BUMPER_CNTR:
self._print_event(Fore.BLUE, _event, message.value)
if self._triggered_bmp_cntr < self._limit:
self._triggered_bmp_cntr += 1
elif _event is Event.BUMPER_STBD:
self._print_event(Fore.GREEN, _event, message.value)
if self._triggered_bmp_stbd < self._limit:
self._triggered_bmp_stbd += 1
elif _event is Event.INFRARED_PORT_SIDE:
self._print_event(Fore.RED, _event, message.value)
if self._triggered_ir_port_side < self._limit:
self._triggered_ir_port_side += 1
elif _event is Event.INFRARED_PORT:
self._print_event(Fore.RED, _event, message.value)
if self._triggered_ir_port < self._limit:
self._triggered_ir_port += 1
elif _event is Event.INFRARED_CNTR:
self._print_event(Fore.BLUE, _event, message.value)
if self._triggered_ir_cntr < self._limit:
self._triggered_ir_cntr += 1
elif _event is Event.INFRARED_STBD:
self._print_event(Fore.GREEN, _event, message.value)
if self._triggered_ir_stbd < self._limit:
self._triggered_ir_stbd += 1
elif _event is Event.INFRARED_STBD_SIDE:
self._print_event(Fore.GREEN, _event, message.value)
if self._triggered_ir_stbd_side < self._limit:
self._triggered_ir_stbd_side += 1
else:
self._log.info(Fore.BLACK + Style.BRIGHT +
'other event: {}'.format(_event.description))
# ......................................................
def _print_event(self, color, event, value):
self._log.info('event:\t' + color + Style.BRIGHT +
'{}; value: {}'.format(event.description, value))
# ..........................................................................
def waiting_for_message(self):
_div = Fore.CYAN + Style.NORMAL + ' | '
self._log.info('waiting for: | ' \
+ self._get_output(Fore.RED, 'PSID', self._triggered_ir_port_side) \
+ _div \
+ self._get_output(Fore.RED, 'PORT', self._triggered_ir_port) \
+ _div \
+ self._get_output(Fore.BLUE, 'CNTR', self._triggered_ir_cntr) \
+ _div \
+ self._get_output(Fore.GREEN, 'STBD', self._triggered_ir_stbd) \
+ _div \
+ self._get_output(Fore.GREEN, 'SSID', self._triggered_ir_stbd_side) \
+ _div \
+ self._get_output(Fore.RED, 'BPRT', self._triggered_bmp_port) \
+ _div \
+ self._get_output(Fore.BLUE, 'BCNT', self._triggered_bmp_cntr) \
+ _div \
+ self._get_output(Fore.GREEN, 'BSTB', self._triggered_bmp_stbd) \
+ _div )
# ......................................................
def _get_output(self, color, label, value):
if (value == 0):
_style = color + Style.BRIGHT
elif (value == 1):
_style = color + Style.NORMAL
elif (value == 2):
_style = color + Style.DIM
else:
_style = Fore.BLACK + Style.DIM
return _style + self._fmt.format(label if
(value < self._limit) else '')
# ......................................................
@property
def all_triggered(self):
return self._triggered_ir_port_side >= self._limit \
and self._triggered_ir_port >= self._limit \
and self._triggered_ir_cntr >= self._limit \
and self._triggered_ir_stbd >= self._limit \
and self._triggered_ir_stbd_side >= self._limit \
and self._triggered_bmp_port >= self._limit \
and self._triggered_bmp_cntr >= self._limit \
and self._triggered_bmp_stbd >= self._limit
# ..........................................................................
@property
def enabled(self):
return self._enabled
# ..........................................................................
def enable(self):
if not self._closed:
if self._enabled:
self._log.warning('already enabled.')
else:
# if we haven't started the thread yet, do so now...
if self._thread is None:
self._enabled = True
# self._thread = Thread(name='mock-ifs', target=MockIntegratedFrontSensor._start_loop, args=[self, lambda: self.enabled], daemon=True)
# self._thread.start()
self._log.info('enabled.')
else:
self._log.warning('cannot enable: thread already exists.')
else:
self._log.warning('cannot enable: already closed.')
# ..........................................................................
def disable(self):
if self._enabled:
self._enabled = False
self._thread = None
self._log.info('disabled.')
else:
self._log.warning('already disabled.')
# ..........................................................................
def close(self):
if not self._closed:
if self._enabled:
self.disable()
self._closed = True
self._log.info('closed.')
else:
self._log.warning('already closed.')
# ..........................................................................
def print_keymap(self):
# 1 2 3 4 5 6 7 8 9 C 1 2
#23456789012345678901234567890123456789012345678901234567890123456789012345678901234567890123456789012345678901234567890
self._log.info('''key map:
o-------------------------------------------------o ---o--------o
| Q | | T | | DEL |
| QUIT | | NOOP | | QUIT |
o--------------------------------------------------------------------------o---------o ---o--------o
| A | S | D | F | G | H | J | K |
| IR_PSID | IR_PORT | IR_CNTR | IR_STBD | IR_SSID | HELP | | |
o-------------------------------------------------------------------------------o------------------------o
| X | C | V | | M | < | > |
| BM_PORT | BM_CNTR | BM_STBD | | TOG_MSG | DN_VELO | UP_VELO |
o-----------------------------o o-----------------------------o
''')
self._log.info('note:\t' + Fore.YELLOW +
'will exit after receiving 3 events on each sensor.')
print('')
# ..........................................................................
def get_event_for_char(self, och):
'''
Below are the mapped characters for IFS-based events, including several others:
oct dec hex char usage
54 44 2C , increase motors speed (both)
56 46 2E . decrease motors speed (both)
141 97 61 a * port side IR
142 98 62 b
143 99 63 c * cntr BMP
144 100 64 d * cntr IR
145 101 65 e
146 102 66 f * stbd IR
147 103 67 g * stbd side IR
150 104 68 h
151 105 69 i
152 106 6A j
153 107 6B k
154 108 6C l
155 109 6D m toggle IFS messaging
156 110 6E n
157 111 6F o
160 112 70 p
161 113 71 q
162 114 72 r
163 115 73 s * port IR
164 116 74 t * noop (test message)
165 117 75 u
166 118 76 v * stbd BMP
167 119 77 w
170 120 78 x * port BMP
171 121 79 y
172 122 7A z
'''
if och == 44: # ,
return Event.DECREASE_SPEED
elif och == 46: # .
return Event.INCREASE_SPEED
elif och == 97: # a
return Event.INFRARED_PORT_SIDE
elif och == 99: # c
return Event.BUMPER_CNTR
elif och == 100: # d
return Event.INFRARED_CNTR
elif och == 102: # f
return Event.INFRARED_STBD
elif och == 103: # g
return Event.INFRARED_STBD_SIDE
elif och == 115: # s
return Event.INFRARED_PORT
elif och == 116: # s
return Event.NOOP
elif och == 118: # v
return Event.BUMPER_STBD
elif och == 120: # x
return Event.BUMPER_PORT
else:
return None
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 __init__(self, level=Level.INFO):
self._log = Logger('flask.wrapper', level)
self._message_factory = MessageFactory(None, level)
self._log.info('ready.')
def main():
global action_A, action_B
# initial states
action_A = False
action_B = True
_loader = ConfigLoader(_level)
_config = _loader.configure('config.yaml')
_pin_A = 12
_button_12 = 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))
try:
_log.info('creating message factory...')
_message_factory = MessageFactory(Level.INFO)
_log.info('creating message bus...')
_message_bus = MessageBus(Level.DEBUG)
_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()
_limit = 90.0
_stbd_setpoint = 0.0
_port_setpoint = 0.0
_scaled_value = 0.0
_value = 0.0
_port_pid_ctrl = None
_stbd_pid_ctrl = None
# rotary controller
_min_rot = 0
_max_rot = 30
_step_rot = 1
_rotary_ctrl = RotaryControl(_config, _min_rot, _max_rot, _step_rot,
Level.WARN)
# configure motors/PID controllers
if ORIENTATION == Orientation.BOTH or ORIENTATION == Orientation.PORT:
_port_motor = _motors.get_motor(Orientation.PORT)
_port_pid_ctrl = PIDController(_config,
_clock,
_port_motor,
level=Level.INFO)
_port_pid_ctrl.limit = _limit
_port_pid_ctrl.enable()
if ORIENTATION == Orientation.BOTH or ORIENTATION == Orientation.STBD:
_stbd_motor = _motors.get_motor(Orientation.STBD)
_stbd_pid_ctrl = PIDController(_config,
_clock,
_stbd_motor,
level=Level.INFO)
_stbd_pid_ctrl.limit = _limit
_stbd_pid_ctrl.enable()
_max_pot = 1.0
_min_pot = 0.0
_pot = Potentiometer(_config, Level.WARN)
_pot.set_output_limits(_min_pot, _max_pot)
_motors.enable()
try:
while action_B:
if action_A:
_log.info(Fore.BLUE + Style.BRIGHT + 'action A.')
action_A = False # trigger once
_stbd_setpoint = 0.0
_port_setpoint = 0.0
while action_B:
# set PID constant
_scaled_value = _pot.get_scaled_value(True)
# _log.info(Style.DIM + 'PID TUNER kp: {:8.5f} ================ '.format(_scaled_value))
if ORIENTATION == Orientation.BOTH or ORIENTATION == Orientation.PORT:
_port_pid_ctrl.kp = _scaled_value
_stbd_pid_ctrl.kp = _scaled_value
_value = _rotary_ctrl.read()
# _log.info(Fore.BLUE + Style.BRIGHT + 'rotary value: {:<5.2f}'.format(_value))
# if _value > -2.0 and _value < 2.0: # hysteresis?
# _value = 0.0
if ORIENTATION == Orientation.BOTH or ORIENTATION == Orientation.PORT:
_port_pid_ctrl.setpoint = ROTATE_VALUE * _value #* 100.0
_port_setpoint = _port_pid_ctrl.setpoint
if ORIENTATION == Orientation.BOTH or ORIENTATION == Orientation.STBD:
_stbd_pid_ctrl.setpoint = _value #* 100.0
_stbd_setpoint = _stbd_pid_ctrl.setpoint
# _log.info(Fore.MAGENTA + 'rotary value: {:<5.2f}; setpoint: '.format(_value) \
# + Fore.RED + ' {:5.2f};'.format(_port_setpoint) + Fore.GREEN + ' {:5.2f};'.format(_stbd_setpoint) \
# + Fore.WHITE + ' action_B: {}'.format(action_B))
# end of loop
action_B = False # quit after action
time.sleep(1.0)
# and we now return to our regularly scheduled broadcast...
else:
pass
_log.info(Fore.BLACK + Style.BRIGHT + 'waiting on button A...')
time.sleep(0.5)
_log.info(Fore.YELLOW + 'end of test.')
except KeyboardInterrupt:
_log.info(Fore.CYAN + Style.BRIGHT + 'Ctrl-C caught: closing...')
finally:
if ORIENTATION == Orientation.BOTH or ORIENTATION == Orientation.PORT:
_port_pid_ctrl.disable()
if ORIENTATION == Orientation.BOTH or ORIENTATION == Orientation.STBD:
_stbd_pid_ctrl.disable()
_motors.brake()
if _rotary_ctrl:
_log.info('resetting rotary encoder...')
_rotary_ctrl.reset()
time.sleep(1.0)
_log.info('complete.')
except Exception as e:
_log.info(Fore.RED + Style.BRIGHT +
'error in test: {}'.format(e, traceback.format_exc()))
finally:
_log.info(Fore.YELLOW + Style.BRIGHT + 'finally.')