def __init__(self, pinBTN=27, pinLED=17, startVolume=70, imuAddress=0x6a, limit=350, changeDelta=1000):
GPIO.setmode(GPIO.BCM)
led = LED(pinLED)
self.ledStrip = LEDStrip()
self.audio = Audio(startVolume)
self.mixer = AudioMixer()
self.initAudioMixer()
self.btn = Button(pinBTN)
self.btn.setListener(self.audioBtnListener)
self.ledControl = LEDController(led)
self.ledControl.start()
self.changeDelta = changeDelta
self.currentMode = -1
# self.imuController = IMUController(imuAddress, limit=limit)
self.imuController = LSMController(imuAddress, limit=limit)
self.initImuController()
self.animDrive = TheaterChaseAnimation(LEDAnimation.COLOR_CYAN, 20, 50, 15, True)
self.animIdle = FadeCycleAnimation(RGBColor(0, 0, 0), RGBColor(0, 80, 0), 25, 20)
self.ledStrip = LEDStrip()
self.ledStrip.start()
self.speed = 0
self.lastLoop = self.getMillis()
def __init__(self, config, image_files):
self.box_surface = pygame.image.load(image_files[config.INPUT_BOX])
self.font = pygame.font.Font(config.FONT_MYRAID_PATH, 30)
self.input_str = []
self.confirm_button = Button(
"Confirm", config.CONFIRM_POS,
pygame.image.load(image_files[config.BUTTON_CONFIRM]), None)
self.return_button = Button(
"Return", config.RETURN_POS,
pygame.image.load(image_files[config.BUTTON_RETURN]), None)
class MyObj:
def __init__(self):
self.value = 0
self.button = Button(XY(5, 5), XY(10, 10), 'Button', self.method, XY)
def click(self, point):
self.button.click(point)
def method(self):
self.value += 1
def _configure_default_features(self):
'''
Import dependencies and configure default features.
'''
self._log.warning('configure default features...')
from lib.button import Button
self._log.info('configuring button...')
self._ros._button = Button(self._ros._config,
self._ros.get_message_queue(),
self._ros._mutex)
from lib.bumpers import Bumpers
self._log.info('configuring bumpers...')
self._ros._bumpers = Bumpers(self._ros._config,
self._ros.get_message_queue(), None,
Level.INFO)
from lib.infrareds import Infrareds
self._log.info('configuring infrared trio...')
self._ros._infrareds = Infrareds(self._ros._config,
self._ros.get_message_queue(),
Level.INFO)
from lib.player import Sound, Player
self._log.info('configuring player...')
self._ros._player = Player(Level.INFO)
self._log.warning('default features ready.')
def main():
global activated
activated = False
print('button_test :' + Fore.CYAN + ' INFO : starting test...' + Style.RESET_ALL)
_pin = 12
_button = Button(_pin, callback_method, Level.INFO)
try:
while not activated:
print(Fore.BLACK + 'waiting for button press on pin {:d}...'.format(_pin) + Style.RESET_ALL)
time.sleep(1.0)
except KeyboardInterrupt:
print(Fore.RED + "caught Ctrl-C." + Style.RESET_ALL)
finally:
print("closing button...")
_button.close()
def read(self, config, data, button_cell, nums_new, nums_new_over):
a, b = self.no % 3 * 3, self.no // 3 * 3
self.index = [(i, j) for i in range(b, b + 3) for j in range(a, a + 3)]
x, y = self.pos
k = 0
for l in range(9):
i, j = self.index[l]
bias_x, bias_y = self.cell_bias[l]
if data[i][j] == 0:
self.buttons.append(Button([i, j, l, k], (x + bias_x, y + bias_y),
button_cell[0], None, button_cell[1], None))
config.CELL_BUTTON_NUM += 1
k += 1
elif data[i][j] < 0:
self.buttons.append(Button([i, j, l, k], (x + bias_x, y + bias_y),
nums_new[-data[i][j] - 1], None,
nums_new_over[-data[i][j] - 1], None))
k += 1
def __init__(self, config, image_files):
self.image_files = image_files
self.difficulty = config.DIFFICULTY
self.cells = None
self.font = pygame.font.Font(config.FONT_MYRAID_PATH, 30)
self.game_buttons = [
Button(0, config.UNDO_POS,
pygame.image.load(image_files[config.BUTTON_UNDO]), None,
pygame.image.load(image_files[config.BUTTON_UNDO_INACTIVE]),
None),
Button(0, config.REDO_POS,
pygame.image.load(image_files[config.BUTTON_REDO]), None,
pygame.image.load(image_files[config.BUTTON_REDO_INACTIVE]),
None),
Button("Menu", config.GAME_MENU_POS,
pygame.image.load(image_files[config.BUTTON_GAME_MENU]),
None),
Button("Flush", config.FLUSH_POS,
pygame.image.load(image_files[config.BUTTON_FLUSH]), None),
]
self.congratulation = pygame.image.load(image_files[config.TEXT_CONG])
self.clear_buttons = [
Button("Restart", config.CLEAR_RE_POS,
pygame.image.load(image_files[config.BUTTON_CLEAR_RESTART]),
None),
Button("Menu", config.CLEAR_MENU_POS,
pygame.image.load(image_files[config.BUTTON_CLEAR_MENU]),
None)
]
def audioTest():
print("Start audio test")
audio = Audio(70)
def onPressed(mode):
vol = audio.getVolume()
if mode is Button.NORMAL_PRESS:
if vol == 100:
audio.setVolume(70)
else:
audio.volumeUp()
else:
if audio.isMute:
audio.unmute()
else:
audio.mute()
print("volume {}".format(audio.getVolume()))
btn = Button(pinBTN)
btn.setListener(onPressed)
try:
print(audio.getVolume())
print(audio.setVolume(20))
print(audio.getVolume())
while True:
btn.loop()
except KeyboardInterrupt:
print("Cancel audio test")
class InputBox:
def __init__(self, config, image_files):
self.box_surface = pygame.image.load(image_files[config.INPUT_BOX])
self.font = pygame.font.Font(config.FONT_MYRAID_PATH, 30)
self.input_str = []
self.confirm_button = Button(
"Confirm", config.CONFIRM_POS,
pygame.image.load(image_files[config.BUTTON_CONFIRM]), None)
self.return_button = Button(
"Return", config.RETURN_POS,
pygame.image.load(image_files[config.BUTTON_RETURN]), None)
def key_input(self, config, event, game):
key = event.key
if key is pygame.K_BACKSPACE:
self.input_str = self.input_str[0:-1]
elif key is pygame.K_RETURN:
self.random_game(config, game)
elif 32 <= key < 127:
self.input_str.append(chr(key))
def check_input_button_is_press(self, config, point, game):
if self.return_button.is_press(point):
config.GAME_STATE = config.GAME_STATE_PREV = config.GAME_STATE_MENU
elif self.confirm_button.is_press(point):
self.random_game(config, game)
def random_game(self, config, game):
seed_str = "".join(self.input_str)
if seed_str == "":
config.GAME_STATE = config.GAME_STATE_PREV = config.GAME_STATE_GAMING
seed = randint(0, 9223372036854775806)
game.init(config, seed=seed)
elif seed_str.isdigit():
config.GAME_STATE = config.GAME_STATE_PREV = config.GAME_STATE_GAMING
seed = int("".join(self.input_str)) % 9223372036854775806
game.init(config, seed=seed)
else:
config.GAME_STATE = config.GAME_STATE_PREV = config.GAME_STATE_MENU
root = tk.Tk()
root.withdraw()
msg.showerror('Error', 'Seed must be a number.')
def render(self, config, surface):
surface.blit(self.box_surface, config.INPUT_BOX_POS)
self.confirm_button.render(surface)
self.return_button.render(surface)
text = "".join(self.input_str[-32:])
height, width = self.font.size(text)
text_render = self.font.render(text, True, config.INPUT_COLOR)
text_x = config.INPUT_TEXT_POS[0] - height // 2
text_y = config.INPUT_TEXT_POS[1]
surface.blit(text_render, (text_x, text_y))
def __init__(self, config, image_files, no, pos):
self.index = None
self.no = no
self.pos = pos
self.cell_bias = config.CELL_BUTTON_BIAS
self.expand_bias = config.EXPAND_NUM_BIAS
self.buttons = []
self.expand_buttons = [Button(i, (0, 0),
pygame.image.load(image_files[config.EXPAND_NUM + i]), None,
pygame.image.load(image_files[config.EXPAND_NUM_OVER + i]), None)
for i in range(9)]
def ledStripTest():
print("Start led strip test")
ledStrip = LEDStrip()
animations = []
animations.append(ColorSetAnimation(LEDAnimation.COLOR_BLACK))
animations.append(TheaterChaseAnimation(LEDAnimation.COLOR_YELLOW, 100, 50, 15, True))
animations.append(TheaterChaseAnimation(LEDAnimation.COLOR_CYAN, 100, 50, 15))
animations.append(TheaterChaseAnimation(LEDAnimation.COLOR_BLUE, 500, 10, 15))
animations.append(TheaterChaseAnimation(LEDAnimation.COLOR_GREEN, 50, 10, 15))
animations.append(TheaterChaseAnimation(LEDAnimation.COLOR_RED, 50, 50, 15))
animations.append(TheaterChaseAnimation(LEDAnimation.COLOR_MAGENTA, 20, 50, 15))
animations.append(TheaterChaseAnimation(LEDAnimation.COLOR_CYAN, 20, 50, 15))
animations.append(TheaterChaseAnimation(LEDAnimation.COLOR_YELLOW, 20, 100, 15))
animations.append(FadeCycleAnimation(RGBColor(0, 0, 0), RGBColor(255, 255, 255), 25, int(1000 / 200)))
animations.append(FadeAnimation(RGBColor(255, 0, 0), RGBColor(0, 255, 0), 50, int(1000 / 50)))
animations.append(FadeAnimation(RGBColor(0, 255, 0), RGBColor(0, 0, 255), 50, int(1000 / 50)))
animations.append(RainbowAnimation())
animations.append(TheaterChaseAnimation(LEDAnimation.COLOR_WHITE))
animations.append(ColorWipeAnimation(LEDAnimation.COLOR_RED))
animations.append(RainbowCycleAnimation())
animations.append(TheaterChaseRainbowAnimation())
animations.append(ColorSetAnimation(LEDAnimation.COLOR_BLACK))
ledStrip.start()
def setNextAnimation():
global animIndex
animIndex += 1
if animIndex >= len(animations):
animIndex = 0
a = animations[animIndex]
ledStrip.setAnimation(a)
def onButtonClick(mode):
if mode is Button.NORMAL_PRESS:
setNextAnimation()
else:
a = ColorWipeAnimation(LEDAnimation.COLOR_BLACK)
ledStrip.setAnimation(a)
button = Button(pinBTN)
button.setListener(onButtonClick)
setNextAnimation()
try:
while True:
button.loop()
except KeyboardInterrupt:
print("Cancel button-LED test")
ledStrip.stop()
def buttonTest():
print("Start button test")
button = Button(pinBTN)
button.setListener(printResult)
try:
while True:
button.loop()
except KeyboardInterrupt:
print("Cancel button test")
def main():
print('button_test :' + Fore.CYAN + ' INFO : starting test...' +
Style.RESET_ALL)
print('button_test :' + Fore.BLUE + Style.BRIGHT +
' INFO : to pass test, press button for both ON and OFF states...' +
Style.RESET_ALL)
# read YAML configuration
_loader = ConfigLoader(Level.INFO)
filename = 'config.yaml'
_config = _loader.configure(filename)
_message_factory = MessageFactory(Level.INFO)
_queue = MessageQueue(_message_factory, Level.INFO)
_button = Button(_config, _queue, _message_factory, threading.Lock())
_button.start()
while not _button.get():
print('button_test : ' + Fore.RED + Style.BRIGHT + "button OFF" +
Fore.CYAN + Style.NORMAL + ' (press button to turn ON)' +
Style.RESET_ALL)
time.sleep(1)
while _button.get():
print('button_test : ' + Fore.GREEN + Style.BRIGHT +
"button ON" + Fore.CYAN + Style.NORMAL +
' (press button to turn OFF)' + Style.RESET_ALL)
time.sleep(1)
while not _button.get():
print('button_test : ' + Fore.RED + Style.BRIGHT + "button OFF" +
Fore.CYAN + Style.NORMAL + ' (press button to turn ON)' +
Style.RESET_ALL)
time.sleep(1)
print("button task close()")
_button.close()
def __init__(self, config, image_files):
self.image_files = image_files
self.box = InputBox(config, image_files)
self.menu_buttons = [
Button(config.DIFFICULTY_EASY, config.EASY_POS,
pygame.image.load(self.image_files[config.BUTTON_EASY]), None),
Button(config.DIFFICULTY_NORMAL, config.NORMAL_POS,
pygame.image.load(self.image_files[config.BUTTON_NORMAL]), None),
Button(config.DIFFICULTY_HARD, config.HARD_POS,
pygame.image.load(self.image_files[config.BUTTON_HARD]), None),
Button("Random", config.RANDOM_POS,
pygame.image.load(self.image_files[config.BUTTON_RANDOM]), None),
Button(True, config.LOAD_POS,
pygame.image.load(self.image_files[config.BUTTON_LOAD]), None,
pygame.image.load(self.image_files[config.BUTTON_LOAD_INACTIVE]), None),
]
self.utils_buttons = [
Button(r"Quit", config.QUIT_POS,
pygame.image.load(self.image_files[config.BUTTON_EXIT]), None),
Button(r"Setting", config.SETTING_POS,
pygame.image.load(self.image_files[config.BUTTON_SETTING]), None)
]
def audioMixerTest():
print("Start audio mixer test")
import os
from os import path
appPath = os.path.dirname(os.path.abspath(__file__))
audioFolder = path.join(appPath, "audio")
audio = Audio(70)
mixer = AudioMixer()
mixer.addSound("start", path.join(audioFolder, "h_start.wav"))
mixer.addSound("stop", path.join(audioFolder, "h_stop.wav"))
mixer.addSound("speedUp", path.join(audioFolder, "h_speedup.wav"))
mixer.addSound("idle", path.join(audioFolder, "h_idle.wav"))
mixer.addSound("drive", path.join(audioFolder, "h_drive.wav"))
def onPressed(mode):
if mode is Button.NORMAL_PRESS:
mixer.clearQueue()
mixer.playSound("speedUp", False, 300)
mixer.queue("drive", True)
else:
mixer.clearQueue()
mixer.playSound("stop", False, 300)
mixer.queue("idle", True)
btn = Button(pinBTN)
btn.setListener(onPressed)
mixer.playSound("start", False, 0)
mixer.queue("idle", True)
try:
while True:
btn.loop()
except KeyboardInterrupt:
print("Cancel audio mixer test")
mixer.stop()
def buttonLedTest():
print("Start Button-LED test")
led = LED(pinLED)
button = Button(pinBTN)
lc = LEDController(led)
lc.start()
def buttonListner(mode):
printResult(mode)
if mode is Button.NORMAL_PRESS:
lc.blink(3, 0.5, 0.5)
else:
lc.blink(1, 3, 0)
button.setListener(buttonListner)
try:
while True:
button.loop()
except KeyboardInterrupt:
print("Cancel button-LED test")
lc.stop()
def main():
global test_triggered, test_enabled
# initial states
test_triggered = False
test_enabled = True
_loader = ConfigLoader(_level)
_config = _loader.configure('config.yaml')
_pin_A = 12
_button_24 = 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))
try:
# .........................................
_motors = Motors(_config, None, Level.INFO)
_pot = Potentiometer(_config, Level.WARN)
_pot_min = 0.0000
_pot_max = 0.1500
_pot.set_output_limits(_pot_min, _pot_max)
_limit = 90.0
# _min = -127
_stbd_setpoint = 0.0
_port_setpoint = 0.0
_scaled_value = 0.0
_value = 0.0
_port_pid = None
_stbd_pid = None
# rotary controller
_min = 90
_max = 0
_step = 5
_rotary_ctrl = RotaryControl(_config, _min, _max, _step, Level.INFO)
# configure motors/PID controllers
_port_motor = _motors.get_motor(Orientation.PORT)
_port_pid = PIDController(_config, _port_motor, level=Level.INFO)
_port_pid.set_limit(_limit)
if ORIENTATION == Orientation.BOTH or ORIENTATION == Orientation.PORT:
_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.INFO)
_stbd_pid.set_limit(_limit)
_stbd_pid.enable()
# sys.exit(0)
_fixed_value = 15.0 # fixed setpoint value
DRY_RUN = False
USE_POTENTIOMETER = False
USE_ROTARY_ENCODER = False
ANYTHING_ELSE = False
try:
while test_enabled:
if test_triggered:
_log.info(Fore.BLUE + Style.BRIGHT + 'action A.')
test_triggered = False # trigger once
rotate_in_place(_rotary_ctrl, _port_pid, _stbd_pid)
test_enabled = False # quit after action
time.sleep(1.0)
# and we now return to our regularly scheduled broadcast...
_kp = _pot.get_scaled_value(True)
_stbd_pid._pid.kp = _kp
_log.info(Fore.YELLOW + 'waiting for button A...;' +
Style.BRIGHT +
' kp: {:8.5f}'.format(_stbd_pid._pid.kp))
time.sleep(1.0)
_log.info(Fore.YELLOW + 'end of test.')
except KeyboardInterrupt:
_log.info(Fore.CYAN + Style.BRIGHT + 'Ctrl-C caught: closing...')
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()
if _rotary_ctrl:
_log.info('resetting rotary encoder...')
_rotary_ctrl.reset()
time.sleep(1.0)
_log.info('complete.')
except Exception as e:
_log.info(Fore.RED + Style.BRIGHT +
'error in test: {}'.format(e, traceback.format_exc()))
finally:
_log.info(Fore.YELLOW + Style.BRIGHT + 'finally.')
def subject(mocker):
mock_method = mocker.stub(name='my_method')
return Button(XY(5, 5), XY(10, 10), 'MAH BUTTON', mock_method, XY)
def main():
global action_A, action_B
# initial states
action_A = False
action_B = True
_loader = ConfigLoader(_level)
_config = _loader.configure('config.yaml')
_pin_A = 12
_button_12 = 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))
try:
_log.info('creating message factory...')
_message_factory = MessageFactory(Level.INFO)
_log.info('creating message bus...')
_message_bus = MessageBus(Level.DEBUG)
_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()
_limit = 90.0
_stbd_setpoint = 0.0
_port_setpoint = 0.0
_scaled_value = 0.0
_value = 0.0
_port_pid_ctrl = None
_stbd_pid_ctrl = None
# rotary controller
_min_rot = 0
_max_rot = 30
_step_rot = 1
_rotary_ctrl = RotaryControl(_config, _min_rot, _max_rot, _step_rot,
Level.WARN)
# configure motors/PID controllers
if ORIENTATION == Orientation.BOTH or ORIENTATION == Orientation.PORT:
_port_motor = _motors.get_motor(Orientation.PORT)
_port_pid_ctrl = PIDController(_config,
_clock,
_port_motor,
level=Level.INFO)
_port_pid_ctrl.limit = _limit
_port_pid_ctrl.enable()
if ORIENTATION == Orientation.BOTH or ORIENTATION == Orientation.STBD:
_stbd_motor = _motors.get_motor(Orientation.STBD)
_stbd_pid_ctrl = PIDController(_config,
_clock,
_stbd_motor,
level=Level.INFO)
_stbd_pid_ctrl.limit = _limit
_stbd_pid_ctrl.enable()
_max_pot = 1.0
_min_pot = 0.0
_pot = Potentiometer(_config, Level.WARN)
_pot.set_output_limits(_min_pot, _max_pot)
_motors.enable()
try:
while action_B:
if action_A:
_log.info(Fore.BLUE + Style.BRIGHT + 'action A.')
action_A = False # trigger once
_stbd_setpoint = 0.0
_port_setpoint = 0.0
while action_B:
# set PID constant
_scaled_value = _pot.get_scaled_value(True)
# _log.info(Style.DIM + 'PID TUNER kp: {:8.5f} ================ '.format(_scaled_value))
if ORIENTATION == Orientation.BOTH or ORIENTATION == Orientation.PORT:
_port_pid_ctrl.kp = _scaled_value
_stbd_pid_ctrl.kp = _scaled_value
_value = _rotary_ctrl.read()
# _log.info(Fore.BLUE + Style.BRIGHT + 'rotary value: {:<5.2f}'.format(_value))
# if _value > -2.0 and _value < 2.0: # hysteresis?
# _value = 0.0
if ORIENTATION == Orientation.BOTH or ORIENTATION == Orientation.PORT:
_port_pid_ctrl.setpoint = ROTATE_VALUE * _value #* 100.0
_port_setpoint = _port_pid_ctrl.setpoint
if ORIENTATION == Orientation.BOTH or ORIENTATION == Orientation.STBD:
_stbd_pid_ctrl.setpoint = _value #* 100.0
_stbd_setpoint = _stbd_pid_ctrl.setpoint
# _log.info(Fore.MAGENTA + 'rotary value: {:<5.2f}; setpoint: '.format(_value) \
# + Fore.RED + ' {:5.2f};'.format(_port_setpoint) + Fore.GREEN + ' {:5.2f};'.format(_stbd_setpoint) \
# + Fore.WHITE + ' action_B: {}'.format(action_B))
# end of loop
action_B = False # quit after action
time.sleep(1.0)
# and we now return to our regularly scheduled broadcast...
else:
pass
_log.info(Fore.BLACK + Style.BRIGHT + 'waiting on button A...')
time.sleep(0.5)
_log.info(Fore.YELLOW + 'end of test.')
except KeyboardInterrupt:
_log.info(Fore.CYAN + Style.BRIGHT + 'Ctrl-C caught: closing...')
finally:
if ORIENTATION == Orientation.BOTH or ORIENTATION == Orientation.PORT:
_port_pid_ctrl.disable()
if ORIENTATION == Orientation.BOTH or ORIENTATION == Orientation.STBD:
_stbd_pid_ctrl.disable()
_motors.brake()
if _rotary_ctrl:
_log.info('resetting rotary encoder...')
_rotary_ctrl.reset()
time.sleep(1.0)
_log.info('complete.')
except Exception as e:
_log.info(Fore.RED + Style.BRIGHT +
'error in test: {}'.format(e, traceback.format_exc()))
finally:
_log.info(Fore.YELLOW + Style.BRIGHT + 'finally.')
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.')
def __init__(self):
self.value = 0
self.button = Button(XY(5, 5), XY(10, 10), 'Button', self.method, XY)
class ROS(AbstractTask):
'''
Extends AbstractTask as a Finite State Machine (FSM) basis of a Robot
Operating System (ROS) or behaviour-based system (BBS), including
spawning the various tasks and an Arbitrator as separate threads,
inter-communicating over a common message queue.
This establishes a message queue, an Arbitrator and a Controller, as
well as an optional RESTful flask-based web service.
The message queue receives Event-containing messages, which are passed
on to the Arbitrator, whose job it is to determine the highest priority
action to execute for that task cycle.
Example usage:
try:
_ros = ROS()
_ros.start()
except Exception:
_ros.close()
There is also a rosd linux daemon, which can be used to start, enable
and disable ros (actually via its Arbitrator).
'''
# ..........................................................................
def __init__(self):
'''
This initialises the ROS and calls the YAML configurer.
'''
self._queue = MessageQueue(Level.INFO)
self._mutex = threading.Lock()
super().__init__("ros", self._queue, None, None, self._mutex)
self._log.info('initialising...')
self._active = False
self._closing = False
self._disable_leds = False
# self._switch = None
self._motors = None
self._arbitrator = None
self._controller = None
self._gamepad = None
self._features = []
# self._flask_wrapper = None
# read YAML configuration
_loader = ConfigLoader(Level.INFO)
filename = 'config.yaml'
self._config = _loader.configure(filename)
self._gamepad_enabled = self._config['ros'].get('gamepad').get(
'enabled')
self._log.info('initialised.')
self._configure()
# ..........................................................................
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.')
# ..........................................................................
def summation():
'''
Displays unaccounted I2C addresses. This is currently non-functional,
as we don't remove a device from the list when its address is found.
'''
self._log.info(Fore.YELLOW + '-- unaccounted for self._addresses:')
for i in range(len(self._addresses)):
hexAddr = self._addrDict.get(self._addresses[i])
self._log.info(Fore.YELLOW + Style.BRIGHT +
'-- address: {}'.format(hexAddr) + Style.RESET_ALL)
# ..........................................................................
def _set_feature_available(self, name, value):
'''
Sets a feature's availability to the boolean value.
'''
self._log.debug(Fore.BLUE + Style.BRIGHT +
'-- set feature available. name: \'{}\' value: \'{}\'.'
.format(name, value))
self.set_property('features', name, value)
# ..........................................................................
def get_message_queue(self):
return self._queue
# ..........................................................................
def get_controller(self):
return self._controller
# ..........................................................................
def get_configuration(self):
return self._config
# ..........................................................................
def get_property(self, section, property_name):
'''
Return the value of the named property of the application
configuration, provided its section and property name.
'''
return self._config[section].get(property_name)
# ..........................................................................
def set_property(self, section, property_name, property_value):
'''
Set the value of the named property of the application
configuration, provided its section, property name and value.
'''
self._log.debug(
Fore.GREEN +
'set config on section \'{}\' for property key: \'{}\' to value: {}.'
.format(section, property_name, property_value))
if section == 'ros':
self._config[section].update(property_name=property_value)
else:
_ros = self._config['ros']
_ros[section].update(property_name=property_value)
# ..........................................................................
def _set_pi_leds(self, enable):
'''
Enables or disables the Raspberry Pi's board LEDs.
'''
sudo_name = self.get_property('pi', 'sudo_name')
led_0_path = self._config['pi'].get('led_0_path')
led_1_path = self._config['pi'].get('led_1_path')
if enable:
self._log.info('re-enabling LEDs...')
os.system('echo 1 | {} tee {}'.format(sudo_name, led_0_path))
os.system('echo 1 | {} tee {}'.format(sudo_name, led_1_path))
else:
self._log.debug('disabling LEDs...')
os.system('echo 0 | {} tee {}'.format(sudo_name, led_0_path))
os.system('echo 0 | {} tee {}'.format(sudo_name, led_1_path))
# ..........................................................................
def _connect_gamepad(self):
if not self._gamepad_enabled:
self._log.info('gamepad disabled.')
return
if self._gamepad is None:
self._log.info('creating gamepad...')
try:
self._gamepad = Gamepad(self._config, self._queue, Level.INFO)
except GamepadConnectException as e:
self._log.error('unable to connect to gamepad: {}'.format(e))
self._gamepad = None
self._gamepad_enabled = False
self._log.info('gamepad unavailable.')
return
if self._gamepad is not None:
self._log.info('enabling gamepad...')
self._gamepad.enable()
_count = 0
while not self._gamepad.has_connection():
_count += 1
if _count == 1:
self._log.info('connecting to gamepad...')
else:
self._log.info(
'gamepad not connected; re-trying... [{:d}]'.format(
_count))
self._gamepad.connect()
time.sleep(0.5)
if self._gamepad.has_connection() or _count > 5:
break
# ..........................................................................
def has_connected_gamepad(self):
return self._gamepad is not None and self._gamepad.has_connection()
# ..........................................................................
def get_arbitrator(self):
return self._arbitrator
# ..........................................................................
def add_feature(self, feature):
'''
Add the feature to the list of features. Features must have
an enable() method.
'''
self._features.append(feature)
self._log.info('added feature {}.'.format(feature.name()))
# ..........................................................................
def _callback_shutdown(self):
_enable_self_shutdown = self._config['ros'].get('enable_self_shutdown')
if _enable_self_shutdown:
self._log.critical('callback: shutting down os...')
self.close()
sys.exit(0)
else:
self._log.critical('self-shutdown disabled.')
# ..........................................................................
def run(self):
'''
This first disables the Pi's status LEDs, establishes the
message queue arbitrator, the controller, enables the set
of features, then starts the main OS loop.
'''
super(AbstractTask, self).run()
loop_count = 0
# display banner!
_banner = '\n' \
'ros\n' \
'ros █▒▒▒▒▒▒▒ █▒▒▒▒▒▒ █▒▒▒▒▒▒ █▒▒ \n' \
+ 'ros █▒▒ █▒▒ █▒▒ █▒▒ █▒▒ █▒▒ \n' \
+ 'ros █▒▒▒▒▒▒ █▒▒ █▒▒ █▒▒▒▒▒▒ █▒▒ \n' \
+ 'ros █▒▒ █▒▒ █▒▒ █▒▒ █▒▒ \n' \
+ 'ros █▒▒ █▒▒ █▒▒▒▒▒▒ █▒▒▒▒▒▒ █▒▒ \n' \
+ 'ros\n'
self._log.info(_banner)
self._disable_leds = self._config['pi'].get('disable_leds')
if self._disable_leds:
# disable Pi LEDs since they may be distracting
self._set_pi_leds(False)
self._log.info('enabling features...')
for feature in self._features:
self._log.info('enabling feature {}...'.format(feature.name()))
feature.enable()
# __enable_player = self._config['ros'].get('enable_player')
# if __enable_player:
# self._log.info('configuring sound player...')
# self._player = Player(Level.INFO)
# else:
# self._player = None
# i2c_slave_address = config['ros'].get('i2c_master').get('device_id') # i2c hex address of I2C slave device
vl53l1x_available = True # self.get_property('features', 'vl53l1x')
ultraborg_available = True # self.get_property('features', 'ultraborg')
if vl53l1x_available and ultraborg_available:
self._log.critical('starting scanner tool...')
self._lidar = Lidar(self._config, Level.INFO)
self._lidar.enable()
else:
self._log.critical(
'lidar scanner tool does not have necessary dependencies.')
# wait to stabilise features?
# configure the Controller and Arbitrator
self._log.info('configuring controller...')
self._controller = Controller(self._config, self._ifs, self._motors,
self._callback_shutdown, Level.INFO)
self._log.info('configuring arbitrator...')
self._arbitrator = Arbitrator(self._config, self._queue,
self._controller, Level.WARN)
_flask_enabled = self._config['flask'].get('enabled')
if _flask_enabled:
self._log.info('starting flask web server...')
self.configure_web_server()
else:
self._log.info(
'not starting flask web server (suppressed from command line).'
)
# bluetooth gamepad controller
if self._gamepad_enabled:
self._connect_gamepad()
self._log.warning('Press Ctrl-C to exit.')
_wait_for_button_press = self._config['ros'].get(
'wait_for_button_press')
self._controller.set_standby(_wait_for_button_press)
# begin main loop ..............................
self._log.info('starting button thread...')
self._button.start()
# self._log.info('enabling bno055 sensor...')
# self._bno055.enable()
# self._bumpers.enable()
self._indicator = Indicator(Level.INFO)
# add indicator as message consumer
self._queue.add_consumer(self._indicator)
self._log.info(Fore.MAGENTA + 'enabling integrated front sensor...')
self._ifs.enable()
# self._log.info('starting info thread...')
# self._info.start()
# self._log.info('starting blinky thread...')
# self._rgbmatrix.enable(DisplayType.RANDOM)
# enable arbitrator tasks (normal functioning of robot)
main_loop_delay_ms = self._config['ros'].get('main_loop_delay_ms')
self._log.info(
'begin main os loop with {:d}ms delay.'.format(main_loop_delay_ms))
_loop_delay_sec = main_loop_delay_ms / 1000
_main_loop_count = 0
self._arbitrator.start()
self._active = True
while self._active:
# The sensors and the flask service sends messages to the message queue,
# which forwards those messages on to the arbitrator, which chooses the
# highest priority message to send on to the controller. So the timing
# of this loop is inconsequential; it exists solely as a keep-alive.
_main_loop_count += 1
self._log.debug(Fore.BLACK + Style.DIM +
'[{:d}] main loop...'.format(_main_loop_count))
time.sleep(_loop_delay_sec)
# end application loop .........................
if not self._closing:
self._log.warning('closing following loop...')
self.close()
# end main ...................................
# ..........................................................................
def configure_web_server(self):
'''
Start flask web server.
'''
try:
self._mutex.acquire()
self._log.info('starting web service...')
self._flask_wrapper = FlaskWrapperService(self._queue,
self._controller)
self._flask_wrapper.start()
except KeyboardInterrupt:
self._log.error('caught Ctrl-C interrupt.')
finally:
self._mutex.release()
time.sleep(1)
self._log.info(Fore.BLUE + 'web service started.' +
Style.RESET_ALL)
# ..........................................................................
def emergency_stop(self):
'''
Stop immediately, something has hit the top feelers.
'''
self._motors.stop()
self._log.info(Fore.RED + Style.BRIGHT +
'emergency stop: contact on upper feelers.')
# ..........................................................................
def send_message(self, message):
'''
Send the Message into the MessageQueue.
'''
self._queue.add(message)
# ..........................................................................
def enable(self):
super(AbstractTask, self).enable()
# ..........................................................................
def disable(self):
super(AbstractTask, self).disable()
# ..........................................................................
def close(self):
'''
This sets the ROS back to normal following a session.
'''
if self._closing:
# this also gets called by the arbitrator so we ignore that
self._log.info('already closing.')
return
else:
self._active = False
self._closing = True
self._log.info(Style.BRIGHT + 'closing...')
if self._gamepad:
self._gamepad.close()
if self._motors:
self._motors.close()
if self._ifs:
self._ifs.close()
# close features
for feature in self._features:
self._log.info('closing feature {}...'.format(feature.name()))
feature.close()
self._log.info('finished closing features.')
if self._arbitrator:
self._arbitrator.disable()
self._arbitrator.close()
# self._arbitrator.join(timeout=1.0)
if self._controller:
self._controller.disable()
# if self._flask_wrapper is not None:
# self._flask_wrapper.close()
super().close()
# self._info.close()
# self._rgbmatrix.close()
# if self._switch:
# self._switch.close()
# super(AbstractTask, self).close()
if self._disable_leds:
# restore LEDs
self._set_pi_leds(True)
# GPIO.cleanup()
# self._log.info('joining main thread...')
# self.join(timeout=0.5)
self._log.info('os closed.')
sys.stderr = DevNull()
sys.exit(0)
class RockingMotorcycleGame:
SOUND_START = "start" # 11 sec
SOUND_STOP = "stop" # 6 sev
SOUND_SPEEDUP = "speedUp" # 13 sec
SOUND_IDLE = "idle" # 40 sec
SOUND_DRIVE = "drive" # 22 sec
MODE_START = 0
MODE_DRIVE = 1
MODE_STOP = 2
LED_SPEED = 100
def __init__(self, pinBTN=27, pinLED=17, startVolume=70, imuAddress=0x6a, limit=350, changeDelta=1000):
GPIO.setmode(GPIO.BCM)
led = LED(pinLED)
self.ledStrip = LEDStrip()
self.audio = Audio(startVolume)
self.mixer = AudioMixer()
self.initAudioMixer()
self.btn = Button(pinBTN)
self.btn.setListener(self.audioBtnListener)
self.ledControl = LEDController(led)
self.ledControl.start()
self.changeDelta = changeDelta
self.currentMode = -1
# self.imuController = IMUController(imuAddress, limit=limit)
self.imuController = LSMController(imuAddress, limit=limit)
self.initImuController()
self.animDrive = TheaterChaseAnimation(LEDAnimation.COLOR_CYAN, 20, 50, 15, True)
self.animIdle = FadeCycleAnimation(RGBColor(0, 0, 0), RGBColor(0, 80, 0), 25, 20)
self.ledStrip = LEDStrip()
self.ledStrip.start()
self.speed = 0
self.lastLoop = self.getMillis()
def initImuController(self):
self.imuChangeTime = 0
self.imuController.setListener(self.imuListener)
self.imuController.start()
def initAudioMixer(self):
appPath = path.dirname(path.abspath(__file__))
audioFolder = path.join(appPath, "audio")
self.mixer.addSound(self.SOUND_START, path.join(audioFolder, "h_start.wav"))
self.mixer.addSound(self.SOUND_STOP, path.join(audioFolder, "h_stop.wav"))
self.mixer.addSound(self.SOUND_SPEEDUP, path.join(audioFolder, "h_speedup.wav"))
self.mixer.addSound(self.SOUND_IDLE, path.join(audioFolder, "h_idle.wav"))
self.mixer.addSound(self.SOUND_DRIVE, path.join(audioFolder, "h_drive.wav"))
self.mixer.setListener(self.mixerListener)
def run(self):
# indicate start
self.ledControl.blink(2, 100, 100)
while True:
self.btn.loop()
mode = self.getMode(self.imuChangeTime)
accl = self.getAcceleration(mode, self.lastLoop)
self.speed += accl
if self.speed < 0:
self.speed = 0
elif self.speed > 100:
self.speed = 100
self.playLEDAnimation(mode, self.speed)
self.playSound(mode)
self.lastLoop = self.getMillis()
self.currentMode = mode
time.sleep(0.1)
def mixerListener(self, id):
return
def playLEDAnimation(self, mode: int, speed: int):
curAnim = self.ledStrip.getAnimation()
if mode == self.MODE_START or speed <= 5:
if curAnim != self.animIdle:
self.ledStrip.setAnimation(self.animIdle)
else:
if curAnim is None or curAnim != self.animDrive:
curAnim = self.animDrive
self.ledStrip.setAnimation(curAnim)
percentage = (100 - speed) / 100
color = LEDAnimation.wheel(int(80 * (1 - percentage)))
self.animDrive.update(color, int(10 + 200 * percentage), curAnim.iterations,
curAnim.distance)
def stop(self):
self.mixer.stop()
self.ledControl.stop()
self.ledStrip.setAnimation(ColorSetAnimation(LEDAnimation.COLOR_BLACK))
time.sleep(1)
self.ledStrip.stop()
self.imuController.stop()
GPIO.cleanup()
def imuListener(self):
self.imuChangeTime = self.getMillis()
def getMillis(self):
return int(round(time.time() * 1000))
def audioBtnListener(self, mode):
vol = self.audio.getVolume()
if mode is Button.NORMAL_PRESS:
if vol == 100:
self.audio.setVolume(70)
vol = 70
else:
vol = self.audio.volumeUp()
else:
if self.audio.isMute:
vol = self.audio.unmute()
else:
self.audio.mute()
vol = 0
if vol >= 70:
self.ledControl.blink(int(vol / 10) - 6, self.LED_SPEED, self.LED_SPEED)
else:
self.ledControl.blink(1, 1000, self.LED_SPEED)
def getMode(self, imuChangeTime: int):
if imuChangeTime == 0:
return self.MODE_START
elif self.getMillis() - imuChangeTime < self.changeDelta:
return self.MODE_DRIVE
else:
return self.MODE_STOP
def playSound(self, mode: int):
if self.currentMode == mode:
return
self.mixer.clearQueue()
if mode == self.MODE_START:
self.mixer.playSound(self.SOUND_START, False, 0)
self.mixer.queue(self.SOUND_IDLE, True)
elif mode == self.MODE_DRIVE:
self.mixer.playSound(self.SOUND_SPEEDUP, False)
self.mixer.queue(self.SOUND_DRIVE, True)
elif mode == self.MODE_STOP:
self.mixer.playSound(self.SOUND_STOP, False)
self.mixer.queue(self.SOUND_IDLE, True)
def getAcceleration(self, mode: int, lastLoop: int):
accl = 0
if mode != self.MODE_START:
# get time between loops
diff = self.getMillis() - lastLoop
speedChange = (diff / 1000) * 5 # 17 kmh per sec
if mode == self.MODE_DRIVE:
# decreases speed
accl = speedChange
elif mode == self.MODE_STOP:
# increase speed
accl -= 2 * speedChange
return accl
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))