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)