def __init__(self, motors, ifs, level):
self._log = Logger("avoid", level)
self._motors = motors
self._ifs = ifs
self._rgbmatrix = RgbMatrix(Level.INFO)
self._fast_speed = 99.0
self._slow_speed = 50.0
self._log.info('ready.')
class Behaviours():
'''
This class provides avoidance and some other relatively simple behaviours.
'''
def __init__(self, motors, ifs, level):
self._log = Logger("avoid", level)
self._motors = motors
self._ifs = ifs
self._rgbmatrix = RgbMatrix(Level.INFO)
self._fast_speed = 99.0
self._slow_speed = 50.0
self._log.info('ready.')
# ..........................................................................
def back_up(self, duration_sec):
'''
Back up for the specified duration so we don't hang our bumper.
'''
self._motors.astern(self._fast_speed)
time.sleep(duration_sec)
# avoid ....................................................................
def avoid(self, orientation):
'''
Avoids differently depending on orientation.
'''
self._log.info(Fore.CYAN + 'avoid {}.'.format(orientation.name))
if orientation is Orientation.CNTR:
orientation = Orientation.PORT if random.choice(
(True, False)) else Orientation.STBD
self._log.info(Fore.YELLOW +
'randomly avoiding to {}.'.format(orientation.name))
self._ifs.suppress(True)
self._motors.halt()
self.back_up(0.5)
if orientation is Orientation.PORT:
self._motors.spin_port(self._fast_speed)
elif orientation is Orientation.STBD:
self._motors.spin_starboard(self._fast_speed)
else:
raise Exception('unexpected center orientation.')
time.sleep(2.0)
self._motors.brake()
self._ifs.suppress(False)
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()
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 configure_rgbmatrix(self):
self._log.warning('configure rgbmatrix...')
from lib.rgbmatrix import RgbMatrix, DisplayType
self._log.debug('configuring random blinky display...')
# self._ros._rgbmatrix = RgbMatrix(Level.INFO)
self._ros._rgbmatrix = RgbMatrix(Level.INFO)
self._ros.add_feature(
self._ros._rgbmatrix) # FIXME this is added twice
def main(argv):
_rgbmatrix = RgbMatrix(Level.INFO)
_rgbmatrix.set_display_type(DisplayType.SOLID)
_rgbmatrix.enable()
try:
# read YAML configuration
_loader = ConfigLoader(Level.INFO)
filename = 'config.yaml'
_config = _loader.configure(filename)
_imu = IMU(_config, Level.INFO)
while True:
# x, y, z = _imu.read_magnetometer()
mag = _imu.read_magnetometer()
# ax, ay, az, gx, gy, gz = _imu.read_accelerometer_gyro()
acc = _imu.read_accelerometer_gyro()
_heading = Convert.heading_from_magnetometer(
_imu._amin, _imu._amax, mag)
_cardinal = Cardinal.get_heading_from_degrees(_heading)
_color = Cardinal.get_color_for_direction(_cardinal)
_rgbmatrix.set_color(_color)
print(Fore.CYAN + 'Accel: {:05.2f} {:05.2f} {:05.2f} '.format(acc[0], acc[1], acc[2]) \
+ Fore.YELLOW + '\tGyro: {:05.2f} {:05.2f} {:05.2f} '.format(acc[3], acc[4], acc[5]) \
+ Fore.MAGENTA + '\tMag: {:05.2f} {:05.2f} {:05.2f} '.format(mag[0], mag[1], mag[2]) \
+ Fore.GREEN + '\tHeading: {:d}°;\tcardinal: {}'.format(_heading, _cardinal.display) + Style.RESET_ALL)
time.sleep(0.25)
except KeyboardInterrupt:
print(Fore.CYAN + Style.BRIGHT + 'caught Ctrl-C; exiting...')
except Exception:
print(Fore.RED + Style.BRIGHT +
'error starting imu: {}'.format(traceback.format_exc()) +
Style.RESET_ALL)
finally:
if _rgbmatrix:
_rgbmatrix.set_color(Color.BLACK)
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 main():
print(
Fore.GREEN +
'main begin. ......................................................... '
+ Style.RESET_ALL)
print('rgbmatrix_test :' + Fore.CYAN + Style.BRIGHT +
' INFO : starting test...' + Style.RESET_ALL)
_rgbmatrix = None
try:
_rgbmatrix = RgbMatrix(Level.WARN)
_rgbmatrix.set_display_type(DisplayType.CPU)
_rgbmatrix.enable()
while True:
time.sleep(1.0)
except KeyboardInterrupt:
print('rgbmatrix_test :' + Fore.YELLOW +
' INFO : Ctrl-C caught: exiting...' + Style.RESET_ALL)
finally:
if _rgbmatrix is not None:
_rgbmatrix.set_color(Color.BLACK)
_rgbmatrix.disable()
_rgbmatrix.close()
def main():
print(Fore.GREEN + 'main begin. ......................................................... ' + Style.RESET_ALL)
print('rgb_sworl_test :' + Fore.CYAN + Style.BRIGHT + ' INFO : starting test...' + Style.RESET_ALL)
try:
_rgbmatrix = RgbMatrix(Level.INFO)
display_type = DisplayType.SWORL
print('rgb_sworl_test :' + Fore.CYAN + Style.BRIGHT + ' INFO : displaying {}...'.format(display_type.name) + Style.RESET_ALL)
_rgbmatrix.set_display_type(display_type)
_rgbmatrix.enable()
time.sleep(2.0)
_rgbmatrix.disable()
count = 0
while not _rgbmatrix.is_disabled():
count += 1
time.sleep(1.0)
if count > 5:
print('rgb_sworl_test :' + Fore.RED + Style.BRIGHT + \
' INFO : timeout waiting to disable rgbmatrix thread for {}.'.format(display_type.name) + Style.RESET_ALL)
sys.exit(1)
_rgbmatrix.set_color(Color.BLACK)
print('rgb_sworl_test :' + Fore.CYAN + Style.BRIGHT + ' INFO : {} complete.'.format(display_type.name) + Style.RESET_ALL)
time.sleep(1.0)
print('rgb_sworl_test :' + Fore.CYAN + Style.BRIGHT + ' INFO : test complete.' + Style.RESET_ALL)
except KeyboardInterrupt:
print('rgb_sworl_test :' + Fore.YELLOW + ' INFO : Ctrl-C caught: exiting...' + Style.RESET_ALL)
finally:
_rgbmatrix.disable()
_rgbmatrix.close()
def test_imu():
_log = Logger("imu-test", Level.INFO)
_i2c_scanner = I2CScanner(Level.WARN)
_addresses = _i2c_scanner.get_int_addresses()
_rgbmatrix = RgbMatrix(Level.INFO) if (0x74 in _addresses) else None
if _rgbmatrix:
_rgbmatrix.set_display_type(DisplayType.SOLID)
_rgbmatrix.enable()
try:
_loader = ConfigLoader(Level.INFO)
filename = 'config.yaml'
_config = _loader.configure(filename)
_cardinal = Cardinal.get_heading_from_degrees(0)
_imu = IMU(_config, Level.INFO)
while True:
_heading = _imu.read_heading()[0]
if _rgbmatrix:
_cardinal = Cardinal.get_heading_from_degrees(_heading)
_color = Cardinal.get_color_for_direction(_cardinal)
_rgbmatrix.set_color(_color)
_log.info(Fore.YELLOW + 'heading: {:05.2f}°;\tcardinal: {}'.format(
_heading, _cardinal.display) + Style.RESET_ALL)
time.sleep(2.0)
except KeyboardInterrupt:
_log.info('Ctrl-C caught: exiting...')
except Exception as e:
_log.error('error closing: {}\n{}'.format(e, traceback.format_exc()))
finally:
if _rgbmatrix:
_rgbmatrix.set_color(Color.BLACK)
_rgbmatrix.disable()
_log.info('complete.')
def test_moth():
'''
Test the basic functionality of the Moth sensor's various outputs.
The test is entirely visual, i.e., something to be looked at, tested
with a bright light source.
'''
_log = Logger("test", Level.INFO)
# read YAML configuration
_loader = ConfigLoader(Level.INFO)
filename = 'config.yaml'
_config = _loader.configure(filename)
_moth = Moth(_config, None, Level.INFO)
_rgbmatrix = RgbMatrix(Level.INFO)
_rgbmatrix.set_display_type(DisplayType.SOLID)
# for i in range(50):
while True:
# orientation and bias .............................
_orientation = _moth.get_orientation()
_bias = _moth.get_bias()
_log.debug('bias: {}; orientation: {}'.format(type(_bias), _orientation))
if _orientation is Orientation.PORT:
_rgbmatrix.show_color(Color.BLACK, Orientation.STBD)
_rgbmatrix.show_color(Color.RED, Orientation.PORT)
_log.info(Fore.RED + Style.BRIGHT + '{}\t {:<6.3f}'.format(_orientation.name, _bias[0]))
elif _orientation is Orientation.STBD:
_rgbmatrix.show_color(Color.BLACK, Orientation.PORT)
_rgbmatrix.show_color(Color.GREEN, Orientation.STBD)
_log.info(Fore.GREEN + Style.BRIGHT + '{}\t {:<6.3f}'.format(_orientation.name, _bias[1]))
else:
_rgbmatrix.show_color(Color.YELLOW, Orientation.PORT)
_rgbmatrix.show_color(Color.YELLOW, Orientation.STBD)
_log.info(Fore.YELLOW + '{}\t {:6.3f}|{:6.3f}'.format(_orientation.name, _bias[0], _bias[1]))
# int values .......................................
_int_values = _moth.get_int_values()
_log.info(Fore.RED + '{:d}\t'.format(_int_values[0]) + Fore.GREEN + '{:d}\t'.format(_int_values[1]) )
# float values .....................................
_values = _moth.get_values()
_log.info(Fore.RED + '{:<6.3f}\t'.format(_values[0]) + Fore.GREEN + '{:<6.3f}\t'.format(_values[1]) )
# time.sleep(1.0)
time.sleep(0.1)
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 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
def test_motors():
global _port_motor, _stbd_motor, action_A, action_B
# read YAML configuration
_loader = ConfigLoader(Level.INFO)
filename = 'config.yaml'
_config = _loader.configure(filename)
_log.info('creating message factory...')
_message_factory = MessageFactory(Level.INFO)
_log.info('creating message bus...')
_message_bus = MessageBus(Level.WARN)
_log.info('creating clock...')
_clock = Clock(_config, _message_bus, _message_factory, Level.WARN)
_motor_configurer = MotorConfigurer(_config, _clock, Level.INFO)
_motors = _motor_configurer.get_motors()
_motors.enable()
_i2c_scanner = I2CScanner(Level.WARN)
if ENABLE_MOTH:
if _i2c_scanner.has_address([0x18]):
_rgbmatrix = RgbMatrix(Level.WARN)
# _rgbmatrix.set_display_type(DisplayType.SOLID)
_moth = Moth(_config, None, Level.WARN)
else:
_log.warning('cannot enable moth: no IO Expander found.')
_moth = None
_pin_A = 16
_button_16 = Button(_pin_A, callback_method_A, Level.INFO)
_log.info(
Style.BRIGHT +
'press button A (connected to pin {:d}) to toggle or initiate action.'.
format(_pin_A))
_pin_B = 24
_button_24 = Button(_pin_B, callback_method_B, Level.INFO)
_log.info(Style.BRIGHT +
'press button B connected to pin {:d}) to exit.'.format(_pin_B))
_log.info('starting motors...')
_port_motor = _motors.get_motor(Orientation.PORT)
_stbd_motor = _motors.get_motor(Orientation.STBD)
if ENABLE_PORT:
_port_motor.enable()
if ENABLE_STBD:
_stbd_motor.enable()
_rot_ctrl = RotaryControl(_config, 0, 50, 2, Level.WARN) # min, max, step
_rate = Rate(5)
_step_limit = 2312
_velocity_stbd = 0.0
_velocity_port = 0.0
_start_time = dt.now()
_moth_port = 1.0
_moth_stbd = 1.0
_moth_offset = 0.6
_moth_bias = [0.0, 0.0]
_moth_fudge = 0.7
try:
action_A = True # if not using buttons at all set to True
action_B = True
while INFINITE or action_B or (_port_motor.steps < _step_limit
and _stbd_motor.steps < _step_limit):
if action_A:
action_A = False # trigger once
while action_B:
if USE_ROTARY_CONTROL:
_target_velocity = _rot_ctrl.read()
else:
_target_velocity = 30.0
# _power = _target_velocity / 100.0
_power = Motor.velocity_to_power(_target_velocity)
if ENABLE_MOTH and _moth:
_moth_bias = _moth.get_bias()
# _log.info(Fore.WHITE + Style.BRIGHT + 'port: {:5.2f}; stbd: {:5.2f}'.format(_moth_port, _moth_stbd))
# _rgbmatrix.show_hue(_moth_hue, Orientation.BOTH)
_orientation = _moth.get_orientation()
if _orientation is Orientation.PORT:
_moth_port = _moth_bias[0] * _moth_fudge
_moth_stbd = 1.0
_rgbmatrix.show_color(Color.BLACK,
Orientation.STBD)
_rgbmatrix.show_color(Color.RED, Orientation.PORT)
elif _orientation is Orientation.STBD:
_moth_port = 1.0
_moth_stbd = _moth_bias[1] * _moth_fudge
_rgbmatrix.show_color(Color.BLACK,
Orientation.PORT)
_rgbmatrix.show_color(Color.GREEN,
Orientation.STBD)
else:
_moth_port = 1.0
_moth_stbd = 1.0
_rgbmatrix.show_color(Color.BLACK,
Orientation.PORT)
_rgbmatrix.show_color(Color.BLACK,
Orientation.STBD)
if ENABLE_STBD:
_stbd_motor.set_motor_power(_stbd_rotate * _power *
_moth_stbd)
_velocity_stbd = _stbd_motor.velocity
if ENABLE_PORT:
_port_motor.set_motor_power(_port_rotate * _power *
_moth_port)
_velocity_port = _port_motor.velocity
_log.info(Fore.YELLOW + 'power: {:5.2f}; bias: '.format(_power) \
+ Fore.RED + ' {:5.2f} '.format(_moth_bias[0]) + Fore.GREEN + '{:5.2f};'.format(_moth_bias[1]) \
+ Fore.BLACK + ' target velocity: {:5.2f};'.format(_target_velocity) \
+ Fore.CYAN + ' velocity: ' \
+ Fore.RED + ' {:5.2f} '.format(_velocity_port) + Fore.GREEN + ' {:5.2f}'.format(_velocity_stbd))
# _log.info(Fore.RED + 'power {:5.2f}/{:5.2f}; {:>4d} steps; \t'.format(_stbd_motor.get_current_power_level(), _power, _port_motor.steps) \
# + Fore.GREEN + 'power {:5.2f}/{:5.2f}; {:>4d} steps.'.format(_port_motor.get_current_power_level(), _power, _stbd_motor.steps))
_rate.wait()
action_B = True # reentry into B loop, waiting for A
_log.info('waiting for A button press...')
time.sleep(1.0)
# end wait loop ....................................................
if ENABLE_PORT:
_log.info('port motor: {:d} steps.'.format(_port_motor.steps))
if ENABLE_STBD:
_log.info('stbd motor: {:d} steps.'.format(_stbd_motor.steps))
except KeyboardInterrupt:
_log.info('Ctrl-C caught; exiting...')
except Exception as e:
_log.error('error: {}'.format(e))
finally:
close_motors(_log)
_elapsed_ms = round((dt.now() - _start_time).total_seconds() * 1000.0)
_log.info(Fore.YELLOW + 'complete: elapsed: {:d}ms'.format(_elapsed_ms))
def _configure(self):
'''
Configures ROS based on both KR01 standard hardware as well as
optional features, the latter based on devices showing up (by
address) on the I²C bus. Optional devices are only enabled at
startup time via registration of their feature availability.
'''
scanner = I2CScanner(Level.WARN)
self._addresses = scanner.getAddresses()
hexAddresses = scanner.getHexAddresses()
self._addrDict = dict(
list(map(lambda x, y: (x, y), self._addresses, hexAddresses)))
for i in range(len(self._addresses)):
self._log.debug(
Fore.BLACK + Style.DIM +
'found device at address: {}'.format(hexAddresses[i]) +
Style.RESET_ALL)
self._log.info('configure default features...')
# standard devices ...........................................
self._log.info('configuring integrated front sensors...')
self._ifs = IntegratedFrontSensor(self._config, self._queue,
Level.INFO)
self._log.info('configure ht0740 switch...')
self._switch = Switch(Level.INFO)
# since we're using the HT0740 Switch, turn it on to enable sensors that rely upon its power
# self._switch.enable()
self._switch.on()
self._log.info('configuring button...')
self._button = Button(self._config, self.get_message_queue(),
self._mutex)
self._log.info('configure battery check...')
_battery_check = BatteryCheck(self._config, self.get_message_queue(),
Level.INFO)
self.add_feature(_battery_check)
self._log.info('configure CPU temperature check...')
_temperature_check = Temperature(self._config, self._queue, Level.INFO)
self.add_feature(_temperature_check)
ultraborg_available = (0x36 in self._addresses)
if ultraborg_available:
self._log.debug(Fore.CYAN + Style.BRIGHT +
'-- UltraBorg available at 0x36.' +
Style.RESET_ALL)
else:
self._log.debug(Fore.RED + Style.BRIGHT +
'-- no UltraBorg available at 0x36.' +
Style.RESET_ALL)
self._set_feature_available('ultraborg', ultraborg_available)
thunderborg_available = (0x15 in self._addresses)
if thunderborg_available: # then configure motors
self._log.debug(Fore.CYAN + Style.BRIGHT +
'-- ThunderBorg available at 0x15' +
Style.RESET_ALL)
_motor_configurer = MotorConfigurer(self, self._config, Level.INFO)
self._motors = _motor_configurer.configure()
else:
self._log.debug(Fore.RED + Style.BRIGHT +
'-- no ThunderBorg available at 0x15.' +
Style.RESET_ALL)
self._set_feature_available('thunderborg', thunderborg_available)
# optional devices ...........................................
# the 5x5 RGB Matrix is at 0x74 for port, 0x77 for starboard
rgbmatrix5x5_stbd_available = (0x74 in self._addresses)
if rgbmatrix5x5_stbd_available:
self._log.debug(Fore.CYAN + Style.BRIGHT +
'-- RGB Matrix available at 0x74.' +
Style.RESET_ALL)
else:
self._log.debug(Fore.RED + Style.BRIGHT +
'-- no RGB Matrix available at 0x74.' +
Style.RESET_ALL)
self._set_feature_available('rgbmatrix5x5_stbd',
rgbmatrix5x5_stbd_available)
rgbmatrix5x5_port_available = (0x77 in self._addresses)
if rgbmatrix5x5_port_available:
self._log.debug(Fore.CYAN + Style.BRIGHT +
'-- RGB Matrix available at 0x77.' +
Style.RESET_ALL)
else:
self._log.debug(Fore.RED + Style.BRIGHT +
'-- no RGB Matrix available at 0x77.' +
Style.RESET_ALL)
self._set_feature_available('rgbmatrix5x5_port',
rgbmatrix5x5_port_available)
if rgbmatrix5x5_stbd_available or rgbmatrix5x5_port_available:
self._log.info('configure rgbmatrix...')
self._rgbmatrix = RgbMatrix(Level.INFO)
self.add_feature(self._rgbmatrix) # FIXME this is added twice
# ............................................
# the 11x7 LED matrix is at 0x75 for starboard, 0x77 for port. The latter
# conflicts with the RGB LED matrix, so both cannot be used simultaneously.
matrix11x7_stbd_available = (0x75 in self._addresses)
if matrix11x7_stbd_available:
self._log.debug(Fore.CYAN + Style.BRIGHT +
'-- 11x7 Matrix LEDs available at 0x75.' +
Style.RESET_ALL)
else:
self._log.debug(Fore.RED + Style.BRIGHT +
'-- no 11x7 Matrix LEDs available at 0x75.' +
Style.RESET_ALL)
self._set_feature_available('matrix11x7_stbd',
matrix11x7_stbd_available)
# device availability ........................................
bno055_available = (0x28 in self._addresses)
if bno055_available:
self._log.info('configuring BNO055 9DoF sensor...')
self._bno055 = BNO055(self._config, self.get_message_queue(),
Level.INFO)
else:
self._log.debug(Fore.RED + Style.BRIGHT +
'no BNO055 orientation sensor available at 0x28.' +
Style.RESET_ALL)
self._set_feature_available('bno055', bno055_available)
# NOTE: the default address for the ICM20948 is 0x68, but this conflicts with the PiJuice
icm20948_available = (0x69 in self._addresses)
if icm20948_available:
self._log.debug(Fore.CYAN + Style.BRIGHT +
'ICM20948 available at 0x69.' + Style.RESET_ALL)
else:
self._log.debug(Fore.RED + Style.BRIGHT +
'no ICM20948 available at 0x69.' + Style.RESET_ALL)
self._set_feature_available('icm20948', icm20948_available)
lsm303d_available = (0x1D in self._addresses)
if lsm303d_available:
self._log.debug(Fore.CYAN + Style.BRIGHT +
'LSM303D available at 0x1D.' + Style.RESET_ALL)
else:
self._log.debug(Fore.RED + Style.BRIGHT +
'no LSM303D available at 0x1D.' + Style.RESET_ALL)
self._set_feature_available('lsm303d', lsm303d_available)
vl53l1x_available = (0x29 in self._addresses)
if vl53l1x_available:
self._log.debug(Fore.CYAN + Style.BRIGHT +
'VL53L1X available at 0x29.' + Style.RESET_ALL)
else:
self._log.debug(Fore.RED + Style.BRIGHT +
'no VL53L1X available at 0x29.' + Style.RESET_ALL)
self._set_feature_available('vl53l1x', vl53l1x_available)
as7262_available = (0x49 in self._addresses)
if as7262_available:
self._log.debug(Fore.CYAN + Style.BRIGHT +
'-- AS7262 Spectrometer available at 0x49.' +
Style.RESET_ALL)
else:
self._log.debug(Fore.RED + Style.BRIGHT +
'-- no AS7262 Spectrometer available at 0x49.' +
Style.RESET_ALL)
self._set_feature_available('as7262', as7262_available)
pijuice_available = (0x68 in self._addresses)
if pijuice_available:
self._log.debug(Fore.CYAN + Style.BRIGHT +
'PiJuice hat available at 0x68.' + Style.RESET_ALL)
else:
self._log.debug(Fore.RED + Style.BRIGHT +
'no PiJuice hat available at 0x68.' +
Style.RESET_ALL)
self._set_feature_available('pijuice', pijuice_available)
self._log.info('configured.')