def main():
_log = Logger("servo-test", Level.INFO)
try:
_log.info('starting ultrasonic scan...')
_loader = ConfigLoader(Level.INFO)
filename = 'config.yaml'
_config = _loader.configure(filename)
_number = 1
_servo = Servo(_config, _number, Level.INFO)
# _servo.enable()
# _thread = threading.Thread(target=_servo.sweep, args=[])
# _thread.start()
for i in numpy.arange(-90.0, 90.0, 5.0):
_servo.set_position(i)
_log.info('position: {}'.format(i))
time.sleep(0.5)
_servo.set_position(0.0)
_servo.disable()
_thread.join()
_log.info('servo test complete.')
except Exception:
_log.info(traceback.format_exc())
sys.exit(1)
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 WallFollower():
def __init__(self, config, level):
self._log = Logger('follower', Level.INFO)
self._config = config
if self._config:
self._log.info('configuration provided.')
_config = self._config['ros'].get('wall_follower')
self._port_angle = _config.get('port_angle')
self._stbd_angle = _config.get('starboard_angle')
_range_value = _config.get('tof_range')
_range = Range.from_str(_range_value)
_servo_number = _config.get('servo_number')
else:
self._log.warning('no configuration provided.')
self._port_angle = -90.0
self._stbd_angle = 90.0
_range = Range.PERFORMANCE
_servo_number = 2
# _range = Range.LONG
# _range = Range.MEDIUM
# _range = Range.SHORT
self._log.info(
'wall follower port angle: {:>5.2f}°; starboard angle: {:>5.2f}°'.
format(self._port_angle, self._stbd_angle))
self._servo = Servo(self._config, _servo_number, level)
self._tof = TimeOfFlight(_range, Level.WARN)
self._max_retries = 10
self._enabled = False
self._closed = False
self._log.info('ready.')
# ..........................................................................
def scan(self):
mm = 0.0
wait_count = 0
while mm <= 0.1 and wait_count < self._max_retries:
mm = self._tof.read_distance()
time.sleep(0.0025)
wait_count += 1
self._log.info('distance: {:>5.0f}mm'.format(mm))
return mm
# ..........................................................................
def set_position(self, orientation):
if not self._enabled:
self._log.warning('cannot scan: disabled.')
return
if orientation == Orientation.PORT:
self._servo.set_position(self._port_angle)
elif orientation == Orientation.STBD:
self._servo.set_position(self._stbd_angle)
else:
raise ValueError(
'only port or starboard acceptable for wall follower orientation'
)
self._log.info('wall follower position set to {}.'.format(orientation))
# ..........................................................................
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.set_position(0.0)
self._servo.disable()
self._tof.disable()
# ..........................................................................
def close(self):
self.disable()
self._servo.close()
self._closed = True
class Lidar():
'''
A combination of a VL53L1X laser distance sensor and a micro servo as a LIDAR.
The zero position is straight forward, with negative (minimum servo position)
sweeping to port, positive (maximum servo position) to starboard.
The 'reverse_movement' parameter is used in case the servo movement is backwards.
'''
def __init__(self, config, level):
self._log = Logger('lidar', Level.INFO)
self._config = config
if self._config:
self._log.info('configuration provided.')
_config = self._config['ros'].get('lidar')
self._reverse_movement = _config.get('reverse_movement')
self._double_sweep = _config.get('double_sweep')
self._min_angle = _config.get('min_angle')
self._max_angle = _config.get('max_angle')
self._degree_step = _config.get('degree_step')
self._step_delay_sec = _config.get('step_delay_sec')
_range_value = _config.get('tof_range')
_range = Range.from_str(_range_value)
_servo_number = _config.get('servo_number')
else:
self._log.warning('no configuration provided.')
self._log.info('play sound disabled.')
self._reverse_movement = False
self._double_sweep = False
self._min_angle = -60.0
self._max_angle = 60.0
self._degree_step = 3.0
self._step_delay_sec = 0.01
_range = Range.PERFORMANCE
# _range = Range.LONG
# _range = Range.MEDIUM
# _range = Range.SHORT
_servo_number = -1
self._log.info(
'scan range of {} from {:>4.1f}° to {:>4.1f}° with step of {:>4.1f}°'
.format(_range, self._min_angle, self._max_angle,
self._degree_step))
if _servo_number > 0:
self._servo = Servo(self._config, _servo_number, level)
else:
self._servo = None
_range = Range.PERFORMANCE
# _range = Range.LONG
# _range = Range.MEDIUM
# _range = Range.SHORT
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 _set_servo_position(self, angle):
if self._servo:
if self._reverse_movement:
self._servo.set_position(-1.0 * angle)
else:
self._servo.set_position(angle)
# ..........................................................................
def _get_servo_position(self, default_angle):
if self._servo:
if self._reverse_movement:
return -1.0 * self._servo.get_position(-1.0 * default_angle)
else:
return self._servo.get_position(default_angle)
# ..........................................................................
def scan(self):
if not self._enabled:
self._log.warning('cannot scan: disabled.')
return None
try:
if self._servo:
start = time.time()
_min_mm = 9999.0
_angle_at_min = 0.0
_max_mm = 0
_angle_at_max = 0.0
self._set_servo_position(self._min_angle)
time.sleep(0.3)
# wait_count = 0
# while ( not self._in_range(self._get_servo_position(self._min_angle), self._min_angle) ) and ( wait_count < 20 ):
# wait_count += 1
# self._set_servo_position(self._min_angle)
# self._log.info(Fore.MAGENTA + Style.BRIGHT + 'waiting for match at degrees: {:>5.2f}°: waited: {:d}'.format(self._get_servo_position(-1), wait_count))
# time.sleep(1.0)
# time.sleep(0.05)
# self._log.info(Fore.YELLOW + Style.BRIGHT + 'starting scan from degrees: {:>5.2f}°: waited: {:d}'.format(self._get_servo_position(-1), wait_count))
# sweep from minimum to maximum ........................................................
self._log.info('sweep fore...')
# self._log.info('sweep min to max...')
for degrees in numpy.arange(self._min_angle,
self._max_angle + 0.01,
self._degree_step):
self._set_servo_position(degrees)
wait_count = 0
while not self._in_range(self._get_servo_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._get_servo_position(degrees), degrees, wait_count))
mm = self._tof.read_distance()
self._log.info('distance at {:>5.2f}°: \t{}mm'.format(
degrees, mm))
# 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._step_delay_sec)
if self._double_sweep:
self._log.info('sweep back...')
# self._log.info('sweep max to min...')
# sweep from maximum to minimum ........................................................
for degrees in numpy.arange(self._max_angle,
self._min_angle + 0.01,
-1.0 * self._degree_step):
self._set_servo_position(degrees)
wait_count = 0
while not self._in_range(
self._get_servo_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._get_servo_position(degrees), degrees, wait_count))
mm = self._tof.read_distance()
self._log.info('distance at {:>5.2f}°: \t{}mm'.format(
degrees, mm))
# 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._step_delay_sec)
time.sleep(0.1)
# self._log.info('complete.')
elapsed = time.time() - start
self._log.info('min. distance at {:>5.2f}°:\t{}mm'.format(
_angle_at_min, _min_mm))
self._log.info('max. distance at {:>5.2f}°:\t{}mm'.format(
_angle_at_max, _max_mm))
self._log.info(
'scan complete: {:>5.2f}sec elapsed.'.format(elapsed))
self._set_servo_position(0.0)
# self._servo.set_position(_angle_at_max)
# self.close()
return [_angle_at_min, _min_mm, _angle_at_max, _max_mm]
else:
_degrees = 0
_start = dt.now()
mm = self._tof.read_distance()
_elapsed_ms = (dt.now() - _start).total_seconds() * 1000.0
self._log.info('distance at {:>5.2f}°: \t{}mm'.format(
_degrees, mm))
self._log.info(
'scan complete: {:>5.2f}ms elapsed.'.format(_elapsed_ms))
return [-1.0, mm, -1.0, mm]
except KeyboardInterrupt:
self._log.info('caught Ctrl-C.')
self.close()
self._log.info('interrupted: incomplete.')
# ..........................................................................
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
if self._servo:
self._servo.disable()
self._tof.disable()
# ..........................................................................
def close(self):
self.disable()
if self._servo:
self._servo.close()
self._closed = True
class CatScan():
'''
Uses an infrared PIR sensor to scan for cats. This involves raising a
servo arm holding the sensor.
'''
def __init__(self, config, queue, level):
self._log = Logger('cat-scan', Level.INFO)
self._config = config
self._queue = queue
if self._config:
self._log.info('configuration provided.')
_config = self._config['ros'].get('cat_scan')
self._active_angle = _config.get('active_angle')
self._inactive_angle = _config.get('inactive_angle')
_pir_pin = _config.get('pir_pin')
_servo_number = _config.get('servo_number')
else:
self._log.warning('no configuration provided.')
self._active_angle = -90.0
self._inactive_angle = 90.0
_pir_pin = 15
_servo_number = 3
_threshold_value = 0.1 # the value above which the device will be considered “on”
self._log.info(
'cat scan active angle: {:>5.2f}°; inactive angle: {:>5.2f}°'.
format(self._active_angle, self._inactive_angle))
self._servo = Servo(self._config, _servo_number, level)
# set up pin where PIR is connected
self._sensor = MotionSensor(_pir_pin,
threshold=_threshold_value,
pull_up=False)
self._sensor.when_motion = self._activated
self._sensor.when_no_motion = None #self._deactivated
self._scan_enabled = False
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._rgbmatrix = RgbMatrix(Level.INFO)
self._rgbmatrix.set_display_type(DisplayType.SCAN)
self._log.info('ready.')
# ..........................................................................
def _warning_display(self):
self._rgbmatrix.disable()
self._rgbmatrix.set_display_type(DisplayType.RANDOM)
self._rgbmatrix.enable()
# ..........................................................................
def _activated(self):
'''
The default function called when the sensor is activated.
'''
if self._enabled and self._scan_enabled:
self._log.info('cat sensed.')
self._log.info(Fore.RED + Style.BRIGHT + 'detected CAT!')
self._queue.add(Message(Event.CAT_SCAN))
self.set_mode(False)
self._warning_display()
else:
self._log.info('[DISABLED] activated catscan.')
# ..........................................................................
def _deactivated(self):
'''
The default function called when the sensor is deactivated.
'''
if self._enabled:
self._log.info('deactivated catscan.')
else:
self._log.debug('[DISABLED] deactivated catscan.')
# ..........................................................................
def is_waiting(self):
'''
Returns true if the scan is in progress.
'''
return self._scan_enabled
# ..........................................................................
def set_mode(self, active):
'''
Sets the mode to active or inactive. This raises or lowers the servo arm.
'''
if not self._enabled:
self._log.warning('cannot scan: disabled.')
return
_current_angle = self._servo.get_position(-999.0)
self._log.critical('current angle: {:>4.1f}°'.format(_current_angle))
if active:
if _current_angle != -999.0 and _current_angle is not self._active_angle:
with _mutex:
# for degrees in numpy.arange(self._inactive_angle, self._active_angle + 0.01, -1.0 * self._arm_movement_degree_step):
for degrees in numpy.arange(
_current_angle, self._active_angle + 0.01,
-1.0 * self._arm_movement_degree_step):
self._log.debug(
'position set to: {:>4.1f}°'.format(degrees))
self._servo.set_position(degrees)
time.sleep(self._arm_up_delay)
self._log.info('position set to active.')
else:
self._log.warning('position already set to active.')
# wait until it settles
self._log.info('waiting for motion detector to settle...')
time.sleep(1.0)
while self._sensor.motion_detected:
self._log.debug('still waiting...')
time.sleep(0.1)
# time.sleep(5.0)
self._scan_enabled = True
self._rgbmatrix.enable()
else:
self._scan_enabled = False
self._rgbmatrix.disable()
if _current_angle != -999.0 and _current_angle is not self._inactive_angle:
with _mutex:
for degrees in numpy.arange(
self._active_angle, self._inactive_angle + 0.1,
self._arm_movement_degree_step):
self._log.debug(
'position set to: {:>4.1f}°'.format(degrees))
self._servo.set_position(degrees)
time.sleep(self._arm_down_delay)
self._log.info('position set to inactive.')
else:
self._log.warning('position already set to inactive.')
# ..........................................................................
def enable(self):
self._log.debug('enabling...')
if self._closed:
self._log.warning('cannot enable: closed.')
return
# self._tof.enable()
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._rgbmatrix.disable()
self._log.debug('disabling...')
self.set_mode(False)
self._servo.disable()
self._servo.close()
self._disabling = False
self._log.debug('disabled.')
# ..........................................................................
def close(self):
self.disable()
self._closed = True
