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, 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 __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 __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 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_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_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_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 _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))
def __init__(self, config, message_bus, message_factory, level): super().__init__() self._log = Logger("clock", level) if message_bus is None: raise ValueError('null message bus argument.') self._message_bus = message_bus if message_factory is None: raise ValueError('null message factory argument.') self._message_factory = message_factory if config is None: raise ValueError('null configuration argument.') _config = config['ros'].get('clock') self._loop_freq_hz = _config.get('loop_freq_hz') self._tock_modulo = _config.get('tock_modulo') self._log.info('tock modulo: {:d}'.format(self._tock_modulo)) self._counter = itertools.count() self._rate = Rate(self._loop_freq_hz) # self._tick_type = type(Tick(None, Event.CLOCK_TICK, None)) # self._tock_type = type(Tock(None, Event.CLOCK_TOCK, None)) self._log.info('tick frequency: {:d}Hz'.format(self._loop_freq_hz)) self._log.info('tock frequency: {:d}Hz'.format( round(self._loop_freq_hz / self._tock_modulo))) self._enable_trim = _config.get('enable_trim') self._log.info('enable clock trim.' if self. _enable_trim else 'disable clock trim.') self._pot = None if self._enable_trim: if SCALE_KP: _min_pot = 0.2 _max_pot = 0.6 _kp = 0.0 _ki = 0.0 _kd = 0.00001 elif SCALE_KI: _min_pot = 0.0 _max_pot = 0.01 _kp = 0.3333 _ki = 0.0 _kd = 0.00001 elif SCALE_KD: _min_pot = 0.0 _max_pot = 0.01 _kp = 0.3333 _ki = 0.0 _kd = 0.0 else: # use these constants for the PID with no potentiometer control _kp = 0.3333 _ki = 0.0 _kd = 0.00001 if (SCALE_KP or SCALE_KI or SCALE_KD): try: self._pot = Potentiometer(config, Level.WARN) except Exception: self._log.info(Fore.YELLOW + 'using mock potentiometer.') self._pot = MockPotentiometer() self._pot.set_output_limits(_min_pot, _max_pot) else: self._log.info('using fixed PID constants; no potentiometer.') # initial constants _setpoint = self._rate.get_period_ms() self._log.info(Style.BRIGHT + 'initial PID setpoint: {:6.3f}'.format(_setpoint)) _min_output = None _max_output = None _sample_time = 0.05 self._pid = PID('clock', _kp, _ki, _kd, _min_output, _max_output, _setpoint, _sample_time, Level.WARN) self._log.info(Style.BRIGHT + 'trim enabled.') else: self._pid = None self._log.info(Style.DIM + 'trim disabled.') # ......................... self._thread = None self._enabled = False self._closed = False self._last_time = dt.now() self._log.info('ready.')
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()
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 test_motors(): global _port_motor, _stbd_motor, action_A, action_B # read YAML configuration _loader = ConfigLoader(Level.INFO) filename = 'config.yaml' _config = _loader.configure(filename) _log.info('creating message factory...') _message_factory = MessageFactory(Level.INFO) _log.info('creating message bus...') _message_bus = MessageBus(Level.WARN) _log.info('creating clock...') _clock = Clock(_config, _message_bus, _message_factory, Level.WARN) _motor_configurer = MotorConfigurer(_config, _clock, Level.INFO) _motors = _motor_configurer.get_motors() _motors.enable() _i2c_scanner = I2CScanner(Level.WARN) if ENABLE_MOTH: if _i2c_scanner.has_address([0x18]): _rgbmatrix = RgbMatrix(Level.WARN) # _rgbmatrix.set_display_type(DisplayType.SOLID) _moth = Moth(_config, None, Level.WARN) else: _log.warning('cannot enable moth: no IO Expander found.') _moth = None _pin_A = 16 _button_16 = Button(_pin_A, callback_method_A, Level.INFO) _log.info( Style.BRIGHT + 'press button A (connected to pin {:d}) to toggle or initiate action.'. format(_pin_A)) _pin_B = 24 _button_24 = Button(_pin_B, callback_method_B, Level.INFO) _log.info(Style.BRIGHT + 'press button B connected to pin {:d}) to exit.'.format(_pin_B)) _log.info('starting motors...') _port_motor = _motors.get_motor(Orientation.PORT) _stbd_motor = _motors.get_motor(Orientation.STBD) if ENABLE_PORT: _port_motor.enable() if ENABLE_STBD: _stbd_motor.enable() _rot_ctrl = RotaryControl(_config, 0, 50, 2, Level.WARN) # min, max, step _rate = Rate(5) _step_limit = 2312 _velocity_stbd = 0.0 _velocity_port = 0.0 _start_time = dt.now() _moth_port = 1.0 _moth_stbd = 1.0 _moth_offset = 0.6 _moth_bias = [0.0, 0.0] _moth_fudge = 0.7 try: action_A = True # if not using buttons at all set to True action_B = True while INFINITE or action_B or (_port_motor.steps < _step_limit and _stbd_motor.steps < _step_limit): if action_A: action_A = False # trigger once while action_B: if USE_ROTARY_CONTROL: _target_velocity = _rot_ctrl.read() else: _target_velocity = 30.0 # _power = _target_velocity / 100.0 _power = Motor.velocity_to_power(_target_velocity) if ENABLE_MOTH and _moth: _moth_bias = _moth.get_bias() # _log.info(Fore.WHITE + Style.BRIGHT + 'port: {:5.2f}; stbd: {:5.2f}'.format(_moth_port, _moth_stbd)) # _rgbmatrix.show_hue(_moth_hue, Orientation.BOTH) _orientation = _moth.get_orientation() if _orientation is Orientation.PORT: _moth_port = _moth_bias[0] * _moth_fudge _moth_stbd = 1.0 _rgbmatrix.show_color(Color.BLACK, Orientation.STBD) _rgbmatrix.show_color(Color.RED, Orientation.PORT) elif _orientation is Orientation.STBD: _moth_port = 1.0 _moth_stbd = _moth_bias[1] * _moth_fudge _rgbmatrix.show_color(Color.BLACK, Orientation.PORT) _rgbmatrix.show_color(Color.GREEN, Orientation.STBD) else: _moth_port = 1.0 _moth_stbd = 1.0 _rgbmatrix.show_color(Color.BLACK, Orientation.PORT) _rgbmatrix.show_color(Color.BLACK, Orientation.STBD) if ENABLE_STBD: _stbd_motor.set_motor_power(_stbd_rotate * _power * _moth_stbd) _velocity_stbd = _stbd_motor.velocity if ENABLE_PORT: _port_motor.set_motor_power(_port_rotate * _power * _moth_port) _velocity_port = _port_motor.velocity _log.info(Fore.YELLOW + 'power: {:5.2f}; bias: '.format(_power) \ + Fore.RED + ' {:5.2f} '.format(_moth_bias[0]) + Fore.GREEN + '{:5.2f};'.format(_moth_bias[1]) \ + Fore.BLACK + ' target velocity: {:5.2f};'.format(_target_velocity) \ + Fore.CYAN + ' velocity: ' \ + Fore.RED + ' {:5.2f} '.format(_velocity_port) + Fore.GREEN + ' {:5.2f}'.format(_velocity_stbd)) # _log.info(Fore.RED + 'power {:5.2f}/{:5.2f}; {:>4d} steps; \t'.format(_stbd_motor.get_current_power_level(), _power, _port_motor.steps) \ # + Fore.GREEN + 'power {:5.2f}/{:5.2f}; {:>4d} steps.'.format(_port_motor.get_current_power_level(), _power, _stbd_motor.steps)) _rate.wait() action_B = True # reentry into B loop, waiting for A _log.info('waiting for A button press...') time.sleep(1.0) # end wait loop .................................................... if ENABLE_PORT: _log.info('port motor: {:d} steps.'.format(_port_motor.steps)) if ENABLE_STBD: _log.info('stbd motor: {:d} steps.'.format(_stbd_motor.steps)) except KeyboardInterrupt: _log.info('Ctrl-C caught; exiting...') except Exception as e: _log.error('error: {}'.format(e)) finally: close_motors(_log) _elapsed_ms = round((dt.now() - _start_time).total_seconds() * 1000.0) _log.info(Fore.YELLOW + 'complete: elapsed: {:d}ms'.format(_elapsed_ms))
def main(): _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 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 _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...') _e_heading = None _e_pitch = None _e_roll = None _e_yaw = None _q_heading = None _q_pitch = None _q_roll = None _q_yaw = 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 ................ _rate = Rate(20) # 20Hz while not self._closed: if self._enabled: _status = self._bno055.calibration_status # status: sys, gyro, accel, mag _sys_calibrated = (_status[0] >= 3) _gyro_calibrated = (_status[1] >= 3) _accelerometer_calibrated = (_status[2] >= 3) _magnetometer_calibrated = (_status[3] >= 3) # "Therefore we recommend that as long as Magnetometer is 3/3, and Gyroscope is 3/3, the data can be trusted." # source: https://community.bosch-sensortec.com/t5/MEMS-sensors-forum/BNO055-Calibration-Staus-not-stable/td-p/8375 self._is_calibrated = _gyro_calibrated and _magnetometer_calibrated # self._is_calibrated = self._bno055.calibrated if self._is_calibrated: self._log.debug('calibrated: s{}-g{}-a{}-m{}.'.format( _status[0], _status[1], _status[2], _status[3])) else: self._log.debug('not calibrated: s{}-g{}-a{}-m{}.'.format( _status[0], _status[1], _status[2], _status[3])) # 1. Absolute Orientation (Euler Vector, 100Hz) Three axis orientation data based on a 360° sphere ........................... _euler = self._bno055.euler if _euler[0] is not None: _e_heading = _euler[0] if _euler[1] is not None: _e_pitch = -1.0 * _euler[1] if _euler[2] is not None: _e_roll = _euler[2] # 2. Absolute Orientation (Quaterion, 100Hz) Four point quaternion output for more accurate data manipulation ................ _quat = self._bno055.quaternion if _quat is not None: if _quat[0] is not None: _quat_w = _quat[0] if _quat[1] is not None: _quat_x = _quat[1] if _quat[2] is not None: _quat_y = _quat[2] if _quat[3] is not None: _quat_z = _quat[3] _sys_calibrated = True # override, just pay attention to values if _sys_calibrated \ and ( None not in (_e_heading, _e_pitch, _e_roll ) ) \ and ( None not in (_quat_w, _quat_x, _quat_y, _quat_z ) ): # _q_euler = self._convert_to_euler(_quat_w, _quat_x, _quat_y, _quat_z) _q = self._quaternion_to_euler_angle( _quat_w, _quat_x, _quat_y, _quat_z) _q_yaw_pitch_roll = _q.yaw_pitch_roll _q_heading = _q.degrees _q_yaw = _q_yaw_pitch_roll[0] _q_pitch = _q_yaw_pitch_roll[1] _q_roll = _q_yaw_pitch_roll[2] time.sleep(0.5) # TEMP # heading ............................................................ if BNO055._in_range(_e_heading, _q_heading): self._heading = _q_heading + self._heading_trim self._log.debug(' ' + Fore.MAGENTA + Style.BRIGHT + 'eh: \t {:>5.4f}° (cal); '.format(_e_heading) \ + Fore.BLACK + Style.NORMAL + '\t qh={:>5.4f}\t p={:>5.4f}\t r={:>5.4f}\t y={:>5.4f}'.format(_q_heading, _q_pitch, _q_roll, _q_yaw)) elif self._quaternion_accept: # if true, we permit Quaternion once we've achieved calibration self._heading = _e_heading + self._heading_trim self._log.debug(' ' + Fore.CYAN + Style.NORMAL + 'eh: \t {:>5.4f}° (qua); '.format(_e_heading) \ + Fore.BLACK + Style.NORMAL + '\t qh={:>5.4f}\t p={:>5.4f}\t r={:>5.4f}\t y={:>5.4f}'.format(_q_heading, _q_pitch, _q_roll, _q_yaw)) else: self._log.debug(' ' + Fore.CYAN + Style.DIM + 'eh: \t {:>5.4f}° (non); '.format(_e_heading) \ + Fore.BLACK + Style.NORMAL + '\t qh={:>5.4f}\t p={:>5.4f}\t r={:>5.4f}\t y={:>5.4f}'.format(_q_heading, _q_pitch, _q_roll, _q_yaw)) self._heading = None # pitch .............................................................. if BNO055._in_range(_e_pitch, _q_pitch): self._pitch = _e_pitch + self._pitch_trim self._log.debug(' ' + Fore.CYAN + Style.BRIGHT + 'pitch: \t{:>5.4f}° (calibrated)'. format(_e_pitch)) else: self._log.debug( ' ' + Fore.YELLOW + Style.BRIGHT + 'pitch: \t{:>5.4f}° (euler)'.format(_e_pitch)) self._log.debug(' ' + Fore.GREEN + Style.BRIGHT + 'pitch: \t{:>5.4f}° (quaternion)'. format(_q_pitch)) self._pitch = None # roll ............................................................... if BNO055._in_range(_e_roll, _q_roll): self._roll = _e_roll + self._roll_trim self._log.debug( ' ' + Fore.CYAN + Style.BRIGHT + 'roll: \t{:>5.4f}° (calibrated)'.format(_e_roll)) else: self._log.debug( ' ' + Fore.YELLOW + Style.BRIGHT + 'roll: \t{:>5.4f}° (euler)'.format(_e_roll)) self._log.debug( ' ' + Fore.GREEN + Style.BRIGHT + 'roll: \t{:>5.4f}° (quaternion)'.format(_q_roll)) self._roll = None # 3. Angular Velocity Vector (20Hz) Three axis of 'rotation speed' in rad/s .................................................. # _gyro = self._bno055.gyro # self._log.info(' ' + Fore.YELLOW + Style.BRIGHT + 'gyroscope 0: \t{:>5.4f} rad/sec'.format(_gyro[0])) # self._log.info(' ' + Fore.YELLOW + Style.BRIGHT + 'gyroscope 1: \t{:>5.4f} rad/sec'.format(_gyro[1])) # self._log.info(' ' + Fore.YELLOW + Style.BRIGHT + 'gyroscope 2: \t{:>5.4f} rad/sec'.format(_gyro[2])) # 4. Acceleration Vector (100Hz) Three axis of acceleration (gravity + linear motion) in m/s^2 ............................... # _accel = self._bno055.acceleration # self._log.info(' ' + Fore.MAGENTA + Style.BRIGHT + 'accelerometer (m/s^2): {}'.format(_accel)) # 5. Magnetic Field Strength Vector (100Hz) Three axis of magnetic field sensing in micro Tesla (uT) ......................... # _magneto = self._bno055.magnetic # _mag_x = _magneto[0] * 100.0 # _mag_y = _magneto[1] * 100.0 # _mag_z = _magneto[2] * 100.0 # _simple_heading = ( 180.0 * math.atan2(_mag_y,_mag_x)/math.pi ) # if _simple_heading < 0: # _simple_heading += 360 # self._log.info(Fore.MAGENTA + Style.BRIGHT + 'simple:\t{:>5.2f}° \t{:>5.2f}°'.format(_simple_heading, _simple_heading - self._heading)) # else: # self._log.info(Fore.GREEN + Style.BRIGHT + 'simple:\t{:>5.2f}° \t{:>5.2f}°'.format(_simple_heading, _simple_heading - self._heading)) # r, p, h = self.getOrientation(_accel, _magneto) # deg = self.to_degrees(h) # # 2pi = 6.283185307 # if h > self._h_max: # self._h_max = h # if h < self._h_min: # self._h_min = h # self._log.debug(Fore.CYAN + Style.BRIGHT + 'pitch/roll/heading: {:>6.3f};\t{:>6.3f}'.format(p, r) \ # + Fore.MAGENTA + '\t{:>6.3f}rad\t{:>6.3f}°'.format(h, deg) + Fore.BLACK + '\t({:>6.3f}->{:>6.3f})'.format(self._h_min, self._h_max)) # 6. Linear Acceleration Vector (100Hz) Three axis of linear acceleration data (acceleration minus gravity) in m/s^2 ......... # self._log.info(' ' + Fore.BLUE + Style.BRIGHT + 'linear acceleration (m/s^2): {}'.format(self._bno055.linear_acceleration)) # 7. Gravity Vector (100Hz) Three axis of gravitational acceleration (minus any movement) in m/s^2 ........................... # self._log.info(' ' + Fore.WHITE + Style.BRIGHT + 'gravity (m/s^2): {}\n'.format(self._bno055.gravity)) # 8. Temperature (1Hz) Ambient temperature in degrees celsius ................................................................ # self._log.info(' ' + Fore.MAGENTA + Style.BRIGHT + 'temperature: {} degrees C'.format(self._bno055.temperature)) # time.sleep(self._loop_delay_sec) _rate.wait() else: self._log.info('disabled loop.') time.sleep(10) count += 1 self._log.debug('read loop ended.')