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_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 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(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)
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 test_external_clock():
_log = Logger("xclock", Level.INFO)
_log.info('starting test...')
# read YAML configuration
_loader = ConfigLoader(Level.INFO)
filename = 'config.yaml'
_config = _loader.configure(filename)
_message_bus = MessageBus(Level.INFO)
_clock = ExternalClock(_config, _message_bus, Level.INFO)
try:
_log.info('starting clock...')
_clock.enable()
while _clock.enabled:
time.sleep(1.0)
except KeyboardInterrupt:
_log.info("caught Ctrl-C.")
finally:
_log.info("closed.")
class ROS(AbstractTask):
'''
Extends AbstractTask as a Finite State Machine (FSM) basis of a Robot
Operating System (ROS) or behaviour-based system (BBS), including
spawning the various tasks and an Arbitrator as separate threads,
inter-communicating over a common message queue.
This establishes the basic subsumption foundation of a MessageBus, a
MessageFactory, a system Clock, an Arbitrator and a Controller.
The MessageBus receives Event-containing messages from sensors and other
message sources, which are passed on to the Arbitrator, whose job it is
to determine the highest priority action to execute for that task cycle,
by passing it on to the Controller.
There is also a rosd linux daemon, which can be used to start, enable
and disable ros.
:param mutex: the optional logging mutex, passed on from rosd
'''
# ..........................................................................
def __init__(self, mutex=None, level=Level.INFO):
'''
This initialises the ROS and calls the YAML configurer.
'''
self._mutex = mutex if mutex is not None else threading.Lock()
super().__init__("ros", None, self._mutex)
self._log.info('setting process as high priority...')
# set ROS as high priority
proc = psutil.Process(os.getpid())
proc.nice(10)
self._config = None
self._active = False
self._closing = False
self._disable_leds = False
self._arbitrator = None
self._controller = None
self._gamepad = None
self._motors = None
self._ifs = None
self._features = []
self._log.info('initialised.')
# ..........................................................................
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 _set_feature_available(self, name, value):
'''
Sets a feature's availability to the boolean value.
'''
self._log.debug(Fore.BLUE + Style.BRIGHT +
'-- set feature available. name: \'{}\' value: \'{}\'.'
.format(name, value))
self.set_property('features', name, value)
# ..........................................................................
def get_device_for_address(self, address):
if address == 0x0E:
return 'RGB Potentiometer'
elif address == 0x0F:
return 'RGB Encoder' # default, moved to 0x16
elif address == 0x15:
return 'ThunderBorg'
elif address == 0x16:
return 'RGB Encoder'
elif address == 0x18:
return 'IO Expander'
elif address == 0x48:
return 'ADS1015'
elif address == 0x4A:
return 'Unknown'
elif address == 0x74:
return '5x5 RGB Matrix'
elif address == 0x77:
return '5x5 RGB Matrix (or 11x7 LED Matrix)'
else:
return 'Unknown'
# ..........................................................................
@property
def controller(self):
return self._controller
# ..........................................................................
@property
def configuration(self):
return self._config
# ..........................................................................
def get_property(self, section, property_name):
'''
Return the value of the named property of the application
configuration, provided its section and property name.
'''
return self._config[section].get(property_name)
# ..........................................................................
def set_property(self, section, property_name, property_value):
'''
Set the value of the named property of the application
configuration, provided its section, property name and value.
'''
self._log.debug(Fore.GREEN + 'set config on section \'{}\' for property key: \'{}\' to value: {}.'.format(\
section, property_name, property_value))
if section == 'ros':
self._config[section].update(property_name=property_value)
else:
_ros = self._config['ros']
_ros[section].update(property_name=property_value)
# ..........................................................................
def _set_pi_leds(self, enable):
'''
Enables or disables the Raspberry Pi's board LEDs.
'''
sudo_name = self.get_property('pi', 'sudo_name')
# led_0_path: '/sys/class/leds/led0/brightness'
_led_0_path = self._config['pi'].get('led_0_path')
_led_0 = Path(_led_0_path)
# led_1_path: '/sys/class/leds/led1/brightness'
_led_1_path = self._config['pi'].get('led_1_path')
_led_1 = Path(_led_1_path)
if _led_0.is_file() and _led_0.is_file():
if enable:
self._log.info('re-enabling LEDs...')
os.system('echo 1 | {} tee {}'.format(sudo_name, _led_0_path))
os.system('echo 1 | {} tee {}'.format(sudo_name, _led_1_path))
else:
self._log.debug('disabling LEDs...')
os.system('echo 0 | {} tee {}'.format(sudo_name, _led_0_path))
os.system('echo 0 | {} tee {}'.format(sudo_name, _led_1_path))
else:
self._log.warning(
'could not change state of LEDs: does not appear to be a Raspberry Pi.'
)
# ..........................................................................
def _connect_gamepad(self):
if not self._gamepad_enabled:
self._log.info('gamepad disabled.')
return
if self._gamepad is None:
self._log.info('creating gamepad...')
try:
self._gamepad = Gamepad(self._config, self._queue, Level.INFO)
except GamepadConnectException as e:
self._log.error('unable to connect to gamepad: {}'.format(e))
self._gamepad = None
self._gamepad_enabled = False
self._log.info('gamepad unavailable.')
return
if self._gamepad is not None:
self._log.info('enabling gamepad...')
self._gamepad.enable()
_count = 0
while not self._gamepad.has_connection():
_count += 1
if _count == 1:
self._log.info('connecting to gamepad...')
else:
self._log.info(
'gamepad not connected; re-trying... [{:d}]'.format(
_count))
self._gamepad.connect()
time.sleep(0.5)
if self._gamepad.has_connection() or _count > 5:
break
# ..........................................................................
def has_connected_gamepad(self):
return self._gamepad is not None and self._gamepad.has_connection()
# ..........................................................................
def get_arbitrator(self):
return self._arbitrator
# ..........................................................................
def add_feature(self, feature):
'''
Add the feature to the list of features. Features must have
an enable() method.
'''
self._features.append(feature)
self._log.info('added feature {}.'.format(feature.name()))
# ..........................................................................
def _callback_shutdown(self):
_enable_self_shutdown = self._config['ros'].get('enable_self_shutdown')
if _enable_self_shutdown:
self._log.critical('callback: shutting down os...')
self.close()
sys.exit(0)
else:
self._log.critical('self-shutdown disabled.')
# ..........................................................................
def _print_banner(self):
'''
Display banner on console.
'''
self._log.info('…')
self._log.info('… █▒▒▒▒▒▒▒ █▒▒▒▒▒▒ █▒▒▒▒▒▒ █▒▒ ')
self._log.info('… █▒▒ █▒▒ █▒▒ █▒▒ █▒▒ █▒▒ ')
self._log.info('… █▒▒▒▒▒▒ █▒▒ █▒▒ █▒▒▒▒▒▒ █▒▒ ')
self._log.info('… █▒▒ █▒▒ █▒▒ █▒▒ █▒▒ ')
self._log.info('… █▒▒ █▒▒ █▒▒▒▒▒▒ █▒▒▒▒▒▒ █▒▒ ')
self._log.info('…')
# ..........................................................................
def run(self):
'''
This first disables the Pi's status LEDs, establishes the
message queue arbitrator, the controller, enables the set
of features, then starts the main OS loop.
'''
super(AbstractTask, self).run()
loop_count = 0
self._print_banner()
self._disable_leds = self._config['pi'].get('disable_leds')
if self._disable_leds:
# disable Pi LEDs since they may be distracting
self._set_pi_leds(False)
_main_loop_freq_hz = self._config['ros'].get('main_loop_freq_hz')
self._main_loop_rate = Rate(_main_loop_freq_hz)
# __enable_player = self._config['ros'].get('enable_player')
# if __enable_player:
# self._log.info('configuring sound player...')
# self._player = Player(Level.INFO)
# else:
# self._player = None
# i2c_slave_address = config['ros'].get('i2c_master').get('device_id') # i2c hex address of I2C slave device
# vl53l1x_available = True # self.get_property('features', 'vl53l1x')
# ultraborg_available = True # self.get_property('features', 'ultraborg')
# if vl53l1x_available and ultraborg_available:
# self._log.critical('starting scanner tool...')
# self._lidar = Lidar(self._config, Level.INFO)
# self._lidar.enable()
# else:
# self._log.critical('lidar scanner tool does not have necessary dependencies.')
# wait to stabilise features?
# _flask_enabled = self._config['flask'].get('enabled')
# if _flask_enabled:
# self._log.info('starting flask web server...')
# self.configure_web_server()
# else:
# self._log.info('not starting flask web server (suppressed from command line).')
# bluetooth gamepad controller
# if self._gamepad_enabled:
# self._connect_gamepad()
# _wait_for_button_press = self._config['ros'].get('wait_for_button_press')
# self._controller.set_standby(_wait_for_button_press)
# begin main loop ..............................
# self._log.info('enabling bno055 sensor...')
# self._bno055.enable()
# self._bumpers.enable()
# self._indicator = Indicator(Level.INFO)
# add indicator as message listener
# self._queue.add_listener(self._indicator)
# self._log.info(Fore.MAGENTA + 'enabling integrated front sensor...')
# self._ifs.enable()
# self._log.info('starting info thread...')
# self._info.start()
# self._log.info('starting blinky thread...')
# self._rgbmatrix.enable(DisplayType.RANDOM)
self._log.info('enabling features...')
for feature in self._features:
self._log.info('enabling feature {}...'.format(feature.name()))
feature.enable()
self._log.notice('Press Ctrl-C to exit.')
self._log.info('begin main os loop.\r')
# enable arbitrator tasks (normal functioning of robot)
# self._arbitrator.start()
_main_loop_counter = itertools.count()
self._active = True
while self._active:
# the timing of this loop is inconsequential; it exists solely as a keep-alive.
self._log.debug('[{:d}] main loop...'.format(
next(_main_loop_counter)))
self._main_loop_rate.wait()
if not self._closing:
self._log.warning('closing following loop...')
self.close()
# end main ...................................
# ..........................................................................
def emergency_stop(self):
'''
Stop immediately, something has hit the top feelers.
'''
self._motors.stop()
self._log.info(Fore.RED + Style.BRIGHT +
'emergency stop: contact on upper feelers.')
# ..........................................................................
def send_message(self, message):
'''
Send the Message into the MessageQueue.
'''
# self._queue.add(message)
raise Exception('unsupported')
# ..........................................................................
def enable(self):
super(AbstractTask, self).enable()
# ..........................................................................
def disable(self):
super(AbstractTask, self).disable()
# ..........................................................................
def close(self):
'''
This sets the ROS back to normal following a session.
'''
if self._closing:
# this also gets called by the arbitrator so we ignore that
self._log.info('already closing.')
return
else:
self._active = False
self._closing = True
self._log.info(Style.BRIGHT + 'closing...')
if self._gamepad:
self._gamepad.close()
if self._motors:
self._motors.close()
if self._ifs:
self._ifs.close()
# close features
for feature in self._features:
self._log.info('closing feature {}...'.format(feature.name()))
feature.close()
self._log.info('finished closing features.')
if self._arbitrator:
self._arbitrator.disable()
self._arbitrator.close()
# self._arbitrator.join(timeout=1.0)
if self._controller:
self._controller.disable()
super().close()
if self._disable_leds:
# restore LEDs
self._set_pi_leds(True)
# GPIO.cleanup()
# self._log.info('joining main thread...')
# self.join(timeout=0.5)
self._log.info('os closed.')
sys.stderr = DevNull()
sys.exit(0)
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):
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 GamepadDemo():
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.')
# ..........................................................................
def get_motors(self):
return self._motors
# ..........................................................................
@property
def enabled(self):
return self._enabled
# ..........................................................................
def enable(self):
if self._enabled:
self._log.warning('already enabled.')
return
self._log.info('enabling...')
self._gamepad.enable()
self._clock.enable()
# if self._enable_compass:
# self._compass.enable()
if self._battery_check:
self._battery_check.enable()
if self._enable_ifs:
self._ifs.enable()
self._ctrl.enable()
self._enabled = True
self._log.info('enabled.')
# ..........................................................................
def get_thread_position(self, thread):
frame = sys._current_frames().get(thread.ident, None)
if frame:
return frame.f_code.co_filename, frame.f_code.co_name, frame.f_code.co_firstlineno
# ..........................................................................
def disable(self):
if not self._enabled:
self._log.warning('already disabled.')
return
self._log.info('disabling...')
self._enabled = False
self._clock.disable()
if self._battery_check:
self._battery_check.disable()
# if self._enable_compass:
# self._compass.disable()
if self._enable_ifs:
self._ifs.disable()
self._pid_motor_ctrl.disable()
self._gamepad.disable()
_show_thread_info = False
if _show_thread_info:
for thread in threading.enumerate():
self._log.info(
Fore.GREEN +
'thread "{}" is alive? {}; is daemon? {}\t😡'.format(
thread.name, thread.is_alive(), thread.isDaemon()))
if thread is not None:
_position = self.get_thread_position(thread)
if _position:
self._log.info(
Fore.GREEN +
' thread "{}" filename: {}; co_name: {}; first_lineno: {}'
.format(thread.name, _position[0], _position[1],
_position[2]))
else:
self._log.info(Fore.GREEN +
' thread "{}" position null.'.format(
thread.name))
else:
self._log.info(Fore.GREEN + ' null thread.')
self._log.info('disabled.')
# ..........................................................................
def _close_demo_callback(self):
self._log.info(Fore.MAGENTA + 'close demo callback...')
# self._queue.disable()
self.disable()
self.close()
# ..........................................................................
def close(self):
if self._enabled:
self.disable()
self._log.info('closing...')
if self._enable_ifs:
self._ifs.close()
self._pid_motor_ctrl.close()
self._gamepad.close()
self._log.info('closed.')
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 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.')