def main(argv): try: # read YAML configuration _level = Level.INFO _loader = ConfigLoader(_level) filename = 'config.yaml' _config = _loader.configure(filename) _motors = Motors(_config, None, Level.INFO) _blob = Blob(_config, _motors, _level) _blob.enable() _rate = Rate(1) # _blob.capture() # for i in range(32): while True: print(Fore.BLACK + Style.DIM + 'capturing...' + Style.RESET_ALL) # _location = _blob.capture() # print(Fore.YELLOW + Style.NORMAL + 'captured location: {}'.format(_location) + Style.RESET_ALL) # print('captured.') _rate.wait() _blob.disable() except KeyboardInterrupt: print(Fore.CYAN + 'Ctrl-C interrupt.' + Style.RESET_ALL) except Exception: print(Fore.RED + Style.BRIGHT + 'error starting ros: {}'.format(traceback.format_exc()) + Style.RESET_ALL)
def main(): try: # read YAML configuration _loader = ConfigLoader(Level.INFO) filename = 'config.yaml' _config = _loader.configure(filename) _loop_delay_sec = 0.7 _loop_count = 10000 _master = I2cMaster(_config, Level.INFO) _master.enable() if not _master.in_loop(): _master.start_front_sensor_loop(_loop_delay_sec, self._callback_front) else: raise Exception( 'unable to establish contact with Arduino on address 0x{:02X}'. format(_device_id)) except KeyboardInterrupt: self._log.warning('Ctrl-C caught; exiting...') finally: if _master: _master.close()
def main(argv): _temp = None _log = Logger('temp-test', Level.INFO) try: # read YAML configuration _loader = ConfigLoader(Level.INFO) filename = 'config.yaml' _config = _loader.configure(filename) _queue = MessageQueue(Level.DEBUG) _temp = Temperature(_config, _queue, Level.INFO) _temp.enable() while True: # _value = _temp.get_cpu_temperature() # _is_hot = _temp.is_hot() time.sleep(1.0) except KeyboardInterrupt: _log.info(Fore.YELLOW + 'exited via Ctrl-C' + Style.RESET_ALL) if _temp: _temp.close() sys.exit(0) except Exception: _log.error(Fore.RED + Style.BRIGHT + 'error getting RPI temperature: {}'.format( traceback.format_exc()) + Style.RESET_ALL) if _temp: _temp.close() sys.exit(1)
def test_fan(): _fan = None _log = Logger("fan test", Level.INFO) try: # read YAML configuration _loader = ConfigLoader(Level.INFO) filename = 'config.yaml' _config = _loader.configure(filename) _fan = Fan(_config, Level.INFO) limit = 2 for i in range(limit): _log.info('Switching fan on! [{:d}/{:d}]'.format(i + 1, limit)) _fan.enable() time.sleep(5) _log.info('Switching fan off! [{:d}/{:d}]'.format(i + 1, limit)) _fan.disable() time.sleep(5) except KeyboardInterrupt: _log.info("Disabling switch") if _fan: _fan.disable() except Exception as e: _log.error('error in fan: {}\n{}'.format(e, traceback.format_exc())) sys.exit(1)
def main(): _log = Logger("status test", Level.INFO) _log.info("status task run()") # read YAML configuration _loader = ConfigLoader(Level.INFO) filename = 'config.yaml' _config = _loader.configure(filename) _status = Status(_config, GPIO, Level.INFO) try: _status.blink(True) for i in range(10): _log.info("blinking...") time.sleep(1) _status.blink(False) _status.enable() time.sleep(5) except KeyboardInterrupt: _log.info('Ctrl-C caught: interrupted.') finally: _log.info("status task close()") _status.close()
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 main(argv): try: # read YAML configuration _loader = ConfigLoader(Level.INFO) filename = 'config.yaml' _config = _loader.configure(filename) _lux = Lux(Level.INFO) _matrix11x7_stbd_available = True _video = Video(_config, _lux, _matrix11x7_stbd_available, Level.INFO) _video.start() while True: time.sleep(1.0) _video.stop() except KeyboardInterrupt: print(Fore.CYAN + Style.BRIGHT + 'caught Ctrl-C; exiting...') if _video is not None: _video.stop() except Exception: print(Fore.RED + Style.BRIGHT + 'error starting ros: {}'.format(traceback.format_exc()) + Style.RESET_ALL)
def test_rot_control(): _log = Logger("rot-ctrl-test", Level.INFO) _rot_ctrl = None try: # read YAML configuration _loader = ConfigLoader(Level.INFO) filename = 'config.yaml' _config = _loader.configure(filename) # def __init__(self, config, minimum, maximum, step, level): # _min = -127 # _max = 127 # _step = 1 _min = 0 _max = 10 _step = 0.5 _rot_ctrl = RotaryControl(_config, _min, _max, _step, Level.INFO) _rate = Rate(20) _log.info(Fore.WHITE + Style.BRIGHT + 'waiting for changes to rotary encoder...') while True: _value = _rot_ctrl.read() _log.info(Fore.YELLOW + ' value: {:5.2f}'.format(_value)) _rate.wait() finally: if _rot_ctrl: _log.info('resetting rotary encoder...') _rot_ctrl.reset()
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(): print('bumpers_test :' + Fore.CYAN + ' INFO : starting test...' + Style.RESET_ALL) print('bumpers_test :' + Fore.YELLOW + Style.BRIGHT + ' INFO : Press Ctrl+C to exit.' + Style.RESET_ALL) signal.signal(signal.SIGINT, signal_handler) print('bumpers_test :' + Fore.BLUE + Style.BRIGHT + ' INFO : to pass test, trigger all bumper sensors...' + Style.RESET_ALL) _queue = MockMessageQueue(Level.INFO) # read YAML configuration _loader = ConfigLoader(Level.INFO) filename = 'config.yaml' _config = _loader.configure(filename) _bumpers = Bumpers(_config, _queue, None, Level.INFO) _bumpers.enable() while not _queue.is_complete(): _queue._display_event_list() time.sleep(2.0) _bumpers.close()
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) _ifs = IntegratedFrontSensor(_config, _queue, _message_factory, Level.INFO) _indicator = Indicator(Level.INFO) # _indicator.set_heading(180) # add indicator as message consumer _queue.add_consumer(_indicator) _ifs.enable() while True: time.sleep(1.0) except KeyboardInterrupt: print(Fore.RED + 'Ctrl-C caught; exiting...' + Style.RESET_ALL) except Exception as e: print(Fore.RED + Style.BRIGHT + 'error starting ifs: {}\n{}'.format(e, traceback.format_exc()) + Style.RESET_ALL) finally: if _ifs is not None: _ifs.close()
def main(argv): try: # read YAML configuration _loader = ConfigLoader(Level.INFO) filename = 'config.yaml' _config = _loader.configure(filename) _imu = IMU(_config, Level.INFO) while True: # x, y, z = _imu.read_magnetometer() mag = _imu.read_magnetometer() # ax, ay, az, gx, gy, gz = _imu.read_accelerometer_gyro() acc = _imu.read_accelerometer_gyro() heading = Convert.heading_from_magnetometer(_imu._amin, _imu._amax, mag) print(Fore.CYAN + 'Accel: {:05.2f} {:05.2f} {:05.2f} '.format(acc[0], acc[1], acc[2]) \ + Fore.YELLOW + '\tGyro: {:05.2f} {:05.2f} {:05.2f} '.format(acc[3], acc[4], acc[5]) \ + Fore.MAGENTA + '\tMag: {:05.2f} {:05.2f} {:05.2f} '.format(mag[0], mag[1], mag[2]) \ + Fore.GREEN + '\tHeading: {:d}°'.format(heading) + Style.RESET_ALL) time.sleep(0.25) except KeyboardInterrupt: print(Fore.CYAN + Style.BRIGHT + 'caught Ctrl-C; exiting...') except Exception: print(Fore.RED + Style.BRIGHT + 'error starting imu: {}'.format(traceback.format_exc()) + Style.RESET_ALL)
def main(): try: _loader = ConfigLoader(Level.INFO) filename = 'config.yaml' _config = _loader.configure(filename) _message_factory = MessageFactory(Level.INFO) # _queue = MessageQueue(_message_factory, Level.INFO) _queue = MockMessageQueue(Level.INFO) _clock = Clock(_config, _queue, _message_factory, Level.INFO) _clock.enable() while True: time.sleep(1.0) except KeyboardInterrupt: print(Fore.RED + 'Ctrl-C caught; exiting...' + Style.RESET_ALL) except Exception as e: print(Fore.RED + Style.BRIGHT + 'error starting ifs: {}\n{}'.format(e, traceback.format_exc()) + Style.RESET_ALL) finally: pass
def main(): signal.signal(signal.SIGINT, signal_handler) try: _loader = ConfigLoader(Level.INFO) filename = 'config.yaml' _config = _loader.configure(filename) _queue = MessageQueue(Level.INFO) _indicator = Indicator(Level.INFO) _compass = Compass(_config, _queue, _indicator, Level.INFO) _compass.enable() _counter = itertools.count() print(Fore.CYAN + 'wave robot in air until it beeps...' + Style.RESET_ALL) while True: _count = next(_counter) _heading = _compass.get_heading() # _log.info(Fore.CYAN + Style.BRIGHT + '{:d}: {:>5.2f}; calibrated? {}'.format(_count, _heading[1], _heading[0])) time.sleep(1.0) except KeyboardInterrupt: if _compass is not None: _compass.close() _log.info('done.') sys.exit(0)
def __init__(self): ''' This initialises the ROS and calls the YAML configurer. ''' self._queue = MessageQueue(Level.INFO) self._mutex = threading.Lock() super().__init__("ros", self._queue, None, None, self._mutex) self._log.info('initialising...') self._active = False self._closing = False self._disable_leds = False # self._switch = None self._motors = None self._arbitrator = None self._controller = None self._gamepad = None self._features = [] # self._flask_wrapper = None # read YAML configuration _loader = ConfigLoader(Level.INFO) filename = 'config.yaml' self._config = _loader.configure(filename) self._gamepad_enabled = self._config['ros'].get('gamepad').get( 'enabled') self._log.info('initialised.') self._configure()
def main(): try: # read YAML configuration _loader = ConfigLoader(Level.INFO) filename = 'config.yaml' _config = _loader.configure(filename) _loop_count = 10000 _master = I2cMaster(_config, Level.INFO) if _master is not None: # set it to some very large number if you want it to go on for a long time... _master.test_configuration( _loop_count) # see documentation for hardware configuration else: raise Exception( 'unable to establish contact with Arduino on address 0x{:02X}'. format(_device_id)) except KeyboardInterrupt: self._log.warning('Ctrl-C caught; exiting...') finally: if _master: _master.close()
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(): _log = Logger("servo-test", Level.INFO) try: _log.info('starting ultrasonic scan...') _loader = ConfigLoader(Level.INFO) filename = 'config.yaml' _config = _loader.configure(filename) _number = 1 _servo = Servo(_config, _number, Level.INFO) # _servo.enable() # _thread = threading.Thread(target=_servo.sweep, args=[]) # _thread.start() for i in numpy.arange(-90.0, 90.0, 5.0): _servo.set_position(i) _log.info('position: {}'.format(i)) time.sleep(0.5) _servo.set_position(0.0) _servo.disable() _thread.join() _log.info('servo test complete.') except Exception: _log.info(traceback.format_exc()) sys.exit(1)
def main(): signal.signal(signal.SIGINT, signal_handler) try: _loader = ConfigLoader(Level.INFO) filename = 'config.yaml' _config = _loader.configure(filename) _queue = MessageQueue(Level.INFO) # _indicator = Indicator(Level.INFO) _nxp9dof = NXP9DoF(_config, _queue, Level.INFO) _nxp9dof.enable() _counter = itertools.count() while True: _count = next(_counter) # _log.info(Fore.BLACK + Style.NORMAL + '{:d}... '.format(_count)) time.sleep(1.0) except KeyboardInterrupt: if _nxp9dof is not None: _nxp9dof.close() _log.info('done.') sys.exit(0)
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(): signal.signal(signal.SIGINT, signal_handler) _log = Logger("infrareds test", Level.INFO) _log.info('starting test...') _log.info(Fore.YELLOW + Style.BRIGHT + 'Press Ctrl+C to exit.') _log.info(Fore.BLUE + Style.BRIGHT + 'to pass test, trigger all infrared sensors...') # read YAML configuration _loader = ConfigLoader(Level.INFO) filename = 'config.yaml' _config = _loader.configure(filename) _queue = MockMessageQueue(Level.INFO) _infrareds = Infrareds(_config, _queue, Level.INFO) _infrareds.enable() while not _queue.is_complete(): _queue._display_event_list() time.sleep(2.0) _infrareds.close() _log.info('complete.')
def main(argv): try: # read YAML configuration _loader = ConfigLoader(Level.INFO) filename = 'config.yaml' _config = _loader.configure(filename) _imu = IMU(_config, Level.INFO) while True: x, y, z = _imu.read_magnetometer() ax, ay, az, gx, gy, gz = _imu.read_accelerometer_gyro() print(Fore.CYAN + 'Accel: {:05.2f} {:05.2f} {:05.2f} '.format(ax, ay, az) \ + Fore.YELLOW + '\tGyro: {:05.2f} {:05.2f} {:05.2f} '.format(gx, gy, gz )\ + Fore.MAGENTA + '\tMag: {:05.2f} {:05.2f} {:05.2f}'.format(x, y, z) + Style.RESET_ALL) time.sleep(0.25) except KeyboardInterrupt: print(Fore.CYAN + Style.BRIGHT + 'caught Ctrl-C; exiting...') except Exception: print(Fore.RED + Style.BRIGHT + 'error starting imu: {}'.format(traceback.format_exc()) + Style.RESET_ALL)
def main(): # read YAML configuration _loader = ConfigLoader(Level.INFO) filename = 'config.yaml' _config = _loader.configure(filename) _accel = Accelerometer(_config, Level.INFO) try: while True: _xyz = _accel.get_xyz() _x = _xyz[0] - _accel._x_offset _y = _xyz[1] - _accel._y_offset _z = _xyz[2] - _accel._z_offset _roll_style = Fore.CYAN + Style.BRIGHT if _accel.is_moving(_x) else Fore.RED + Style.NORMAL _yaw_style = Fore.MAGENTA + Style.BRIGHT if _accel.is_moving(_y) else Fore.GREEN + Style.NORMAL _pitch_style = Fore.YELLOW + Style.BRIGHT if _accel.is_moving(_z) else Fore.BLUE + Style.NORMAL _at_rest = _accel.at_rest() print( _roll_style + 'roll: {:+06.2f}g '.format(_x) \ + _yaw_style + '\tyaw: {:+06.2f}g '.format(_y) \ + _pitch_style + '\tpitch: {:+06.2f}g '.format(_z) + Fore.WHITE + Style.NORMAL \ + '\tat rest? {}'.format(_at_rest) + Style.RESET_ALL) time.sleep(1.0/50) except KeyboardInterrupt: print(Fore.RED + 'caught Ctrl-C; exiting...' + Style.RESET_ALL)
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): _loader = ConfigLoader(Level.WARN) filename = 'config.yaml' _config = _loader.configure(filename) _gp_scan = GamepadScan(_config, Level.INFO) _matches = _gp_scan.check_gamepad_device() print(Fore.CYAN + 'done: {}'.format(_matches) + Style.RESET_ALL)
def main(): try: _log = Logger("test", Level.INFO) _log.info(Fore.BLUE + 'configuring...') # read YAML configuration _loader = ConfigLoader(Level.INFO) filename = 'config.yaml' _config = _loader.configure(filename) _queue = MockMessageQueue(Level.INFO) _motors = Motors(_config, None, Level.INFO) _pid_motor_ctrl = PIDMotorController(_config, _motors, Level.WARN) _motor_ctrl = MotorController(_pid_motor_ctrl, Level.INFO) _motor_ctrl.set_cruising_velocity(25.0) _message_factory = MessageFactory(Level.INFO) _ifs = IntegratedFrontSensor(_config, _queue, _message_factory, Level.INFO) try: _log.info(Fore.BLUE + 'starting to cruise...') _motor_ctrl.cruise() time.sleep(2) _log.info(Fore.BLUE + 'cruising...') for i in range(5): # while True: _motor_ctrl.set_cruising_velocity(40.0) _log.info(Fore.BLUE + '{} cruising...'.format(i)) time.sleep(1) _motor_ctrl.disable() _log.info(Fore.BLUE + 'exited loop.') # sys.exit(0) # _ifs.enable() # while True: # time.sleep(1.0) except KeyboardInterrupt: print(Fore.RED + 'Ctrl-C caught; exiting...' + Style.RESET_ALL) _motor_ctrl.disable() _log.info(Fore.BLUE + 'post-try...') time.sleep(3) _pid_motor_ctrl.disable() _log.info(Fore.BLUE + 'exiting...') except Exception as e: print(Fore.RED + Style.BRIGHT + 'error: {}'.format(e)) traceback.print_exc(file=sys.stdout)
def main(): signal.signal(signal.SIGINT, signal_handler) _log = Logger("scanner", Level.INFO) _log.info(Fore.CYAN + Style.BRIGHT + ' INFO : starting test...') _log.info(Fore.YELLOW + Style.BRIGHT + ' INFO : Press Ctrl+C to exit.') try: _i2c_scanner = I2CScanner(Level.WARN) _addresses = _i2c_scanner.getAddresses() hexAddresses = _i2c_scanner.getHexAddresses() _addrDict = dict( list(map(lambda x, y: (x, y), _addresses, hexAddresses))) for i in range(len(_addresses)): _log.debug(Fore.BLACK + Style.DIM + 'found device at address: {}'.format(hexAddresses[i])) vl53l1x_available = (0x29 in _addresses) ultraborg_available = (0x36 in _addresses) if not vl53l1x_available: raise OSError('VL53L1X hardware dependency not available.') elif not ultraborg_available: raise OSError('UltraBorg hardware dependency not available.') _log.info('starting scan...') # _player = Player(Level.INFO) _loader = ConfigLoader(Level.INFO) filename = 'config.yaml' _config = _loader.configure(filename) _lidar = Lidar(_config, Level.INFO) _lidar.enable() values = _lidar.scan() _angle_at_min = values[0] _min_mm = values[1] _angle_at_max = values[2] _max_mm = values[3] _log.info( Fore.CYAN + Style.BRIGHT + 'min. distance at {:>5.2f}°:\t{}mm'.format(_angle_at_min, _min_mm)) _log.info( Fore.CYAN + Style.BRIGHT + 'max. distance at {:>5.2f}°:\t{}mm'.format(_angle_at_max, _max_mm)) time.sleep(1.0) _lidar.close() _log.info(Fore.CYAN + Style.BRIGHT + 'test complete.') except Exception: _log.info(traceback.format_exc()) sys.exit(1)
def main(): _log = Logger("cat-scan test", Level.INFO) _log.info('starting test...') _log.info(Fore.YELLOW + Style.BRIGHT + ' INFO : Press Ctrl+C to exit.') try: _i2c_scanner = I2CScanner(Level.WARN) _addresses = _i2c_scanner.getAddresses() hexAddresses = _i2c_scanner.getHexAddresses() _addrDict = dict(list(map(lambda x, y:(x,y), _addresses, hexAddresses))) for i in range(len(_addresses)): _log.debug(Fore.BLACK + Style.DIM + 'found device at address: {}'.format(hexAddresses[i]) + Style.RESET_ALL) ultraborg_available = ( 0x36 in _addresses ) if not ultraborg_available: raise OSError('UltraBorg hardware dependency not available.') _log.info('starting cat scan...') _loader = ConfigLoader(Level.INFO) filename = 'config.yaml' _config = _loader.configure(filename) _queue = MockMessageQueue(Level.INFO) _catscan = CatScan(_config, _queue, Level.INFO) _catscan.enable() _catscan.set_mode(True) _count = 0 while not _queue.is_complete(): if _count % 5 == 0: _log.info('waiting for cat... ' + Fore.BLACK + Style.DIM + '({:d} sec)'.format(_count)) time.sleep(1.0) _count += 1 time.sleep(9.0) _catscan.close() _log.info('test complete.') except KeyboardInterrupt: if _catscan: _catscan.close() except Exception: _log.info(traceback.format_exc()) finally: if _catscan: _catscan.close()
def test_ioe(): ''' Test the basic functionality of the IO Expander's connections to the IR and bumper sensors. ''' _log = Logger("test-ioe", Level.INFO) # read YAML configuration _loader = ConfigLoader(Level.INFO) filename = 'config.yaml' _config = _loader.configure(filename) _ioe = IoExpander(_config, Level.INFO) assert _ioe is not None _show_ir = True _show_bmp = True _bmp_port_triggered = False _bmp_cntr_triggered = False _bmp_stbd_triggered = False for i in range(100000): # infrared sensors ......................................................... if _show_ir: _ir_port_side_value = _ioe.get_raw_port_side_ir_value() _ir_port_value = _ioe.get_raw_port_ir_value() _ir_cntr_value = _ioe.get_raw_center_ir_value() _ir_stbd_value = _ioe.get_raw_stbd_ir_value() _ir_stbd_side_value = _ioe.get_raw_stbd_side_ir_value() _log.info(Fore.RED + 'IR {:8.5f}\t'.format(_ir_port_side_value) \ + '{:8.5f}\t'.format(_ir_port_value) \ + Fore.BLUE + '{:8.5f}\t'.format(_ir_cntr_value) \ + Fore.GREEN + '{:8.5f}\t'.format(_ir_stbd_value) \ + '{:8.5f}'.format(_ir_stbd_side_value)) # # bumpers .................................................................. if _show_bmp: _bmp_port_value = _ioe.get_raw_port_bmp_value() if _bmp_port_value == 0: _bmp_port_triggered = True _bmp_cntr_value = _ioe.get_raw_center_bmp_value() if _bmp_cntr_value == 0: _bmp_cntr_triggered = True _bmp_stbd_value = _ioe.get_raw_stbd_bmp_value() if _bmp_stbd_value == 0: _bmp_stbd_triggered = True # _log.info(Fore.RED + 'BMP {}\t'.format(_bmp_port_value) \ # + Fore.BLUE + '{}\t'.format(_bmp_cntr_value) \ # + Fore.GREEN + '{}'.format(_bmp_stbd_value)) _log.info(Fore.RED + 'BMP Triggered? {}\t'.format(_bmp_port_triggered) \ + Fore.BLUE + '{}\t'.format(_bmp_cntr_triggered) \ + Fore.GREEN + '{}'.format(_bmp_stbd_triggered)) _log.info('i={:d}\n'.format(i)) time.sleep(0.33)
def main(): signal.signal(signal.SIGINT, signal_handler) print('follower test :' + Fore.CYAN + Style.BRIGHT + ' INFO : starting test...' + Style.RESET_ALL) print('follower test :' + Fore.YELLOW + Style.BRIGHT + ' INFO : Press Ctrl+C to exit.' + Style.RESET_ALL) _log = Logger("follower", Level.INFO) try: _i2c_scanner = I2CScanner(Level.WARN) _addresses = _i2c_scanner.getAddresses() hexAddresses = _i2c_scanner.getHexAddresses() _addrDict = dict(list(map(lambda x, y:(x,y), _addresses, hexAddresses))) for i in range(len(_addresses)): _log.debug(Fore.BLACK + Style.DIM + 'found device at address: {}'.format(hexAddresses[i]) + Style.RESET_ALL) vl53l1x_available = ( 0x29 in _addresses ) ultraborg_available = ( 0x36 in _addresses ) if not vl53l1x_available: raise OSError('VL53L1X hardware dependency not available.') elif not ultraborg_available: raise OSError('UltraBorg hardware dependency not available.') _log.info('starting wall follower...') _loader = ConfigLoader(Level.INFO) filename = 'config.yaml' _config = _loader.configure(filename) _follower = WallFollower(_config, Level.INFO) _follower.enable() _follower.set_position(Orientation.PORT) time.sleep(1.5) _follower.set_position(Orientation.STBD) time.sleep(1.5) # for i in range(10): # mm = _follower.scan() # time.sleep(1.0) _follower.close() print('follower test :' + Fore.CYAN + Style.BRIGHT + ' INFO : test complete.' + Style.RESET_ALL) except Exception: _log.info(traceback.format_exc()) sys.exit(1)