def __init__(self, config, queue, message_factory, level): ''' Parameters: config: the YAML-based application configuration queue: the message queue to receive messages from this task message_factory: the factory for creating messages mutex: vs godzilla ''' if config is None: raise ValueError('no configuration provided.') self._level = level self._log = Logger("gamepad", level) self._log.info('initialising...') self._config = config _config = config['ros'].get('gamepad') # config _loop_freq_hz = _config.get('loop_freq_hz') self._rate = Rate(_loop_freq_hz) self._device_path = _config.get('device_path') self._queue = queue self._message_factory = message_factory self._gamepad_closed = False self._closed = False self._enabled = False self._thread = None self._gamepad = None
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 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: # 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 __init__(self, config, queue, level): self._log = Logger("bno055", level) if config is None: raise ValueError("no configuration provided.") self._queue = queue self._config = config # config _config = self._config['ros'].get('bno055') self._loop_delay_sec = _config.get('loop_delay_sec') _i2c_device = _config.get('i2c_device') self._pitch_trim = _config.get('pitch_trim') self._roll_trim = _config.get('roll_trim') self._heading_trim = _config.get('heading_trim') self._error_range = 5.0 # permitted error between Euler and Quaternion (in degrees) to allow setting value, was 3.0 self._rate = Rate(_config.get('sample_rate')) # e.g., 20Hz # use `ls /dev/i2c*` to find out what i2c devices are connected self._bno055 = adafruit_bno055.BNO055_I2C( I2C(_i2c_device)) # Device is /dev/i2c-1 # some of the other modes provide accurate compass calibration but are very slow to calibrate # self._bno055.mode = BNO055Mode.COMPASS_MODE.mode # self._bno055.mode = BNO055Mode.NDOF_FMC_OFF_MODE.mode self._counter = itertools.count() self._thread = None self._enabled = False self._closed = False self._heading = 0.0 # we jauntily default at zero so we don't return a None self._pitch = None self._roll = None self._is_calibrated = Calibration.NEVER self._log.info('ready.')
def _read(self): ''' The function that reads sensor values in a loop. This checks to see if the 'sys' calibration is at least 3 (True), and if the Euler and Quaternion values are within an error range of each other, this sets the class variable for heading, pitch and roll. If they aren't within range for more than n polls, the values revert to None. ''' self._log.info('starting sensor read...') _heading = None _pitch = None _roll = None _quat_w = None _quat_x = None _quat_y = None _quat_z = None count = 0 # 2. Absolute Orientation (Quaterion, 100Hz) Four point quaternion output for more accurate data manipulation ................ # grab data at 10Hz rate = Rate(10) while not self._closed: if self._enabled: header = 91 print(Fore.CYAN + ('-'*header) + Style.RESET_ALL) print(Fore.CYAN + "| {:17} | {:20} | {:20} | {:21} |".format("Accels [g's]", " Magnet [uT]", "Gyros [dps]", "Roll, Pitch, Heading") + Style.RESET_ALL) print(Fore.CYAN + ('-'*header) + Style.RESET_ALL) for _ in range(10): a, m, g = self._imu.get() r, p, h = self._imu.getOrientation(a, m) deg = Convert.to_degrees(h) # self._log.info(Fore.GREEN + '| {:>6.1f} {:>6.1f} {:>6.1f} | {:>6.1f} {:>6.1f} {:>6.1f} |'.format(a[0], a[1], a[2], r, p, h) + Style.RESET_ALL) print(Fore.CYAN + '| {:>5.2f} {:>5.2f} {:>5.2f} '.format(a[0], a[1], a[2]) + Fore.YELLOW + '| {:>6.1f} {:>6.1f} {:>6.1f} '.format(m[0], m[1], m[2]) + Fore.MAGENTA + '| {:>6.1f} {:>6.1f} {:>6.1f} '.format(g[0], g[1], g[2]) + Fore.GREEN + '| {:>6.1f} {:>6.1f} {:>6.1f}° |'.format(r, p, deg) + Style.RESET_ALL) time.sleep(0.50) print('-'*header) print(' uT: micro Tesla') print(' g: gravity') print('dps: degrees per second') print('') # accel_x, accel_y, accel_z = self._fxos.accelerometer # m/s^2 # mag_x, mag_y, mag_z = self._fxos.magnetometer # uTesla # gyro_x, gyro_y, gyro_z = self._fxas.gyroscope # radians/s # # print(Fore.MAGENTA + 'acc: ({0:0.3f}, {1:0.3f}, {2:0.3f})'.format(accel_x, accel_y, accel_z)\ # + Fore.YELLOW + ' \tmag: ({0:0.3f}, {1:0.3f}, {2:0.3f})'.format(mag_x, mag_y, mag_z)\ # + Fore.CYAN + ' \tgyr: ({0:0.3f}, {1:0.3f}, {2:0.3f})'.format(gyro_x, gyro_y, gyro_z) + Style.RESET_ALL) rate.wait() else: self._log.info('disabled loop.') time.sleep(10) count += 1 self._log.debug('read loop ended.')
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 __init__(self, config, motor, setpoint=0.0, sample_time=0.01, level=Level.INFO): ''' :param config: The application configuration, read from a YAML file. :param motor: The motor to be controlled. :param setpoint: The initial setpoint or target output :param sample_time: The time in seconds before generating a new output value. This PID is expected to be called at a constant rate. :param level: The log level, e.g., Level.INFO. ''' if motor is None: raise ValueError('null motor argument.') self._motor = motor self._log = Logger('pidc:{}'.format(motor.orientation.label), level) if sys.version_info < (3, 0): self._log.error('PID class requires Python 3.') sys.exit(1) # PID configuration ................................ if config is None: raise ValueError('null configuration argument.') _config = config['ros'].get('motors').get('pid-controller') self._pot_ctrl = _config.get('pot_ctrl') if self._pot_ctrl: self._pot = Potentiometer(config, Level.INFO) self._rate = Rate(_config.get('sample_freq_hz'), level) _sample_time = self._rate.get_period_sec() self._pid = PID(config, self._motor.orientation, sample_time=_sample_time, level=level) self._enable_slew = True #_config.get('enable_slew') self._slewlimiter = SlewLimiter(config, orientation=self._motor.orientation, level=Level.INFO) _slew_rate = SlewRate.NORMAL # TODO _config.get('slew_rate') self._slewlimiter.set_rate_limit(_slew_rate) if self._enable_slew: self._log.info('slew limiter enabled, set to {:7.4f}.'.format( _slew_rate.limit)) self._slewlimiter.enable() else: self._log.info('slew limiter disabled.') self._power = 0.0 self._last_power = 0.0 self._failures = 0 self._enabled = False self._disabling = False self._closed = False self._thread = None # self._last_steps = 0 # self._max_diff_steps = 0 self._log.info('ready.')
def __init__(self, config, motor, setpoint=0.0, sample_time=0.01, level=Level.INFO): ''' :param config: The application configuration, read from a YAML file. :param motor: The motor to be controlled. :param setpoint: The initial setpoint or target output :param sample_time: The time in seconds before generating a new output value. This PID is expected to be called at a constant rate. :param level: The log level, e.g., Level.INFO. ''' if motor is None: raise ValueError('null motor argument.') self._motor = motor self._log = Logger('pid:{}'.format(motor.get_orientation().label), level) if sys.version_info < (3, 0): self._log.error('PID class requires Python 3.') sys.exit(1) # PID configuration ................................ if config is None: raise ValueError('null configuration argument.') _config = config['ros'].get('motors').get('pid') self.kp = _config.get('kp') # proportional gain self.ki = _config.get('ki') # integral gain self.kd = _config.get('kd') # derivative gain self._min_output = _config.get('min_output') self._max_output = _config.get('max_output') self._setpoint = setpoint self._log.info( 'kp:{:7.4f}; ki:{:7.4f}; kd:{:7.4f};\tmin={:>5.2f}; max={:>5.2f}'. format(self._kp, self.ki, self.kd, self._min_output, self._max_output)) if sample_time is None: raise Exception('no sample time argument provided') self._rate = Rate(_config.get('sample_freq_hz'), level) self._sample_time = self._rate.get_period_sec() self._log.info('sample time: {:7.4f} sec'.format(self._sample_time)) self._current_time = time.monotonic # to ensure time deltas are always positive self.reset() # Velocity.CONFIG.configure(_config) self._enable_slew = _config.get('enable_slew') if self._enable_slew: self._slewlimiter = SlewLimiter(config, self._motor.get_orientation(), Level.INFO) _slew_rate = SlewRate.NORMAL # TODO _config.get('slew_rate') self._slewlimiter.set_rate_limit(_slew_rate) self._failures = 0 self._enabled = False self._closed = False self._thread = None # self._last_steps = 0 # self._max_diff_steps = 0 self._log.info('ready.')
def _monitor(self, f_is_enabled): ''' A 20Hz loop that prints PID statistics to the log while enabled. Note that this loop is not synchronised with the two PID controllers, which each have their own loop. ''' _indent = ' ' self._log.info('PID monitor header:\n' \ + _indent + 'name : description\n' \ + _indent + 'kp : proportional constant\n' \ + _indent + 'ki : integral constant\n' \ + _indent + 'kd : derivative constant\n' \ + _indent + 'p_cp : port proportial value\n' \ + _indent + 'p_ci : port integral value\n' \ + _indent + 'p_cd : port derivative value\n' \ + _indent + 'p_lpw : port last power\n' \ + _indent + 'p_cpw : port current motor power\n' \ + _indent + 'p_spwr : port set power\n' \ + _indent + 'p_cvel : port current velocity\n' \ + _indent + 'p_stpt : port velocity setpoint\n' \ + _indent + 's_cp : starboard proportial value\n' \ + _indent + 's_ci : starboard integral value\n' \ + _indent + 's_cd : starboard derivative value\n' \ + _indent + 's_lpw : starboard proportional value\n' \ + _indent + 's_cpw : starboard integral value\n' \ + _indent + 's_spw : starboard derivative value\n' \ + _indent + 's_cvel : starboard current velocity\n' \ + _indent + 's_stpt : starboard velocity setpoint\n' \ + _indent + 'p_stps : port encoder steps\n' \ + _indent + 's_stps : starboard encoder steps') if self._log_to_file: self._file_log = Logger("pid", Level.INFO, log_to_file=True) # write header self._file_log.file( "kp|ki|kd|p_cp|p_ci|p_cd|p_lpw|p_cpw|p_spwr|p_cvel|p_stpt|s_cp|s_ci|s_cd|s_lpw|s_cpw|s_spw|s_cvel|s_stpt|p_stps|s_stps|" ) self._log.info(Fore.GREEN + 'starting PID monitor...') _rate = Rate(20) while f_is_enabled(): kp, ki, kd, p_cp, p_ci, p_cd, p_last_power, p_current_motor_power, p_power, p_current_velocity, p_setpoint, p_steps = self._port_pid.stats _x, _y, _z, s_cp, s_ci, s_cd, s_last_power, s_current_motor_power, s_power, s_current_velocity, s_setpoint, s_steps = self._stbd_pid.stats _msg = ('{:7.4f}|{:7.4f}|{:7.4f}|{:7.4f}|{:7.4f}|{:7.4f}|{:5.2f}|{:5.2f}|{:5.2f}|{:<5.2f}|{:>5.2f}|{:d}|{:7.4f}|{:7.4f}|{:7.4f}|{:5.2f}|{:5.2f}|{:5.2f}|{:<5.2f}|{:>5.2f}|{:d}|').format(\ kp, ki, kd, p_cp, p_ci, p_cd, p_last_power, p_current_motor_power, p_power, p_current_velocity, p_setpoint, p_steps, \ s_cp, s_ci, s_cd, s_last_power, s_current_motor_power, s_power, s_current_velocity, s_setpoint, s_steps) _p_hilite = Style.BRIGHT if p_power > 0.0 else Style.NORMAL _s_hilite = Style.BRIGHT if s_power > 0.0 else Style.NORMAL _msg2 = (Fore.RED+_p_hilite+'{:7.4f}|{:7.4f}|{:7.4f}|{:<5.2f}|{:<5.2f}|{:<5.2f}|{:>5.2f}|{:d}'+Fore.GREEN+_s_hilite+'|{:7.4f}|{:7.4f}|{:7.4f}|{:5.2f}|{:5.2f}|{:<5.2f}|{:<5.2f}|{:d}|').format(\ p_cp, p_ci, p_cd, p_last_power, p_power, p_current_velocity, p_setpoint, p_steps, \ s_cp, s_ci, s_cd, s_last_power, s_power, s_current_velocity, s_setpoint, s_steps) _rate.wait() if self._log_to_file: self._file_log.file(_msg) if self._log_to_console: self._log.info(_msg2) self._log.info('PID monitor stopped.')
def __init__(self, loop_freq_hz, callback, level=Level.INFO): super().__init__() self._log = Logger("clock", level) self._loop_freq_hz = loop_freq_hz self._rate = Rate(self._loop_freq_hz) self._log.info('tick frequency: {:d}Hz'.format(self._loop_freq_hz)) self._callbacks = [] self._thread = None self._enabled = False self._closed = False self._log.info('ready.')
def capture(self): if not self._enabled: raise Exception('not enabled.') self._log.info(Fore.GREEN + Style.BRIGHT + 'snapshot start...') _rate = Rate(3) # 10Hz self._capturing = True while self._capturing == True: # wait til we're done self._log.info(Fore.BLACK + '...') _rate.wait() _location = self._location self._log.info(Fore.GREEN + Style.BRIGHT + 'snapshot complete; location: '.format(self._location))
def __init__(self, config, motor, level): self._motor = motor self._orientation = motor.get_orientation() self._log = Logger('pid:{}'.format(self._orientation.label), level) # PID configuration ................................ if config is None: raise ValueError('null configuration argument.') _config = config['ros'].get('motors').get('pid') self._enable_slew = _config.get('enable_slew') if self._enable_slew: self._slewlimiter = SlewLimiter(config, self._orientation, Level.INFO) # geometry configuration ........................... _geo_config = config['ros'].get('geometry') self._wheel_diameter = _geo_config.get('wheel_diameter') self._wheelbase = _geo_config.get('wheelbase') self._step_per_rotation = _geo_config.get('steps_per_rotation') _clip_max = _config.get('clip_limit') _clip_min = -1.0 * _clip_max self._clip = lambda n: _clip_min if n <= _clip_min else _clip_max if n >= _clip_max else n self._enable_p = _config.get('enable_p') self._enable_i = _config.get('enable_i') self._enable_d = _config.get('enable_d') _kp = _config.get('kp') _ki = _config.get('ki') _kd = _config.get('kd') self.set_tuning(_kp, _ki, _kd) self._rate = Rate( _config.get('sample_freq_hz')) # default sample rate is 20Hz # self._millis = lambda: int(round(time.time() * 1000)) self._counter = itertools.count() self._last_error = 0.0 self._sum_errors = 0.0 self._step_limit = 0 self._stats_queue = None self._filewriter = None # optional: for statistics self._filewriter_closed = False _wheel_circumference = math.pi * self._wheel_diameter # mm self._steps_per_m = 1000.0 * self._step_per_rotation / _wheel_circumference self._log.info(Fore.BLUE + Style.BRIGHT + '{:5.2f} steps per m.'.format(self._steps_per_m)) self._steps_per_cm = 10.0 * self._step_per_rotation / _wheel_circumference self._log.info(Fore.BLUE + Style.BRIGHT + '{:5.2f} steps per cm.'.format(self._steps_per_cm)) self._log.info('ready.')
def test_selector(): _log = Logger("sel-test", Level.INFO) # _i2c_scanner = I2CScanner(Level.WARN) # if not _i2c_scanner.has_address([0x0F]): # _log.warning('test ignored: no rotary encoder found.') # return _selector = None try: # read YAML configuration _loader = ConfigLoader(Level.INFO) filename = 'config.yaml' _config = _loader.configure(filename) _selector = Selector(_config, Level.INFO) _count = 0 _updates = 0 _rate = Rate(20) _last_value = 0 _limit = 45 _log.info('live-updating from rotary encoder...') while _updates < _limit: _value = _selector.read() if _value != _last_value: _updates += 1 _log.info(Style.BRIGHT + 'returned value: {:d}; updates remaining: {:d}'. format(_value, _limit - _updates)) else: _log.info(Style.DIM + 'returned value: {:d}; updates remaining: {:d}'. format(_value, _limit - _updates)) _last_value = _value _count += 1 if _count % 33 == 0: _log.info('waiting…') _rate.wait() finally: if _selector: _selector.reset()
def _loop(self, _enabled): self._log.info(Fore.RED + 'start _loop...') # TEMP _rate = Rate(1) # 1Hz _thread_count = 4 try: # summarise elements on a column basis ......... self._log.info('starting image capture...') with picamera.PiCamera() as camera: camera.resolution = (self._width, self._height) camera.iso = 800 camera.exposure_mode = 'auto' camera.framerate = 15 # https://picamera.readthedocs.io/en/release-1.13/_modules/picamera/array.html#PiRGBArray camera.start_preview() # time.sleep(2) self._log.info(Style.BRIGHT + '1. creating processor pool...') self._pool = [ImageProcessor(self._config, camera, i, _enabled) for i in range(_thread_count)] self._log.info(Style.BRIGHT + '2. calling camera.capture_continuous...') # size = ( self._width, self._height ) # camera.capture_continuous(self.outputs, format='rgb', resize=size, use_video_port=True) # camera.capture_sequence(self.outputs, format='rgb', resize=size, use_video_port=True) # camera.capture_sequence(self.outputs, format='rgb', use_video_port=True) camera.capture_sequence(self.outputs, format='rgb', use_video_port=True) self._log.info(Style.BRIGHT + '3. called camera.capture_continuous...') time.sleep(3) _rate.wait() # self._close_pool() self._log.info('end _loop.') # TEMP except picamera.PiCameraMMALError: self._log.error('could not get camera: in use by another process.') except Exception as e: self._log.error('error in blob loop: {}'.format(e)) self._log.error('traceback in blob loop: {}'.format(traceback.format_exc())) finally: self._log.set_suppress(False) self._motors.set_led_show_battery(True) self._log.info('finis.')
def test_rot_encoder(): _log = Logger("rot-test", Level.INFO) _i2c_scanner = I2CScanner(Level.WARN) if not _i2c_scanner.has_address([0x16]): _log.warning('test ignored: no rotary encoder found.') return _rot = None try: # read YAML configuration _loader = ConfigLoader(Level.INFO) filename = 'config.yaml' _config = _loader.configure(filename) _rot = RotaryEncoder(_config, 0x0F, Level.INFO) _count = 0 _updates = 0 _update_led = True _last_value = 0 _rate = Rate(20) _log.info(Fore.WHITE + Style.BRIGHT + 'waiting for rotary encoder to make 10 ticks...') while _updates < 10: # _value = _rot.update() # original method _value = _rot.read(_update_led) # improved method if _value != _last_value: _log.info(Style.BRIGHT + 'returned value: {:d}'.format(_value)) _updates += 1 _last_value = _value _count += 1 if _count % 33 == 0: _log.info(Fore.BLACK + Style.BRIGHT + 'waiting…') _rate.wait() finally: if _rot: _log.info('resetting rotary encoder...') _rot.reset()
def test_pot(): ''' Tests reading the wiper of a potentiometer attached to a GPIO pin, where its other two connections are to Vcc and ground. ''' _loader = ConfigLoader(Level.INFO) filename = 'config.yaml' _config = _loader.configure(filename) _pot = Potentiometer(_config, Level.DEBUG) _value = 0 # start... _rate = Rate(20) # Hz # i = 0 # while True: for i in range(20): _value = _pot.get_value() _scaled_value = _pot.get_scaled_value() _log.info('[{:d}] analog value: {:03d};'.format(i, _value) + Fore.GREEN + '\tscaled: {:>7.4f};'.format(_scaled_value)) _rate.wait()
def test_ioe_potentiometer(): _log = Logger("test-ioe-pot", Level.INFO) _log.info(Fore.RED + 'to kill type Ctrl-C') # read YAML configuration _loader = ConfigLoader(Level.INFO) _config = _loader.configure('config.yaml') _pot = Potentiometer(_config, Level.INFO) _pot.set_output_limits(0.00, 0.150) _log.info('starting test...') _hz = 10 _rate = Rate(_hz, Level.ERROR) while True: # _value = self.get_value() # self.set_rgb(_value) # _scaled_value = self.scale_value() # as float _scaled_value = _pot.get_scaled_value(True) # _scaled_value = math.floor(self.scale_value()) # as integer _log.info(Fore.YELLOW + 'scaled value: {:9.6f}'.format(_scaled_value)) _rate.wait()
def main(argv): try: # read YAML configuration _loader = ConfigLoader(Level.INFO) filename = 'config.yaml' _config = _loader.configure(filename) _pot = Potentiometer(_config, Level.INFO) # start... _rate = Rate(20) # Hz while True: _value = _pot.get_value() _scaled_value = _pot.get_scaled_value() _log.info(' analog value: {:03d};'.format(_value) + Fore.GREEN + '\tscaled: {:>7.4f};'.format(_scaled_value)) _rate.wait() except KeyboardInterrupt: _log.info('caught Ctrl-C; exiting...') except Exception: _log.error('error starting potentiometer: {}'.format(traceback.format_exc()))
def __init__(self, config, clock, level): if config is None: raise ValueError('no configuration provided.') if clock is None: raise ValueError('no clock provided.') self._clock = clock self._message_bus = clock.message_bus self._message_factory = clock.message_factory self._log = Logger("ifs", level) self._log.info('configuring integrated front sensor...') self._config = config['ros'].get('integrated_front_sensor') self._ignore_duplicates = self._config.get('ignore_duplicates') _use_pot = self._config.get('use_potentiometer') self._pot = Potentiometer(config, Level.INFO) if _use_pot else None self._loop_freq_hz = self._config.get('loop_freq_hz') self._rate = Rate(self._loop_freq_hz) # event thresholds: self._cntr_raw_min_trigger = self._config.get('cntr_raw_min_trigger') self._oblq_raw_min_trigger = self._config.get('oblq_raw_min_trigger') self._side_raw_min_trigger = self._config.get('side_raw_min_trigger') self._cntr_trigger_distance_cm = self._config.get( 'cntr_trigger_distance_cm') self._oblq_trigger_distance_cm = self._config.get( 'oblq_trigger_distance_cm') self._side_trigger_distance_cm = self._config.get( 'side_trigger_distance_cm') self._log.info('event thresholds: \t' \ + Fore.RED + ' port side={:>5.2f}; port={:>5.2f};'.format(self._side_trigger_distance_cm, self._oblq_trigger_distance_cm) \ + Fore.BLUE + ' center={:>5.2f};'.format(self._cntr_trigger_distance_cm) \ + Fore.GREEN + ' stbd={:>5.2f}; stbd side={:>5.2f}'.format(self._oblq_trigger_distance_cm, self._side_trigger_distance_cm)) # hardware pin assignments are defined in IO Expander # create/configure IO Expander self._ioe = IoExpander(config, Level.INFO) # these are used to support running averages _queue_limit = 2 # larger number means it takes longer to change self._deque_cntr = Deque([], maxlen=_queue_limit) self._deque_port = Deque([], maxlen=_queue_limit) self._deque_stbd = Deque([], maxlen=_queue_limit) self._deque_port_side = Deque([], maxlen=_queue_limit) self._deque_stbd_side = Deque([], maxlen=_queue_limit) self._counter = itertools.count() self._thread = None self._group = 0 self._enabled = False self._suppressed = False self._closed = False self._count = 0 self._log.info('ready.')
def _cruise(self, pid, cruising_velocity, f_is_enabled): ''' Threaded method for roaming. ''' _rate = Rate(20) if not pid.enabled: pid.enable() pid.set_slew_rate(SlewRate.NORMAL) pid.enable_slew(True) self._cruising_velocity = cruising_velocity self._log.info(Fore.YELLOW + '{} motor accelerating to velocity: {:5.2f}...'.format( pid.orientation.label, self._cruising_velocity)) pid.velocity = self._cruising_velocity while pid.velocity < self._cruising_velocity: pid.velocity += 5.0 self._log.info(Fore.GREEN + 'accelerating from {} to {}...'.format( pid.velocity, self._cruising_velocity)) time.sleep(0.5) # _rate.wait() self._log.info(Fore.YELLOW + '{} motor cruising...'.format(pid.orientation.label)) while f_is_enabled: self._log.info(Fore.BLACK + '{} motor cruising at velocity: {:5.2f}...'.format( pid.orientation.label, pid.velocity)) time.sleep(0.5) self._log.info( Fore.YELLOW + '{} motor decelerating...'.format(pid.orientation.label)) pid.velocity = 0.0 while pid.velocity > 0.0: pid.velocity -= 5.0 self._log.info(Fore.GREEN + 'decelerating from {} to {}...'.format( pid.velocity, self._cruising_velocity)) time.sleep(0.01) # _rate.wait() self._log.info(Fore.YELLOW + '{} motor at rest.'.format(pid.orientation.label)) time.sleep(1.0) pid.set_slew_rate(SlewRate.NORMAL) self._log.info(Fore.YELLOW + 'cruise complete.')
def main(): _log = Logger('main', Level.INFO) _rate = Rate(20) # Hz _loader = ConfigLoader(Level.INFO) filename = 'config.yaml' _config = _loader.configure(filename) _pot = Potentiometer(_config, Level.INFO) # _pot.set_out_max(3.0) try: # motors ............................................................... _motors = Motors(_config, None, Level.INFO) _port_motor = _motors.get_motor(Orientation.PORT) _stbd_motor = _motors.get_motor(Orientation.STBD) # _motor = _stbd_motor # pid .................................................................. _pid_config = _config['ros'].get('motors').get('pid') _target_velocity = 0.0 _port_pid = PID(_config, setpoint=_target_velocity, sample_time=_rate.get_period_sec(), level=Level.INFO) _stbd_pid = PID(_config, setpoint=_target_velocity, sample_time=_rate.get_period_sec(), level=Level.INFO) # _stbd_pid.auto_mode = True # _stbd_pid.proportional_on_measurement = True # _stbd_pid.set_auto_mode(False) # _stbd_pid.tunings = ( _kp, _ki, _kd ) # _stbd_pid.output_limits = ( -10.0, 10.0 ) # _stbd_pid.sample_time = _rate.get_period_sec() # _log.info(Fore.GREEN + 'sample time: {:>6.3f} sec.'.format(_stbd_pid.sample_time)) # _clip_max = 1.0 # _clip_min = -1.0 * _clip_max # _clip = lambda n: _clip_min if n <= _clip_min else _clip_max if n >= _clip_max else n # _limiter = SlewLimiter(_config, None, Level.WARN) # _limiter.enable() # _stbd_pid.setpoint = _target_velocity _kp = 0.0 _ki = 0.0 _kd = 0.0 _power = 0.0 _limit = -1 _count = -1 try: while True: _count = 0 _counter = itertools.count() while _limit < 0 or _count < _limit: _count = next(_counter) _pot_value = _pot.get_scaled_value() _target_velocity = _pot_value _port_pid.setpoint = _target_velocity # _port_pid.tunings = ( _kp, _ki, _kd ) _last_power = _port_motor.get_current_power_level() _current_velocity = _port_motor.get_velocity() _power += _port_pid(_current_velocity) / 100.0 _set_power = _power / 100.0 _port_motor.set_motor_power(_power) # ......................... kp, ki, kd = _port_pid.constants print(Fore.RED + '{:d}\tPID:{:6.3f}|{:6.3f}|{:6.3f};\tLAST={:>7.4f}\tSET={:>7.4f}; \tvel: {:>6.3f}/{:>6.3f}'.format(\ _count, kp, ki, kd, _last_power, _power, _current_velocity, _target_velocity) + Style.RESET_ALL) _stbd_pid.setpoint = _target_velocity # _stbd_pid.tunings = ( _kp, _ki, _kd ) _last_power = _stbd_motor.get_current_power_level() _current_velocity = _stbd_motor.get_velocity() _power += _stbd_pid(_current_velocity) / 100.0 # _set_power = _power / 100.0 _stbd_motor.set_motor_power(_power) # ......................... kp, ki, kd = _stbd_pid.constants print(Fore.GREEN + '{:d}\tPID:{:6.3f}|{:6.3f}|{:6.3f};\tLAST={:>7.4f}\tSET={:>7.4f}; \tvel: {:>6.3f}/{:>6.3f}'.format(\ _count, kp, ki, kd, _last_power, _power, _current_velocity, _target_velocity) + Style.RESET_ALL) _rate.wait() _motors.brake() time.sleep(1.0) except KeyboardInterrupt: _log.info(Fore.CYAN + Style.BRIGHT + 'B. motor test complete.') finally: _motors.brake() ''' Kpro = 0.380 Kdrv = 0.0 last_proportional_error = 0.0 power = 0.0 while True: Kdrv = _pot.get_scaled_value() _current_velocity = _motor.get_velocity() proportional_error = _target_velocity - _current_velocity derivative_error = proportional_error - last_proportional_error last_proportional_error = proportional_error power += (proportional_error * Kpro) + (derivative_error * Kdrv) _motor.set_motor_power( power / 100.0 ) _log.info(Fore.GREEN + ' P:{:6.3f}; D:{:6.3f};'.format(Kpro, Kdrv) \ + Fore.YELLOW + ' power: {:>8.5f};'.format(power) \ + Fore.MAGENTA + ' velocity: {:>6.3f}/{:>6.3f}'.format(_current_velocity, _target_velocity)) _rate.wait() ''' except Exception as e: _log.info(Fore.RED + Style.BRIGHT + 'error in PID controller: {}'.format(e)) traceback.print_exc(file=sys.stdout) finally: _log.info(Fore.YELLOW + Style.BRIGHT + 'C. finally.')
def main(): _level = Level.INFO _log = Logger('main', _level) _loader = ConfigLoader(_level) _config = _loader.configure('config.yaml') try: _motors = Motors(_config, None, Level.WARN) _pot = Potentiometer(_config, Level.WARN) _pot.set_output_limits(0.0, 127.0) # motor configuration: starboard, port or both? _orientation = Orientation.BOTH if _orientation == Orientation.BOTH or _orientation == Orientation.PORT: _port_motor = _motors.get_motor(Orientation.PORT) _port_pid = PIDController(_config, _port_motor, level=Level.WARN) _port_pid.enable() if _orientation == Orientation.BOTH or _orientation == Orientation.STBD: _stbd_motor = _motors.get_motor(Orientation.STBD) _stbd_pid = PIDController(_config, _stbd_motor, level=Level.WARN) _stbd_pid.enable() ROTATE = True _rotate = -1.0 if ROTATE else 1.0 # sys.exit(0) _stbd_velocity = 0.0 _port_velocity = 0.0 _step = 0.5 _min = 0.0 _max = 70.0 _rate = Rate(10) try: # for _value in numpy.arange(_min, _max, _step): while True: # update RGB LED # _pot.set_rgb(_pot.get_value()) _value = 127.0 - _pot.get_scaled_value(True) if _value > 125.0: _value = 127.0 _velocity = Gamepad.convert_range(_value) if _orientation == Orientation.BOTH or _orientation == Orientation.PORT: _port_pid.setpoint = _velocity * 100.0 _port_velocity = _port_pid.setpoint if _orientation == Orientation.BOTH or _orientation == Orientation.STBD: _stbd_pid.setpoint = _rotate * _velocity * 100.0 _stbd_velocity = _stbd_pid.setpoint _log.info(Fore.WHITE + 'value: {:<5.2f}; set-point: {:5.2f}; velocity: '.format(_value, _velocity) \ + Fore.RED + ' port: {:5.2f}\t'.format(_port_velocity) + Fore.GREEN + ' stbd: {:5.2f}'.format(_stbd_velocity)) _rate.wait() _log.info(Fore.YELLOW + 'resting...') time.sleep(10.0) # for _value in numpy.arange(_min, _max, _step): while True: # update RGB LED # _pot.set_rgb(_pot.get_value()) _value = 127.0 - _pot.get_scaled_value(True) if _value > 125.0: _value = 127.0 _velocity = Gamepad.convert_range(_value) if _orientation == Orientation.BOTH or _orientation == Orientation.PORT: _port_pid.setpoint = _rotate * _velocity * 100.0 _port_velocity = _port_pid.setpoint if _orientation == Orientation.BOTH or _orientation == Orientation.STBD: _stbd_pid.setpoint = _velocity * 100.0 _stbd_velocity = _stbd_pid.setpoint _log.info(Fore.MAGENTA + 'value: {:<5.2f}; set-point: {:5.2f}; velocity: '.format(_value, _velocity) \ + Fore.RED + ' port: {:5.2f}\t'.format(_port_velocity) + Fore.GREEN + ' stbd: {:5.2f}'.format(_stbd_velocity)) _rate.wait() _log.info(Fore.YELLOW + 'end of cruising.') except KeyboardInterrupt: _log.info(Fore.CYAN + Style.BRIGHT + 'PID test complete.') finally: if _orientation == Orientation.BOTH or _orientation == Orientation.PORT: _port_pid.disable() if _orientation == Orientation.BOTH or _orientation == Orientation.STBD: _stbd_pid.disable() _motors.brake() except Exception as e: _log.info(Fore.RED + Style.BRIGHT + 'error in PID controller: {}\n{}'.format(e, traceback.format_exc())) finally: _log.info(Fore.YELLOW + Style.BRIGHT + 'C. finally.')
def _handbuilt_imu(self): ''' The function that reads sensor values in a loop. This checks to see if the 'sys' calibration is at least 3 (True), and if the Euler and Quaternion values are within an error range of each other, this sets the class variable for heading, pitch and roll. If they aren't within range for more than n polls, the values revert to None. ''' self._log.info('starting sensor read...') _heading = None _pitch = None _roll = None _quat_w = None _quat_x = None _quat_y = None _quat_z = None count = 0 # 2. Absolute Orientation (Quaterion, 100Hz) Four point quaternion output for more accurate data manipulation ................ # grab data at 20Hz _rate = Rate(1) header = 90 print('-' * header) # print("| {:17} | {:20} |".format("Accels [g's]", " IMU Accels")) print("| {:17} |".format("Mag [xyz]")) while not self._closed: if self._enabled: accel_x, accel_y, accel_z = self._fxos.accelerometer # m/s^2 gyro_x, gyro_y, gyro_z = self._fxas.gyroscope # radians/s a, m, g = self._imu.get() mag_x, mag_y, mag_z = self._fxos.magnetometer # uTesla mag_x_g = m[0] * 100.0 mag_y_g = m[1] * 100.0 mag_z_g = m[2] * 100.0 # rate_gyro_x_dps = Convert.rps_to_dps(gyro_x) # rate_gyro_y_dps = Convert.rps_to_dps(gyro_y) # rate_gyro_z_dps = Convert.rps_to_dps(gyro_z) # mag_x_g = mag_x * 100.0 # mag_y_g = mag_y * 100.0 # mag_z_g = mag_z * 100.0 heading = 180.0 * math.atan2(mag_y_g, mag_x_g) / math.pi print(Fore.CYAN + '| x{:>6.3f} y{:>6.3f} z{:>6.3f} |'.format( mag_x_g, mag_y_g, mag_z_g) \ + Style.BRIGHT + '\t{:>5.2f}'.format(heading) + Style.RESET_ALL) # a, m, g = self._imu.get() # print(Fore.CYAN + '| {:>6.3f} {:>6.3f} {:>6.3f} | {:>6.3f} {:>6.3f} {:>6.3f} |'.format( accel_x, accel_y, accel_z, a[0], a[1], a[2]) + Style.RESET_ALL) _rate.wait() # + Style.BRIGHT + ' {:>6.1f} {:>6.1f} {:>6.1f}° |'.format(r, p, deg) + Style.RESET_ALL) # time.sleep(0.50) # mag_x, mag_y, mag_z = self._fxos.magnetometer # uTesla # gyro_x, gyro_y, gyro_z = self._fxas.gyroscope # radians/s # header = 90 # print('-'*header) # print("| {:17} | {:20} | {:20} | {:20} |".format("Accels [g's]", " Magnet [uT]", "Gyros [dps]", "Roll, Pitch, Heading")) # print('-'*header) # for _ in range(10): # a, m, g = self._imu.get() # r, p, h = self._imu.getOrientation(a, m) # deg = Convert.to_degrees(h) # print(Fore.CYAN + '| {:>5.2f} {:>5.2f} {:>5.2f} | {:>6.1f} {:>6.1f} {:>6.1f} | {:>6.1f} {:>6.1f} {:>6.1f} |'.format( # a[0], a[1], a[2], # m[0], m[1], m[2], # g[0], g[1], g[2]) # + Style.BRIGHT + ' {:>6.1f} {:>6.1f} {:>6.1f}° |'.format(r, p, deg) + Style.RESET_ALL) # time.sleep(0.50) # print('-'*header) # print(' uT: micro Tesla') # print(' g: gravity') # print('dps: degrees per second') # print('') # # print(Fore.MAGENTA + 'acc: ({0:0.3f}, {1:0.3f}, {2:0.3f})'.format(accel_x, accel_y, accel_z)\ # + Fore.YELLOW + ' \tmag: ({0:0.3f}, {1:0.3f}, {2:0.3f})'.format(mag_x, mag_y, mag_z)\ # + Fore.CYAN + ' \tgyr: ({0:0.3f}, {1:0.3f}, {2:0.3f})'.format(gyro_x, gyro_y, gyro_z) + Style.RESET_ALL) else: self._log.info('disabled loop.') time.sleep(10) count += 1 self._log.debug('read loop ended.')
def _one_meter(self, pid, callback): ''' Threaded method for one_meter, one for each PID controller. This will enabled the PID if not already enabled. ''' _current_steps = pid.steps # get this quickly _rate = Rate(20) if not pid.enabled: pid.enable() pid.set_slew_rate(SlewRate.SLOWER) pid.enable_slew(True) # self._geo.steps_for_distance_cm _distance_cm = 200.0 # should be 1m _target_step_count = _distance_cm * self._geo.steps_per_cm _target_steps = round(_current_steps + _target_step_count) _closing_target_steps = _target_steps - self._geo.steps_per_rotation # last wheel rotation _final_target_step_count = _target_steps - ( 1 * self._geo.steps_per_rotation) _proposed_accel_range_cm = _distance_cm / 4.0 if _proposed_accel_range_cm * 2.0 >= self._accel_range_cm: # is the proposed range greater than the standard range? # then we use standard acceleration/deceleration self._log.info( 'using standard accel/decel range (compressed: {:5.2f}cm; standard: {:5.2f}cm)' .format(_proposed_accel_range_cm, self._accel_range_cm)) _accel_range_cm = self._accel_range_cm else: # otherwise compress to just one fourth of distance self._log.info( 'using compressed accel/decel range (compressed: {:5.2f}cm; standard: {:5.2f}cm)' .format(_proposed_accel_range_cm, self._accel_range_cm)) _accel_range_cm = _proposed_accel_range_cm _accel_range_steps = round(_accel_range_cm * self._geo.steps_per_cm) self._log.info( 'using accel/decel range of {:5.2f}cm, or {:d} steps.'.format( _accel_range_cm, _accel_range_steps)) _accel_target_steps = _current_steps + _accel_range_steps # accelerate til this step count _decel_target_steps = _target_steps - _accel_range_steps # step count when we begin to decelerate self._log.info(Fore.YELLOW + 'begin one meter travel for {} motor accelerating from {:d} to {:d} steps, then cruise until {:d} steps, when we decelerate to {:d} steps and halt.'.format(\ pid.orientation.label, _current_steps, _accel_target_steps, _decel_target_steps, _target_steps)) _actually_do_this = True if _actually_do_this: # _accel_velocity = 0 # accelerate to cruising velocity ............................................ # self._rgbmatrix.show_color(Color.CYAN, Orientation.STBD) self._log.info( Fore.YELLOW + '{} motor accelerating...'.format(pid.orientation.label)) while pid.steps < _accel_target_steps: pid.velocity = self._cruising_velocity _rate.wait() # cruise until 3/4 of range .................................................. self._log.info(Fore.YELLOW + '{} motor reached cruising velocity...'.format( pid.orientation.label)) # self._rgbmatrix.show_color(Color.GREEN, Orientation.STBD) pid.velocity = self._cruising_velocity while pid.steps < _decel_target_steps: # ..................................... _rate.wait() # slow down until we reach 9/10 of range # self._rgbmatrix.show_color(Color.YELLOW, Orientation.STBD) pid.velocity = round( (self._cruising_velocity + self._targeting_velocity) / 2.0) while pid.steps < _decel_target_steps: # ..................................... _rate.wait() # self._rgbmatrix.show_color(Color.ORANGE, Orientation.STBD) self._log.info( Fore.YELLOW + '{} motor reached 9/10 of target, decelerating to targeting velocity...' .format(pid.orientation.label)) pid.set_slew_rate(SlewRate.NORMAL) # decelerate to targeting velocity when we get to one wheel revo of target ... pid.velocity = self._targeting_velocity while pid.steps < _closing_target_steps: # ................................... _rate.wait() # self._rgbmatrix.show_color(Color.RED, Orientation.STBD) pid.velocity = self._targeting_velocity while pid.steps < _target_steps: # ........................................... # self._log.info(Fore.YELLOW + Style.DIM + '{:d} steps...'.format(pid.steps)) time.sleep(0.001) pid.velocity = 0.0 # self._rgbmatrix.show_color(Color.BLACK, Orientation.STBD) self._log.info( Fore.YELLOW + '{} motor stopping...'.format(pid.orientation.label)) time.sleep(1.0) pid.set_slew_rate(SlewRate.NORMAL) self._log.info( Fore.YELLOW + 'executing {} callback...'.format(pid.orientation.label)) callback() self._rgbmatrix.disable() # pid.reset() # pid.disable() self._log.info(Fore.YELLOW + 'one meter on {} motor complete: {:d} of {:d} steps.'. format(pid.orientation.label, pid.steps, _target_steps))
class PIDController(object): ''' Provides a configurable PID motor controller via a threaded fixed-time loop. If the 'ros:motors:pid:enable_slew' property is True, this will set a slew limiter on the velocity setter. ''' def __init__(self, config, motor, setpoint=0.0, sample_time=0.01, level=Level.INFO): ''' :param config: The application configuration, read from a YAML file. :param motor: The motor to be controlled. :param setpoint: The initial setpoint or target output :param sample_time: The time in seconds before generating a new output value. This PID is expected to be called at a constant rate. :param level: The log level, e.g., Level.INFO. ''' if motor is None: raise ValueError('null motor argument.') self._motor = motor self._log = Logger('pidc:{}'.format(motor.orientation.label), level) if sys.version_info < (3, 0): self._log.error('PID class requires Python 3.') sys.exit(1) # PID configuration ................................ if config is None: raise ValueError('null configuration argument.') _config = config['ros'].get('motors').get('pid-controller') self._pot_ctrl = _config.get('pot_ctrl') if self._pot_ctrl: self._pot = Potentiometer(config, Level.INFO) self._rate = Rate(_config.get('sample_freq_hz'), level) _sample_time = self._rate.get_period_sec() self._pid = PID(config, self._motor.orientation, sample_time=_sample_time, level=level) self._enable_slew = True #_config.get('enable_slew') self._slewlimiter = SlewLimiter(config, orientation=self._motor.orientation, level=Level.INFO) _slew_rate = SlewRate.NORMAL # TODO _config.get('slew_rate') self._slewlimiter.set_rate_limit(_slew_rate) if self._enable_slew: self._log.info('slew limiter enabled, set to {:7.4f}.'.format( _slew_rate.limit)) self._slewlimiter.enable() else: self._log.info('slew limiter disabled.') self._power = 0.0 self._last_power = 0.0 self._failures = 0 self._enabled = False self._disabling = False self._closed = False self._thread = None # self._last_steps = 0 # self._max_diff_steps = 0 self._log.info('ready.') # .......................................................................... def reset(self): self._pid.velocity = 0.0 self._pid.reset() # self._motor.reset_steps() self._motor.stop() self._log.info(Fore.GREEN + 'reset.') # .......................................................................... @property def orientation(self): return self._motor.orientation # .............................................................................. def enable_slew(self, enable): _old_value = self._enable_slew if not self._slewlimiter.is_enabled(): self._slewlimiter.enable() self._enable_slew = enable return _old_value # .............................................................................. def set_slew_rate(self, slew_rate): if type(slew_rate) is SlewRate: self._slewlimiter.set_rate_limit(slew_rate) else: raise Exception('expected SlewRate argument, not {}'.format( type(slew_rate))) # .............................................................................. @property def steps(self): return self._motor.steps def reset_steps(self): self._motor._steps = 0 # .......................................................................... @property def velocity(self): ''' Getter for the velocity (PID set point). ''' return self._pid.velocity @velocity.setter def velocity(self, velocity): ''' Setter for the velocity (PID set point). ''' if self._enable_slew: velocity = self._slewlimiter.slew(self.velocity, velocity) self._pid.velocity = velocity # .......................................................................... def set_max_velocity(self, max_velocity): ''' Setter for the maximum velocity of both PID controls. Set to None (the default) to disable this feature. ''' if max_velocity == None: self._log.info(Fore.YELLOW + 'max velocity: NONE') else: self._log.info(Fore.YELLOW + 'max velocity: {:5.2f}'.format(max_velocity)) self._pid.set_max_velocity(max_velocity) # .......................................................................... @property def stats(self): ''' Returns statistics a tuple for this PID contrroller: [kp, ki, kd, cp, ci, cd, last_power, current_motor_power, power, current_velocity, setpoint, steps] ''' kp, ki, kd = self._pid.constants cp, ci, cd = self._pid.components return kp, ki, kd, cp, ci, cd, self._last_power, self._motor.get_current_power_level( ), self._power, self._motor.velocity, self._pid.setpoint, self._motor.steps # .......................................................................... def _loop(self): ''' The PID loop that continues while the enabled flag is True. ''' _doc = Documentation.NONE _power_color = Fore.BLACK _lim = 0.2 _last_velocity = 0.0 _counter = itertools.count() while self._enabled: _count = next(_counter) _current_power = self._motor.get_current_power_level() _current_velocity = self._motor.velocity _vpr = round(_current_velocity / _current_power) if _current_power > 0 else -1.0 if self._pot_ctrl: _pot_value = self._pot.get_scaled_value() self._pid.kd = _pot_value self._log.debug('pot: {:8.5f}.'.format(_pot_value)) if _vpr < 0.0: self._failures += 1 elif self._failures > 0: self._failures -= 1 self._power += self._pid(_current_velocity) #/ 100.0 self._motor.set_motor_power(self._power / 100.0) if _doc is Documentation.MINIMAL: self._log.info('velocity: {:>5.2f}\t{:<5.2f}'.format( _current_velocity, self._pid.setpoint) + Style.RESET_ALL) elif _doc is Documentation.SIMPLE: if (_last_velocity * 0.97) <= _current_velocity <= ( _last_velocity * 1.03): # within 3% _velocity_color = Fore.YELLOW + Style.NORMAL elif _last_velocity < _current_velocity: _velocity_color = Fore.MAGENTA + Style.NORMAL elif _last_velocity > _current_velocity: _velocity_color = Fore.CYAN + Style.NORMAL else: _velocity_color = Fore.BLACK self._log.info('velocity:\t' + _velocity_color + '{:>5.2f}\t{:<5.2f}'.format( _current_velocity, self._pid.setpoint) + Style.RESET_ALL) elif _doc is Documentation.FULL: if self._last_power < self._power: _power_color = Fore.GREEN + Style.BRIGHT elif self._last_power > self._power: _power_color = Fore.RED else: _power_color = Fore.YELLOW if (_last_velocity * 0.97) <= _current_velocity <= ( _last_velocity * 1.03): # within 3% _velocity_color = Fore.YELLOW + Style.NORMAL elif _last_velocity < _current_velocity: _velocity_color = Fore.MAGENTA + Style.NORMAL elif _last_velocity > _current_velocity: _velocity_color = Fore.CYAN + Style.NORMAL else: _velocity_color = Fore.BLACK kp, ki, kd = self._pid.constants cp, ci, cd = self._pid.components self._log.info(Fore.BLUE + 'cp={:7.4f}|ci={:7.4f}|cd={:7.4f}'.format(cp, ci, cd)\ + Fore.CYAN + '|kd={:<7.4f} '.format(kd) \ + _power_color + '\tpwr: {:>5.2f}/{:<5.2f}'.format(_current_power, self._power)\ + _velocity_color + '\tvel: {:>5.2f}/{:<5.2f}'.format(_current_velocity, self._pid.setpoint)\ # + Fore.CYAN + Style.NORMAL + ' \tvp ratio: {:>5.2f}%; {:d} fails;'.format(_vpr, self._failures)\ + Style.RESET_ALL) # + '\t{:d}/{:d} steps.'.format(_diff_steps, self._max_diff_steps) + Style.RESET_ALL) elif _doc is Documentation.CSV: kp, ki, kd = self._pid.constants cp, ci, cd = self._pid.components self._log.info('{:7.4f}|{:7.4f}|{:7.4f}'.format(kp, ki, kd)\ + '|{:7.4f}|{:7.4f}|{:7.4f}'.format(cp, ci, cd) \ + '|{:>5.2f}|{:<5.2f}'.format(_current_power, self._power)\ + '|{:>5.2f}|{:<5.2f}'.format(_current_velocity, self._pid.setpoint)) self._last_power = self._power _last_velocity = _current_velocity self._rate.wait() # 20Hz # ...................... self._log.info('exited PID loop.') # .......................................................................... @property def enabled(self): return self._enabled # .......................................................................... def enable(self): if not self._closed: if self._enabled: self._log.warning('PID loop already enabled.') return self._log.info('enabling PID loop...') self._enabled = True # if we haven't started the thread yet, do so now... if self._thread is None: self._thread = threading.Thread(target=PIDController._loop, args=[self]) self._thread.start() self._log.info(Style.BRIGHT + 'PID loop enabled.') else: self._log.warning( 'cannot enable PID loop: thread already exists.') else: self._log.warning('cannot enable PID loop: already closed.') # .......................................................................... def disable(self): if self._disabling: self._log.warning('already disabling.') return elif self._closed: self._log.warning('already closed.') return self._disabling = True time.sleep(0.06) # just a bit more than 1 loop in 20Hz if self._thread is not None: self._thread.join(timeout=1.0) self._log.debug('pid thread joined.') self._thread = None self._disabling = False self._enabled = False self._log.info('disabled.') # .......................................................................... def close(self): if self._enabled: self.disable() self._closed = True # self.close_filewriter() self._log.info('closed.')
def _loop(self, _enabled, _capturing): ''' Captures an image, optionally writing it to the console in color, then post-processing the array to enumerate the peak of matches in a single linear array. We don't necessarily process the whole image (it takes a long time), processing from start_row to end_row only. ''' self._start_time = dt.now() self._column_summary = [0] * self._width try: # summarise elements on a column basis ......... self._motors.set_led_show_battery(False) self._motors.set_led_color(Color.BLACK) self._log.debug('starting image capture...') with picamera.PiCamera() as camera: with CaptureArray(camera, Level.INFO) as output: camera.resolution = (self._width, self._height) camera.iso = 800 camera.exposure_mode = 'auto' # time.sleep(1.0) if self._take_snapshot: # take JPEG image and save to file _ts = dt.utcfromtimestamp( dt.utcnow().timestamp()).isoformat().replace( ':', '_').replace('-', '_').replace('.', '_') _filename = './snapshot-{}.jpg'.format(_ts) self._log.info( 'writing snapshot to file: {}'.format(_filename)) camera.capture(_filename) # output.truncate(0) _rate = Rate(5) # 10Hz while _enabled: self._log.info(Fore.GREEN + Style.BRIGHT + 'looping snapshot...') # capture an image # camera.capture(output, 'rgb') _use_video_port = False for x in camera.capture_continuous( output, format='rgb', use_video_port=_use_video_port): image = output.array if image is not None: self._log.info( 'captured size {:d}x{:d} image.'.format( image.shape[1], image.shape[0])) else: self._log.info('captured no image.') output.truncate(0) # self._process_image(image) # self._log.set_suppress(True) # while not _capturing: # self._log.info(Fore.MAGENTA + Style.DIM + 'waiting...') time.sleep(0.5) # _rate.wait() # _capturing = False # self._log.info(Fore.CYAN + Style.DIM + 'loop end, repeating...') except picamera.PiCameraMMALError: self._log.error('could not get camera: in use by another process.') except Exception: self._log.error('error starting ros: {}'.format( traceback.format_exc())) finally: self._log.set_suppress(False) self._motors.set_led_show_battery(True) self._log.info('finis.')
def run(self): ''' This first disables the Pi's status LEDs, establishes the message queue arbitrator, the controller, enables the set of features, then starts the main OS loop. ''' super(AbstractTask, self).run() loop_count = 0 self._print_banner() self._disable_leds = self._config['pi'].get('disable_leds') if self._disable_leds: # disable Pi LEDs since they may be distracting self._set_pi_leds(False) _main_loop_freq_hz = self._config['ros'].get('main_loop_freq_hz') self._main_loop_rate = Rate(_main_loop_freq_hz) # __enable_player = self._config['ros'].get('enable_player') # if __enable_player: # self._log.info('configuring sound player...') # self._player = Player(Level.INFO) # else: # self._player = None # i2c_slave_address = config['ros'].get('i2c_master').get('device_id') # i2c hex address of I2C slave device # vl53l1x_available = True # self.get_property('features', 'vl53l1x') # ultraborg_available = True # self.get_property('features', 'ultraborg') # if vl53l1x_available and ultraborg_available: # self._log.critical('starting scanner tool...') # self._lidar = Lidar(self._config, Level.INFO) # self._lidar.enable() # else: # self._log.critical('lidar scanner tool does not have necessary dependencies.') # wait to stabilise features? # _flask_enabled = self._config['flask'].get('enabled') # if _flask_enabled: # self._log.info('starting flask web server...') # self.configure_web_server() # else: # self._log.info('not starting flask web server (suppressed from command line).') # bluetooth gamepad controller # if self._gamepad_enabled: # self._connect_gamepad() # _wait_for_button_press = self._config['ros'].get('wait_for_button_press') # self._controller.set_standby(_wait_for_button_press) # begin main loop .............................. # self._log.info('enabling bno055 sensor...') # self._bno055.enable() # self._bumpers.enable() # self._indicator = Indicator(Level.INFO) # add indicator as message listener # self._queue.add_listener(self._indicator) # self._log.info(Fore.MAGENTA + 'enabling integrated front sensor...') # self._ifs.enable() # self._log.info('starting info thread...') # self._info.start() # self._log.info('starting blinky thread...') # self._rgbmatrix.enable(DisplayType.RANDOM) self._log.info('enabling features...') for feature in self._features: self._log.info('enabling feature {}...'.format(feature.name())) feature.enable() self._log.notice('Press Ctrl-C to exit.') self._log.info('begin main os loop.\r') # enable arbitrator tasks (normal functioning of robot) # self._arbitrator.start() _main_loop_counter = itertools.count() self._active = True while self._active: # the timing of this loop is inconsequential; it exists solely as a keep-alive. self._log.debug('[{:d}] main loop...'.format( next(_main_loop_counter))) self._main_loop_rate.wait() if not self._closing: self._log.warning('closing following loop...') self.close()
class Gamepad(): _NOT_AVAILABLE_ERROR = 'gamepad device not found (not configured, paired, powered or otherwise available)' def __init__(self, config, queue, message_factory, level): ''' Parameters: config: the YAML-based application configuration queue: the message queue to receive messages from this task message_factory: the factory for creating messages mutex: vs godzilla ''' if config is None: raise ValueError('no configuration provided.') self._level = level self._log = Logger("gamepad", level) self._log.info('initialising...') self._config = config _config = config['ros'].get('gamepad') # config _loop_freq_hz = _config.get('loop_freq_hz') self._rate = Rate(_loop_freq_hz) self._device_path = _config.get('device_path') self._queue = queue self._message_factory = message_factory self._gamepad_closed = False self._closed = False self._enabled = False self._thread = None self._gamepad = None # .......................................................................... def connect(self): ''' Scan for likely gamepad device, and if found, connect. Otherwise we raise an OSError. ''' _scan = GamepadScan(self._config, self._level) _matches = _scan.check_gamepad_device() if not _matches: self._log.warning('no connection attempted: gamepad is not the most recent device (configured at: {}).'.format(self._device_path )) raise OSError('no gamepad available.') else: self._connect() # .......................................................................... def has_connection(self): return self._gamepad != None # .......................................................................... def _connect(self): self._log.header('gamepad','Connecting Gamepad...',None) try: self._gamepad = InputDevice(self._device_path) # display device info self._log.info(Fore.GREEN + "gamepad: {}".format(self._gamepad)) self._log.info('connected.') except Exception as e: self._enabled = False self._gamepad = None raise GamepadConnectException('unable to connect to input device path {}: {}'.format(self._device_path, e)) # .......................................................................... def enable(self): if not self._closed: self._log.info('enabled gamepad.') if not self.in_loop(): self._enabled = True self._start_gamepad_loop() else: self._log.error('cannot start gamepad.') self._enabled = False else: self._log.warning('cannot enable gamepad: already closed.') self._enabled = False # .......................................................................... def in_loop(self): ''' Returns true if the main loop is active (the thread is alive). ''' return self._thread != None and self._thread.is_alive() # ...................................................... @staticmethod def convert_range(value): return ( (value - 127.0) / 255.0 ) * -2.0 # .......................................................................... def _gamepad_loop(self, f_is_enabled): self._log.info('starting event loop...') __enabled = True while __enabled and f_is_enabled(): try: if self._gamepad is None: raise Exception(Gamepad._NOT_AVAILABLE_ERROR + ' [gamepad no longer available]') # loop and filter by event code and print the mapped label for event in self._gamepad.read_loop(): self._handleEvent(event) if not f_is_enabled(): self._log.info(Fore.BLACK + 'event loop.') break except Exception as e: self._log.error('gamepad device error: {}'.format(e)) except OSError as e: self._log.error(Gamepad._NOT_AVAILABLE_ERROR + ' [lost connection to gamepad]') finally: ''' Note that closing the InputDevice is a bit tricky, and we're currently masking an exception that's always thrown. As there is no data loss on a gamepad event loop being closed suddenly this is not an issue. ''' try: self._log.info(Fore.YELLOW + 'closing gamepad device...') self._gamepad.close() self._log.info(Fore.YELLOW + 'gamepad device closed.') except Exception as e: self._log.debug('error closing gamepad device: {}'.format(e)) finally: __enabled = False self._gamepad_closed = True self._rate.wait() self._log.info('exited event loop.') # .......................................................................... @property def enabled(self): return self._enabled # .......................................................................... def _start_gamepad_loop(self): ''' This is the method to call to actually start the loop. ''' if not self._enabled: self._log.error('attempt to start gamepad event loop while disabled.') elif self._gamepad is None: self._log.error(Gamepad._NOT_AVAILABLE_ERROR + ' [no gamepad found]') sys.exit(3) elif not self._closed: if self._thread is None: self._enabled = True self._thread = Thread(name='gamepad', target=Gamepad._gamepad_loop, args=[self, lambda: self._enabled], daemon=True) # self._thread.setDaemon(False) self._thread.start() self._log.info('started.') else: self._log.warning('cannot enable: process already running.') else: self._log.warning('cannot enable: already closed.') # .......................................................................... def disable(self): if self._closed: self._log.warning('can\'t disable: already closed.') elif not self._enabled: self._log.warning('already disabled.') else: self._enabled = False time.sleep(0.2) # we'll wait a bit for the gamepad device to close... _i = 0 while not self._gamepad_closed and _i < 20: _i += 1 self._log.info('_i: {:d}'.format(_i)) time.sleep(0.1) self._log.info('disabled.') # .......................................................................... def close(self): ''' Permanently close and disable the gamepad. ''' if self._enabled: self.disable() if not self._closed: self._closed = True self._log.info('closed.') else: self._log.warning('already closed.') # .......................................................................... def _handleEvent(self, event): ''' Handles the incoming event by filtering on event type and code. There's possibly a more elegant way of doing this but for now this works just fine. ''' _message = None _control = None if event.type == ecodes.EV_KEY: _control = GamepadControl.get_by_code(self, event.code) if event.value == 1: if event.code == GamepadControl.A_BUTTON.code: self._log.info(Fore.RED + "A Button") # _control = GamepadControl.A_BUTTON elif event.code == GamepadControl.B_BUTTON.code: self._log.info(Fore.RED + "B Button") # _control = GamepadControl.B_BUTTON elif event.code == GamepadControl.X_BUTTON.code: self._log.info(Fore.RED + "X Button") # _control = GamepadControl.X_BUTTON elif event.code == GamepadControl.Y_BUTTON.code: self._log.info(Fore.RED + "Y Button") # _control = GamepadControl.Y_BUTTON elif event.code == GamepadControl.L1_BUTTON.code: self._log.info(Fore.YELLOW + "L1 Button") # _control = GamepadControl.L1_BUTTON elif event.code == GamepadControl.L2_BUTTON.code: self._log.info(Fore.YELLOW + "L2 Button") # _control = GamepadControl.L2_BUTTON elif event.code == GamepadControl.R1_BUTTON.code: self._log.info(Fore.YELLOW + "R1 Button") # _control = GamepadControl.R1_BUTTON elif event.code == GamepadControl.R2_BUTTON.code: self._log.info(Fore.YELLOW + "R2 Button") # _control = GamepadControl.R2_BUTTON elif event.code == GamepadControl.START_BUTTON.code: self._log.info(Fore.GREEN + "Start Button") # _control = GamepadControl.START_BUTTON elif event.code == GamepadControl.SELECT_BUTTON.code: self._log.info(Fore.GREEN + "Select Button") # _control = GamepadControl.SELECT_BUTTON elif event.code == GamepadControl.HOME_BUTTON.code: self._log.info(Fore.MAGENTA + "Home Button") # _control = GamepadControl.HOME_BUTTON else: self._log.info(Fore.BLACK + "event type: EV_KEY; event: {}; value: {}".format(event.code, event.value)) else: # self._log.info(Fore.BLACK + Style.DIM + "event type: EV_KEY; value: {}".format(event.value)) pass elif event.type == ecodes.EV_ABS: _control = GamepadControl.get_by_code(self, event.code) if event.code == GamepadControl.DPAD_HORIZONTAL.code: if event.value == 1: self._log.info(Fore.CYAN + Style.BRIGHT + "D-Pad Horizontal(Right) {}".format(event.value)) elif event.value == -1: self._log.info(Fore.CYAN + Style.NORMAL + "D-Pad Horizontal(Left) {}".format(event.value)) else: self._log.info(Fore.BLACK + "D-Pad Horizontal(N) {}".format(event.value)) elif event.code == GamepadControl.DPAD_VERTICAL.code: if event.value == -1: self._log.info(Fore.CYAN + Style.NORMAL + "D-Pad Vertical(Up) {}".format(event.value)) elif event.value == 1: self._log.info(Fore.CYAN + Style.BRIGHT + "D-Pad Vertical(Down) {}".format(event.value)) else: self._log.info(Fore.BLACK + "D-Pad Vertical(N) {}".format(event.value)) elif event.code == GamepadControl.L3_VERTICAL.code: self._log.debug(Fore.MAGENTA + "L3 Vertical {}".format(event.value)) elif event.code == GamepadControl.L3_HORIZONTAL.code: self._log.debug(Fore.YELLOW + "L3 Horizontal {}".format(event.value)) elif event.code == GamepadControl.R3_VERTICAL.code: self._log.debug(Fore.CYAN + "R3 Vertical {}".format(event.value)) # _control = GamepadControl.R3_VERTICAL elif event.code == GamepadControl.R3_HORIZONTAL.code: self._log.debug(Fore.GREEN + "R3 Horizontal {}".format(event.value)) # _control = GamepadControl.R3_HORIZONTAL else: # self._log.info(Fore.BLACK + "type: EV_ABS; event code: {}; value: {}".format(event.code, event.value)) pass else: # self._log.info(Fore.BLACK + Style.DIM + "ZZ. event type: {}; code: {}; value: {}".format(event.type, event.code, event.value)) pass if _control != None: _message = self._message_factory.get_message(_control.event, event.value) self._log.debug(Fore.CYAN + Style.BRIGHT + "triggered control with message {}".format(_message)) self._queue.add(_message)
class ROS(AbstractTask): ''' Extends AbstractTask as a Finite State Machine (FSM) basis of a Robot Operating System (ROS) or behaviour-based system (BBS), including spawning the various tasks and an Arbitrator as separate threads, inter-communicating over a common message queue. This establishes the basic subsumption foundation of a MessageBus, a MessageFactory, a system Clock, an Arbitrator and a Controller. The MessageBus receives Event-containing messages from sensors and other message sources, which are passed on to the Arbitrator, whose job it is to determine the highest priority action to execute for that task cycle, by passing it on to the Controller. There is also a rosd linux daemon, which can be used to start, enable and disable ros. :param mutex: the optional logging mutex, passed on from rosd ''' # .......................................................................... def __init__(self, mutex=None, level=Level.INFO): ''' This initialises the ROS and calls the YAML configurer. ''' self._mutex = mutex if mutex is not None else threading.Lock() super().__init__("ros", None, self._mutex) self._log.info('setting process as high priority...') # set ROS as high priority proc = psutil.Process(os.getpid()) proc.nice(10) self._config = None self._active = False self._closing = False self._disable_leds = False self._arbitrator = None self._controller = None self._gamepad = None self._motors = None self._ifs = None self._features = [] self._log.info('initialised.') # .......................................................................... def configure(self, arguments): ''' Provided with a set of configuration arguments, configures ROS based on both KD01/KR01 standard hardware as well as optional features, the latter based on devices showing up (by address) on the I²C bus. Optional devices are only enabled at startup time via registration of their feature availability. ''' self._log.heading('configuration', 'configuring ros...', '[1/2]' if arguments.start else '[1/1]') self._log.info('application log level: {}'.format( self._log.level.name)) # configuration from command line arguments self._using_mocks = False self._permit_mocks = arguments.mock self._enable_camera = arguments.camera # TODO # read YAML configuration _loader = ConfigLoader(self._log.level) _config_file = arguments.config_file if arguments.config_file is not None else 'config.yaml' self._config = _loader.configure(_config_file) # scan I2C bus self._log.info('scanning I²C address bus...') scanner = I2CScanner(self._log.level) self._addresses = scanner.get_int_addresses() _hex_addresses = scanner.get_hex_addresses() self._addrDict = dict( list(map(lambda x, y: (x, y), self._addresses, _hex_addresses))) # for i in range(len(self._addresses)): for _address in self._addresses: _device_name = self.get_device_for_address(_address) self._log.info('found device at I²C address 0x{:02X}: {}'.format( _address, _device_name) + Style.RESET_ALL) # TODO look up address and make assumption about what the device is # establish basic subsumption components self._log.info('configure application messaging...') self._message_factory = MessageFactory(self._log.level) self._message_bus = MessageBus(self._log.level) self._log.info('configuring system clock...') self._clock = Clock(self._config, self._message_bus, self._message_factory, Level.WARN) self.add_feature(self._clock) # standard devices ........................................... self._log.info('configure default features...') self._log.info('configure CPU temperature check...') _temperature_check = Temperature(self._config, self._clock, self._log.level) if _temperature_check.get_cpu_temperature() > 0: self.add_feature(_temperature_check) else: self._log.warning('no support for CPU temperature.') motors_enabled = not arguments.no_motors and (0x15 in self._addresses) if motors_enabled: # then configure motors self._log.debug(Fore.CYAN + Style.BRIGHT + '-- ThunderBorg available at 0x15' + Style.RESET_ALL) _motor_configurer = MotorConfigurer(self._config, self._clock, self._log.level) self._motors = _motor_configurer.get_motors() self.add_feature(self._motors) self._set_feature_available('motors', motors_enabled) elif self._permit_mocks: self._using_mocks = True self._log.debug(Fore.RED + Style.BRIGHT + '-- no ThunderBorg available, using mocks.' + Style.RESET_ALL) from mock.motor_configurer import MockMotorConfigurer _motor_configurer = MockMotorConfigurer(self._config, self._clock, self._log.level) self._motors = _motor_configurer.get_motors() self.add_feature(self._motors) self._set_feature_available('motors', motors_enabled) ifs_available = (0x0E in self._addresses) if ifs_available: self._log.info('configuring integrated front sensor...') self._ifs = IntegratedFrontSensor(self._config, self._clock, self._log.level) self.add_feature(self._ifs) elif self._permit_mocks: self._using_mocks = True self._log.info( 'integrated front sensor not available; loading mock sensor.') from mock.ifs import MockIntegratedFrontSensor self._ifs = MockIntegratedFrontSensor(self._message_bus, exit_on_complete=False, level=self._log.level) self._message_bus.set_ifs(self._ifs) self.add_feature(self._ifs) else: self._ifs = None self._log.warning('no integrated front sensor available.') # ultraborg_available = ( 0x36 in self._addresses ) # if ultraborg_available: # self._log.debug(Fore.CYAN + Style.BRIGHT + '-- UltraBorg available at 0x36.' + Style.RESET_ALL) # else: # self._log.debug(Fore.RED + Style.BRIGHT + '-- no UltraBorg available at 0x36.' + Style.RESET_ALL) # self._set_feature_available('ultraborg', ultraborg_available) # # optional devices ........................................... self._log.info('configure optional features...') self._gamepad_enabled = arguments.gamepad and self._config['ros'].get( 'gamepad').get('enabled') # # the 5x5 RGB Matrix is at 0x74 for port, 0x77 for starboard # rgbmatrix5x5_stbd_available = ( 0x74 in self._addresses ) # if rgbmatrix5x5_stbd_available: # self._log.debug(Fore.CYAN + Style.BRIGHT + '-- RGB Matrix available at 0x74.' + Style.RESET_ALL) # else: # self._log.debug(Fore.RED + Style.BRIGHT + '-- no RGB Matrix available at 0x74.' + Style.RESET_ALL) # self._set_feature_available('rgbmatrix5x5_stbd', rgbmatrix5x5_stbd_available) # rgbmatrix5x5_port_available = ( 0x77 in self._addresses ) # if rgbmatrix5x5_port_available: # self._log.debug(Fore.CYAN + Style.BRIGHT + '-- RGB Matrix available at 0x77.' + Style.RESET_ALL) # else: # self._log.debug(Fore.RED + Style.BRIGHT + '-- no RGB Matrix available at 0x77.' + Style.RESET_ALL) # self._set_feature_available('rgbmatrix5x5_port', rgbmatrix5x5_port_available) # if rgbmatrix5x5_stbd_available or rgbmatrix5x5_port_available: # self._log.info('configure rgbmatrix...') # self._rgbmatrix = RgbMatrix(Level.INFO) # self.add_feature(self._rgbmatrix) # FIXME this is added twice # # ............................................ # # the 11x7 LED matrix is at 0x75 for starboard, 0x77 for port. The latter # # conflicts with the RGB LED matrix, so both cannot be used simultaneously. # matrix11x7_stbd_available = ( 0x75 in self._addresses ) # if matrix11x7_stbd_available: # self._log.debug(Fore.CYAN + Style.BRIGHT + '-- 11x7 Matrix LEDs available at 0x75.' + Style.RESET_ALL) # else: # self._log.debug(Fore.RED + Style.BRIGHT + '-- no 11x7 Matrix LEDs available at 0x75.' + Style.RESET_ALL) # self._set_feature_available('matrix11x7_stbd', matrix11x7_stbd_available) # # device availability ........................................ # bno055_available = ( 0x28 in self._addresses ) # if bno055_available: # self._log.info('configuring BNO055 9DoF sensor...') # self._bno055 = BNO055(self._config, self.get_message_queue(), Level.INFO) # else: # self._log.debug(Fore.RED + Style.BRIGHT + 'no BNO055 orientation sensor available at 0x28.' + Style.RESET_ALL) # self._set_feature_available('bno055', bno055_available) # # NOTE: the default address for the ICM20948 is 0x68, but this conflicts with the PiJuice # icm20948_available = ( 0x69 in self._addresses ) # if icm20948_available: # self._log.debug(Fore.CYAN + Style.BRIGHT + 'ICM20948 available at 0x69.' + Style.RESET_ALL) # else: # self._log.debug(Fore.RED + Style.BRIGHT + 'no ICM20948 available at 0x69.' + Style.RESET_ALL) # self._set_feature_available('icm20948', icm20948_available) # lsm303d_available = ( 0x1D in self._addresses ) # if lsm303d_available: # self._log.debug(Fore.CYAN + Style.BRIGHT + 'LSM303D available at 0x1D.' + Style.RESET_ALL) # else: # self._log.debug(Fore.RED + Style.BRIGHT + 'no LSM303D available at 0x1D.' + Style.RESET_ALL) # self._set_feature_available('lsm303d', lsm303d_available) # # vl53l1x_available = ( 0x29 in self._addresses ) # if vl53l1x_available: # self._log.debug(Fore.CYAN + Style.BRIGHT + 'VL53L1X available at 0x29.' + Style.RESET_ALL) # else: # self._log.debug(Fore.RED + Style.BRIGHT + 'no VL53L1X available at 0x29.' + Style.RESET_ALL) # self._set_feature_available('vl53l1x', vl53l1x_available) # as7262_available = ( 0x49 in self._addresses ) # if as7262_available: # self._log.debug(Fore.CYAN + Style.BRIGHT + '-- AS7262 Spectrometer available at 0x49.' + Style.RESET_ALL) # else: # self._log.debug(Fore.RED + Style.BRIGHT + '-- no AS7262 Spectrometer available at 0x49.' + Style.RESET_ALL) # self._set_feature_available('as7262', as7262_available) # pijuice_available = ( 0x68 in self._addresses ) # if pijuice_available: # self._log.debug(Fore.CYAN + Style.BRIGHT + 'PiJuice hat available at 0x68.' + Style.RESET_ALL) # else: # self._log.debug(Fore.RED + Style.BRIGHT + 'no PiJuice hat available at 0x68.' + Style.RESET_ALL) # self._set_feature_available('pijuice', pijuice_available) self._log.info(Fore.YELLOW + 'configure subsumption support...') # configure the MessageQueue, Controller and Arbitrator self._log.info('configuring message queue...') self._queue = MessageQueue(self._message_bus, self._log.level) self._message_bus.add_handler(Message, self._queue.handle) self._log.info('configuring controller...') self._controller = Controller(self._config, self._ifs, self._motors, self._callback_shutdown, self._log.level) self._log.info('configuring arbitrator...') self._arbitrator = Arbitrator(self._config, self._queue, self._controller, self._log.level) self._log.info('configured.') # .......................................................................... def _set_feature_available(self, name, value): ''' Sets a feature's availability to the boolean value. ''' self._log.debug(Fore.BLUE + Style.BRIGHT + '-- set feature available. name: \'{}\' value: \'{}\'.' .format(name, value)) self.set_property('features', name, value) # .......................................................................... def get_device_for_address(self, address): if address == 0x0E: return 'RGB Potentiometer' elif address == 0x0F: return 'RGB Encoder' # default, moved to 0x16 elif address == 0x15: return 'ThunderBorg' elif address == 0x16: return 'RGB Encoder' elif address == 0x18: return 'IO Expander' elif address == 0x48: return 'ADS1015' elif address == 0x4A: return 'Unknown' elif address == 0x74: return '5x5 RGB Matrix' elif address == 0x77: return '5x5 RGB Matrix (or 11x7 LED Matrix)' else: return 'Unknown' # .......................................................................... @property def controller(self): return self._controller # .......................................................................... @property def configuration(self): return self._config # .......................................................................... def get_property(self, section, property_name): ''' Return the value of the named property of the application configuration, provided its section and property name. ''' return self._config[section].get(property_name) # .......................................................................... def set_property(self, section, property_name, property_value): ''' Set the value of the named property of the application configuration, provided its section, property name and value. ''' self._log.debug(Fore.GREEN + 'set config on section \'{}\' for property key: \'{}\' to value: {}.'.format(\ section, property_name, property_value)) if section == 'ros': self._config[section].update(property_name=property_value) else: _ros = self._config['ros'] _ros[section].update(property_name=property_value) # .......................................................................... def _set_pi_leds(self, enable): ''' Enables or disables the Raspberry Pi's board LEDs. ''' sudo_name = self.get_property('pi', 'sudo_name') # led_0_path: '/sys/class/leds/led0/brightness' _led_0_path = self._config['pi'].get('led_0_path') _led_0 = Path(_led_0_path) # led_1_path: '/sys/class/leds/led1/brightness' _led_1_path = self._config['pi'].get('led_1_path') _led_1 = Path(_led_1_path) if _led_0.is_file() and _led_0.is_file(): if enable: self._log.info('re-enabling LEDs...') os.system('echo 1 | {} tee {}'.format(sudo_name, _led_0_path)) os.system('echo 1 | {} tee {}'.format(sudo_name, _led_1_path)) else: self._log.debug('disabling LEDs...') os.system('echo 0 | {} tee {}'.format(sudo_name, _led_0_path)) os.system('echo 0 | {} tee {}'.format(sudo_name, _led_1_path)) else: self._log.warning( 'could not change state of LEDs: does not appear to be a Raspberry Pi.' ) # .......................................................................... def _connect_gamepad(self): if not self._gamepad_enabled: self._log.info('gamepad disabled.') return if self._gamepad is None: self._log.info('creating gamepad...') try: self._gamepad = Gamepad(self._config, self._queue, Level.INFO) except GamepadConnectException as e: self._log.error('unable to connect to gamepad: {}'.format(e)) self._gamepad = None self._gamepad_enabled = False self._log.info('gamepad unavailable.') return if self._gamepad is not None: self._log.info('enabling gamepad...') self._gamepad.enable() _count = 0 while not self._gamepad.has_connection(): _count += 1 if _count == 1: self._log.info('connecting to gamepad...') else: self._log.info( 'gamepad not connected; re-trying... [{:d}]'.format( _count)) self._gamepad.connect() time.sleep(0.5) if self._gamepad.has_connection() or _count > 5: break # .......................................................................... def has_connected_gamepad(self): return self._gamepad is not None and self._gamepad.has_connection() # .......................................................................... def get_arbitrator(self): return self._arbitrator # .......................................................................... def add_feature(self, feature): ''' Add the feature to the list of features. Features must have an enable() method. ''' self._features.append(feature) self._log.info('added feature {}.'.format(feature.name())) # .......................................................................... def _callback_shutdown(self): _enable_self_shutdown = self._config['ros'].get('enable_self_shutdown') if _enable_self_shutdown: self._log.critical('callback: shutting down os...') self.close() sys.exit(0) else: self._log.critical('self-shutdown disabled.') # .......................................................................... def _print_banner(self): ''' Display banner on console. ''' self._log.info('…') self._log.info('… █▒▒▒▒▒▒▒ █▒▒▒▒▒▒ █▒▒▒▒▒▒ █▒▒ ') self._log.info('… █▒▒ █▒▒ █▒▒ █▒▒ █▒▒ █▒▒ ') self._log.info('… █▒▒▒▒▒▒ █▒▒ █▒▒ █▒▒▒▒▒▒ █▒▒ ') self._log.info('… █▒▒ █▒▒ █▒▒ █▒▒ █▒▒ ') self._log.info('… █▒▒ █▒▒ █▒▒▒▒▒▒ █▒▒▒▒▒▒ █▒▒ ') self._log.info('…') # .......................................................................... def run(self): ''' This first disables the Pi's status LEDs, establishes the message queue arbitrator, the controller, enables the set of features, then starts the main OS loop. ''' super(AbstractTask, self).run() loop_count = 0 self._print_banner() self._disable_leds = self._config['pi'].get('disable_leds') if self._disable_leds: # disable Pi LEDs since they may be distracting self._set_pi_leds(False) _main_loop_freq_hz = self._config['ros'].get('main_loop_freq_hz') self._main_loop_rate = Rate(_main_loop_freq_hz) # __enable_player = self._config['ros'].get('enable_player') # if __enable_player: # self._log.info('configuring sound player...') # self._player = Player(Level.INFO) # else: # self._player = None # i2c_slave_address = config['ros'].get('i2c_master').get('device_id') # i2c hex address of I2C slave device # vl53l1x_available = True # self.get_property('features', 'vl53l1x') # ultraborg_available = True # self.get_property('features', 'ultraborg') # if vl53l1x_available and ultraborg_available: # self._log.critical('starting scanner tool...') # self._lidar = Lidar(self._config, Level.INFO) # self._lidar.enable() # else: # self._log.critical('lidar scanner tool does not have necessary dependencies.') # wait to stabilise features? # _flask_enabled = self._config['flask'].get('enabled') # if _flask_enabled: # self._log.info('starting flask web server...') # self.configure_web_server() # else: # self._log.info('not starting flask web server (suppressed from command line).') # bluetooth gamepad controller # if self._gamepad_enabled: # self._connect_gamepad() # _wait_for_button_press = self._config['ros'].get('wait_for_button_press') # self._controller.set_standby(_wait_for_button_press) # begin main loop .............................. # self._log.info('enabling bno055 sensor...') # self._bno055.enable() # self._bumpers.enable() # self._indicator = Indicator(Level.INFO) # add indicator as message listener # self._queue.add_listener(self._indicator) # self._log.info(Fore.MAGENTA + 'enabling integrated front sensor...') # self._ifs.enable() # self._log.info('starting info thread...') # self._info.start() # self._log.info('starting blinky thread...') # self._rgbmatrix.enable(DisplayType.RANDOM) self._log.info('enabling features...') for feature in self._features: self._log.info('enabling feature {}...'.format(feature.name())) feature.enable() self._log.notice('Press Ctrl-C to exit.') self._log.info('begin main os loop.\r') # enable arbitrator tasks (normal functioning of robot) # self._arbitrator.start() _main_loop_counter = itertools.count() self._active = True while self._active: # the timing of this loop is inconsequential; it exists solely as a keep-alive. self._log.debug('[{:d}] main loop...'.format( next(_main_loop_counter))) self._main_loop_rate.wait() if not self._closing: self._log.warning('closing following loop...') self.close() # end main ................................... # .......................................................................... def emergency_stop(self): ''' Stop immediately, something has hit the top feelers. ''' self._motors.stop() self._log.info(Fore.RED + Style.BRIGHT + 'emergency stop: contact on upper feelers.') # .......................................................................... def send_message(self, message): ''' Send the Message into the MessageQueue. ''' # self._queue.add(message) raise Exception('unsupported') # .......................................................................... def enable(self): super(AbstractTask, self).enable() # .......................................................................... def disable(self): super(AbstractTask, self).disable() # .......................................................................... def close(self): ''' This sets the ROS back to normal following a session. ''' if self._closing: # this also gets called by the arbitrator so we ignore that self._log.info('already closing.') return else: self._active = False self._closing = True self._log.info(Style.BRIGHT + 'closing...') if self._gamepad: self._gamepad.close() if self._motors: self._motors.close() if self._ifs: self._ifs.close() # close features for feature in self._features: self._log.info('closing feature {}...'.format(feature.name())) feature.close() self._log.info('finished closing features.') if self._arbitrator: self._arbitrator.disable() self._arbitrator.close() # self._arbitrator.join(timeout=1.0) if self._controller: self._controller.disable() super().close() if self._disable_leds: # restore LEDs self._set_pi_leds(True) # GPIO.cleanup() # self._log.info('joining main thread...') # self.join(timeout=0.5) self._log.info('os closed.') sys.stderr = DevNull() sys.exit(0)