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.')
class IMU():
'''
Composite IMU.
'''
def __init__(self, config, level):
self._log = Logger("imu", level)
if config is None:
raise ValueError("no configuration provided.")
# ICM20948 configuration .....................................
_icm20948_config = config['ros'].get('icm20948')
self._icm20948_heading_trim = _icm20948_config.get('heading_trim')
self._log.info('trim: heading: {:<5.2f}° (ICM20948)'.format(
self._icm20948_heading_trim))
self._icm20948 = ICM20948(i2c_addr=0x69)
self._amin = list(self._icm20948.read_magnetometer_data())
self._amax = list(self._icm20948.read_magnetometer_data())
# BNO055 configuration .......................................
self._bno055 = BNO055(config, Level.INFO)
self._log.info('ready.')
# ..........................................................................
def read_icm20948_magnetometer(self):
return self._icm20948.read_magnetometer_data()
# ..........................................................................
def read_icm20948_accelerometer_gyro_data(self):
# ax, ay, az, gx, gy, gz = _imu.read_accelerometer_gyro()
return self._icm20948.read_accelerometer_gyro_data()
# ..........................................................................
def read_icm20948_heading(self):
_mag = self._icm20948.read_magnetometer_data()
_orig_heading = Convert.heading_from_magnetometer(
self._amin, self._amax, _mag)
_heading = Convert.offset_in_degrees(_orig_heading,
self._icm20948_heading_trim)
self._log.info(Fore.GREEN + 'heading:\t{:>9.2f}°\t'.format(_heading) + Style.DIM \
+ 'orig: {:>9.2f}°\ttrim: {:>9.2f}°; icm20948'.format(_orig_heading, self._icm20948_heading_trim))
return _heading
# ..........................................................................
def read_bno055_heading(self):
_bno_reading = self._bno055.read()
return _bno_reading
# ..........................................................................
def read_heading(self):
'''
Read a composite heading, returning the BNO055 and ICM20948 values as a tuple.
'''
_bno_reading = self.read_bno055_heading()
_bno_heading = _bno_reading[1]
_icm20948_heading = self.read_icm20948_heading()
if _bno_heading and _icm20948_heading:
_diff = _bno_heading - _icm20948_heading
if _diff < 90.0:
self._log.info(Fore.CYAN + 'diff: {:5.2f}°\t'.format(_diff) +
Fore.BLACK +
'(bno: {:5.2f}°; icm: {:5.2f}°)'.format(
_bno_heading, _icm20948_heading))
else:
self._log.info(Fore.YELLOW + 'diff: {:5.2f}°\t'.format(_diff) +
Fore.BLACK +
'(bno: {:5.2f}°; icm: {:5.2f}°)'.format(
_bno_heading, _icm20948_heading))
return [_bno_heading, _icm20948_heading]
else:
self._log.info('unavailable.')
return [-1.0, -1.0]
class Blinka():
def __init__(self, GPIO, level):
self._log = Logger('blinka', level)
self._log.debug('initialising...')
self._pin_0 = 5
self._pin_1 = 6
self._pin_2 = 13
self.GPIO = GPIO
self.GPIO.setwarnings(False)
self.GPIO.setmode(GPIO.BCM)
self.GPIO.setup(self._pin_0, GPIO.OUT, initial=GPIO.LOW)
self.GPIO.setup(self._pin_1, GPIO.OUT, initial=GPIO.LOW)
self.GPIO.setup(self._pin_2, GPIO.OUT, initial=GPIO.LOW)
self._blink_thread = None
self._enabled = False
self._delay_sec = 0.3
self._log.info('ready.')
# ..........................................................................
def __blink__(self):
'''
The blinking thread.
'''
while self._enabled:
for i in range(0, 8):
_s = '{:03b}'.format(i)
_b0 = (_s[2] == '1')
_b1 = (_s[1] == '1')
_b2 = (_s[0] == '1')
self._log.info(Fore.GREEN + Style.BRIGHT + '{}: '.format(_s) +
Fore.CYAN +
'\t{}\t{}\t{}'.format(_b2, _b1, _b0))
self.GPIO.output(self._pin_0, _b0)
self.GPIO.output(self._pin_1, _b1)
self.GPIO.output(self._pin_2, _b2)
time.sleep(self._delay_sec)
# self.GPIO.output(self._pin_0,False)
# self.GPIO.output(self._pin_1,False)
# self.GPIO.output(self._pin_2,False)
# time.sleep(0.2)
self._log.info('and again...')
self._log.info('blink complete.')
# ..........................................................................
def blink(self, active):
if active:
if self._blink_thread is None:
self._log.debug('starting blink...')
self._enabled = True
self._blink_thread = threading.Thread(target=Blinka.__blink__,
args=[
self,
])
self._blink_thread.start()
else:
self._log.warning('ignored: blink already started.')
else:
if self._blink_thread is None:
self._log.debug('ignored: blink thread does not exist.')
else:
self._log.info('stop blinking...')
self._enabled = False
self._blink_thread.join()
self._blink_thread = None
self._log.info('blink thread ended.')
# ..........................................................................
def on(self):
self.GPIO.output(self._pin_0, True)
self.GPIO.output(self._pin_1, True)
self.GPIO.output(self._pin_2, True)
# ..........................................................................
def off(self):
self.GPIO.output(self._pin_0, False)
self.GPIO.output(self._pin_1, False)
self.GPIO.output(self._pin_2, False)
# ..........................................................................
def enable(self):
self._log.info('enable blinka.')
self.on()
# ..........................................................................
def disable(self):
self._log.info('disable blinka.')
self.off()
self._enabled = False
class Video():
'''
Provides a video-to-file and optional video-to-stream (web) functionality,
with options for dynamically altering the camera settings to account for
light levels based on a Pimoroni LTR-559 lux sensor (at 0x23), as well as
automatic and manual controls for camera lighting using either one or two
Pimoroni Matrix 11x7 displays (white LEDs, at 0x75 and 0x77).
The camera image is annotated with a title and timestamp. The output filename
is timestamped and written to a './videos' directory in H.264 video format.
'''
def __init__(self, config, level):
super().__init__()
global video_width, video_height, annotation_title
self._log = Logger('video', level)
if config is None:
raise ValueError("no configuration provided.")
self._config = config
_config = self._config['ros'].get('video')
self._enable_streaming = _config.get('enable_streaming')
self._port = _config.get('port')
self._lux_threshold = _config.get('lux_threshold')
self._counter = itertools.count()
self._server = None
# camera configuration
self._ctrl_lights = _config.get('ctrl_lights')
self._width = _config.get('width')
self._height = _config.get('height')
self._resolution = (self._width, self._height)
self._framerate = _config.get('framerate')
self._convert_mp4 = _config.get('convert_mp4')
self._remove_h264 = _config.get('remove_h264')
self._quality = _config.get('quality')
self._annotate = _config.get('annotate')
self._title = _config.get('title')
self._basename = _config.get('basename')
self._dirname = _config.get('dirname')
# set globals
video_width = self._width
video_height = self._height
annotation_title = self._title
self._filename = None
self._thread = None
self._killer = None
# scan I2c bus for devices
_i2c_scanner = I2CScanner(Level.DEBUG)
if _i2c_scanner.has_address([0x23]):
self._lux = Lux(Level.INFO)
self._log.info(
'LTR-559 lux sensor found: camera adjustment active.')
else:
self._lux = None
self._log.warning(
'no LTR-559 lux sensor found: camera adjustment inactive.')
# lighting configuration
self._port_light = None
self._stbd_light = None
if self._ctrl_lights:
if _i2c_scanner.has_address([0x77]): # port
self._port_light = Matrix(Orientation.PORT, Level.INFO)
self._log.info('port-side camera lighting available.')
else:
self._log.warning('no port-side camera lighting available.')
if _i2c_scanner.has_address([0x75]): # starboard
self._stbd_light = Matrix(Orientation.STBD, Level.INFO)
self._log.info('starboard-side camera lighting available.')
else:
self._log.warning(
'no starboard-side camera lighting available.')
else:
self._log.info('lighting control is disabled.')
if self._enable_streaming:
self._log.info('ready: streaming on port {:d}'.format(self._port))
else:
self._log.info('ready: save to file only, no streaming.')
# ..........................................................................
def set_compass(self, compass):
global g_compass
g_compass = compass # used by annotation
self._compass = compass
# ..........................................................................
@staticmethod
def get_annotation():
global annotation_title, g_compass
_heading = '' #g_compass.get_heading_message() if g_compass is not None else ''
return '{} {} {}'.format(
annotation_title,
dt.now(tzlocal.get_localzone()).strftime('%Y-%m-%d %H:%M:%S %Z'),
_heading)
# ..........................................................................
def is_night_mode(self):
if self._lux is None:
return False
_value = self._lux.get_value()
_threshold = self._lux_threshold
self._log.debug('lux: {:>5.2f} < threshold: {:>5.2f}?'.format(
_value, _threshold))
return _value < _threshold
# ..........................................................................
def _get_timestamp(self):
return dt.utcfromtimestamp(
dt.utcnow().timestamp()).isoformat().replace(':', '_').replace(
'-', '_').replace('.', '_')
# ..........................................................................
def get_filename(self):
return self._filename
# ..........................................................................
def get_ip_address(self):
_socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
try:
_socket.connect(('10.255.255.255', 1))
_ip = _socket.getsockname()[0]
except Exception:
_ip = '127.0.0.1'
finally:
_socket.close()
return _ip
# ..........................................................................
def _start(self, output_splitter, f_is_enabled):
global output # necessary for access from StreamingHandler, passed as type
output = output_splitter
self._output = output_splitter
self._filename = output_splitter.get_filename()
if self._enable_streaming:
self._log.info('starting video with capture to file: {}'.format(
output_splitter.get_filename()))
else:
self._log.info('starting capture to file: {}'.format(
output_splitter.get_filename()))
with picamera.PiCamera(resolution=self._resolution,
framerate=self._framerate) as camera:
self._log.info('camera framerate: {}'.format(camera.framerate))
self._log.info('camera ISO: {}'.format(camera.iso))
self._log.info('camera mode: {}'.format(camera.exposure_mode))
self._log.info('camera shutter speed: {}'.format(
camera.shutter_speed))
if self._annotate:
camera.annotate_text_size = 12
self.set_night_mode(camera, self.is_night_mode())
# camera.annotate_text = Video.get_annotation() # initial text
# start video annotation thread
self._annot = threading.Thread(
target=Video._annotate, args=[self, camera, f_is_enabled])
self._annot.setDaemon(True)
self._annot.start()
if self._quality > 0:
# values 1 (highest quality) to 40 (lowest quality), with typical values between 20 and 25
self._log.info('camera quality: {}'.format(self._quality))
camera.start_recording(output_splitter,
format='mjpeg',
quality=self._quality)
else:
camera.start_recording(output_splitter, format='mjpeg')
# ............
try:
if self._enable_streaming:
if self._server is None:
self._log.info('starting streaming server...')
address = ('', self._port)
self._server = StreamingServer(address,
StreamingHandler,
f_is_enabled)
self._killer = lambda: self.close(
camera, output_splitter)
self._server.serve_forever()
else:
self._log.info('streaming server already active.')
else: # keepalive
while f_is_enabled():
self._log.debug('keep-alive...')
time.sleep(1.0)
self._log.info(Fore.RED + 'exited video loop.')
except Exception:
self._log.error('error streaming video: {}'.format(
traceback.format_exc()))
finally:
self._log.info(Fore.RED + 'finally.')
# self.close(camera, output_splitter)
self._log.info('_start: complete.')
# ..........................................................................
def _annotate(self, camera, f_is_enabled):
'''
Update the video annotation every second.
'''
self._camera = camera
_count = 0
while f_is_enabled():
# _count = next(self._counter)
self._camera.annotate_text = Video.get_annotation()
# if ( _count % 5 ) == 0: # every five seconds
self.set_night_mode(camera, self.is_night_mode())
time.sleep(1.0)
# ..........................................................................
def set_night_mode(self, camera, enabled):
# NOTE: setting 'iso' overrides exposure mode
_compass_calibrated = False # True if self._compass and self._compass.is_calibrated() else False
if enabled:
self._log.debug('night mode.')
# camera.exposure_mode = 'nightpreview'
'''
off
auto: use automatic exposure mode
night: select setting for night shooting
nightpreview:
backlight: select setting for backlit subject
spotlight:
sports: select setting for sports (fast shutter etc.)
snow: select setting optimised for snowy scenery
beach: select setting optimised for beach
verylong: select setting for long exposures
fixedfps: constrain fps to a fixed value
antishake: antishake mode
fireworks: select setting optimised for fireworks
source: https://www.raspberrypi.org/documentation/raspbian/applications/camera.md
'''
camera.iso = 800
camera.led = False
camera.annotate_foreground = Color.from_string('#ffdada')
if _compass_calibrated:
camera.annotate_background = Color.from_string('#440000')
else:
camera.annotate_background = Color.from_string('#440088')
if self._ctrl_lights:
self.set_lights(True)
else:
self._log.debug('day mode.')
camera.iso = 100
# camera.exposure_mode = 'off'
camera.led = True
camera.annotate_foreground = Color.from_string('#111111')
if _compass_calibrated:
camera.annotate_background = Color.from_string('#ffffff')
else:
camera.annotate_background = Color.from_string('#ffff00')
if self._ctrl_lights:
self.set_lights(False)
# ..........................................................................
def set_lights(self, on):
if self._port_light:
if on:
self._port_light.light()
else:
self._port_light.clear()
if self._stbd_light:
if on:
self._stbd_light.light()
else:
self._stbd_light.clear()
# ..........................................................................
def is_enabled(self):
return self._enabled
# ..........................................................................
@property
def active(self):
return self._thread is not None
# ..........................................................................
def start(self):
if self._thread is not None:
self._log.info('video already started.')
return
self._log.info('start.')
self._enabled = True
if not os.path.isdir(self._dirname):
os.makedirs(self._dirname)
self._filename = os.path.join(
self._dirname,
self._basename + '_' + self._get_timestamp() + '.h264')
_output = OutputSplitter(self._filename)
self._thread = threading.Thread(target=Video._start,
args=[
self,
_output,
lambda: self.is_enabled(),
])
self._thread.setDaemon(True)
self._thread.start()
_ip = self.get_ip_address()
self._log.info(
Fore.MAGENTA + Style.BRIGHT +
'video started on:\thttp://{}:{:d}/'.format(_ip, self._port))
# ..........................................................................
def stop(self):
if self._thread is None:
self._log.info('video already stopped.')
return
self._log.info('stopping video capture on file: {}'.format(
self._filename))
print(Fore.GREEN + 'setting enabled flag to False.' + Style.RESET_ALL)
self._enabled = False
if self._killer is not None:
print(Fore.GREEN + 'KILLING...' + Style.RESET_ALL)
self._killer()
else:
print(Fore.GREEN + 'NO KILLER.' + Style.RESET_ALL)
if self._output is not None:
print(Fore.GREEN + 'flushing and closing...' + Style.RESET_ALL)
self._output.flush()
self._output.close()
self._output = None
print(Fore.GREEN + 'closed.' + Style.RESET_ALL)
self._log.info('joining thread...')
self._thread.join(timeout=1.0)
self._thread = None
if self._convert_mp4:
self._convert_to_mp4()
self._log.info('stopped.')
# ..........................................................................
def _convert_to_mp4(self):
if os.path.exists(self._filename):
self._log.info('converting file {} to mp4...'.format(
self._filename))
_mp4_filename = Path(self._filename).stem + ".mp4"
os.system('ffmpeg -loglevel panic -hide_banner -r {:d} -i {:s} -vcodec copy {:s}'.format(\
self._framerate, self._filename, _mp4_filename))
self._log.info('mp4 conversion complete.')
if self._remove_h264:
os.remove(self._filename)
self._log.info('removed h264 video source.')
else:
self._log.warning(
'could not convert to mp4: file {} did not exist.'.format(
self._filename))
# ..........................................................................
def close(self, camera, output):
self._log.info('closing video...')
if self._ctrl_lights:
self.set_lights(False)
if camera:
if camera.recording:
camera.stop_recording()
self._log.debug('camera stopped recording.')
if not camera.closed:
camera.close()
self._log.debug('camera closed.')
if output:
output.flush()
self._log.debug('output flushed.')
output.close()
self._log.debug('output closed.')
if self._server is not None:
self._log.info('shutting down server...')
self._server.shutdown()
self._log.info('server shut down.')
self._log.info(Fore.MAGENTA + Style.BRIGHT + 'video closed.')
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.')
class MotionDetector():
'''
Uses an infrared PIR sensor to scan for cats, or other causes of motion.
'''
def __init__(self, config, message_factory, message_bus, level):
self._log = Logger('motion-detect', Level.INFO)
if config is None:
raise ValueError('no configuration provided.')
self._message_factory = message_factory
self._message_bus = message_bus
_config = config['ros'].get('motion_detect')
_pin = _config.get('pin')
_threshold_value = 0.1 # the value above which the device will be considered “on”
# set up pin where PIR is connected
self._sensor = MotionSensor(_pin, threshold=_threshold_value, pull_up=False)
self._sensor.when_motion = self._activated
self._sensor.when_no_motion = self._deactivated
self._disabling = False
self._enabled = False
self._closed = False
# arm behaviour
self._arm_movement_degree_step = 5.0
self._arm_up_delay = 0.09
self._arm_down_delay = 0.04
self._log.info('ready.')
# ..........................................................................
def _activated(self):
'''
The default function called when the sensor is activated.
'''
if self._enabled:
self._log.info(Fore.YELLOW + 'detected motion!')
self._message_bus.handle(self._message_factory.get_message(Event.MOTION_DETECT, True))
else:
self._log.info('motion detector not enabled.')
# ..........................................................................
def _deactivated(self):
'''
The default function called when the sensor is deactivated.
'''
if self._enabled:
self._log.info('deactivated motion detector.')
else:
self._log.debug('motion detector not enabled.')
# ..........................................................................
def enable(self):
self._log.debug('enabling...')
if self._closed:
self._log.warning('cannot enable: closed.')
return
self._enabled = True
self._log.debug('enabled.')
# ..........................................................................
def disable(self):
if self._disabling:
self._log.warning('already disabling.')
else:
self._disabling = True
self._enabled = False
self._log.debug('disabling...')
self._disabling = False
self._log.debug('disabled.')
# ..........................................................................
def close(self):
self.disable()
self._closed = True
def test_matrix():
_log = Logger('matrix-test', Level.INFO)
_matrices = None
try:
_log.info(Fore.CYAN + 'start matrices test...')
_matrices = Matrices(Level.INFO)
_log.info('matrix write text...')
_matrices.text('HE', 'LP')
time.sleep(3)
_matrices.clear()
time.sleep(1)
_log.info('matrix on...')
_matrices.on()
time.sleep(2)
_log.info('matrix off...')
_matrices.clear()
time.sleep(1)
_log.info('manual gradient wipes...')
for i in range(1, 8):
_matrices.vertical_gradient(i)
time.sleep(0.02)
for i in range(7, -1, -1):
_matrices.vertical_gradient(i)
time.sleep(0.02)
time.sleep(1)
for i in range(1, 11):
_matrices.horizontal_gradient(i)
time.sleep(0.02)
for i in range(11, -1, -1):
_matrices.horizontal_gradient(i)
time.sleep(0.02)
time.sleep(1)
_log.info('starting matrix vertical wipe...')
_matrices.wipe(Matrices.DOWN, True, 0.00)
time.sleep(0.0)
_matrices.wipe(Matrices.DOWN, False, 0.00)
_matrices.clear()
time.sleep(1)
_log.info('starting matrix horizontal wipe right...')
_matrices.wipe(Matrices.RIGHT, True, 0.00)
time.sleep(0.0)
_matrices.wipe(Matrices.RIGHT, False, 0.00)
_matrices.clear()
# UP and LEFT not implemented
except KeyboardInterrupt:
_log.info(Fore.MAGENTA + 'Ctrl-C caught: interrupted.')
finally:
_log.info('closing matrix test...')
if _matrices:
_matrices.clear()
class HardwareClock(object):
'''
A hardware clock based on using one of the Raspberry Pi's hardware
clocks as a 20Hz system clock. In order to enable hardware-based
pulse-width modulation (PWM):
* Edit /boot/config.txt.
* Add the line dtoverlay=pwm-2chan
* Save the file.
* Reboot.
This automagically creates a new directory at:
/sys/class/pwm/pwmchip0
containing a bunch of files. If you write a "1" to the 'export' file,
this will create a new directory:
/sys/class/pwm/pwmchip0/pwm1/
which will contain a number of configuration files, generally each
containing a single line containing a single value.
For a 20Hz system clock the period is 50ms, which must be written to
the 'period' configuration file as a value in nanoseconds. Likewise,
the 50% duty cycle is expressed as half of the period and written to
the 'duty_cycle' file. Then a "1" (True) is written to the 'enable'
file to start the timer. This is shown below:
echo 50000000 > /sys/class/pwm/pwmchip0/pwm1/period
echo 25000000 > /sys/class/pwm/pwmchip0/pwm1/duty_cycle
echo 1 > /sys/class/pwm/pwmchip0/pwm1/enable
And that's it. You can connect an LED between BCM pin 19 and ground
through an appropriate resistor and you'll see it vibrating away at
20Hz.
'''
def __init__(self, level):
self._log = Logger("hwclock", level)
self.check_boot_config()
self.configure()
self._callbacks = []
_pin = 21 # BCM 21
self._sensor = DigitalInputDevice(_pin, pull_up=False)
self._sensor.when_activated = self._activated
# self._sensor.when_deactivated = self._deactivated
self._enabled = False
# testing ....................
self._dt_ms = 50.0 # loop delay in milliseconds
self._queue_len = 50 # larger number means it takes longer to change
self._queue = Deque([], maxlen=self._queue_len)
self._counter = itertools.count()
self._last_time = dt.now()
self._max_error = 0.0
self._max_vari = 0.0
self._log.info('ready.')
# ..........................................................................
def _get_mean(self, value):
'''
Returns the mean of measured values.
'''
self._queue.append(value)
_n = 0
_mean = 0.0
for x in self._queue:
_n += 1
_mean += (x - _mean) / _n
if _n < 1:
return float('nan')
else:
return _mean
# ..........................................................................
def add_callback(self, callback):
self._callbacks.append(callback)
# ..........................................................................
def _activated(self):
'''
The default function called when the sensor is activated.
'''
if self._enabled:
for callback in self._callbacks:
callback()
else:
self._log.info('hardware clock disabled.')
# ..........................................................................
def _deactivated(self):
'''
The default function called when the sensor is deactivated.
'''
if self._enabled:
self._log.info('tick off!')
else:
self._log.info('hardware clock disabled.')
# ..........................................................................
def set_messaging(self, message_bus, message_factory):
self._message_factory = message_factory
# ..........................................................................
def check_boot_config(self):
_term = 'dtoverlay=pwm-2chan'
with open(HWCLOCK_BOOTCONFIG_PATH, ) as f:
lines = [line.rstrip() for line in f]
for line in lines:
if line.startswith(_term):
self._log.info(Fore.CYAN +
'PWM support found in boot configuration.' +
Style.RESET_ALL)
return
sys.exit(Fore.RED + '''
For a hardware clock, PWM support needs to be added to the boot configuration:
* Edit the file: /boot/config.txt
* Add the line: dtoverlay=pwm-2chan
* Save the file.
* Reboot.
''' + Style.RESET_ALL)
# ..........................................................................
def configure(self):
# static logger specific to configure()
if not path.exists(HWCLOCK_PWM1_PATH):
if os.geteuid() != 0:
sys.exit(
Fore.RED +
'You need to have root privileges to run this script.\nPlease try again, this time using \'sudo\'. Exiting.'
+ Style.RESET_ALL)
self._log.info(Fore.CYAN +
'no hardware clock configured; configuring...' +
Style.RESET_ALL)
HardwareClock._write_to_file(HWCLOCK_EXPORT_PATH, '1')
if path.exists(HWCLOCK_PWM1_PATH):
self._log.info(Fore.CYAN +
'created PWM configuration files at: {}'.format(
HWCLOCK_PWM1_PATH) + Style.RESET_ALL)
else:
raise Exception(
'cannot continue: expected creation of pwm1 directory.')
sys.exit(1)
else:
self._log.info('hardware clock already configured.' +
Style.RESET_ALL)
self._log.info('found existing PWM configuration files at: {}'.format(
HWCLOCK_PWM1_PATH) + Style.RESET_ALL)
# now continue: we'll overwrite existing values since it doesn't hurt.
#
# For a 20Hz system clock the period is 50ms, but must be written to the
# configuration file in nanoseconds. Likewise, the 50% duty cycle is
# expressed as half of the period and written to a separate file. Finally,
# a "1" is written to the 'enable' file to start the timer:
#
# echo 50000000 > /sys/class/pwm/pwmchip0/pwm1/period
HardwareClock._write_to_file(HWCLOCK_PERIOD_PATH, '50000000')
# echo 25000000 > /sys/class/pwm/pwmchip0/pwm1/duty_cycle
HardwareClock._write_to_file(HWCLOCK_DUTY_CYCLE_PATH, '25000000')
# echo 1 > /sys/class/pwm/pwmchip0/pwm1/enable
if self.enabled:
self._log.info(Fore.CYAN + 'currently enabled.')
else:
self._log.info(Fore.CYAN + 'currently disabled; enabling...')
self.enable()
self._log.info(Fore.CYAN +
'configuration complete: enabled hardware clock: {}'.
format(HWCLOCK_ENABLE_PATH) + Style.RESET_ALL)
# ..........................................................................
@staticmethod
def _write_to_file(path, data):
f = open(path, "w")
f.write(data)
f.close()
# ..........................................................................
def add_test_callback(self):
'''
Adds a callback function used for testing.
'''
self.add_callback(self.test_callback_method)
# ..........................................................................
def test_callback_method(self):
if self._enabled:
_now = dt.now()
self._count = next(self._counter)
_delta_ms = 1000.0 * (_now - self._last_time).total_seconds()
_mean = self._get_mean(_delta_ms)
if len(self._queue) > 5:
_error_ms = _delta_ms - self._dt_ms
self._max_error = max(self._max_error, _error_ms)
_vari1 = statistics.variance(
self._queue) # if len(self._queue) > 5 else 0.0
_vari = sum((item - _mean)**2
for item in self._queue) / (len(self._queue) - 1)
self._max_vari = max(self._max_vari, _vari)
if self._count <= self._queue_len:
_vari = 0.0
self._max_vari = 0.0
else:
self._max_vari = max(self._max_vari, _vari)
if _error_ms > 0.5000:
self._log.info(
Fore.RED + Style.BRIGHT +
'[{:05d}]\tΔ {:8.5f}; err: {:8.5f}; '.format(
self._count, _delta_ms, _error_ms) + Fore.CYAN +
Style.NORMAL +
'max err: {:8.5f}; mean: {:8.5f}; max vari: {:8.5f}'.
format(self._max_error, _mean, self._max_vari))
elif _error_ms > 0.1000:
self._log.info(
Fore.YELLOW + Style.BRIGHT +
'[{:05d}]\tΔ {:8.5f}; err: {:8.5f}; '.format(
self._count, _delta_ms, _error_ms) + Fore.CYAN +
Style.NORMAL +
'max err: {:8.5f}; mean: {:8.5f}; max vari: {:8.5f}'.
format(self._max_error, _mean, self._max_vari))
else:
self._log.info(
Fore.GREEN +
'[{:05d}]\tΔ {:8.5f}; err: {:8.5f}; '.format(
self._count, _delta_ms, _error_ms) + Fore.CYAN +
'max err: {:8.5f}; mean: {:8.5f}; max vari: {:8.5f}'.
format(self._max_error, _mean, self._max_vari))
else:
self._log.info(
Fore.CYAN +
'[{:05d}]\tΔ {:8.5f}; '.format(self._count, _delta_ms) +
Fore.CYAN + 'mean: {:8.5f}'.format(_mean))
self._last_time = _now
if self._count > 1000:
self._log.info('reached count limit, exiting...')
self.disable()
# sys.exit(0)
# ..........................................................................
@property
def enabled(self):
if not path.exists(HWCLOCK_ENABLE_PATH):
raise Exception(
'hardware clock enable file \'{}\' not found.'.format(
HWCLOCK_ENABLE_PATH))
f = open(HWCLOCK_ENABLE_PATH, "r")
line = f.readline().strip()
# self._log.info(Fore.GREEN + 'line: "{}"'.format(line) + Style.RESET_ALL)
return line == '1'
# ..........................................................................
def enable(self):
if not path.exists(HWCLOCK_ENABLE_PATH):
raise Exception(
'hardware clock enable file \'{}\' not found.'.format(
HWCLOCK_ENABLE_PATH))
self._enabled = True
HardwareClock._write_to_file(HWCLOCK_ENABLE_PATH, '1')
self._log.info('enabled.')
# ..........................................................................
def disable(self):
self._callbacks = []
if not path.exists(HWCLOCK_ENABLE_PATH):
raise Exception(
'hardware clock enable file \'{}\' not found.'.format(
HWCLOCK_ENABLE_PATH))
HardwareClock._write_to_file(HWCLOCK_ENABLE_PATH, '0')
self._enabled = False
self._log.info('disabled.')
# ..........................................................................
def close(self):
self.disable()
class BNO055:
'''
Reads from a BNO055 9DoF sensor. In order for this to be used by a
Raspberry Pi you must slow your I2C bus down to around 10kHz.
Note that this has been calibrated based on the orientation of the
BNO055 board as installed on the KR01 robot, with the dot on the
chip (mounted horizontally) as follows:
Fore
--------------
| • |
| |
| |
Port | | Starboard
| |
--------------
Aft
If you mount the chip in a different orientation you will likely
need to multiple one or more of the axis by -1.0.
'''
# permitted error between Euler and Quaternion (in degrees) to allow setting value
ERROR_RANGE = 5.0 # was 3.0
# ..........................................................................
def __init__(self, config, queue, level):
self._log = Logger("bno055", level)
if config is None:
raise ValueError("no configuration provided.")
self._config = config
self._queue = queue
_config = self._config['ros'].get('bno055')
self._quaternion_accept = _config.get(
'quaternion_accept') # permit Quaternion once calibrated
self._loop_delay_sec = _config.get('loop_delay_sec')
_i2c_device = _config.get('i2c_device')
# 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
self._thread = None
self._enabled = False
self._closed = False
self._heading = None
self._pitch = None
self._roll = None
self._is_calibrated = False
self._log.info('ready.')
# ..........................................................................
def enable(self):
if not self._closed:
self._enabled = True
# if we haven't started the thread yet, do so now...
if self._thread is None:
self._thread = threading.Thread(target=BNO055._read,
args=[self])
self._thread.start()
self._log.info('enabled.')
else:
self._log.warning('cannot enable: already closed.')
# ..........................................................................
def to_degrees(radians):
return math.degrees(radians)
# ..........................................................................
def to_radians(degrees):
return math.radians(degrees)
# ..........................................................................
def get_heading(self):
return self._heading
# ..........................................................................
def get_pitch(self):
return self._pitch
# ..........................................................................
def get_roll(self):
return self._roll
# ..........................................................................
def is_calibrated(self):
return self._is_calibrated
# ..........................................................................
def _quaternion_to_euler_angle(self, w, x, y, z):
q = Quaternion(w, x, y, z)
self._log.info(
Fore.BLUE +
'{:>6.3f},\t{:>6.3f},\t{:>6.3f},\t{:>6.3f}'.format(x, x, y, z) +
Style.RESET_ALL)
deg = q.degrees
return deg
# ..........................................................................
def _quaternion_to_euler_angle_other(self, w, x, y, z):
ysqr = y * y
t0 = +2.0 * (w * x + y * z)
t1 = +1.0 - 2.0 * (x * x + ysqr)
X = numpy.degrees(numpy.arctan2(t0, t1))
t2 = +2.0 * (w * y - z * x)
t2 = numpy.clip(t2, a_min=-1.0, a_max=1.0)
Y = numpy.degrees(numpy.arcsin(t2))
t3 = +2.0 * (w * z + x * y)
t4 = +1.0 - 2.0 * (ysqr + z * z)
Z = numpy.degrees(numpy.arctan2(t3, t4))
return X, Y, Z
# ..........................................................................
def _quaternion_to_euler(self, w, x, y, z):
t0 = +2.0 * (w * x + y * z)
t1 = +1.0 - 2.0 * (x * x + y * y)
roll = math.atan2(t0, t1)
t2 = +2.0 * (w * y - z * x)
t2 = +1.0 if t2 > +1.0 else t2
t2 = -1.0 if t2 < -1.0 else t2
pitch = math.asin(t2)
t3 = +2.0 * (w * z + x * y)
t4 = +1.0 - 2.0 * (y * y + z * z)
heading = math.atan2(t3, t4)
return [heading, pitch, roll]
# ..........................................................................
def _convert_to_euler(self, qw, qx, qy, qz):
# can get the euler angles back out in degrees (set to True)
_euler = quat2euler(qw, qx, qy, qz, degrees=True)
_heading = -1.0 * _euler[2]
_pitch = _euler[1]
_roll = -1.0 * _euler[0]
return [_heading, _pitch, _roll]
# ..........................................................................
@staticmethod
def _in_range(p, q):
return p >= (q - BNO055.ERROR_RANGE) and p <= (q + BNO055.ERROR_RANGE)
# ..........................................................................
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 ................
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:
_heading = _euler[0]
if _euler[1] is not None:
_pitch = -1.0 * _euler[1]
if _euler[2] is not None:
_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 (_heading, _pitch, _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_euler = self._quaternion_to_euler_angle(
_quat_w, _quat_x, _quat_y, _quat_z)
_q_heading = _q_euler #[0]
# _q_pitch = 0.0 # _q_euler[1]
# _q_roll = 0.0 # _q_euler[2]
# heading ............................................................
if BNO055._in_range(_heading, _q_heading):
self._heading = _heading
self._log.debug(
' ' + Fore.CYAN + Style.NORMAL +
'heading: {:>5.4f}° (calibrated)'.format(_heading))
elif self._quaternion_accept: # if true, we permit Quaternion once we've achieved calibration
self._heading = _q_heading
self._log.debug(' ' + Fore.CYAN + Style.DIM +
'heading: {:>5.4f}° (quaternion-only)'.
format(_q_heading))
else:
self._log.debug(' ' + Fore.YELLOW + Style.DIM + 'heading: {:>5.4f}° (euler); '.format(_heading) \
+ Fore.GREEN + Style.DIM + '{:>5.4f}° (quaternion)'.format(_q_heading))
self._heading = None
# # pitch ..............................................................
# if BNO055._in_range(_pitch, _q_pitch):
# self._pitch = _pitch
# self._log.debug(' ' + Fore.CYAN + Style.BRIGHT + 'pitch: \t{:>5.4f}° (calibrated)'.format(_pitch))
# else:
# self._log.debug(' ' + Fore.YELLOW + Style.BRIGHT + 'pitch: \t{:>5.4f}° (euler)'.format(_pitch))
# self._log.debug(' ' + Fore.GREEN + Style.BRIGHT + 'pitch: \t{:>5.4f}° (quaternion)'.format(_q_pitch))
# self._pitch = None
# # roll ...............................................................
# if BNO055._in_range(_roll, _q_roll):
# self._roll = _roll
# self._log.debug(' ' + Fore.CYAN + Style.BRIGHT + 'roll: \t{:>5.4f}° (calibrated)'.format(_roll))
# else:
# self._log.debug(' ' + Fore.YELLOW + Style.BRIGHT + 'roll: \t{:>5.4f}° (euler)'.format(_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 ...............................
# self._log.info(' ' + Fore.MAGENTA + Style.BRIGHT + 'accelerometer (m/s^2): {}'.format(self._bno055.acceleration))
# 5. Magnetic Field Strength Vector (100Hz) Three axis of magnetic field sensing in micro Tesla (uT) .........................
# _magneto = self._bno055.magnetic
# self._log.info(' ' + Fore.RED + Style.BRIGHT + 'magnetometer 0:\t{:>5.2f} microteslas'.format(_magneto[0]))
# self._log.info(' ' + Fore.GREEN + Style.BRIGHT + 'magnetometer 1:\t{:>5.2f} microteslas'.format(_magneto[1]))
# self._log.info(' ' + Fore.BLUE + Style.BRIGHT + 'magnetometer 2:\t{:>5.2f} microteslas'.format(_magneto[2]))
# 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)
else:
self._log.info('disabled loop.')
time.sleep(10)
count += 1
self._log.debug('read loop ended.')
# ..........................................................................
def disable(self):
self._enabled = False
self._log.info('disabled.')
# ..........................................................................
def close(self):
self._closed = True
self._log.info('closed.')
class SlewLimiter():
'''
A general purpose slew limiter that limits the rate of change of a value.
This uses the ros:slew: section of the YAML configuration.
Parameters:
:param config: application configuration
:param orientation: motor orientation, used only for logging label (optional)
:param level: the logging Level
'''
def __init__(self, config, orientation, level):
if orientation is None:
self._log = Logger('slew', level)
else:
self._log = Logger('slew:{}'.format(orientation.label), level)
self._millis = lambda: int(round(time.time() * 1000))
self._seconds = lambda: int(round(time.time()))
self._clamp = lambda n: self._minimum_output if n <= self._minimum_output else self._maximum_output if n >= self._maximum_output else n
# Slew configuration .........................................
cfg = config['ros'].get('slew')
self._minimum_output = cfg.get('minimum_output')
self._maximum_output = cfg.get('maximum_output')
self._log.info('minimum output: {:5.2f}; maximum output: {:5.2f}'.format(self._minimum_output, self._maximum_output))
self._rate_limit = SlewRate.FAST.limit # default rate_limit: 0.0025 # value change permitted per millisecond
self._stats_queue = None
self._start_time = None
self._enabled = False
self._log.info('ready.')
# ..........................................................................
def set_rate_limit(self, slew_rate):
'''
Sets the slew rate limit to the argument, in value/second. This
overrides the value set in configuration. The default is NORMAL.
'''
if not isinstance(slew_rate, SlewRate):
raise Exception('expected SlewRate argument, not {}'.format(type(slew_rate)))
self._rate_limit = slew_rate.limit
self._log.info('slew rate limit set to {:>6.4f}/cycle.'.format(self._rate_limit))
# ..........................................................................
def is_enabled(self):
return self._enabled
# ..........................................................................
def enable(self):
self._log.info('starting slew limiter with rate limit of {:5.3f}/cycle.'.format(self._rate_limit))
self._enabled = True
self._start_time = self._millis()
self._log.info('enabled.')
# ..........................................................................
def disable(self):
self._log.info(Fore.MAGENTA + 'slew disabled.')
self._enabled = False
# ..........................................................................
def reset(self, value):
'''
Resets the elapsed timer.
'''
self._start_time = self._millis()
# ..........................................................................
def slew(self, current_value, target_value):
'''
The returned result is the maximum amount of change between the current value
and the target value based on the amount of elapsed time between calls (in
milliseconds) multiplied by a constant set in configuration.
If not enabled this returns the passed target value argument.
'''
if not self._enabled:
self._log.warning('disabled; returning original target value {:+06.2f}.'.format(target_value))
return target_value
self._log.debug(Fore.BLACK + 'slew from current {:+06.2f} to target value {:+06.2f}.'.format(current_value, target_value))
_now = self._millis()
_elapsed = _now - self._start_time
if target_value == current_value:
_value = current_value
# self._log.debug(Fore.BLACK + 'already there; returning target: {:+06.2f})'.format(_value))
elif target_value > current_value: # increasing ..........
_min = current_value - ( self._rate_limit * _elapsed )
_max = current_value + ( self._rate_limit * _elapsed )
_value = numpy.clip(target_value, _min, _max)
# self._log.debug(Fore.GREEN + '-value: {:+06.2f} = numpy.clip(target_value: {:+06.2f}), _min: {:+06.2f}), _max: {:+06.2f}); elapsed: {:+06.2f}'.format(_value, target_value, _min, _max, _elapsed))
else: # decreasing .......................................
_min = current_value - ( self._rate_limit * _elapsed )
_max = current_value + ( self._rate_limit * _elapsed )
_value = numpy.clip(target_value, _min, _max)
# self._log.debug(Fore.RED + '-value: {:+06.2f} = numpy.clip(target_value: {:+06.2f}), _min: {:+06.2f}), _max: {:+06.2f}); elapsed: {:+06.2f}'.format(_value, target_value, _min, _max, _elapsed))
# clip the output between min and max set in config (if negative we fix it before and after)
if _value < 0:
_clamped_value = -1.0 * self._clamp(-1.0 * _value)
else:
_clamped_value = self._clamp(_value)
# self._log.debug('slew from current {:>6.2f} to target {:>6.2f} returns:'.format(current_value, target_value) \
# + Fore.MAGENTA + '\t{:>6.2f}'.format(_clamped_value) + Fore.BLACK + ' (clamped from {:>6.2f})'.format(_value))
return _clamped_value
class Fan(object):
'''
Uses an HT0740 switch to turn a cooling fan on or off.
'''
def __init__(self, config, level):
self._log = Logger("fan", level)
if config is None:
raise ValueError('no configuration provided.')
_config = config['ros'].get('fan')
_i2c_address = _config.get('i2c_address')
self._fan_threshold = _config.get('fan_threshold')
self._hysteresis = _config.get('hysteresis')
self._log.info('setpoint temperature: {:5.2f}°C; hysteresis: {:2.1f}°C.'.format(self._fan_threshold, self._hysteresis))
self._switch = HT0740(i2c_addr=_i2c_address)
self._log.info('enabling fan.')
self._switch.enable()
self._enabled = False
self._log.info('ready.')
# ..........................................................................
def react_to_temperature(self, temperature):
'''
Performs typical thermostat behaviour:
* If the CPU temperature is higher than the setpoint, turn the fan on.
* If the CPU temperature is lower than the setpoint minus hysteresis, turn the fan off.
We don't use the hysteresis when turning the fan on.
'''
if temperature > self._fan_threshold:
if self.enable():
self._log.info(Fore.YELLOW + 'enable fan: cpu temperature: {:5.2f}°C.'.format(temperature))
elif temperature < ( self._fan_threshold - self._hysteresis ):
if self.disable():
self._log.info(Fore.YELLOW + Style.DIM + 'disable fan: cpu temperature: {:5.2f}°C.'.format(temperature))
# ..........................................................................
@property
def enabled(self):
return self._enabled
# ..........................................................................
def enable(self):
'''
Enables the fan, returning True if calling the function
resulted in a change in state.
'''
if self._enabled:
self._log.debug('already enabled.')
return False
else:
self._switch.switch.on()
self._switch.led.on()
self._enabled = True
self._log.info('enabled.')
return True
# ..........................................................................
def disable(self):
'''
Disables the fan, returning True if calling the function
resulted in a change in state.
'''
if self._enabled:
self._switch.switch.off()
self._switch.led.off()
self._enabled = False
self._log.info('disabled.')
return True
else:
self._log.debug('already disabled.')
return False
# ..........................................................................
def close(self):
if self._enabled:
self.disable()
self._log.info('closed.')
class FileWriter():
'''
The FileWriter writes content to a file located in a directory (set
in configuration) which will be created if it doesn't yet exist.
It must be enabled in order to start its data write thread.
It two options:
* filename: the optional filename of the output file. If unspecified,
the filename used will include a timestamp.
* level: the Log level
'''
def __init__(self, config, filename, level):
self._log = Logger("filewriter", level)
self._enabled = False
self._active = False
self._thread = None
# configuration ..............................................
cfg = config['ros'].get('filewriter')
_extension = cfg.get('extension')
_directory_name = cfg.get('directory_name')
_default_filename_prefix = cfg.get('default_filename_prefix')
self._gnuplot_template_file = cfg.get(
'gnuplot_template_file') # template for gnuplot settings
self._gnuplot_output_file = cfg.get(
'gnuplot_output_file') # output file for gnuplot settings
if not os.path.exists(_directory_name):
os.makedirs(_directory_name)
if filename is not None:
if filename.endswith(_extension):
self._filename = _directory_name + '/' + filename
else:
self._filename = _directory_name + '/' + filename + _extension
else:
# create os-friendly filename including current timestamp
self._filename = _directory_name + '/' + _default_filename_prefix \
+ datetime.utcfromtimestamp(datetime.utcnow().timestamp()).isoformat().replace(':','_').replace('-','_').replace('.','_') + _extension
self._log.info('ready.')
# ..........................................................................
def get_filename(self):
return self._filename
# ..........................................................................
def is_enabled(self):
return self._enabled
# ..........................................................................
def enable(self, queue):
'''
Enables and starts the data collection thread using a
producer-consumer pattern. The passed parameters include the
deque that will be actively populated with data by the producer.
'''
if self._thread is None:
self._enabled = True
self._log.debug('starting file writer thread...')
self._thread = threading.Thread(target=FileWriter._writer,
args=[
self,
queue,
lambda: self.is_enabled(),
])
self._thread.start()
self._log.debug('enabled.')
else:
self._log.warning('already enabled.')
# ..........................................................................
def disable(self):
if self._enabled:
self._enabled = False
self._log.info('closing...')
while self._active:
self._log.warning('waiting for file writer to close...')
time.sleep(0.5)
self._log.info('joining file write thread...')
self._thread.join()
self._thread = None
self._log.info('file write thread joined.')
self._log.info('closed.')
else:
self._log.info('already closed.')
# ..........................................................................
def write_gnuplot_settings(self, data):
'''
Using the provided array of data, reads in the gnuplot template,
substitutes the data arguments into the imported string, then
writes the output file as the gnuplot settings file.
'''
try:
# read template for gnuplot settings
_fin = open(self._gnuplot_template_file, "r")
_string = _fin.read()
_template = Template(_string)
_elapsed = data[0]
self._log.debug('ELAPSED={}'.format(_elapsed))
_output = _template.substitute(ELAPSED_SEC=_elapsed)
_fout = open(self._gnuplot_output_file, "w")
_fout.write(_output)
_fout.close()
self._log.info('wrote gnuplot settings to file: {}'.format(
self._gnuplot_output_file))
except Exception as e:
self._log.error('error writing gnuplot settings: {}'.format(
traceback.format_exc()))
# ..........................................................................
def _writer(self, queue, f_is_enabled):
self._log.info('writing to file: {}...'.format(self._filename))
self._active = True
fieldnames = ['time', 'value']
try:
_file = open(self._filename, mode='w')
with _file as output_file:
while f_is_enabled():
while len(queue) > 0:
_data = queue.pop()
_file.write(_data)
self._log.debug('wrote row {}'.format(_data))
time.sleep(0.01)
self._log.info('exited file write loop.')
finally:
if _file:
_file.close()
self._log.info('file closed.')
self._active = False
self._log.info('file write thread complete.')
class UltrasonicScanner():
def __init__(self, config, level):
self._log = Logger('uscanner', Level.INFO)
self._config = config
if self._config:
self._log.info('configuration provided.')
_config = self._config['ros'].get('ultrasonic_scanner')
self._min_angle = _config.get('min_angle')
self._max_angle = _config.get('max_angle')
self._degree_step = _config.get('degree_step')
self._use_raw_distance = _config.get('use_raw_distance')
self._read_delay_sec = _config.get('read_delay_sec')
self._log.info('read delay: {:>4.1f} sec'.format(
self._read_delay_sec))
_servo_number = _config.get('servo_number')
else:
self._log.warning('no configuration provided.')
self._min_angle = -60.0
self._max_angle = 60.0
self._degree_step = 3.0
self._use_raw_distance = False
self._read_delay_sec = 0.1
_servo_number = 2
self._log.info(
'scan from {:>5.2f} to {:>5.2f} with step of {:>4.1f}°'.format(
self._min_angle, self._max_angle, self._degree_step))
self._servo = Servo(self._config, _servo_number, level)
self._ub = self._servo.get_ultraborg()
# self._tof = TimeOfFlight(_range, Level.WARN)
self._error_range = 0.067
self._enabled = False
self._closed = False
self._log.info('ready.')
# ..........................................................................
def _in_range(self, p, q):
return p >= (q - self._error_range) and p <= (q + self._error_range)
# ..........................................................................
def scan(self):
if not self._enabled:
self._log.warning('cannot scan: disabled.')
return
start = time.time()
_min_mm = 9999.0
_angle_at_min = 0.0
_max_mm = 0
_angle_at_max = 0.0
_max_retries = 10
self._servo.set_position(self._min_angle)
time.sleep(0.3)
_slack = 0.1
self._log.info(
Fore.BLUE + Style.BRIGHT +
'scanning from {:>5.2f} to {:>5.2f} with step of {:>4.1f}°...'.
format(self._min_angle, self._max_angle, self._degree_step))
for degrees in numpy.arange(self._min_angle, self._max_angle + _slack,
self._degree_step):
self._log.debug('loop set to : {:>5.2f}°...'.format(degrees))
if self._servo.is_in_range(degrees):
self._servo.set_position(degrees)
wait_count = 0
while not self._in_range(self._servo.get_position(degrees),
degrees) and wait_count < 10:
time.sleep(0.0025)
wait_count += 1
self._log.debug(Fore.GREEN + Style.BRIGHT + 'measured degrees: {:>5.2f}°: \ttarget: {:>5.2f}°; waited: {:d}'.format(\
self._servo.get_position(degrees), degrees, wait_count))
_mm = self._servo.get_distance(_max_retries,
self._use_raw_distance)
# if we get a zero we keep trying...
if _mm is None:
self._log.info(
'failed to read distance at {:>5.2f}°.'.format(
degrees))
else:
if _mm <= 0.01:
retry_count = 0
while _mm == 0.0 and retry_count < _max_retries:
_mm = self._servo.get_distance(
_max_retries, self._use_raw_distance)
if _mm is None:
self._log.info(
'failed to read distance at {:>5.2f}°: '.
format(degrees) + Fore.RED +
'\t retries: {:d}'.format(retry_count))
else:
if _mm <= 0.01:
self._log.info(
'distance at {:>5.2f}°: '.format(
degrees) + Fore.RED +
'\t{:>6.0f}mm'.format(_mm) +
'\t retries: {:d}'.format(retry_count))
else:
self._log.info(
'distance at {:>5.2f}°: \t{:>6.0f}mm'.
format(degrees, _mm) + Fore.BLACK +
' (on retry)')
retry_count += 1
time.sleep(0.1)
else:
self._log.info(
'distance at {:>5.2f}°: \t{:>6.0f}mm'.format(
degrees, _mm))
if _mm is not None:
# capture min and max at angles
_min_mm = min(_min_mm, _mm)
if _mm == _min_mm:
_angle_at_min = degrees
_max_mm = max(_max_mm, _mm)
if _mm == _max_mm:
_angle_at_max = degrees
time.sleep(self._read_delay_sec)
# time.sleep(0.1)
else:
self._log.warning(
'requested position: {:>5.2f}° out range of servo movement.'
.format(degrees))
time.sleep(0.1)
# self._log.info('complete.')
elapsed = time.time() - start
self._log.info('scan complete: {:>5.2f}sec elapsed.'.format(elapsed))
self._log.info(
Fore.CYAN + Style.BRIGHT +
'mix. distance at {:>5.2f}°:\t{}mm'.format(_angle_at_min, _min_mm))
self._log.info(
Fore.CYAN + Style.BRIGHT +
'max. distance at {:>5.2f}°:\t{}mm'.format(_angle_at_max, _max_mm))
self._servo.set_position(0.0)
return [_angle_at_min, _min_mm, _angle_at_max, _max_mm]
# ..........................................................................
def enable(self):
if self._closed:
self._log.warning('cannot enable: closed.')
return
# self._tof.enable()
self._enabled = True
# ..........................................................................
def disable(self):
self._enabled = False
self._servo.disable()
# self._tof.disable()
# ..........................................................................
def close(self):
self.disable()
self._servo.close()
self._closed = True
class PID(object):
'''
The PID controller itself.
'''
def __init__(
self,
config,
orientation, # used only for logging
setpoint=0.0,
sample_time=0.01,
level=Level.INFO):
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._orientation = orientation
self._max_velocity = None
self._log = Logger('pid:{}'.format(orientation.label), level)
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._sample_time = sample_time
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()
self._log.info('ready.')
# ..........................................................................
@property
def kp(self):
return self._kp
@kp.setter
def kp(self, kp):
self._kp = kp
# ..........................................................................
@property
def ki(self):
return self._ki
@ki.setter
def ki(self, ki):
self._ki = ki
# ..........................................................................
@property
def kd(self):
return self._kd
@kd.setter
def kd(self, kd):
self._kd = kd
# ..........................................................................
@property
def velocity(self):
'''
Getter for the velocity (PID set point).
'''
return self.setpoint
@velocity.setter
def velocity(self, velocity):
'''
Setter for the velocity (PID set point).
'''
self._log.debug(Fore.BLACK + 'set velocity: {:5.2f}'.format(velocity))
self._setpoint = velocity
def set_max_velocity(self, max_velocity):
'''
Setter for the maximum velocity. Set to None (the default) to
disable this feature. Note that this doesn't affect the setting
of the velocity but rather the getting of the setpoint.
'''
if max_velocity:
self._log.debug(Fore.CYAN +
'max velocity: {:5.2f}'.format(max_velocity))
else:
self._log.debug(Fore.CYAN + Style.DIM + 'max velocity: DISABLED')
self._max_velocity = max_velocity
# ..........................................................................
@property
def setpoint(self):
'''
The setpoint used by the controller as a tuple: (Kp, Ki, Kd)
If a maximum velocity has been set the returned value is
limited by the set value.
'''
if self._max_velocity:
return min(self._max_velocity, self._setpoint)
else:
return self._setpoint
@setpoint.setter
def setpoint(self, setpoint):
'''
Setter for the set point.
'''
self._setpoint = setpoint
# ..........................................................................
def __call__(self, target, dt=None):
'''
Call the PID controller with *target* and calculate and return a
control output if sample_time seconds has passed since the last
update. If no new output is calculated, return the previous output
instead (or None if no value has been calculated yet).
:param dt: If set, uses this value for timestep instead of real
time. This can be used in simulations when simulation time
is different from real time.
'''
_now = self._current_time()
if dt is None:
dt = _now - self._last_time if _now - self._last_time else 1e-16
elif dt <= 0:
raise ValueError(
"dt has nonpositive value {}. Must be positive.".format(dt))
if dt < self._sample_time and self._last_output is not None:
# only update every sample_time seconds
return self._last_output
# compute error terms
error = self.setpoint - target
d_input = target - (self._last_input
if self._last_input is not None else target)
# compute the proportional term
self._proportional = self.kp * error
# compute integral and derivative terms
self._integral += self.ki * error * dt
self._integral = self.clamp(self._integral) # avoid integral windup
self._derivative = -self.kd * d_input / dt
# compute output
output = self._proportional + self._integral + self._derivative
output = self.clamp(output)
# kp, ki, kd = self.constants
# cp, ci, cd = self.components
# self._log.info(Fore.CYAN + 'error={:6.3f};'.format(error) \
# + Fore.MAGENTA + '\tKD={:8.5f};'.format(kd) \
# + Fore.CYAN + Style.BRIGHT + '\tP={:8.5f}; I={:8.5f}; D={:8.5f}; setpoint={:6.3f};'.format(cp, ci, cd, self._setpoint) \
# + Style.DIM + ' output: {:>6.3f};'.format(output))
# keep track of state
self._last_output = output
self._last_input = target
self._last_time = _now
return output
# ..........................................................................
@property
def sample_time(self):
'''
Return the sample time as a property.
'''
return self._sample_time
# ..........................................................................
@property
def constants(self):
'''
The P-, I- and D- fixed terms, as a tuple.
'''
return self.kp, self.ki, self.kd
# ..........................................................................
@property
def components(self):
'''
The P-, I- and D-terms from the last computation as separate
components as a tuple. Useful for visualizing what the controller
is doing or when tuning hard-to-tune systems.
'''
return self._proportional, self._integral, self._derivative
# ..........................................................................
@property
def tunings(self):
'''
The tunings used by the controller as a tuple: (Kp, Ki, Kd)
'''
return self.kp, self.ki, self.kd
# ..........................................................................
@tunings.setter
def tunings(self, tunings):
'''
Setter for the PID tunings.
'''
self._kp, self._ki, self._kd = tunings
# ..........................................................................
@property
def output_limits(self):
'''
The curre_pom output limits as a 2-tuple: (lower, upper). See also
the *output_limts* parameter in :meth:`PID.__init__`.
'''
return self._min_output, self._max_output
# ..........................................................................
@output_limits.setter
def output_limits(self, limits):
'''
Setter for the output limits.
'''
if limits is None:
self._min_output, self._max_output = None, None
return
min_output, max_output = limits
if None not in limits and max_output < min_output:
raise ValueError('lower limit must be less than upper limit')
self._min_output = min_output
self._max_output = max_output
self._integral = self.clamp(self._integral)
self._last_output = self.clamp(self._last_output)
# ..........................................................................
def reset(self):
'''
Reset the PID controller internals, setting each term to 0 as well
as cleaning the integral, the last output and the last input
(derivative calculation).
'''
self._proportional = 0.0
self._integral = 0.0
self._derivative = 0.0
self._last_time = self._current_time()
self._last_output = None
self._last_input = None
# ..........................................................................
def clamp(self, value):
'''
Clamp the value between the lower and upper limits.
'''
if value is None:
return None
elif self._max_output is not None and value > self._max_output:
return self._max_output
elif self._min_output is not None and value < self._min_output:
return self._min_output
else:
return value
def brake():
_log.info('braking...')
_motors.brake()
_motors.stop()
_motors.close()
_log.info('done.')
# ..............................................................................
_motors = None
_log = Logger('brake', Level.INFO)
try:
_log.info('configuring...')
# read YAML configuration
_loader = ConfigLoader(Level.WARN)
filename = 'config.yaml'
_config = _loader.configure(filename)
_motors = Motors(_config, None, Level.WARN)
except KeyboardInterrupt:
_log.error('Ctrl-C caught in main: exiting...')
finally:
_log.info('complete.')
if __name__ == '__main__':
brake()
#EOF
class MockMessageQueue():
'''
This message queue just displays IFS events as they arrive.
'''
def __init__(self, level):
super().__init__()
self._count = 0
self._counter = itertools.count()
self._log = Logger("queue", Level.INFO)
self._listeners = []
self._triggered_ir_port_side = False
self._triggered_ir_port = False
self._triggered_ir_cntr = False
self._triggered_ir_stbd = False
self._triggered_ir_stbd_side = False
self._triggered_bmp_port = False
self._triggered_bmp_cntr = False
self._triggered_bmp_stbd = False
self._log.info('ready.')
# ......................................................
def add(self, message):
self._log.debug(Fore.BLUE + 'add message {}'.format(message))
self.add(message)
# ......................................................
def add(self, message):
self._count = next(self._counter)
message.number = self._count
_event = message.event
self._log.debug('added message #{}; priority {}: {}; event: {}'.format(message.number, message.priority, message.description, _event))
_value = message.value
if _event is Event.BUMPER_PORT and not self._triggered_bmp_port:
self._log.info(Fore.RED + Style.BRIGHT + 'BUMPER_PORT: {}; value: {}'.format(_event.description, _value))
self._triggered_bmp_port = True
elif _event is Event.BUMPER_CNTR and not self._triggered_bmp_cntr:
self._log.info(Fore.BLUE + Style.BRIGHT + 'BUMPER_CNTR: {}; value: {}'.format(_event.description, _value))
self._triggered_bmp_cntr = True
elif _event is Event.BUMPER_STBD and not self._triggered_bmp_stbd:
self._log.info(Fore.GREEN + Style.BRIGHT + 'BUMPER_STBD: {}; value: {}'.format(_event.description, _value))
self._triggered_bmp_stbd = True
elif _event is Event.INFRARED_PORT_SIDE and not self._triggered_ir_port_side:
self._log.debug(Fore.RED + '> INFRARED_PORT_SIDE: {}; value: {}'.format(_event.description, _value))
self._triggered_ir_port_side = True
elif _event is Event.INFRARED_PORT and not self._triggered_ir_port:
self._log.debug(Fore.RED + '> INFRARED_PORT: {}; value: {}'.format(_event.description, _value))
self._triggered_ir_port = True
elif _event is Event.INFRARED_CNTR and not self._triggered_ir_cntr:
self._log.debug(Fore.BLUE + '> INFRARED_CNTR: distance: {:>5.2f}cm'.format(_value))
self._triggered_ir_cntr = True
elif _event is Event.INFRARED_STBD and not self._triggered_ir_stbd:
self._log.debug(Fore.GREEN + '> INFRARED_STBD: {}; value: {}'.format(_event.description, _value))
self._triggered_ir_stbd = True
elif _event is Event.INFRARED_STBD_SIDE and not self._triggered_ir_stbd_side:
self._log.debug(Fore.GREEN + '> INFRARED_STBD_SIDE: {}; value: {}'.format(_event.description, _value))
self._triggered_ir_stbd_side = True
else:
self._log.debug(Fore.BLACK + Style.BRIGHT + 'other event: {}'.format(_event.description))
# ..........................................................................
def add_listener(self, listener):
'''
Add a listener to the optional list of message listeners.
'''
return self._listeners.append(listener)
# ..........................................................................
def remove_listener(self, listener):
'''
Remove the listener from the list of message listeners.
'''
try:
self._listeners.remove(listener)
except ValueError:
self._log.warn('message listener was not in list.')
# ......................................................
@property
def all_triggered(self):
_all_ir_triggered = self._triggered_ir_port_side and self._triggered_ir_port and self._triggered_ir_cntr and self._triggered_ir_stbd and self._triggered_ir_stbd_side
_all_bmp_triggered = self._triggered_bmp_port and self._triggered_bmp_cntr and self._triggered_bmp_stbd
return _all_ir_triggered and _all_bmp_triggered
# ......................................................
@property
def count(self):
return self._count
# ..........................................................................
def waiting_for_message(self):
_fmt = '{0:>9}'
self._log.info('waiting for: | ' \
+ Fore.RED + _fmt.format( 'PORT_SIDE' if not self._triggered_ir_port_side else '' ) \
+ Fore.CYAN + ' | ' \
+ Fore.RED + _fmt.format( 'PORT' if not self._triggered_ir_port else '' ) \
+ Fore.CYAN + ' | ' \
+ Fore.BLUE + _fmt.format( 'CNTR' if not self._triggered_ir_cntr else '' ) \
+ Fore.CYAN + ' | ' \
+ Fore.GREEN + _fmt.format( 'STBD' if not self._triggered_ir_stbd else '' ) \
+ Fore.CYAN + ' | ' \
+ Fore.GREEN + _fmt.format( 'STBD_SIDE' if not self._triggered_ir_stbd_side else '' )
+ Fore.CYAN + ' || ' \
+ Fore.RED + _fmt.format( 'BMP_PORT' if not self._triggered_bmp_port else '' ) \
+ Fore.CYAN + ' | ' \
+ Fore.BLUE + _fmt.format( 'BMP_CNTR' if not self._triggered_bmp_cntr else '' ) \
+ Fore.CYAN + ' | ' \
+ Fore.GREEN + _fmt.format( 'BMP_STBD' if not self._triggered_bmp_stbd else '' ) \
+ Fore.CYAN + ' |' )
class Rate():
'''
Loops at a fixed rate, specified in hertz (Hz).
:param hertz: the frequency of the loop in Hertz
:param level: the log level
:param use_ns: (optional) if True use a nanosecond counter instead of milliseconds
'''
def __init__(self, hertz, level=Level.INFO, use_ns=False):
self._log = Logger('rate', level)
self._last_ns = time.perf_counter_ns()
self._last_time = time.time()
self._dt = 1 / hertz
self._dt_ms = self._dt * 1000
self._dt_ns = self._dt_ms * 1000000
self._use_ns = use_ns
if self._use_ns:
self._log.info(
'nanosecond rate set for {:d}Hz (period: {:>4.2f}sec/{:d}ms)'.
format(hertz, self.get_period_sec(), self.get_period_ms()))
else:
self._log.info(
'millisecond rate set for {:d}Hz (period: {:>4.2f}sec/{:d}ms)'.
format(hertz, self.get_period_sec(), self.get_period_ms()))
# ..........................................................................
def get_period_sec(self):
'''
Returns the period in seconds, as a float.
'''
return self._dt
# ..........................................................................
def get_period_ms(self):
'''
Returns the period in milliseconds, rounded to an int.
'''
return round(self._dt * 1000)
# ..........................................................................
def waiting(self):
'''
Return True if still waiting for the current loop to complete.
'''
return self._dt < (time.time() - self._last_time)
# ..........................................................................
def wait(self):
"""
If called before the allotted period will wait the remaining time
so that the loop is no faster than the specified frequency. If
called after the allotted period has passed, no waiting takes place.
E.g.:
# execute loop at 60Hz
rate = Rate(60)
while True:
# do something...
rate.wait()
"""
if self._use_ns:
_ns_diff = time.perf_counter_ns() - self._last_ns
_delay_sec = (self._dt_ns - _ns_diff) / (1000 * 1000000)
if self._dt_ns > _ns_diff:
# print('...')
time.sleep(_delay_sec)
# self._log.info(Fore.BLACK + Style.BRIGHT + 'dt_ns: {:7.4f}; diff: {:7.4f}ns; delay: {:7.4f}s'.format(self._dt_ns, _ns_diff, _delay_sec))
self._last_ns = time.perf_counter_ns()
else:
_diff = time.time() - self._last_time
_delay_sec = self._dt - _diff
if self._dt > _diff:
time.sleep(_delay_sec)
# self._log.info(Fore.BLACK + Style.BRIGHT + 'dt: {:7.4f}; diff: {:7.4f}ms; delay: {:7.4f}s'.format(self._dt, _diff, _delay_sec))
self._last_time = time.time()
class __impl:
def __init__(self, vs, imgOutput):
self.__vs = vs
self.__imgOutput = imgOutput
self.image = None
self.logger = Logger()
self.state = State()
self.tesseract = PyTessBaseAPI(psm=PSM.SINGLE_CHAR,
oem=OEM.LSTM_ONLY,
lang="digits")
self.filter = Filter()
self.signalThresholdY = 160
self.LAPPatternSesibility = 5
self.recordStamp = time.strftime(self.logger.timeFormat)
self.recordNum = 0
self.recordFolder = None
self.cntNum = 0
if (self.state.RecordImage):
root = 'record'
if not os.path.isdir(root):
os.mkdir(root)
self.recordFolder = os.path.join(root, self.recordStamp)
if not os.path.isdir(self.recordFolder):
os.mkdir(self.recordFolder)
def showImg(self, window, image):
if self.__imgOutput:
cv2.imshow(window, image)
def warmup(self):
time.sleep(2.0)
self.tesserOCR(np.zeros((1, 1, 3), np.uint8))
def tesserOCR(self, image):
self.tesseract.SetImage(Image.fromarray(image))
return self.tesseract.GetUTF8Text(
), self.tesseract.AllWordConfidences()
def dominantColor(self, img, clusters=2):
data = np.reshape(img, (-1, 3))
data = np.float32(data)
criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 10,
1.0)
flags = cv2.KMEANS_RANDOM_CENTERS
_, _, centers = cv2.kmeans(data, 1, None, criteria, 10, flags)
return centers[0].astype(np.int32)
def analyzeRect(self, image, warped, box, x, y):
# find amount of color blue in warped area, assuming over X% is the lap signal
if (self.getAmountOfColor(warped, Colors.lower_blue_color,
Colors.upper_blue_color, True) > 0.1):
self.logger.info("Rundensignal")
self.state.setCurrentSignal(Signal.LAP)
return "Rundensignal"
def analyzeSquare(self, image, warped, box, x, y):
#dominantColor, percent, _ = self.dominantColor(warped, 3)
# dominantColor = self.dominantColor(
# cv2.cvtColor(warped, cv2.COLOR_BGR2HSV), 3)
""" color = 'k'
# find amount of color black in warped area, assuming over X% is a numeric signal
if ((dominantColor <= 70).all()):
color = 'Black'
elif ((dominantColor >= 180).all()):
color = 'White'
if (color): """
resizedWarp = cv2.resize(warped,
None,
fx=2.0,
fy=2.0,
interpolation=cv2.INTER_CUBIC)
# gray
optimized = cv2.cvtColor(resizedWarp, cv2.COLOR_BGR2GRAY)
# blur
optimized = cv2.GaussianBlur(optimized, (5, 5), 0)
# binary image
optimized = cv2.threshold(optimized, 0, 255,
cv2.THRESH_BINARY + cv2.THRESH_OTSU)[1]
# binary inversion if dominant color is black
""" if (color == 'Black'):
optimized = cv2.bitwise_not(optimized) """
# now check the frame (1px) of the image.. there shouldn't be any noise since its a clean signal background
h, w = optimized.shape[0:2]
clean = optimized[0, 0]
for iFrame in range(0, 2):
for iHeight in range(h):
if not (optimized[iHeight, iFrame] == clean) or not (
optimized[iHeight, w - 1 - iFrame] == clean):
return False
for iWidth in range(w):
# or not(optimized[h - iFrame, iWidth])
if not (optimized[iFrame, iWidth] == clean):
return False
# cv2.imwrite("records/opt/" + str(self.cntNum) + ".jpg", optimized)
output, confidence = self.tesserOCR(optimized)
# if the resulting text is below X% confidence threshold, we skip it
if not output or confidence[0] < 95:
return False
# clean up output from tesseract
output = output.replace('\n', '')
output = output.replace(' ', '')
if output.isdigit() and 0 < int(output) < 10:
""" self.showImg("opt " + str(self.cntNum),
np.hstack((resizedWarp, cv2.cvtColor(optimized, cv2.COLOR_GRAY2BGR)))) """
if y <= self.signalThresholdY:
self.logger.info('Stop Signal OCR: ' + output + ' X: ' +
str(x) + ' Y: ' + str(y) +
' Confidence: ' + str(confidence[0]) +
'%') # + ' DC: ' + str(dominantColor))
self.state.setStopSignal(int(output))
return 'S: ' + output
elif self.state.StopSignalNum != 0:
self.logger.info('Info Signal OCR: ' + output + ' X: ' +
str(x) + ' Y: ' + str(y) +
' Confidence: ' + str(confidence[0]) +
'%') # + ' DC: ' + str(dominantColor))
self.state.setCurrentSignal(Signal.UPPER, int(output))
return 'I: ' + output
def getAmountOfColor(self,
img,
lowerColor,
upperColor,
convert2hsv=True):
if (convert2hsv):
img = cv2.cvtColor(img, cv2.COLOR_BGR2HSV)
# create mask from color range
maskColor = cv2.inRange(img, lowerColor, upperColor)
# get ratio of active pixels
ratio_color = cv2.countNonZero(maskColor) / (img.size)
return ratio_color
# color picker for manual debugging
def pick_color(self, event, x, y, flags, param):
if event == cv2.EVENT_LBUTTONDOWN:
pixel = self.image[y, x]
color = np.array([pixel[0], pixel[1], pixel[2]])
self.logger.info(pixel)
# capture frames from the camera
def capture(self, savedImg=None):
if (savedImg is not None):
image = savedImg
else:
image = self.__vs.read()
if (self.state.InvertCamera):
image = imutils.rotate(image, angle=180)
self.image = image
if (self.state.RecordImage):
self.recordNum += 1
cv2.imwrite(
os.path.join(self.recordFolder,
str(self.recordNum) + ".jpg"), image)
return
if (self.state.Approaching == Signal.UPPER
or self.state.Approaching == Signal.LOWER):
self.findNumberSignal(image)
elif (self.state.Approaching == Signal.LAP):
self.findLapSignal(image)
def findLapSignal(self, image):
contourImage = image.copy()
blur = cv2.GaussianBlur(image, (3, 3), 0)
hsv = cv2.cvtColor(blur, cv2.COLOR_BGR2HSV)
self.image = hsv
mask = cv2.inRange(hsv, Colors.lower_blue_color,
Colors.upper_blue_color)
cnts = imutils.grab_contours(
cv2.findContours(mask.copy(), cv2.RETR_LIST,
cv2.CHAIN_APPROX_SIMPLE))
if len(cnts) > 0:
# transform all contours to rects
rects = [cv2.boundingRect(cnt) for cnt in cnts]
# now iterate all of the rects, trying to find an approximated sibiling shifted in Y-direction
for rect in rects:
(x, y, w, h) = rect
cv2.rectangle(contourImage, (x, y), (x + w, y + h),
(0, 0, 255), 2)
# try to match the pattern from a given rect in all rects
counterPart = [
counterRect for counterRect in rects
if (counterRect != rect and x - 5 <= counterRect[0] <=
x + 5 and 2 * -(h + 5) <= y - counterRect[1] <= 2 *
(h + 5) and w - 5 <= counterRect[2] <= w + 5)
and h - 5 <= counterRect[3] <= h + 5
]
if (counterPart):
(x, y, w, h) = counterPart[0]
cv2.rectangle(contourImage, (x, y), (x + w, y + h),
(0, 255, 0), 2)
self.logger.info('LAP Signal')
self.state.captureLapSignal()
break
self.showImg(
'contourImage',
np.hstack(
(hsv, contourImage, cv2.cvtColor(mask,
cv2.COLOR_GRAY2BGR))))
cv2.setMouseCallback('contourImage', self.pick_color)
def findNumberSignal(self, image):
image_height = np.size(image, 0)
image_width = np.size(image, 1)
contourImage = image.copy()
# focus only on the part of the image, where a signal could occur
# image = image[int(image.shape[0] * 0.2):int(image.shape[0] * 0.8), 0:int(image.shape[1]*0.666)]
mask = self.filter.autoCanny(image, 2, 3)
# get a list of contours in the mask, chaining to just endpoints
cnts = imutils.grab_contours(
cv2.findContours(mask.copy(), cv2.RETR_LIST,
cv2.CHAIN_APPROX_SIMPLE))
# only proceed if at least one contour was found
if len(cnts) > 0:
# loop contours
for self.cntNum, cnt in enumerate(cnts):
rect = cv2.minAreaRect(cnt)
_, _, angle = rect
# approximate shape
approx = cv2.approxPolyDP(cnt,
0.02 * cv2.arcLength(cnt, True),
True)
# the rectangle must not have a too big rotation (+/-10)
# and more than 3 connecting points
if len(approx) >= 3 and (-90 <= angle <= -80
or angle >= -10):
box = cv2.boxPoints(rect)
box = np.int0(box)
(x, y, w, h) = cv2.boundingRect(approx)
# limit viewing range
if (y <= image_height * 0.2 or x >= image_width * 0.8):
continue
if (w <= 5 or h <= 5):
continue
# we are in approaching mode, thus we only care for the lower signals <= threshold
if ((self.state.Approaching == Signal.UPPER
and y >= self.signalThresholdY)
and not self.state.Standalone):
continue
elif ((self.state.Approaching == Signal.LOWER
and y <= self.signalThresholdY)
and not self.state.Standalone):
continue
sideRatio = w / float(h)
absoluteSizeToImageRatio = (
100 / (image_width * image_height)) * (w * h)
# calculate area of the bounding rectangle
rArea = w * float(h)
# calculate area of the contour
cArea = cv2.contourArea(cnt)
if (cArea):
rectContAreaRatio = (100 / rArea) * cArea
else:
continue
# cv2.drawContours(contourImage, [box], 0, (255, 0, 0), 1)
result = None
# is the rectangle sideways, check for lap signal
# if (h*2 < w and y <= self.signalThresholdY and rectContAreaRatio >= 80):
#result = self.analyzeRect(image, four_point_transform(image, [box][0]), box, x, y)
# find all contours looking like a signal with minimum area (1%)
if absoluteSizeToImageRatio >= 0.01:
# is it approx a square, or standing rect? then check for info or stop signal
if 0.2 <= sideRatio <= 1.1:
# transform ROI
if (sideRatio <= 0.9):
coords, size, angle = rect
size = size[0] + 8, size[1] + 4
coords = coords[0] + 1, coords[1] + 1
rect = coords, size, angle
box = cv2.boxPoints(rect)
box = np.int0(box)
""" cv2.drawContours(
contourImage, [box], 0, (0, 255, 0), 1) """
warp = four_point_transform(image, [box][0])
result = self.analyzeSquare(
image, warp, box, x, y)
if (result):
if (self.__imgOutput):
color = None
if (y >= self.signalThresholdY):
color = (0, 0, 255)
else:
color = (255, 0, 0)
cv2.drawContours(contourImage, [box], 0, color,
1)
cv2.drawContours(contourImage, [cnt], -1,
color, 2)
""" M = cv2.moments(cnt)
cX = int(M["m10"] / M["m00"])
cY = int(M["m01"] / M["m00"])
cv2.putText(contourImage, str(
self.cntNum), (cX - 30, cY - 30), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 255, 0), 1) """
self.logger.debug(
"[" + str(self.cntNum) + "] SideRatio: " +
str(sideRatio) + " AreaRatio: " +
str(rectContAreaRatio) + " ContArea: " +
str(cArea) + " RectArea: " + str(rArea) +
" AbsSize: " +
str(absoluteSizeToImageRatio) +
" CntPoints: " + str(len(approx)) +
" Size: " + str(w) + "x" + str(h))
""" if (self.__imgOutput): # hsv img output
hsv = cv2.cvtColor(image, cv2.COLOR_BGR2HSV)
cv2.namedWindow('contourImage')
cv2.setMouseCallback('contourImage', self.pick_color)
# self.showImg("hsv", hsv) """
self.showImg(
"contourImage",
np.hstack((contourImage, cv2.cvtColor(mask,
cv2.COLOR_GRAY2BGR))))
class Behaviours():
'''
This class provides avoidance and some other relatively simple behaviours.
This version uses the PID controller rathen than directly driving the motors.
'''
def __init__(self, config, pid_motor_controller, level):
self._log = Logger("behave", level)
if config is None:
raise ValueError('null configuration argument.')
self._geo = Geometry(config, level)
_config = config['ros'].get('behaviours')
self._accel_range_cm = _config.get('accel_range_cm')
self._cruising_velocity = _config.get('cruising_velocity')
self._targeting_velocity = _config.get('targeting_velocity')
self._pid_motor_controller = pid_motor_controller
_pid_controllers = pid_motor_controller.get_pid_controllers()
self._port_pid = _pid_controllers[0]
self._stbd_pid = _pid_controllers[1]
self._rgbmatrix = RgbMatrix(Level.INFO)
self._fast_speed = 99.0
self._slow_speed = 50.0
self._log.info('ready.')
# ..........................................................................
def get_cruising_velocity(self):
return self._cruising_velocity
# ..........................................................................
def back_up(self, duration_sec):
'''
Back up for the specified duration so we don't hang our bumper.
'''
self._log.info('back up.')
# self._motors.astern(self._fast_speed)
# time.sleep(duration_sec)
pass
# ==========================================================================
# one_meter .....................................................................
'''
The configuration defines an 'acceleration range', which is the range used
for both acceleration and deceleration. If the travel distance is greater
than twice this range we have enough room to reach cruising speed before
beginning to decelerate to the step target. We also define a targeting speed,
which is a low velocity from which we are prepared to immediately halt upon
reaching our step target.
'''
'''
Geometry Notes ...................................
494 encoder steps per rotation (maybe 493)
68.5mm diameter tires
215.19mm/21.2cm wheel circumference
1 wheel rotation = 215.2mm
2295 steps per meter
2295 steps per second = 1 m/sec
2295 steps per second = 100 cm/sec
229.5 steps per second = 10 cm/sec
22.95 steps per second = 1 cm/sec
1 rotation = 215mm = 494 steps
1 meter = 4.587 rotations
2295.6 steps per meter
22.95 steps per cm
'''
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 one_meter(self):
'''
A behaviour that drives one meter forward in a straight line, accelerating and decelerating to hit the target exactly.
'''
self._log.info('start one meter travel...')
self._one_meter_port_complete = False
self._one_meter_stbd_complete = False
_tp = threading.Thread(target=self._one_meter,
args=(self._port_pid,
self._one_meter_callback_port))
_ts = threading.Thread(target=self._one_meter,
args=(self._stbd_pid,
self._one_meter_callback_stbd))
_tp.start()
_ts.start()
_tp.join()
_ts.join()
self._log.info('one meter travel complete.')
# ..........................................................................
def _one_meter_callback_port(self):
self._log.info(Fore.RED + 'port one meter complete.')
self._one_meter_port_complete = True
# ..........................................................................
def _one_meter_callback_stbd(self):
self._log.info(Fore.GREEN + 'stbd one meter complete.')
self._one_meter_stbd_complete = True
# ..........................................................................
def is_one_metering(self):
return not self._one_meter_port_complete and not self._one_meter_stbd_complete
# ==========================================================================
# roam .....................................................................
# ..........................................................................
def _roam_callback_port(self):
self._log.info(Fore.RED + 'port roam complete.')
self._roam_port_complete = True
# ..........................................................................
def _roam_callback_stbd(self):
self._log.info(Fore.GREEN + 'stbd roam complete.')
self._roam_stbd_complete = True
# ..........................................................................
def is_roaming(self):
return not self._roam_port_complete and not self._roam_stbd_complete
def _roam(self, pid, velocity, steps, orientation, callback):
'''
Thread method for each PID controller.
'''
_current_steps = pid.steps
_target_steps = _current_steps + steps
self._log.info(Fore.YELLOW +
'begin {} roaming from {:d} to {:d} steps.'.format(
orientation.label, _current_steps, _target_steps))
pid.velocity = velocity
while pid.steps < _target_steps:
# TODO accelerate
# TODO cruise
time.sleep(0.01)
# TODO decelerate
pid.velocity = 0.0
callback()
time.sleep(1.0)
self._log.info('{} roaming complete: {:d} of {:d} steps.'.format(
orientation.label, pid.steps, _target_steps))
def roam(self):
'''
A pseudo-roam behaviour.
'''
'''
Geometry Notes ...................................
494 encoder steps per rotation (maybe 493)
68.5mm diameter tires
215.19mm/21.2cm wheel circumference
1 wheel rotation = 215.2mm
2295 steps per meter
2295 steps per second = 1 m/sec
2295 steps per second = 100 cm/sec
229.5 steps per second = 10 cm/sec
22.95 steps per second = 1 cm/sec
1 rotation = 215mm = 494 steps
1 meter = 4.587 rotations
2295.6 steps per meter
22.95 steps per cm
'''
self._log.info('roam.')
self._roam_port_complete = False
self._roam_stbd_complete = False
_port_current_velocity = 0.0
self._port_pid
self._stbd_pid
_forward_steps_per_rotation = 494
_rotations = 5
_forward_steps = _forward_steps_per_rotation * _rotations
_velocity = 35.0 #Velocity.HALF
self._log.info(
'start roaming at velocity {:5.2f} for {:d} steps'.format(
_velocity, _forward_steps))
_tp = threading.Thread(target=self._roam,
args=(self._port_pid, _velocity, _forward_steps,
Orientation.PORT,
self._roam_callback_port))
_ts = threading.Thread(target=self._roam,
args=(self._stbd_pid, _velocity, _forward_steps,
Orientation.STBD,
self._roam_callback_stbd))
_tp.start()
_ts.start()
_tp.join()
_ts.join()
self._log.info('complete.')
pass
# avoid ....................................................................
def avoid(self, orientation):
'''
Avoids differently depending on orientation.
'''
self._log.info(Fore.CYAN + 'avoid {}.'.format(orientation.name))
self._log.info('action complete: avoid.')
# sniff ....................................................................
def sniff(self):
if self._rgbmatrix.is_disabled():
self._rgbmatrix.set_solid_color(Color.BLUE)
self._rgbmatrix.set_display_type(DisplayType.SOLID)
# self._rgbmatrix.set_display_type(DisplayType.RANDOM)
self._rgbmatrix.enable()
else:
self._rgbmatrix.disable()
class Servo():
'''
Provided with configuration this permits setting of the center position offset.
The UltraBorg supports four servos, numbered 1-4.
'''
def __init__(self, config, number, level):
self._number = number
self._log = Logger("servo-{:d}".format(number), level)
self._config = config
if self._config:
self._log.info('configuration provided.')
_config = self._config['ros'].get('servo{:d}'.format(number))
self._center_offset = _config.get('center_offset')
else:
self._log.warning('no configuration provided.')
self._center_offset = 0.0
self._log.info('center offset: {:>3.1f}'.format(self._center_offset))
self._UB = UltraBorg() # create a new UltraBorg object
self._UB.Init() # initialise the board (checks it is connected)
# settings .......................................................................
self._min_angle = -90.1 # Smallest position to use (n/90)
self._max_angle = +90.1 # Largest position to use (n/90)
self._startup_delay = 0.5 # Delay before making the initial move
self._step_delay = 0.05 # Delay between steps
self._rate_start = 0.05 # Step distance for all servos during initial move
self._step_distance = 1.0 # Step distance for servo #1
self._enabled = True
self._closed = False
self._log.info('ready.')
# ..........................................................................
def get_min_angle(self):
'''
Return the configured minimum servo angle.
'''
return self._min_angle
# ..........................................................................
def get_max_angle(self):
'''
Return the configured maximum servo angle.
'''
return self._max_angle
# ..........................................................................
def is_in_range(self, degrees):
'''
A convenience method that returns true if the argument is within the
minimum and maximum range (inclusive of endpoints) of the servo.
'''
return degrees >= self._min_angle and degrees <= self._max_angle
# ..........................................................................
def set_position(self, position):
'''
Set the position of the servo to a value between -90.0 and 90.0 degrees.
Values outside this range set to their respective min/max.
This adjusts the argument by the value of the center offset.
'''
self._log.debug('set servo position to: {:+5.2f}° (center offset {:+5.2f}°)'.format(position, self._center_offset))
position += self._center_offset
if position < self._min_angle:
self._log.warning('cannot set position {:+5.2f}° less than minimum {:+5.2f}°'.format(position, self._min_angle))
position = self._min_angle
if position > self._max_angle:
self._log.warning('cannot set position {:+5.2f}° greater than maximum {:+5.2f}°'.format(position, self._max_angle))
position = self._max_angle
# values for servo range from -1.0 to 1.0
_value = position / 90.0
if self._number == 1:
self._UB.SetServoPosition1(_value)
elif self._number == 2:
self._UB.SetServoPosition2(_value)
elif self._number == 3:
self._UB.SetServoPosition3(_value)
elif self._number == 4:
self._UB.SetServoPosition4(_value)
# ..........................................................................
def get_position(self, defaultValue):
'''
Get the position of the servo. If unavailable return the default.
'''
if self._number == 1:
_position = self._UB.GetServoPosition1()
elif self._number == 2:
_position = self._UB.GetServoPosition2()
elif self._number == 3:
_position = self._UB.GetServoPosition3()
elif self._number == 4:
_position = self._UB.GetServoPosition4()
if _position is None:
return defaultValue
else:
return ( _position * 90.0 ) + self._center_offset
# ..........................................................................
def get_distance(self, retry_count, useRawDistance):
'''
Gets the filtered distance for the ultrasonic module in millimeters.
Parameters:
retry_count: how many times to attempt to read from the sensor
before giving up (this value is ignored when
useRawDistance is True)
useRawDistance: if you need a faster response use this instead
(no filtering) e.g.:
0 -> No object in range
25 -> Object 25 mm away
1000 -> Object 1000 mm (1 m) away
3500 -> Object 3500 mm (3.5 m) away
Returns 0 for no object detected or no ultrasonic module attached.
This admittedly is not a servo function but since we have an UltraBorg
supporting this servo, this becomes a convenience method. The ultrasonic
sensor should be connected to the same number as the servo.
'''
if self._number == 1:
if useRawDistance:
return self._UB.GetRawDistance1()
else:
return self._UB.GetWithRetry(self._UB.GetDistance1, retry_count)
elif self._number == 2:
if useRawDistance:
return self._UB.GetRawDistance2()
else:
return self._UB.GetWithRetry(self._UB.GetDistance2, retry_count)
elif self._number == 3:
if useRawDistance:
return self._UB.GetRawDistance3()
else:
return self._UB.GetWithRetry(self._UB.GetDistance3, retry_count)
elif self._number == 4:
if useRawDistance:
return self._UB.GetRawDistance4()
else:
return self._UB.GetWithRetry(self._UB.GetDistance4, retry_count)
# ..........................................................................
def sweep(self):
'''
Repeatedly sweeps between minimum and maximum until disabled.
'''
self._log.info('press CTRL+C to quit.')
try:
self._log.info('move to center position.')
position = 0.0
self.set_position(position)
# wait to be sure the servo has caught up
time.sleep(self._startup_delay)
self._log.info('sweep to start position.')
while position > self._min_angle:
position -= self._rate_start
if position < self._min_angle:
position = self._min_angle
self.set_position(position)
time.sleep(self._step_delay)
self._log.info('sweep through the range.')
while True:
position += self._step_distance
# check if too large, if so wrap to the over end
if position > self._max_angle:
position -= (self._max_angle - self._min_angle)
self.set_position(position)
time.sleep(self._step_delay)
except KeyboardInterrupt:
self.close()
self._log.info('done.')
# ..........................................................................
def get_ultraborg(self):
'''
Return the UltraBorg supporting this Servo.
'''
return self._UB
# ..........................................................................
def disable(self):
self._enabled = False
self._log.info('disabled.')
# ..........................................................................
def reset(self):
if self._number == 1:
self._UB.SetServoPosition1(0.0)
elif self._number == 2:
self._UB.SetServoPosition2(0.0)
elif self._number == 3:
self._UB.SetServoPosition3(0.0)
elif self._number == 4:
self._UB.SetServoPosition4(0.0)
self._log.info('reset position.')
# ..........................................................................
def close(self):
'''
Close the servo. If it has not already been disabled its position
will reset to zero.
'''
if self._enabled:
self.reset()
if not self._closed:
self._closed = True
self.disable()
self._log.info('closed.')
class RoamBehaviour():
'''
This action class provides a roaming behaviour.
'''
def __init__(self, motors, level):
self._log = Logger("roam", level)
self._port_pid = motors._port_pid
self._stbd_pid = motors._stbd_pid
self._roam_port_complete = False
self._roam_stbd_complete = False
self._log.info('ready.')
# ..........................................................................
def _callback_port(self):
self._log.info(Fore.RED + 'port roam complete.')
self._roam_port_complete = True
# ..........................................................................
def _callback_stbd(self):
self._log.info(Fore.GREEN + 'stbd roam complete.')
self._roam_stbd_complete = True
# ..........................................................................
def is_roaming(self):
return not self._roam_port_complete and not self._roam_stbd_complete
def _roam(self, velocity, direction, slew_rate, steps, callback, orientation):
self._log.info('begin roaming...')
for i in range(10):
self._log.info(Fore.GREEN + 'roam [{:d}]'.format(i))
if orientation is Orientation.PORT:
self._port_pid.step_to(velocity, direction, slew_rate, steps)
elif orientation is Orientation.STBD:
self._stbd_pid.step_to(velocity, direction, slew_rate, steps)
time.sleep(1.0)
callback()
self._log.info('stop roaming.')
# ..........................................................................
def roam(self):
'''
Begin roaming.
'''
self._log.info('roaming...')
self._roam_port_complete = False
self._roam_stbd_complete = False
_forward_steps_per_rotation = 494
_rotations = 2
_forward_steps = _forward_steps_per_rotation * _rotations
_velocity = 35.0 #Velocity.HALF
_direction = Direction.FORWARD
_slew_rate = SlewRate.SLOW
self._log.info('start roaming at velocity: {}'.format(_velocity))
_tp = Thread(target=self._roam, args=(_velocity, _direction, _slew_rate, _forward_steps, self._callback_port, Orientation.PORT))
_ts = Thread(target=self._roam, args=(_velocity, _direction, _slew_rate, _forward_steps, self._callback_stbd, Orientation.STBD))
_tp.start()
_ts.start()
_tp.join()
_ts.join()
self._log.info('complete.')
class IntegratedFrontSensor():
'''
IntegratedFrontSensor: communicates with the integrated front bumpers and
infrared sensors, receiving messages from the IO Expander board or I²C
Arduino slave, sending the messages with its events onto the message bus.
This listens to the Clock and at a frequency of ( TICK % tick_modulo )
calls the _poll() method.
Parameters:
:param config: the YAML based application configuration
:param queue: the message queue receiving activation notifications
:param clock: the clock providing the polling loop trigger
:param message_bus: the message bus to send event messages
:param message_factory: optional MessageFactory
:param level: the logging Level
'''
# ..........................................................................
def __init__(self, config, queue, clock, message_bus, message_factory,
level):
if config is None:
raise ValueError('no configuration provided.')
self._log = Logger("ifs", level)
self._log.info('configuring integrated front sensor...')
_cruise_config = config['ros'].get('cruise_behaviour')
self._cruising_velocity = _cruise_config.get('cruising_velocity')
self._config = config['ros'].get('integrated_front_sensor')
self._clock = clock
self._clock.add_consumer(self)
self._message_bus = message_bus
# _queue = queue
# _queue.add_consumer(self)
self._device_id = self._config.get(
'device_id'
) # i2c hex address of slave device, must match Arduino's SLAVE_I2C_ADDRESS
self._channel = self._config.get('channel')
self._ignore_duplicates = self._config.get('ignore_duplicates')
self._tick_modulo = self._config.get('tick_modulo')
_max_workers = self._config.get('max_workers')
self._log.info('tick modulo: {:d}'.format(self._tick_modulo))
# event thresholds:
self._callback_cntr_min_trigger = self._config.get(
'callback_center_minimum_trigger')
self._callback_side_min_trigger = self._config.get(
'callback_side_minimum_trigger')
self._callback_min_trigger = self._config.get(
'callback_minimum_trigger')
self._port_side_trigger_distance = self._config.get(
'port_side_trigger_distance')
self._port_trigger_distance = self._config.get('port_trigger_distance')
self._center_trigger_distance = self._config.get(
'center_trigger_distance')
self._stbd_trigger_distance = self._config.get('stbd_trigger_distance')
self._stbd_side_trigger_distance = self._config.get(
'stbd_side_trigger_distance')
self._log.info('event thresholds: \t' \
+Fore.RED + ' port side={:>5.2f}; port={:>5.2f};'.format(self._port_side_trigger_distance, self._port_trigger_distance) \
+Fore.BLUE + ' center={:>5.2f};'.format(self._center_trigger_distance) \
+Fore.GREEN + ' stbd={:>5.2f}; stbd side={:>5.2f}'.format(self._stbd_trigger_distance, self._stbd_side_trigger_distance))
# hardware pin assignments
self._port_side_ir_pin = self._config.get('port_side_ir_pin')
self._port_ir_pin = self._config.get('port_ir_pin')
self._center_ir_pin = self._config.get('center_ir_pin')
self._stbd_ir_pin = self._config.get('stbd_ir_pin')
self._stbd_side_ir_pin = self._config.get('stbd_side_ir_pin')
self._log.info('infrared pin assignments:\t' \
+Fore.RED + ' port side={:d}; port={:d};'.format(self._port_side_ir_pin, self._port_ir_pin) \
+Fore.BLUE + ' center={:d};'.format(self._center_ir_pin) \
+Fore.GREEN + ' stbd={:d}; stbd side={:d}'.format(self._stbd_ir_pin, self._stbd_side_ir_pin))
self._port_bmp_pin = self._config.get('port_bmp_pin')
self._center_bmp_pin = self._config.get('center_bmp_pin')
self._stbd_bmp_pin = self._config.get('stbd_bmp_pin')
self._log.info('bumper pin assignments:\t' \
+Fore.RED + ' port={:d};'.format(self._port_bmp_pin) \
+Fore.BLUE + ' center={:d};'.format(self._center_bmp_pin) \
+Fore.GREEN + ' stbd={:d}'.format(self._stbd_bmp_pin))
if message_factory:
self._message_factory = message_factory
else:
self._message_factory = MessageFactory(level)
# self._executor = ProcessPoolExecutor(max_workers=_max_workers)
self._log.info(
'creating thread pool executor with maximum of {:d} workers.'.
format(_max_workers))
self._executor = ThreadPoolExecutor(max_workers=_max_workers,
thread_name_prefix='ifs')
# config IO Expander
self._ioe = IoExpander(config, Level.INFO)
# calculating means for IR sensors
self._pot = Potentiometer(config, Level.INFO)
_queue_limit = 2 # larger number means it takes longer to change
self._deque_port_side = Deque([], maxlen=_queue_limit)
self._deque_port = Deque([], maxlen=_queue_limit)
self._deque_cntr = Deque([], maxlen=_queue_limit)
self._deque_stbd = Deque([], maxlen=_queue_limit)
self._deque_stbd_side = Deque([], maxlen=_queue_limit)
# ...
self._last_event = None
self._last_value = None
self._enabled = False
self._suppressed = False
self._closed = False
self._log.info(Fore.MAGENTA + 'ready.')
# ......................................................
def add(self, message):
'''
Reacts to every nth (modulo) TICK message, submitting a _poll Thread
from the thread pool executor, which polls the various sensors and
sending callbacks for each.
Note that if the loop frequency is set this method is disabled.
'''
if self._enabled and message.event is Event.CLOCK_TICK:
if (message.value % self._tick_modulo) == 0:
_future = self._executor.submit(self._poll, message.value,
lambda: self.enabled)
# ......................................................
def _poll(self, count, f_is_enabled):
'''
Poll the various infrared and bumper sensors, executing callbacks for each.
In tests this typically takes 173ms from ItsyBitsy, 85ms from the Pimoroni IO Expander.
We add a messsage for the bumpers immediately (rather than use a callback) after reading
the sensors since their response must be as fast as possible.
'''
if not f_is_enabled():
self._log.warning('[{:04d}] poll not enabled'.format(count))
return
self._log.info(Fore.BLACK + '[{:04d}] ifs poll start.'.format(count))
_current_thread = threading.current_thread()
_current_thread.name = 'poll'
_start_time = dt.datetime.now()
# pin 10: digital bumper sensor ........................................
_port_bmp_data = self.get_input_for_event_type(Event.BUMPER_PORT)
_cntr_bmp_data = self.get_input_for_event_type(Event.BUMPER_CNTR)
_stbd_bmp_data = self.get_input_for_event_type(Event.BUMPER_STBD)
_port_side_ir_data = self.get_input_for_event_type(
Event.INFRARED_PORT_SIDE)
_port_ir_data = self.get_input_for_event_type(Event.INFRARED_PORT)
_cntr_ir_data = self.get_input_for_event_type(Event.INFRARED_CNTR)
_stbd_ir_data = self.get_input_for_event_type(Event.INFRARED_STBD)
_stbd_side_ir_data = self.get_input_for_event_type(
Event.INFRARED_STBD_SIDE)
# self._callback(Event.BUMPER_CNTR, _cntr_bmp_data)
if _cntr_bmp_data == 1:
_message = self._message_factory.get_message(
Event.BUMPER_CNTR, _cntr_bmp_data)
self._log.info(Fore.BLUE + Style.BRIGHT +
'adding new message eid#{} for BUMPER_CNTR event.'.
format(_message.eid))
self._message_bus.handle(_message)
# pin 9: digital bumper sensor .........................................
# self._callback(Event.BUMPER_PORT, _port_bmp_data)
if _port_bmp_data == 1:
_message = self._message_factory.get_message(
Event.BUMPER_PORT, _port_bmp_data)
self._log.info(Fore.BLUE + Style.BRIGHT +
'adding new message eid#{} for BUMPER_PORT event.'.
format(_message.eid))
self._message_bus.handle(_message)
# pin 11: digital bumper sensor ........................................
# self._callback(Event.BUMPER_STBD, _stbd_bmp_data)
if _stbd_bmp_data == 1:
_message = self._message_factory.get_message(
Event.BUMPER_STBD, _stbd_bmp_data)
self._log.info(Fore.BLUE + Style.BRIGHT +
'adding new message eid#{} for BUMPER_STBD event.'.
format(_message.eid))
self._message_bus.handle(_message)
# port side analog infrared sensor .....................................
if _port_side_ir_data > self._callback_side_min_trigger:
# _message = self._message_factory.get_message(Event.INFRARED_PORT_SIDE, _port_side_ir_data)
# self._log.info(Fore.RED + Style.BRIGHT + 'adding new message eid#{} for INFRARED_PORT_SIDE event.'.format(_message.eid))
# self._message_bus.handle(_message)
self._log.info(Fore.RED + '[{:04d}] ANALOG IR ({:d}): \t'.format(count, 1) + (Fore.RED if (_port_side_ir_data > 100.0) else Fore.YELLOW) \
+ Style.BRIGHT + '{:d}'.format(_port_side_ir_data) + Style.DIM + '\t(analog value 0-255)')
self._callback(Event.INFRARED_PORT_SIDE, _port_side_ir_data)
# port analog infrared sensor ..........................................
if _port_ir_data > self._callback_min_trigger:
# _message = self._message_factory.get_message(Event.INFRARED_PORT, _port_ir_data)
# self._log.info(Fore.RED + Style.BRIGHT + 'adding new message eid#{} for INFRARED_PORT event.'.format(_message.eid))
# self._message_bus.handle(_message)
self._log.info('[{:04d}] ANALOG IR ({:d}): \t'.format(count, 2) + (Fore.RED if (_port_ir_data > 100.0) else Fore.YELLOW) \
+ Style.BRIGHT + '{:d}'.format(_port_ir_data) + Style.DIM + '\t(analog value 0-255)')
self._callback(Event.INFRARED_PORT, _port_ir_data)
# center analog infrared sensor ........................................
if _cntr_ir_data > self._callback_cntr_min_trigger:
# _message = self._message_factory.get_message(Event.INFRARED_CNTR, _cntr_ir_data)
# self._log.info(Fore.BLUE + Style.BRIGHT + 'adding new message eid#{} for INFRARED_CNTR event.'.format(_message.eid))
# self._message_bus.handle(_message)
self._log.info(Fore.BLUE + '[{:04d}] ANALOG IR ({:d}): \t'.format(count, 3) + (Fore.RED if (_cntr_ir_data > 100.0) else Fore.YELLOW) \
+ Style.BRIGHT + '{:d}'.format(_cntr_ir_data) + Style.DIM + '\t(analog value 0-255)')
self._callback(Event.INFRARED_CNTR, _cntr_ir_data)
# starboard analog infrared sensor .....................................
if _stbd_ir_data > self._callback_min_trigger:
# _message = self._message_factory.get_message(Event.INFRARED_STBD, _stbd_ir_data)
# self._log.info(Fore.GREEN + Style.BRIGHT + 'adding new message eid#{} for INFRARED_STBD event.'.format(_message.eid))
# self._message_bus.handle(_message)
self._log.info('[{:04d}] ANALOG IR ({:d}): \t'.format(count, 4) + (Fore.RED if (_stbd_ir_data > 100.0) else Fore.YELLOW) \
+ Style.BRIGHT + '{:d}'.format(_stbd_ir_data) + Style.DIM + '\t(analog value 0-255)')
self._callback(Event.INFRARED_STBD, _stbd_ir_data)
# starboard side analog infrared sensor ................................
if _stbd_side_ir_data > self._callback_side_min_trigger:
# _message = self._message_factory.get_message(Event.INFRARED_STBD_SIDE, _stbd_side_ir_data)
# self._log.info(Fore.GREEN + Style.BRIGHT + 'adding new message eid#{} for INFRARED_STBD_SIDE event.'.format(_message.eid))
# self._message_bus.handle(_message)
self._log.info(
'[{:04d}] ANALOG IR ({:d}): \t'.format(count, 5) +
(Fore.RED if (_stbd_side_ir_data > 100.0) else Fore.YELLOW) +
Style.BRIGHT + '{:d}'.format(_stbd_side_ir_data) + Style.DIM +
'\t(analog value 0-255)')
self._callback(Event.INFRARED_STBD_SIDE, _stbd_side_ir_data)
_delta = dt.datetime.now() - _start_time
_elapsed_ms = int(_delta.total_seconds() * 1000)
self._log.info(Fore.BLACK +
'[{:04d}] poll end; elapsed processing time: {:d}ms'.
format(count, _elapsed_ms))
# return True
# ..........................................................................
def _get_mean_distance(self, orientation, value):
'''
Returns the mean of values collected in the queue for the specified IR sensor.
'''
if value == None or value == 0:
return None
if orientation is Orientation.PORT_SIDE:
_deque = self._deque_port_side
elif orientation is Orientation.PORT:
_deque = self._deque_port
elif orientation is Orientation.CNTR:
_deque = self._deque_cntr
elif orientation is Orientation.STBD:
_deque = self._deque_stbd
elif orientation is Orientation.STBD_SIDE:
_deque = self._deque_stbd_side
else:
raise ValueError('unsupported orientation.')
_deque.append(value)
_n = 0
_mean = 0.0
for x in _deque:
_n += 1
_mean += (x - _mean) / _n
if _n < 1:
return float('nan')
else:
return _mean
# ..........................................................................
def _convert_to_distance(self, value):
'''
Converts the value returned by the IR sensor to a distance in centimeters.
Distance Calculation ---------------
This is reading the distance from a 3 volt Sharp GP2Y0A60SZLF infrared
sensor to a piece of white A4 printer paper in a low ambient light room.
The sensor output is not linear, but its accuracy is not critical. If
the target is too close to the sensor the values are not valid. According
to spec 10cm is the minimum distance, but we get relative variability up
until about 5cm. Values over 150 clearly indicate the robot is less than
10cm from the target.
0cm = unreliable
5cm = 226.5
7.5cm = 197.0
10cm = 151.0
20cm = 92.0
30cm = 69.9
40cm = 59.2
50cm = 52.0
60cm = 46.0
70cm = 41.8
80cm = 38.2
90cm = 35.8
100cm = 34.0
110cm = 32.9
120cm = 31.7
130cm = 30.7 *
140cm = 30.7 *
150cm = 29.4 *
* Maximum range on IR is about 130cm, after which there is diminishing
stability/variability, i.e., it's hard to determine if we're dealing
with a level of system noise rather than data. Different runs produce
different results, with values between 28 - 31 on a range of any more
than 130cm.
See: http://ediy.com.my/blog/item/92-sharp-gp2y0a21-ir-distance-sensors
'''
if value == None or value == 0:
return None
self._use_pot = False
# if self._use_pot:
# _pot_value = self._pot.get_scaled_value()
# _EXPONENT = _pot_value
# else:
# _pot_value = 0.0
_EXPONENT = 1.33
_NUMERATOR = 1000.0
_distance = pow(_NUMERATOR / value, _EXPONENT) # 900
# self._log.debug(Fore.BLACK + Style.NORMAL + 'value: {:>5.2f}; pot value: {:>5.2f}; distance: {:>5.2f}cm'.format(value, _pot_value, _distance))
return _distance
# ..........................................................................
def _callback(self, event, value):
'''
This is the callback method from the I²C Master, whose events are
being returned from the source, e.g., Arduino or IO Expander board.
'''
if not self._enabled or self._suppressed:
self._log.info(Fore.BLACK + Style.DIM +
'SUPPRESSED callback: event {}; value: {:d}'.format(
event.name, value))
return
try:
# self._log.debug(Fore.BLACK + 'callback: event {}; value: {:d}'.format(event.name, value))
_event = None
_value = value
# bumpers ..................................................................................
if event == Event.BUMPER_PORT:
if value == 1:
_event = Event.BUMPER_PORT
_value = 1
elif event == Event.BUMPER_CNTR:
if value == 1:
_event = Event.BUMPER_CNTR
_value = 1
elif event == Event.BUMPER_STBD:
if value == 1:
_event = Event.BUMPER_STBD
_value = 1
# ..........................................................................................
# For IR sensors we rewrite the value with a dynamic mean distance (cm),
# setting the event type only if the value is less than a distance threshold.
elif event == Event.INFRARED_PORT_SIDE:
_value = self._get_mean_distance(
Orientation.PORT_SIDE, self._convert_to_distance(value))
self._log.info(
Fore.RED + Style.BRIGHT +
'mean distance: {:5.2f}cm;\tPORT SIDE'.format(_value))
if _value != None and _value < self._port_side_trigger_distance:
_fire_message(event, _value)
# _event = event
# else:
# self._log.info(Fore.RED + 'mean distance: {:5.2f}cm;\tPORT SIDE'.format(_value))
elif event == Event.INFRARED_PORT:
_value = self._get_mean_distance(
Orientation.PORT, self._convert_to_distance(value))
self._log.info(
Fore.RED + Style.BRIGHT +
'mean distance: {:5.2f}cm;\tPORT'.format(_value))
if _value != None and _value < self._port_trigger_distance:
_fire_message(event, _value)
# _event = event
# else:
# self._log.info(Fore.RED + 'mean distance: {:5.2f}cm;\tPORT'.format(_value))
elif event == Event.INFRARED_CNTR:
_value = self._get_mean_distance(
Orientation.CNTR, self._convert_to_distance(value))
self._log.info(
Fore.BLUE + Style.BRIGHT +
'mean distance: {:5.2f}cm;\tCNTR'.format(_value))
if _value != None and _value < self._center_trigger_distance:
_fire_message(event, _value)
# _event = event
# else:
# self._log.info(Fore.BLUE + 'mean distance: {:5.2f}cm;\tCNTR'.format(_value))
elif event == Event.INFRARED_STBD:
_value = self._get_mean_distance(
Orientation.STBD, self._convert_to_distance(value))
self._log.info(
Fore.GREEN + Style.BRIGHT +
'mean distance: {:5.2f}cm;\tSTBD'.format(_value))
if _value != None and _value < self._stbd_trigger_distance:
_fire_message(event, _value)
# _event = event
# else:
# self._log.info(Fore.GREEN + 'mean distance: {:5.2f}cm;\tSTBD'.format(_value))
elif event == Event.INFRARED_STBD_SIDE:
_value = self._get_mean_distance(
Orientation.STBD_SIDE, self._convert_to_distance(value))
self._log.info(
Fore.GREEN + Style.BRIGHT +
'mean distance: {:5.2f}cm;\tSTBD SIDE'.format(_value))
if _value != None and _value < self._stbd_side_trigger_distance:
_fire_message(event, _value)
# _event = event
# else:
# self._log.info(Fore.GREEN + 'mean distance: {:5.2f}cm;\tSTBD SIDE'.format(_value))
# if _event is not None and _value is not None:
# # if not self._ignore_duplicates or ( _event != self._last_event and _value != self._last_value ):
# _message = self._message_factory.get_message(_event, _value)
# self._log.info(Fore.WHITE + Style.BRIGHT + 'adding new message eid#{} for event {}'.format(_message.eid, _event.description))
# self._message_bus.handle(_message)
# # else:
# # self._log.warning(Fore.CYAN + Style.NORMAL + 'ignoring message for event {}'.format(_event.description))
# self._last_event = _event
# self._last_value = _value
# else:
# self._log.info(Fore.WHITE + Style.BRIGHT + 'NOT ADDING message eid#{} for event {}'.format(_message.eid, _event.description))
except Exception as e:
self._log.error('callback error: {}\n{}'.format(
e, traceback.format_exc()))
# ..........................................................................
def _fire_message(self, event, value):
self._log.info(
Fore.YELLOW + Style.BRIGHT +
'fire message with event {} and value {}'.format(event, value))
if event is not None and value is not None:
_message = self._message_factory.get_message(event, value)
self._log.info(Fore.WHITE + Style.BRIGHT +
'adding new message eid#{} for event {}'.format(
_message.eid, _event.description))
self._message_bus.handle(_message)
else:
self._log.info(Fore.RED + Style.BRIGHT +
'ignoring message with event {} and value {}'.
format(event, value))
# ..........................................................................
def get_input_for_event_type(self, event):
'''
Return the current value of the pin corresponding to the Event type.
'''
if event is Event.INFRARED_PORT_SIDE:
return self._ioe.get_port_side_ir_value()
elif event is Event.INFRARED_PORT:
return self._ioe.get_port_ir_value()
elif event is Event.INFRARED_CNTR:
return self._ioe.get_center_ir_value()
elif event is Event.INFRARED_STBD:
return self._ioe.get_stbd_ir_value()
elif event is Event.INFRARED_STBD_SIDE:
return self._ioe.get_stbd_side_ir_value()
elif event is Event.BUMPER_PORT:
return self._ioe.get_port_bmp_value()
elif event is Event.BUMPER_CNTR:
return self._ioe.get_center_bmp_value()
elif event is Event.BUMPER_STBD:
return self._ioe.get_stbd_bmp_value()
else:
raise Exception('unexpected event type.')
# ..........................................................................
def suppress(self, state):
self._log.info('suppress {}.'.format(state))
self._suppressed = state
# ..........................................................................
@property
def enabled(self):
return self._enabled
# ..........................................................................
def enable(self):
if not self._enabled:
if not self._closed:
self._log.info('enabled.')
self._enabled = True
else:
self._log.warning('cannot enable: already closed.')
else:
self._log.warning('already enabled.')
# ..........................................................................
def disable(self):
if self._enabled:
self._enabled = False
self._log.info(Fore.YELLOW +
'shutting down thread pool executor...')
self._executor.shutdown(
wait=False) # python 3.9: , cancel_futures=True)
self._log.info(
Fore.YELLOW +
'disabled: thread pool executor has been shut down.')
else:
self._log.debug('already disabled.')
# ..........................................................................
def close(self):
'''
Permanently close and disable the integrated front sensor.
'''
if not self._closed:
self.disable()
self._closed = True
self._log.info('closed.')
else:
self._log.warning('already closed.')
# ..........................................................................
@staticmethod
def clamp(n, minn, maxn):
return max(min(maxn, n), minn)
# ..........................................................................
@staticmethod
def remap(x, in_min, in_max, out_min, out_max):
return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min
class NXP9DoF:
'''
Reads from an Adafruit NXP 9-DoF FXOS8700 + FXAS21002 sensor.
Note that this has been calibrated based on the orientation of the
NXP9DoF board as installed on the KR01 robot, with the Adafruit
logo on the chip (mounted horizontally) as follows:
Fore
_---------------
| * |
| |
| |
Port | | Starboard
| |
----------------
Aft
If you mount the chip in a different orientation you will likely
need to multiply one or more of the axis by -1.0.
Depending on where you are on the Earth, true North changes because
the Earth is not a perfect sphere. You can get the declination angle
for your location from:
http://www.magnetic-declination.com/
E.g., for Pukerua Bay, New Zealand:
Latitude: 41° 1' 57.7" S
Longitude: 174° 53' 11.8" E
PUKERUA BAY
Magnetic Declination: +22° 37'
Declination is POSITIVE (EAST)
Inclination: 66° 14'
Magnetic field strength: 55716.5 nT
E.g., for Wellington, New Zealand:
Latitude: 41° 17' 11.9" S
Longitude: 174° 46' 32.1" E
KELBURN
Magnetic Declination: +22° 47'
Declination is POSITIVE (EAST)
Inclination: 66° 28'
Magnetic field strength: 55863.3 nT
'''
# permitted error between Euler and Quaternion (in degrees) to allow setting value
ERROR_RANGE = 5.0 # was 3.0
# ..........................................................................
def __init__(self, config, queue, level):
self._log = Logger("nxp9dof", level)
if config is None:
raise ValueError("no configuration provided.")
self._config = config
self._queue = queue
_config = self._config['ros'].get('nxp9dof')
self._quaternion_accept = _config.get(
'quaternion_accept') # permit Quaternion once calibrated
self._loop_delay_sec = _config.get('loop_delay_sec')
# verbose will print some start-up info on the IMU sensors
self._imu = IMU(gs=4, dps=2000, verbose=True)
# setting a bias correction for the accel only; the mags/gyros are not getting a bias correction
self._imu.setBias((0.0, 0.0, 0.0), None, None)
# self._imu.setBias((0.1,-0.02,.25), None, None)
_i2c = busio.I2C(board.SCL, board.SDA)
self._fxos = adafruit_fxos8700.FXOS8700(_i2c)
self._log.info('accelerometer and magnetometer ready.')
self._fxas = adafruit_fxas21002c.FXAS21002C(_i2c)
self._log.info('gyroscope ready.')
self._thread = None
self._enabled = False
self._closed = False
self._heading = None
self._pitch = None
self._roll = None
self._is_calibrated = False
self._log.info('ready.')
# ..........................................................................
def get_imu(self):
return self._imu
# ..........................................................................
def get_fxos(self):
return self._fxos
# ..........................................................................
def get_fxas(self):
return self._fxas
# ..........................................................................
def enable(self):
if not self._closed:
self._enabled = True
# if we haven't started the thread yet, do so now...
if self._thread is None:
self._thread = threading.Thread(target=NXP9DoF._read,
args=[self])
self._thread.start()
self._log.info('enabled.')
else:
self._log.warning('cannot enable: already closed.')
# ..........................................................................
def get_heading(self):
return self._heading
# ..........................................................................
def get_pitch(self):
return self._pitch
# ..........................................................................
def get_roll(self):
return self._roll
# ..........................................................................
def is_calibrated(self):
return self._is_calibrated
# ..........................................................................
@staticmethod
def _in_range(p, q):
return p >= (q - NXP9DoF.ERROR_RANGE) and p <= (q +
NXP9DoF.ERROR_RANGE)
# ..........................................................................
def imu_enable(self):
'''
Enables the handbuilt IMU on a 20Hz loop.
'''
if not self._closed:
self._enabled = True
# if we haven't started the thread yet, do so now...
if self._thread is None:
self._thread = threading.Thread(target=NXP9DoF._handbuilt_imu,
args=[self])
self._thread.start()
self._log.info('enabled.')
else:
self._log.warning('cannot enable: already closed.')
# ..........................................................................
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 _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 disable(self):
self._enabled = False
self._log.info('disabled.')
# ..........................................................................
def close(self):
self._closed = True
self._log.info('closed.')
class IoExpander():
'''
Wraps an IO Expander board as input from an integrated front sensor
array of infrareds and bumper switches.
'''
def __init__(self, config, level):
super().__init__()
if config is None:
raise ValueError('no configuration provided.')
_config = config['ros'].get('io_expander')
self._log = Logger('ioe', level)
# infrared
self._port_side_ir_pin = _config.get(
'port_side_ir_pin') # pin connected to port side infrared
self._port_ir_pin = _config.get(
'port_ir_pin') # pin connected to port infrared
self._center_ir_pin = _config.get(
'center_ir_pin') # pin connected to center infrared
self._stbd_ir_pin = _config.get(
'stbd_ir_pin') # pin connected to starboard infrared
self._stbd_side_ir_pin = _config.get(
'stbd_side_ir_pin') # pin connected to starboard side infrared
self._log.info('infrared pin assignments:\t' \
+ Fore.RED + ' port side={:d}; port={:d};'.format(self._port_side_ir_pin, self._port_ir_pin) \
+ Fore.BLUE + ' center={:d};'.format(self._center_ir_pin) \
+ Fore.GREEN + ' stbd={:d}; stbd side={:d}'.format(self._stbd_ir_pin, self._stbd_side_ir_pin))
# bumpers
self._port_bmp_pin = _config.get(
'port_bmp_pin') # pin connected to port bumper
self._center_bmp_pin = _config.get(
'center_bmp_pin') # pin connected to center bumper
self._stbd_bmp_pin = _config.get(
'stbd_bmp_pin') # pin connected to starboard bumper
self._log.info('bumper pin assignments:\t' \
+ Fore.RED + ' port={:d};'.format(self._port_ir_pin) \
+ Fore.BLUE + ' center={:d};'.format(self._center_ir_pin ) \
+ Fore.GREEN + ' stbd={:d}'.format(self._stbd_ir_pin))
# configure board
self._ioe = io.IOE(i2c_addr=0x18)
self.board = self._ioe # TEMP
self._ioe.set_adc_vref(
3.3
) # input voltage of IO Expander, this is 3.3 on Breakout Garden
# analog infrared sensors
self._ioe.set_mode(self._port_side_ir_pin, io.ADC)
self._ioe.set_mode(self._port_ir_pin, io.ADC)
self._ioe.set_mode(self._center_ir_pin, io.ADC)
self._ioe.set_mode(self._stbd_ir_pin, io.ADC)
self._ioe.set_mode(self._stbd_side_ir_pin, io.ADC)
# digital bumpers
self._ioe.set_mode(self._port_bmp_pin, io.PIN_MODE_PU)
self._ioe.set_mode(self._center_bmp_pin, io.PIN_MODE_PU)
self._ioe.set_mode(self._stbd_bmp_pin, io.PIN_MODE_PU)
self._log.info('ready.')
def get_port_side_ir_value(self):
return int(round(self._ioe.input(self._port_side_ir_pin) * 100.0))
def get_port_ir_value(self):
return int(round(self._ioe.input(self._port_ir_pin) * 100.0))
def get_center_ir_value(self):
return int(round(self._ioe.input(self._center_ir_pin) * 100.0))
def get_stbd_ir_value(self):
return int(round(self._ioe.input(self._stbd_ir_pin) * 100.0))
def get_stbd_side_ir_value(self):
return int(round(self._ioe.input(self._stbd_side_ir_pin) * 100.0))
def get_port_bmp_value(self):
return self._ioe.input(self._port_bmp_pin) == 0
def get_center_bmp_value(self):
return self._ioe.input(self._center_bmp_pin) == 0
def get_stbd_bmp_value(self):
return self._ioe.input(self._stbd_bmp_pin) == 0
def main():
signal.signal(signal.SIGINT, signal_handler)
_log = Logger("scanner", Level.INFO)
_log.info(Fore.CYAN + Style.BRIGHT + ' INFO : starting test...')
_log.info(Fore.YELLOW + Style.BRIGHT + ' INFO : Press Ctrl+C to exit.')
try:
_i2c_scanner = I2CScanner(Level.WARN)
_addresses = _i2c_scanner.get_int_addresses()
_hex_addresses = _i2c_scanner.get_hex_addresses()
_addrDict = dict(
list(map(lambda x, y: (x, y), _addresses, _hex_addresses)))
for i in range(len(_addresses)):
_log.debug(Fore.BLACK + Style.DIM +
'found device at address: {}'.format(_hex_addresses[i]))
vl53l1x_available = (0x29 in _addresses)
ultraborg_available = (0x36 in _addresses)
if not vl53l1x_available:
raise OSError('VL53L1X hardware dependency not available.')
if not ultraborg_available:
# raise OSError('UltraBorg hardware dependency not available.')
print('UltraBorg hardware dependency not available.')
_log.info('starting scan...')
# _player = Player(Level.INFO)
_loader = ConfigLoader(Level.INFO)
filename = 'config.yaml'
_config = _loader.configure(filename)
_lidar = Lidar(_config, Level.INFO)
_lidar.enable()
for i in range(5):
values = _lidar.scan()
_angle_at_min = values[0]
if _angle_at_min < 0:
_mm = values[3]
_log.info(Fore.CYAN + Style.BRIGHT +
'distance:\t{}mm'.format(_mm))
else:
_min_mm = values[1]
_angle_at_max = values[2]
_max_mm = values[3]
_log.info(Fore.CYAN + Style.BRIGHT +
'min. distance at {:>5.2f}°:\t{}mm'.format(
_angle_at_min, _min_mm))
_log.info(Fore.CYAN + Style.BRIGHT +
'max. distance at {:>5.2f}°:\t{}mm'.format(
_angle_at_max, _max_mm))
time.sleep(1.0)
_lidar.close()
_log.info(Fore.CYAN + Style.BRIGHT + 'test complete.')
except Exception:
_log.info(traceback.format_exc())
sys.exit(1)
def making_host_decision(self, application, decision, release_node=False):
"""
Make decision on which host to run container
Args:
application(str)
decision(str)
Returns:
host(str)
"""
logger = Logger(filename="orchastrator",
logger_name="DecisionMaker making_host_decision")
swarm_manager = SwarmManagment()
app_per_node = "{}_per_node".format(application)
app_by_hosts = self.counting_app_by_host(application)
if release_node:
del (app_by_hosts[release_node])
host_number = len(app_by_hosts.keys())
if decision is 'up':
application_number = 0
for host in app_by_hosts.keys():
if app_by_hosts[host][application] == 0:
return host
else:
application_number += app_by_hosts[host][application]
average_app_number = application_number / host_number
# print("Average => {}".format(average_app_number))
# print("Appp => {}".format(parse_config('orchastrator.json')[app_per_node]))
logger.info("Aplication {} ||| Average => {}\tApp_per_node => {}". \
format(application, average_app_number, parse_config('orchastrator.json')[app_per_node]))
logger.clear_handler()
###logic for adding node to the swarm
if average_app_number >= parse_config(
'orchastrator.json')[app_per_node]:
if len(parse_config('orchastrator.json')
['available_servers']) != 0:
new_server = parse_config(
'orchastrator.json')['available_servers'][0]
swarm_manager.join_server_swarm(host_ip=parse_config(
'orchastrator.json')['available_servers'][0])
return new_server
else:
logger.critical("There are not any available servers should" \
"look at host stat to run on the lowest" \
"loaded host a container")
logger.clear_handler()
# print("There are not any available servers should \
# look at host stat to run on the lowest \
# loaded host a container")
###logic for adding node to the swarm
for host in app_by_hosts.keys():
if app_by_hosts[host][application] < average_app_number and \
app_by_hosts[host][application] < parse_config('orchastrator.json')[app_per_node]:
return host
for host in app_by_hosts.keys():
return host
elif decision is 'down':
application_number = 0
for host in app_by_hosts.keys():
application_number += app_by_hosts[host][application]
min_app = "{}_min".format(application)
# print("Min_app => {}\t app_num {}".format(parse_config('orchastrator.json')[min_app], application_number))
logger.warning("Application => {} ||| min_apps on platform=> {}\tcurrent app_num {}". \
format(application, parse_config('orchastrator.json')[min_app], application_number))
logger.clear_handler()
if application_number == parse_config(
'orchastrator.json')[min_app]:
return None
average_app_number = application_number / host_number
for host in app_by_hosts.keys():
if app_by_hosts[host][application] > average_app_number and \
app_by_hosts[host][application] < parse_config('orchastrator.json')[app_per_node]:
return host
for host in app_by_hosts.keys():
return host
class Elastic(object):
'''
Configurable access to an elasticsearch service with support for
basic search engine API calls.
'''
# ..........................................................................
def __init__(self, config, level):
self._log = Logger("elastic", level)
if config is None:
raise ValueError('no configuration provided.')
_config = config['ros'].get('elastic')
self._host = _config.get('host')
self._port = _config.get('port')
self._log.info('service address: {}:{}'.format(self._host, self._port))
self._schema = _config.get('schema')
self._index = _config.get('index')
self._doc_type = _config.get('doc_type')
self._log.info('schema: \'{}\';\tindex: \'{}\';\tdoctype: \'{}\t'.format(self._schema, self._index, self._doc_type))
self._log.info('connecting to elastic search cluster...')
self._es = Elasticsearch(host=self._host, port=self._port)
# self._es = Elasticsearch(
# ['esnode1', 'esnode2'],
# # sniff before doing anything
# sniff_on_start=True,
# # refresh nodes after a node fails to respond
# sniff_on_connection_fail=True,
# # and also every 60 seconds
# sniffer_timeout=60
# )
self._log.info('ready.')
# ..........................................................................
def ping(self):
if self._es.ping():
self._log.info('successfully pinged server.')
return True
else:
self._log.warning('unable to ping server.')
return False
# ..........................................................................
def create_index(self):
_response = self._es.indices.exists(index=self._index)
if _response:
self._log.info('index exists?; response: {}'.format(_response))
else:
# ignore 400 cause by IndexAlreadyExistsException when creating an index
_response = self._es.indices.create(index=self._index, ignore=400)
self._log.info('created index; response: {}'.format(_response))
# ..........................................................................
def get_index(self):
_response = self._es.indices.get(index=self._index)
if _response:
self._log.info('GET index {} successful.'.format(self._index))
else:
self._log.info('index did not exist.')
return _response
# ..........................................................................
def delete_index(self):
_response = self._es.indices.exists(index=self._index)
if _response:
self._log.info('index exists?; response: {}'.format(_response))
_response = self._es.indices.delete(index=self._index)
self._log.info('index deleted; response: {}'.format(_response))
else:
self._log.info('index did not exist.')
# ..........................................................................
def insert_document(self, oid, content):
# index and doc_type you can customize by yourself
_response = self._es.index(index=self._index, doc_type=self._doc_type, id=oid, body=content)
# index will return insert info: like as created is True or False
# self._log.info(Style.BRIGHT + 'inserted document with oid {}; response:\n'.format(oid) + Fore.GREEN + Style.NORMAL + json.dumps(_response, indent=4))
_version = _response.get('_version')
self._log.info('inserted document with oid {}; version: {}'.format(oid, _version))
return _response
# ..........................................................................
def get(self):
_response = requests.get(self._host)
self._log.info('response: {}'.format(_response.content))
pass
# ..........................................................................
def put(self, message):
'''
Sends the message to the Elasticsearch service.
'''
self._log.info('message: {}'.format(message))
pass
class I2cMaster():
'''
A Raspberry Pi Master for a corresponding Arduino Slave.
Parameters:
device_id: the I²C address over which the master and slave communicate
level: the log level, e.g., Level.INFO
'''
CMD_RESET_CONFIGURATION = 234
CMD_RETURN_IS_CONFIGURED = 235
def __init__(self, config, level):
super().__init__()
if config is None:
raise ValueError('no configuration provided.')
self._config = config['ros'].get('i2c_master')
self._device_id = self._config.get('device_id') # i2c hex address of slave device, must match Arduino's SLAVE_I2C_ADDRESS
self._channel = self._config.get('channel')
self._log = Logger('i²cmaster-0x{:02x}'.format(self._device_id), level)
self._log.info('initialising...')
self._counter = itertools.count()
self._loop_count = 0 # currently only used in testing
self._thread = None
self._enabled = False
self._closed = False
self._i2c = SMBus(self._channel)
self._log.info('ready: imported smbus2 and obtained I²C bus at address 0x{:02X}...'.format(self._device_id))
# ..........................................................................
def enable(self):
self._enabled = True
self._log.info('enabled.')
# ..........................................................................
def disable(self):
self._enabled = False
self._log.info('disabled.')
# ..............................................................................
def read_i2c_data(self):
'''
Read a byte from the I²C device at the specified handle, returning the value.
## Examples:
int.from_bytes(b'\x00\x01', "big") # 1
int.from_bytes(b'\x00\x01', "little") # 256
'''
# self._log.info(Fore.BLUE + Style.BRIGHT + '1. reading byte...')
with SMBus(self._channel) as bus:
# _byte = bus.read_byte(self._device_id)
_byte = bus.read_byte_data(self._device_id, 0)
time.sleep(0.01)
# self._log.info(Fore.BLUE + Style.BRIGHT + '2. read byte:\t{:08b}'.format(_byte))
# self._log.info(Fore.BLUE + Style.BRIGHT + '2. read byte:\t{}'.format(_byte))
return _byte
# ..........................................................................
def write_i2c_data(self, data):
'''
Write a byte to the I²C device at the specified handle.
'''
# self._log.info(Fore.RED + '1. writing byte:\t{:08b}'.format(data))
with SMBus(self._channel) as bus:
bus.write_byte(self._device_id, data)
time.sleep(0.01)
# bus.write_byte_data(self._device_id, 0, data)
# self._log.info(Fore.RED + '2. wrote byte:\t{}'.format(data))
# ..........................................................................
def get_input_from_pin(self, pinPlusOffset):
'''
Sends a message to the pin (which should already include an offset if
this is intended to return a non-pin value), returning the result as
a byte.
'''
self.write_i2c_data(pinPlusOffset)
_received_data = self.read_i2c_data()
self._log.debug('received response from pin {:d} of {:08b}.'.format(pinPlusOffset, _received_data))
return _received_data
# ..........................................................................
def set_output_on_pin(self, pin, value):
'''
Set the output on the pin as true (HIGH) or false (LOW). The
corresponding pin must have already been configured as OUTPUT.
This returns the response from the Arduino.
'''
if value is True:
self.write_i2c_data(pin + 192)
self._log.debug('set pin {:d} as HIGH.'.format(pin))
else:
self.write_i2c_data(pin + 160)
self._log.debug('set pin {:d} as LOW.'.format(pin))
_received_data = self.read_i2c_data()
self._log.debug('received response on pin {:d} of {:08b}.'.format(pin, _received_data))
return _received_data
# ..........................................................................
def configure_pin_as_digital_input(self, pin):
'''
Equivalent to calling pinMode(pin, INPUT), which sets the pin as a digital input pin.
This configuration treats the output as a digital value, returning a 0 (inactive) or 1 (active).
32-63: set the pin (n-32) as an INPUT pin, return pin number
'''
self._log.debug('configuring pin {:d} for DIGITAL_INPUT...'.format(pin))
self.write_i2c_data(pin + 32)
_received_data = self.read_i2c_data()
if pin == _received_data:
self._log.info('configured pin {:d} for DIGITAL_INPUT'.format(pin))
else:
_err_msg = self.get_error_message(_received_data)
# self._log.error('failed to configure pin {:d} for DIGITAL_INPUT; response: {}'.format(pin, _err_msg))
raise Exception('failed to configure pin {:d} for DIGITAL_INPUT; response: {}'.format(pin, _err_msg))
# ..........................................................................
def configure_pin_as_digital_input_pullup(self, pin):
'''
Equivalent to calling pinMode(pin, INPUT_PULLUP), which sets the pin as a
digital input pin with an internal pullup resister.
This configuration treats the output as a digital value, returning a 0 (active) or 1 (inactive).
64-95: set the pin (n-64) as an INPUT_PULLUP pin, return pin number
'''
self._log.debug('configuring pin {:d} for DIGITAL_INPUT_PULLUP'.format(pin))
self.write_i2c_data(pin + 64)
_received_data = self.read_i2c_data()
if pin == _received_data:
self._log.info('configured pin {:d} for DIGITAL_INPUT_PULLUP'.format(pin))
else:
_err_msg = self.get_error_message(_received_data)
# self._log.error('failed to configure pin {:d} for DIGITAL_INPUT_PULLUP; response: {}'.format(pin, _err_msg))
raise Exception('failed to configure pin {:d} for DIGITAL_INPUT_PULLUP; response: {}'.format(pin, _err_msg))
# ..........................................................................
def configure_pin_as_analog_input(self, pin):
'''
Equivalent to calling pinMode(pin, INPUT);
which sets the pin as an input pin.
This configuration treats the output as an analog value, returning a value from 0 - 255.
96-127: set the pin (n-96) as an INPUT_ANALOG pin, return pin number
'''
self._log.debug('configuring pin {:d} for ANALOG_INPUT...'.format(pin))
self.write_i2c_data(pin + 96)
_received_data = self.read_i2c_data()
print(Fore.GREEN + 'RX: ' + Fore.MAGENTA + Style.BRIGHT + '_received_data: {}'.format(_received_data) + Style.RESET_ALL)
if pin == _received_data:
self._log.info('configured pin {:d} for ANALOG_INPUT'.format(pin))
else:
_err_msg = self.get_error_message(_received_data)
# self._log.error('failed to configure pin {:d} for ANALOG_INPUT; response: {}'.format(pin, _err_msg))
print(Fore.MAGENTA + Style.BRIGHT + 'failed to configure pin {:d} for ANALOG_INPUT; response: {}'.format(pin, _err_msg))
raise Exception('failed to configure pin {:d} for ANALOG_INPUT; response: {}'.format(pin, _err_msg))
# ..........................................................................
def configure_pin_as_output(self, pin):
'''
Equivalent to calling pinMode(pin, OUTPUT), which sets the pin as an output pin.
The output from calling this pin returns the current output setting. To set the
value to 0 or 1 call setOutputForPin().
128-159: set the pin (n-128) as an OUTPUT pin, return pin number
'''
self._log.debug('configuring pin {:d} for OUTPUT...'.format(pin))
self.write_i2c_data(pin + 128)
_received_data = self.read_i2c_data()
if pin == _received_data:
self._log.info('configured pin {:d} for OUTPUT'.format(pin))
else:
_err_msg = self.get_error_message(_received_data)
# self._log.error('failed to configure pin {:d} for OUTPUT; response: {}'.format(pin, _err_msg))
raise Exception('failed to configure pin {:d} for OUTPUT; response: {}'.format(pin, _err_msg))
# ..........................................................................
def get_error_message(self, n):
'''
These match the error numbers in the Arduino sketch.
'''
switcher = {
249: "249: EMPTY_QUEUE", # returned on error
250: "250: PIN_UNASSIGNED", # configuration error
251: "251: PIN_ASSIGNED_AS_OUTPUT", # configuration error
252: "252: PIN_ASSIGNED_AS_INPUT", # configuration error
253: "253: SYNCHRONISATION_ERROR", # returned on communications error
254: "254: UNRECOGNISED_COMMAND", # returned on communications error
255: "255: NO_DATA" # considered as a 'null'
}
return switcher.get(n, "{}: UNRECOGNISED_ERROR".format(n))
# ..........................................................................
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()
# ..........................................................................
def _front_sensor_loop(self, assignments, loop_delay_sec, callback):
self._log.info('starting event loop...\n')
while self._enabled:
_count = next(self._counter)
# print(CLEAR_SCREEN)
for pin in assignments:
pin_type = assignments[pin]
self._log.debug('reading from pin {}:\ttype: {}.'.format( pin, pin_type.description))
# write and then read response to/from Arduino...
_data_to_send = pin
self.write_i2c_data(_data_to_send)
time.sleep(0.2)
_received_data = self.read_i2c_data()
time.sleep(0.2)
callback(pin, pin_type, _received_data)
time.sleep(loop_delay_sec)
# we never get here if using 'while True:'
self._log.info('exited event loop.')
# ..........................................................................
def start_front_sensor_loop(self, assignments, loop_delay_sec, callback):
'''
This is the method to call to actually start the loop.
Configuration must have already occurred.
'''
if not self._enabled:
raise Exception('attempt to start front sensor event loop while disabled.')
elif not self._closed:
if self._thread is None:
enabled = True
self._thread = threading.Thread(target=I2cMaster._front_sensor_loop, args=[self, assignments, loop_delay_sec, callback])
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 configure_front_sensor_loop(self):
'''
Configures each of the pin types based on the application-level YAML
configuration file, then starts a loop that continues while enabled.
This passes incoming events from the Arduino to the message queue.
The prototype (KR01) configuration includes:
* 0: starboard side analog infrared sensor.
* 1: starboard analog infrared sensor.
* 2: center analog infrared sensor.
* 3: port analog infrared sensor.
* 4: port side analog infrared sensor.
* 9: starboard bumper (digital).
* 10: center bumper (digital).
* 11: port bumper (digital).
Analog sensors return a value from 0-255, digital 0 or 1.
'''
try:
self._log.info('front_sensor_loop: configuring the arduino and then reading the results...')
_is_configured = self.is_configured()
_assignment_config = self._config.get('assignments')
_assignments = {}
# configure pins...
for pin, pin_type_param in _assignment_config.items():
pin_type = PinType.from_str(pin_type_param)
_assignments.update({ pin : pin_type })
if not _is_configured:
self._log.info('configuring pin {}:\ttype: {}.'.format( pin, pin_type.description))
if pin_type is PinType.DIGITAL_INPUT: # configure pin as DIGITAL_INPUT
self.configure_pin_as_digital_input(pin)
elif pin_type is PinType.DIGITAL_INPUT_PULLUP: # configure pin as DIGITAL_INPUT_PULLUP
self.configure_pin_as_digital_input_pullup(pin)
elif pin_type is PinType.ANALOG_INPUT: # configure pin as ANALOG_INPUT
self.configure_pin_as_analog_input(pin)
# elif pin_type is PinType.OUTPUT: # configure pin as OUTPUT
# self.configure_pin_as_output(pin)
else:
self._log.info('already configuring pin {}:\ttype: {}.'.format( pin, pin_type.description))
self._log.info('front sensor loop configured.')
return _assignments
except KeyboardInterrupt:
self._log.warning('Ctrl-C caught; exiting...')
return None
except Exception as e:
self._log.error('error in master: {}'.format(e))
traceback.print_exc(file=sys.stdout)
# sys.exit(1)
return None
# ..........................................................................
def is_configured(self):
'''
Sends a 235 (congiguration check) code to the slave, returning a
value as 1 (as True) or 0 (as False).
'''
try:
self._log.info('starting configuration check...')
_data_to_send = I2cMaster.CMD_RETURN_IS_CONFIGURED
self.write_i2c_data(_data_to_send)
_received_data = self.read_i2c_data()
if _received_data == 1:
self._log.info('configuration check returned: ' + Fore.GREEN + 'true.')
return True
else:
self._log.info('configuration check returned: ' + Fore.CYAN + '{}'.format(_received_data))
return False
except Exception as e:
_data_to_send = I2cMaster.CMD_RESET_CONFIGURATION
self.write_i2c_data(_data_to_send)
_received_data = self.read_i2c_data()
traceback.print_exc(file=sys.stdout)
if _received_data == 1:
self._log.info('configuration recovery returned: ' + Fore.GREEN + 'true.')
return True
else:
self._log.error('error in is_configured check: {}'.format(e) + '; attempted recovery returned: {}'.format(_received_data))
return False
# ..........................................................................
def close(self):
'''
Close the instance.
'''
self._log.debug('closing I²C master.')
if not self._closed:
try:
if self._thread != None:
self._thread.join(timeout=1.0)
self._log.debug('front sensor loop thread joined.')
self._thread = None
self._closed = True
# self._pi.i2c_close(self._handle) # close device
self._log.debug('I²C master closed.')
except Exception as e:
self._log.error('error closing master: {}'.format(e))
else:
self._log.debug('I²C master already closed.')
# tests ====================================================================
# ..........................................................................
def test_echo(self):
'''
Performs a simple test, sending a series of bytes to the slave, then
reading the return values. The Arduino slave's 'isEchoTest' boolean
must be set to true. This does not require any hardware configuration
save that the Raspberry Pi must be connected to the Arduino via I²C
and that there is no contention on address 0x08.
'''
try:
self._log.info('starting echo test...')
_data = [ 0, 1, 2, 4, 32, 63, 64, 127, 128, 228, 254, 255 ]
for i in range(0,len(_data)):
_data_to_send = _data[i]
self.write_i2c_data(_data_to_send)
_received_data = self.read_i2c_data()
if _data_to_send == _received_data:
self._log.info('echo succeeded: {} == {}'.format(_data_to_send, _received_data))
else:
self._log.error('echo failed: {} != {}'.format(_data_to_send, _received_data))
# self._pi.i2c_close()
self._log.info('echo test complete.')
except Exception as e:
self._log.error('error in echo test: {}'.format(e))
traceback.print_exc(file=sys.stdout)
# ..........................................................................
def test_blink_led(self, pin, blink_count):
'''
Blinks an LED connected to the specified pin of the Arduino. If you're
using a 5V Arduino a 330 ohm resistor should be connected in series
with the LED, as LEDs cannot directly handle a 5v source.
'''
try:
self._log.info(Fore.CYAN + Style.BRIGHT + 'blink the LED {:d} times...'.format(blink_count))
self._log.info(Fore.YELLOW + Style.BRIGHT + 'type Ctrl-C to cancel the test.')
# configure an LED as OUTPUT on specified pin
self.configure_pin_as_output(pin)
# 225: set request count to zero
request_count = self.get_input_from_pin(225)
self._log.info('set loop count to zero; response: {:>5.2f}'.format(request_count))
# while True: # if you want to blink foreever
for i in range(blink_count):
_received_data = self.set_output_on_pin(pin, True)
self._log.debug(Fore.YELLOW + 'blink ON on pin {:d}; response: {:>5.2f}'.format(pin, _received_data))
time.sleep(0.025)
_received_data = self.set_output_on_pin(pin, False)
self._log.debug(Fore.MAGENTA + 'blink OFF on pin {:d}; response: {:>5.2f}'.format(pin, _received_data))
time.sleep(0.015)
# 226: return request count (we expect 2x the blink count plus one for the request count itself
_expected = 2 * ( blink_count * 2 + 1 )
request_count = self.get_input_from_pin(226)
if _expected == request_count:
self._log.info(Style.BRIGHT + 'request count: ' + Fore.CYAN + Style.NORMAL + 'expected: {:d}; actual: {:d}'.format(_expected, request_count))
else:
self._log.warning('request count: ' + Fore.CYAN + Style.NORMAL + 'expected: {:d}; actual: {:d}'.format(_expected, request_count))
except KeyboardInterrupt:
self._log.warning('Ctrl-C caught; exiting...')
except Exception as e:
self._log.error('error in master: {}'.format(e))
traceback.print_exc(file=sys.stdout)
# ..........................................................................
def test_configuration(self, loop_count):
'''
Configures each of the pin types based on the application-level YAML
configuration. You can see the result of the configuration on the
Arduino IDE's Serial Monitor (even without any associated hardware).
If you set up the hardware to match this configuration you can also
see the input and output results on the Serial Monitor.
The prototype hardware configuration is as follows:
* An LED (and a 330 ohm resistor) connected between pin 7 and ground, configured as OUTPUT.
* A pushbutton connected between pin 9 and ground, configured as INPUT_PULLUP.
* A digital infrared sensor connected to pin 10, configured as INPUT.
* A digital infrared sensor connected to pin 11, configured as INPUT_PULLUP.
* An analog infrared sensor connected to pin A1, configured as INPUT.
'''
try:
self._log.info(Fore.CYAN + Style.BRIGHT + 'test configuring the arduino and then reading the results...')
self._log.info(Fore.YELLOW + Style.BRIGHT + 'type Ctrl-C to cancel the test.')
LOOP_DELAY_SEC = 1
_assignment_config = self._config.get('assignments')
_assignments = {}
for pin, pin_type_param in _assignment_config.items():
pin_type = PinType.from_str(pin_type_param)
self._log.warning('configuring pin {}:\ttype: {}.'.format( pin, pin_type.description))
_assignments.update({ pin : pin_type })
if pin_type is PinType.DIGITAL_INPUT: # configure pin as DIGITAL_INPUT
self.configure_pin_as_digital_input(pin)
elif pin_type is PinType.DIGITAL_INPUT_PULLUP: # configure pin as DIGITAL_INPUT_PULLUP
self.configure_pin_as_digital_input_pullup(pin)
elif pin_type is PinType.ANALOG_INPUT: # configure pin as ANALOG_INPUT
self.configure_pin_as_analog_input(pin)
elif pin_type is PinType.OUTPUT: # configure pin as OUTPUT
self.configure_pin_as_output(pin)
self._log.info('configured. starting loop to repeat {:d} times...\n'.format(loop_count))
for x in range(loop_count):
_count = next(self._counter)
print(CLEAR_SCREEN)
for pin in _assignments:
pin_type = _assignments[pin]
self._log.debug('reading from pin {}:\ttype: {}.'.format( pin, pin_type.description))
_data_to_send = pin
# write and then read response to/from Arduino...
self.write_i2c_data(_data_to_send)
_received_data = self.read_i2c_data()
_is_close = _received_data > 100.0
if pin_type is PinType.DIGITAL_INPUT: # configure pin as DIGITAL_INPUT
self._log.info('[{:04d}] DIGITAL IR ({:d}): \t'.format(_count, pin) + Fore.CYAN + Style.BRIGHT + '{:d}'.format(_received_data) + Style.DIM + '\t(displays digital value 0|1)')
elif pin_type is PinType.DIGITAL_INPUT_PULLUP: # configure pin as DIGITAL_INPUT_PULLUP
self._log.info('[{:04d}] DIGITAL IR ({:d}): \t'.format(_count, pin) + Fore.GREEN + Style.BRIGHT + '{:d}'.format(_received_data) + Style.DIM + '\t(displays digital pup value 0|1)')
elif pin_type is PinType.ANALOG_INPUT: # configure pin as ANALOG_INPUT
_close_color = Fore.RED if _is_close else Fore.YELLOW
self._log.info('[{:04d}] ANALOG IR ({:d}): \t'.format(_count, pin) + _close_color + Style.BRIGHT + '{:d}'.format(_received_data) + Style.DIM + '\t(analog value 0-255)')
elif pin_type is PinType.OUTPUT: # configure pin as OUTPUT
self._log.info('[{:04d}] LED ({:d}): \t'.format(_count, pin) + Fore.MAGENTA + Style.BRIGHT + '{:d}'.format(_received_data) + Style.DIM + '\t(expected 251/PIN_ASSIGNED_AS_OUTPUT)')
print('')
time.sleep(LOOP_DELAY_SEC)
# we never get here if using 'while True:'
self._log.info('test complete.')
except KeyboardInterrupt:
self._log.warning('Ctrl-C caught; exiting...')
except Exception as e:
self._log.error('error in master: {}'.format(e))
traceback.print_exc(file=sys.stdout)
from state_machine_handler import TransitGateway, VPC, DynamoDb, ResourceAccessManager, ApprovalNotification
from lib.logger import Logger
import os
import inspect
import os.path
import sys
# initialise logger
LOG_LEVEL = os.environ['LOG_LEVEL']
logger = Logger(loglevel=LOG_LEVEL)
import botocore
import boto3
logger.info("boto3 version:" + boto3.__version__)
logger.info("botocore version:" + botocore.__version__)
def transit_gateway(event, function_name):
logger.info("Router FunctionName: {}".format(function_name))
tgw = TransitGateway(event, logger)
if function_name == 'describe_transit_gateway_vpc_attachments':
response = tgw.describe_transit_gateway_vpc_attachments()
elif function_name == 'tgw_attachment_crud_operations':
response = tgw.tgw_attachment_crud_operations()
elif function_name == 'describe_transit_gateway_route_tables':
response = tgw.describe_transit_gateway_route_tables()
elif function_name == 'disassociate_transit_gateway_route_table':
response = tgw.disassociate_transit_gateway_route_table()
class MessageHandler(object):
def __init__(self, level):
super().__init__()
self._log = Logger('msg-hand', level)
self._log.info('ready.')
# ..........................................................................
def add(self, message):
event = message.event
self._log.info(Fore.MAGENTA + 'rx message: {}; event: {}; value: {:>5.2f}'.format(\
message.description, event.description, message.value))
if event is Event.INFRARED_PORT_SIDE:
self._log.info(Fore.RED + Style.BRIGHT +
'event: {};\tvalue: {:5.2f}cm'.format(
event.description, message.value))
elif event is Event.INFRARED_PORT:
self._log.info(Fore.RED + Style.BRIGHT +
'event: {};\tvalue: {:5.2f}cm'.format(
event.description, message.value))
elif event is Event.INFRARED_CNTR:
self._log.info(Fore.BLUE +
'INFRARED_CNTR RECEIVED;\tvalue: {:5.2f}cm'.format(
message.value))
elif event is Event.INFRARED_STBD:
self._log.info(Fore.GREEN + Style.BRIGHT +
'event: {};\tvalue: {:5.2f}cm'.format(
event.description, message.value))
elif event is Event.INFRARED_STBD_SIDE:
self._log.info(Fore.GREEN + Style.BRIGHT +
'event: {};\tvalue: {:5.2f}cm'.format(
event.description, message.value))
else:
pass
pass
