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 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(): try: _log = Logger("test", Level.INFO) _log.info(Fore.BLUE + 'configuring...') _loader = ConfigLoader(Level.INFO) filename = 'config.yaml' _config = _loader.configure(filename) _message_factory = MessageFactory(Level.INFO) _message_bus = MessageBus(Level.INFO) _clock = Clock(_config, _message_bus, _message_factory, Level.WARN) _clock.enable() _ifs = IntegratedFrontSensor(_config, _clock, Level.INFO) _ifs.enable() _handler = MessageHandler(Level.INFO) _message_bus.add_handler(Message, _handler.handle) try: _log.info(Fore.BLUE + 'starting loop... type Ctrl-C to exit.') _rate = Rate(1) # while True: for i in range(360): _rate.wait() _log.info(Fore.BLUE + 'exited loop.') except KeyboardInterrupt: print(Fore.RED + 'Ctrl-C caught; exiting...' + Style.RESET_ALL) except Exception as e: print(Fore.RED + Style.BRIGHT + 'error: {}'.format(e)) traceback.print_exc(file=sys.stdout)
def test_temperature(): _temperature = None _fan = None _log = Logger('temp-test', Level.INFO) try: # scan I2C bus _log.info('scanning I²C address bus...') scanner = I2CScanner(_log.level) _addresses = scanner.get_int_addresses() # load configuration _loader = ConfigLoader(Level.INFO) filename = 'config.yaml' _config = _loader.configure(filename) # create Fan if available _fan_config = _config['ros'].get('fan') _fan_i2c_address = _fan_config.get('i2c_address') fan_available = (_fan_i2c_address in _addresses) if fan_available: _fan = Fan(_config, Level.INFO) _log.info('fan is available at I²C address of 0x{:02X}.'.format( _fan_i2c_address)) else: _fan = None _log.info( 'fan is not available at I²C address of 0x{:02X}.'.format( _fan_i2c_address)) _message_factory = MessageFactory(Level.INFO) _message_bus = MessageBus(Level.DEBUG) _clock = Clock(_config, _message_bus, _message_factory, Level.WARN) _temperature = Temperature(_config, _clock, _fan, Level.INFO) _temperature.enable() _counter = itertools.count() _clock.enable() while True: _count = next(_counter) # _temperature.display_temperature() # _value = _temperature.get_cpu_temperature() # _is_warning_temp = _temperature.is_warning_temperature() # _is_max_temp = _temperature.is_max_temperature() # if _is_warning_temp: # _log.warning('[{:04d}] loop; is warning temp? {}; is max temp? {}'.format(_count, _is_warning_temp, _is_max_temp)) # else: # _log.info('[{:04d}] loop; is warning temp? {}; is max temp? {}'.format(_count, _is_warning_temp, _is_max_temp)) time.sleep(1.0) except KeyboardInterrupt: _log.info(Fore.YELLOW + 'exited via Ctrl-C' + Style.RESET_ALL) except Exception: _log.error(Fore.RED + Style.BRIGHT + 'error getting RPI temperature: {}'.format( traceback.format_exc()) + Style.RESET_ALL) finally: if _temperature: _temperature.close() if _fan: _fan.disable()
def get_end_of_current_lecture(self) -> Clock: self._maybe_not_implemented() ret: Clock = Clock() localt: Clock = Clock(time.strftime("%H:%M")) for lecture in self._lectures: # type: Lecture_digest if localt in lecture.important_bespan: ret = lecture.important_bespan.end return ret
def __init__(self, begin: str, end: str, weekday: int, room: Optional[str] = None) -> NoReturn: self.begin = Clock(begin) self.end = Clock(end) self.bespan = Span(self.begin, self.end) self.weekday = weekday self.room = room
def print_remaining_length_of_hour_and_day( end_of_current_lecture: Clock, end_of_current_day: Clock) -> NoReturn: # Bestimmt, wie lange die Stunde/der Tag noch geht localt: Clock = Clock(time.strftime("%H:%M", time.localtime())) end_of_current_lecture -= localt end_of_current_day -= localt if end_of_current_lecture.hash > 0: print("\nDiese Stunde noch:", end_of_current_lecture.pprint()) print("Heute noch:", end_of_current_day.pprint()) elif end_of_current_day.hash > 0: print("\nHeute noch:", end_of_current_day.pprint())
class Sexting(): def __init__(self, contacts_file, message, start_hour): self.message = message self.transformers = all_transformers() self.contacts = list(ContactLoader().load(contacts_file)) self.clock = Clock(int(start_hour)) def process(self): for character in self.message: transformersWithContacts = list(self.__possible_transformers_and_contacts(character)) while (not transformersWithContacts): print "Nothing to do at {0}".format(self.clock.block_range_str()) self.clock = self.clock.next_block() if (self.clock.day() > 10): raise Exception("There doesn't seem to be anyone who can process {0}".format(character)) transformersWithContacts = list(self.__possible_transformers_and_contacts(character)) transformer, contacts = self.__choose_transformer_and_contacts(transformersWithContacts) instruction = transformer.transform(character, contacts, self.clock) yield instruction self.clock = self.clock.next_block() def __possible_transformers_and_contacts(self, character): for t in self.transformers: if not t.can_handle_character(character): continue viableContacts = filter(lambda c: not c.is_busy(self.clock) and t.can_handle_contact(c, self.clock), self.contacts) if len(viableContacts) < t.num_required_contacts(): continue yield (t, viableContacts) def __choose_transformer_and_contacts(self, transformersWithContacts): shuffle(transformersWithContacts) transformer, possibleContacts = transformersWithContacts[0] shuffle(possibleContacts) contacts = possibleContacts[:transformer.num_required_contacts()] return (transformer, contacts)
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 processor(_date: Date) -> Tuple[Clock, Clock, int, int]: # Gibt Fächerliste für den aktuellen Tag aus. Gibt Ende der aktuellen # Stunde, Tages zurück. thisfaecher: Self_correcting_lecture_list = Self_correcting_lecture_list( _date) thisfaecher.extend(database.faecher) thisfaecher.apply_all_correctives_and_sort() if thisfaecher.is_empty(): print("frei") return Clock(), Clock(), 0, 0 print(thisfaecher.pprint()) return thisfaecher.get_end_of_current_lecture(), \ thisfaecher.get_end_of_last_lecture(), \ thisfaecher.get_length_of_current_lecture(), \ thisfaecher.get_length_of_day()
def test_clock(): global tock_count _log = Logger('clock-test', Level.INFO) _loader = ConfigLoader(Level.INFO) filename = 'config.yaml' _config = _loader.configure(filename) tock_count = 0 _message_factory = MessageFactory(Level.INFO) _message_bus = MockMessageBus(Level.INFO) _clock = Clock(_config, _message_bus, _message_factory, Level.INFO) _message_bus.set_clock(_clock) _clock.enable() _log.info('ready; begin test.') _loops = 3 while INFINITE or tock_count < _loops: time.sleep(1.0) _log.info('test complete.')
def test_battery_check(): _log = Logger("batcheck test", Level.INFO) _log.heading('battery check', 'Starting test...', '[1/3]') _log.info(Fore.RED + 'Press Ctrl+C to exit.') # read YAML configuration _loader = ConfigLoader(Level.INFO) filename = 'config.yaml' _config = _loader.configure(filename) _log.heading('battery check', 'Creating objects...', '[2/3]') _log.info('creating message factory...') _message_factory = MessageFactory(Level.INFO) _log.info('creating message queue...') _queue = MessageQueue(_message_factory, Level.INFO) # _consumer = MockConsumer() # _queue.add_consumer(_consumer) _log.info('creating message bus...') _message_bus = MessageBus(Level.INFO) _log.info('creating clock...') _clock = Clock(_config, _message_bus, _message_factory, Level.INFO) _log.info('creating battery check...') _battery_check = BatteryCheck(_config, _clock, _queue, _message_factory, Level.INFO) _battery_check.enable() # _battery_check.set_enable_messaging(True) _log.heading('battery check', 'Enabling battery check...', '[3/3]') _clock.enable() while _battery_check.count < 40: time.sleep(0.5) _battery_check.close() # _log.info('consumer received {:d} messages.'.format(_consumer.count)) _log.info('complete.')
def main(argv): try: # read YAML configuration _loader = ConfigLoader(Level.INFO) filename = 'config.yaml' _config = _loader.configure(filename) _message_factory = MessageFactory(Level.INFO) _queue = MessageQueue(_message_factory, Level.INFO) _msg_handler = MessageHandler(Level.INFO) # _tick_handler = TickHandler(_ifs, Level.INFO) # create message bus... _message_bus = MessageBus(Level.INFO) _message_bus.add_handler(Message, _msg_handler.add) # _message_bus.add_handler(Tick, _tick_handler.tick) # _message_bus.add_handler(Tock, _tick_handler.tock) # _message_bus.add_handler(Tick, _tick_handler.add) # _message_bus.add_consumer(_ifs) _clock = Clock(_config, _message_bus, _message_factory, Level.INFO) _ifs = IntegratedFrontSensor(_config, _queue, _clock, _message_bus, _message_factory, Level.INFO) _clock.enable() _ifs.enable() while True: time.sleep(1.0) except KeyboardInterrupt: print(Fore.CYAN + Style.BRIGHT + 'caught Ctrl-C; exiting...') except Exception: print(Fore.RED + Style.BRIGHT + 'error starting ros: {}'.format(traceback.format_exc()) + Style.RESET_ALL)
def __init__(self, opengl=True, width=None, height=None, cols=None, rows=None, tile_size=None, limit_fps=None, window_color=None): self.width = width or SCREEN_WIDTH self.height = height or SCREEN_HEIGHT self.limit_fps = 60 #limit_fps or LIMIT_FPS self.window_color = window_color or WINDOW_COLOR if self.limit_fps == self.DEFAULT_FPS: # print("self.limit_fps == self.DEFAULT_FPS") set_global_fps_modifier(1) else: # 3 Precision # print(str(self.limit_fps)) # print(str(self.DEFAULT_FPS)) set_global_fps_modifier(round(self.DEFAULT_FPS / self.limit_fps, 3)) # global global_fps_modifier # global_fps_modifier = fps_modifier print("FPS LIMIT: " + str(self.limit_fps) + " with modifier " + str(get_global_fps_modifier())) set_screen_dimensions(self.width, self.height) # test = get_global_fps_modifier() # print("HOT HERE: " + str(test)) # raise "stop here" # Initialize with no scene self.scene = None self.show_fps = True # self.show_fps = False if opengl: # No hardware accelerated renderers available, on python 3.7 flags = sdl2.SDL_WINDOW_OPENGL else: flags = sdl2.SDL_RENDERER_SOFTWARE self.window = sdl2.ext.Window("Tiles", size=(self.width, self.height), flags=flags) # Create a renderer that supports hardware-accelerated sprites. # AKA texture_renderer self.renderer = sdl2.ext.Renderer(self.window) # Create a sprite factory that allows us to create visible 2D elements # easily. self.factory = sdl2.ext.SpriteFactory(sdl2.ext.TEXTURE, renderer=self.renderer) # self.text = self.factory.from_text("Unisung Softworks",fontmanager= ARIAL_FONT_WHITE) # Creates a simple rendering system for the Window. The # SpriteRenderSystem can draw Sprite objects on the window. # By default, every Window is hidden, not shown on the screen right # after creation. Thus we need to tell it to be shown now. self.window.show() # SpriteRenderSystem can draw Sprite objects on the window. # TextureSpriteRenderSystem uses SDL_RenderCopy by default, but you can change it to use SDL_RenderCopyEx # Using the TextureSpriteRenderSystem drastically increased the framerate. # self.spriterenderer = self.factory.create_sprite_render_system(self.window) # Switching over to new renderer for sprite rotation. self.spriterenderer = TextureRenderer(self.renderer) # self.software_renderer = SoftwareRenderer(self.window) # Enforce window raising just to be sure. sdl2.SDL_RaiseWindow(self.window.window) # Initialize the keyboard state controller. # PySDL2/SDL2 shouldn't need this but the basic procedure for getting # key mods and locks is not working for me atm. # So I've implemented my own controller. self.kb_state = KeyboardStateController() # Initialize a mouse starting position. From here on the manager will # be able to work on distances from previous positions. self._get_mouse_state() # Initialize a clock utility to help us control the framerate self.clock = Clock() # Make the Manager alive. This is used on the main loop. self.alive = True
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 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.')
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.')
class Manager(): DEFAULT_FPS = 60 # OPENGL = False, makes it full screen. def __init__(self, opengl=True, width=None, height=None, cols=None, rows=None, tile_size=None, limit_fps=None, window_color=None): self.width = width or SCREEN_WIDTH self.height = height or SCREEN_HEIGHT self.limit_fps = 60 #limit_fps or LIMIT_FPS self.window_color = window_color or WINDOW_COLOR if self.limit_fps == self.DEFAULT_FPS: # print("self.limit_fps == self.DEFAULT_FPS") set_global_fps_modifier(1) else: # 3 Precision # print(str(self.limit_fps)) # print(str(self.DEFAULT_FPS)) set_global_fps_modifier(round(self.DEFAULT_FPS / self.limit_fps, 3)) # global global_fps_modifier # global_fps_modifier = fps_modifier print("FPS LIMIT: " + str(self.limit_fps) + " with modifier " + str(get_global_fps_modifier())) set_screen_dimensions(self.width, self.height) # test = get_global_fps_modifier() # print("HOT HERE: " + str(test)) # raise "stop here" # Initialize with no scene self.scene = None self.show_fps = True # self.show_fps = False if opengl: # No hardware accelerated renderers available, on python 3.7 flags = sdl2.SDL_WINDOW_OPENGL else: flags = sdl2.SDL_RENDERER_SOFTWARE self.window = sdl2.ext.Window("Tiles", size=(self.width, self.height), flags=flags) # Create a renderer that supports hardware-accelerated sprites. # AKA texture_renderer self.renderer = sdl2.ext.Renderer(self.window) # Create a sprite factory that allows us to create visible 2D elements # easily. self.factory = sdl2.ext.SpriteFactory(sdl2.ext.TEXTURE, renderer=self.renderer) # self.text = self.factory.from_text("Unisung Softworks",fontmanager= ARIAL_FONT_WHITE) # Creates a simple rendering system for the Window. The # SpriteRenderSystem can draw Sprite objects on the window. # By default, every Window is hidden, not shown on the screen right # after creation. Thus we need to tell it to be shown now. self.window.show() # SpriteRenderSystem can draw Sprite objects on the window. # TextureSpriteRenderSystem uses SDL_RenderCopy by default, but you can change it to use SDL_RenderCopyEx # Using the TextureSpriteRenderSystem drastically increased the framerate. # self.spriterenderer = self.factory.create_sprite_render_system(self.window) # Switching over to new renderer for sprite rotation. self.spriterenderer = TextureRenderer(self.renderer) # self.software_renderer = SoftwareRenderer(self.window) # Enforce window raising just to be sure. sdl2.SDL_RaiseWindow(self.window.window) # Initialize the keyboard state controller. # PySDL2/SDL2 shouldn't need this but the basic procedure for getting # key mods and locks is not working for me atm. # So I've implemented my own controller. self.kb_state = KeyboardStateController() # Initialize a mouse starting position. From here on the manager will # be able to work on distances from previous positions. self._get_mouse_state() # Initialize a clock utility to help us control the framerate self.clock = Clock() # Make the Manager alive. This is used on the main loop. self.alive = True def _get_mouse_state(self): """Get the mouse state. This is only required during initialization. Later on the mouse position will be passed through events. """ # This is an example of what PySDL2, below the hood, does for us. # Here we create a ctypes int (i.e. a C type int) x = ctypes.c_int(0) y = ctypes.c_int(0) # And pass it by reference to the SDL C function (i.e. pointers) sdl2.mouse.SDL_GetMouseState(ctypes.byref(x), ctypes.byref(y)) # The variables were modified by SDL, but are still of C type # So we need to get their values as python integers self._mouse_x = x.value self._mouse_y = y.value # Now we hope we're never going to deal with this kind of stuff again return self._mouse_x, self._mouse_y def run(self): # Calculate our framerate. tick = 0 update_tick = 0 if self.show_fps: time_new = time.time() time_old = time.time() time_track = [] """Main loop handling events and updates.""" while self.alive: tick += 1 if self.show_fps: time_elapsed = time_new - time_old time_track.append(time_elapsed) time_old = time_new time_new = time.time() self.clock.tick(self.limit_fps) self.on_event() self.on_update() if self.show_fps: if len(time_track) == 60: average = sum(time_track) / len(time_track) print("FPS:", int(1 / average)) time_track = [] if self.limit_fps == tick: tick = 0 self.occasional_update() return sdl2.ext.quit() def on_update(self): """Update the active scene.""" if self.alive: self.renderer.clear(self.window_color) self.scene.on_update() self.scene.on_draw() # for text in self.scene.on_draw_text(): # self.renderer.copy(text.value, dstrect = (text.x, text.y, text.value.size[0], text.value.size[1])) # self.spriterenderer.render(sprites = self.scene.on_draw()) self.renderer.present() # Appears to no longer be necessary. # sdl2.timer.SDL_Delay(12) # if self.alive: # # clear the window with its color # self.renderer.clear(self.window_color) # if self.scene: # # call the active scene's on_update # self.scene.on_update() # self.renderer.copy(self.text, dstrect= (0,0,self.text.size[0],self.text.size[1])) # # present what we have to the screen # self.renderer.present() # # self.window.refresh() # # https://pysdl2.readthedocs.io/en/latest/tutorial/pygamers.html#pygame-time # # SDL_Delay(1000//FPS - ((SDL_GetTicks()-starttime))) # # frameTime = sdl2.timer.SDL_GetTicks() - frameStart; # # if frameDelay > frameTime: # # sdl2.timer.SDL_Delay(frameDelay - frameTime); # sdl2.timer.SDL_Delay(100) def occasional_update(self): if self.alive: self.scene.occasional_update() def present(self): """Flip the GPU buffer.""" sdl2.render.SDL_RenderPresent(self.spriterenderer.sdlrenderer) def set_scene(self, scene=None, **kwargs): """Set the scene. Args: scene (SceneBase): the scene to be initialized kwargs: the arguments that should be passed to the scene """ self.scene = scene(manager=self, **kwargs) def quit(self): self.alive = False def get_keyboard_state(self): """ Returns a list with the current SDL keyboard state, which is updated on SDL_PumpEvents. """ numkeys = ctypes.c_int() keystate = sdl2.keyboard.SDL_GetKeyboardState(ctypes.byref(numkeys)) ptr_t = ctypes.POINTER(ctypes.c_uint8 * numkeys.value) return ctypes.cast(keystate, ptr_t)[0] def on_event(self): """Handle the events and pass them to the active scene.""" scene = self.scene if scene is None: return # events = [] # for test in sdl2.ext.get_events(): # events.append(test.key.keysym.sym) # if len(events) > 0: # print("BEING PRESSED") # print(events) # events = [] # test2 = ctypes.c_int(8) # for test in sdl2.SDL_GetKeyboardState(test2): # # print(test) # events.append(test) # if len(events) > 0: # print("BEING PRESSED") # print(events) keystatus = self.get_keyboard_state() scene.key_status(keystatus) # if keystatus[sdl2.SDL_SCANCODE_W]: # print("the w key was pressed") for event in sdl2.ext.get_events(): # print("EVENT LOOP HERE") # Exit events if event.type == sdl2.SDL_QUIT: self.quit() return # Redraw in case the focus was lost and now regained if event.type == sdl2.SDL_WINDOWEVENT_FOCUS_GAINED: self.on_update() continue # on_mouse_motion, on_mouse_drag if event.type == sdl2.SDL_MOUSEMOTION: x = event.motion.x y = event.motion.y buttons = event.motion.state self._mouse_x = x self._mouse_y = y dx = x - self._mouse_x dy = y - self._mouse_y if buttons & sdl2.SDL_BUTTON_LMASK: scene.on_mouse_drag(event, x, y, dx, dy, "LEFT") elif buttons & sdl2.SDL_BUTTON_MMASK: scene.on_mouse_drag(event, x, y, dx, dy, "MIDDLE") elif buttons & sdl2.SDL_BUTTON_RMASK: scene.on_mouse_drag(event, x, y, dx, dy, "RIGHT") else: scene.on_mouse_motion(event, x, y, dx, dy) continue # on_mouse_press elif event.type == sdl2.SDL_MOUSEBUTTONUP: x = event.button.x y = event.button.y button_n = event.button.button button = 'UNDEFINED' if button_n == sdl2.SDL_BUTTON_LEFT: button = "LEFT" elif button_n == sdl2.SDL_BUTTON_RIGHT: button = "RIGHT" elif button_n == sdl2.SDL_BUTTON_MIDDLE: button = "MIDDLE" double = bool(event.button.clicks - 1) scene.on_mouse_release(event, x, y, button, double) elif event.type == sdl2.SDL_MOUSEBUTTONDOWN: x = event.button.x y = event.button.y button_n = event.button.button button = 'UNDEFINED' if button_n == sdl2.SDL_BUTTON_LEFT: button = "LEFT" elif button_n == sdl2.SDL_BUTTON_RIGHT: button = "RIGHT" elif button_n == sdl2.SDL_BUTTON_MIDDLE: button = "MIDDLE" double = bool(event.button.clicks - 1) scene.on_mouse_press(event, x, y, button, double) continue # on_mouse_scroll (wheel) elif event.type == sdl2.SDL_MOUSEWHEEL: offset_x = event.wheel.x offset_y = event.wheel.y scene.on_mouse_scroll(event, offset_x, offset_y) continue # for keyboard input, set the key symbol and keyboard modifiers mod = self.kb_state.process(event) sym = event.key.keysym.sym # on_key_release if event.type == sdl2.SDL_KEYUP: # print("TEST") scene.on_key_release(event, sym, mod) # on_key_press elif event.type == sdl2.SDL_KEYDOWN: scene.on_key_press(event, sym, mod)
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 __init__(self, contacts_file, message, start_hour): self.message = message self.transformers = all_transformers() self.contacts = list(ContactLoader().load(contacts_file)) self.clock = Clock(int(start_hour))
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.')