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)
