class launcher:
def __init__(self):
self.reading_calibration = True
# Initialise wiimote, will be created at beginning of loop.
self.wiimote = None
# Instantiate CORE / Chassis module and store in the launcher.
self.core = core.Core(VL53L0X.tof_lib)
GPIO.setwarnings(False)
self.GPIO = GPIO
self.challenge = None
self.challenge_thread = None
# Shutting down status
self.shutting_down = False
self.killed = False
# WiiMote LED counter to indicate mode
# NOTE: the int value will be shown as binary on the wiimote.
self.MODE_NONE = 1
self.MODE_RC = 2
self.MODE_WALL = 3
self.MODE_MAZE = 4
self.MODE_CALIBRATION = 5
self.mode = self.MODE_NONE
# create oled object, nominating the correct I2C bus, default address
self.oled = ssd1306(VL53L0X.i2cbus)
def stop_threads(self):
""" Single point of call to stop any RC or Challenge Threads """
if self.challenge:
if (self.mode == self.MODE_CALIBRATION):
# Write the config file when exiting the calibration module.
self.challenge.write_config()
self.challenge.stop()
self.challenge = None
self.challenge_thread = None
logging.info("Stopping Challenge Thread")
else:
logging.info("No Challenge Thread")
# Reset LED to NO MODE
self.mode = self.MODE_NONE
if self.wiimote and self.wiimote.wm:
self.wiimote.wm.led = self.mode
# Safety setting
self.core.enable_motors(False)
# Show state on OLED display
self.show_mode()
def show_message(self, message):
""" Show state on OLED display """
self.oled.cls() # Clear Screen
self.oled.canvas.text((10, 10), message, fill=1)
# Now show the mesasge on the screen
self.oled.display()
def show_mode(self):
""" Show state on OLED display """
self.oled.cls() # Clear Screen
# self.oled.canvas.text((10, 10), 'mode', fill=1)
# Show appropriate mode
if self.mode == self.MODE_NONE:
self.oled.canvas.text((10, 10), 'Mode:', fill=1)
elif self.mode == self.MODE_RC:
self.oled.canvas.text((10, 10), 'Mode: RC', fill=1)
elif self.mode == self.MODE_WALL:
self.oled.canvas.text((10, 10), 'Mode: Wall', fill=1)
elif self.mode == self.MODE_MAZE:
self.oled.canvas.text((10, 10), 'Mode: Maze', fill=1)
elif self.mode == self.MODE_CALIBRATION:
self.oled.canvas.text((10, 10), 'Mode: Calibration', fill=1)
# Now show the mesasge on the screen
self.oled.display()
def show_motor_config(self, left):
""" Show motor/aux config on OLED display """
if left:
title = "Left Motor:"
message = str(self.core.left_servo.servo_min) + '/'\
+ str(self.core.left_servo.servo_mid) + '/'\
+ str(self.core.left_servo.servo_max)
else:
title = "Right Motor:"
message = str(self.core.right_servo.servo_min) + '/'\
+ str(self.core.right_servo.servo_mid) + '/'\
+ str(self.core.right_servo.servo_max)
self.oled.cls() # Clear Screen
self.oled.canvas.text((10, 10), title, fill=1)
self.oled.canvas.text((10, 30), message, fill=1)
# Now show the mesasge on the screen
self.oled.display()
def show_aux_1_config(self, left):
""" Show motor/aux config on OLED display """
if left:
title = "Left Aux 1:"
message = str(self.core.left_aux_1_servo.servo_min) + '/'\
+ str(self.core.left_aux_1_servo.servo_mid) + '/'\
+ str(self.core.left_aux_1_servo.servo_max)
else:
title = "Right Aux 1:"
message = str(self.core.right_aux_1_servo.servo_min) + '/'\
+ str(self.core.right_aux_1_servo.servo_mid) + '/'\
+ str(self.core.right_aux_1_servo.servo_max)
self.oled.cls() # Clear Screen
self.oled.canvas.text((10, 10), title, fill=1)
self.oled.canvas.text((10, 30), message, fill=1)
# Now show the mesasge on the screen
self.oled.display()
def read_config(self):
# Read the config file when starting up.
if self.reading_calibration:
calibration = Calibration.Calibration(self.core, self.wiimote,
self)
calibration.read_config()
def start_rc_mode(self):
# Kill any previous Challenge / RC mode
self.stop_threads()
# Set Wiimote LED to RC Mode index
self.mode = self.MODE_RC
if self.wiimote and self.wiimote.wm:
self.wiimote.wm.led = self.mode
# Inform user we are about to start RC mode
logging.info("Entering into RC Mode")
self.challenge = rc.rc(self.core, self.wiimote)
# Create and start a new thread
# running the remote control script
logging.info("Starting RC Thread")
self.challenge_thread = threading.Thread(target=self.challenge.run)
self.challenge_thread.start()
logging.info("RC Thread Running")
# Show state on OLED display
self.show_mode()
def start_calibration_mode(self):
# Kill any previous Challenge / RC mode
self.stop_threads()
# Set Wiimote LED to RC Mode index
self.mode = self.MODE_CALIBRATION
if self.wiimote and self.wiimote.wm:
self.wiimote.wm.led = self.mode
# Inform user we are about to start RC mode
logging.info("Entering into Calibration Mode")
self.challenge = \
Calibration.Calibration(self.core, self.wiimote, self)
# Create and start a new thread
# running the remote control script
logging.info("Starting Calibration Thread")
self.challenge_thread = threading.Thread(target=self.challenge.run)
self.challenge_thread.start()
logging.info("Calibration Thread Running")
# Show state on OLED display
self.show_mode()
def run(self):
""" Main Running loop controling bot mode and menu state """
# Show state on OLED display
self.show_message('Booting...')
# Read config file FIRST
self.read_config()
self.show_message('Initialising Bluetooth...')
# Never stop looking for wiimote.
while not self.killed:
# Show state on OLED display
self.oled.cls() # Clear screen
self.oled.canvas.text((10, 10), 'Waiting for WiiMote...', fill=1)
self.oled.canvas.text((10, 30), '***Press 1+2 now ***', fill=1)
self.oled.display()
self.wiimote = None
try:
self.wiimote = Wiimote()
except WiimoteException:
logging.error("Could not connect to wiimote. please try again")
# Reset LED to NO MODE
self.mode = self.MODE_NONE
if self.wiimote and self.wiimote.wm:
self.wiimote.wm.led = self.mode
# Show state on OLED display
self.show_mode()
# Constantly check wiimote for button presses
while self.wiimote:
buttons_state = self.wiimote.get_buttons()
classic_buttons_state = self.wiimote.get_classic_buttons()
if buttons_state is not None:
if (buttons_state & cwiid.BTN_A):
self.start_rc_mode()
if (buttons_state & cwiid.BTN_HOME):
self.start_calibration_mode()
if (buttons_state & cwiid.BTN_B):
# Kill any previous Challenge / RC mode
self.stop_threads()
if (buttons_state & cwiid.BTN_HOME):
self.start_calibration_mode()
if (buttons_state & cwiid.BTN_UP):
logging.info("BUTTON_UP")
if (buttons_state & cwiid.BTN_DOWN):
logging.info("BUTTON_DOWN")
if (buttons_state & cwiid.BTN_LEFT):
logging.info("BUTTON_LEFT")
if (buttons_state & cwiid.BTN_RIGHT):
logging.info("BUTTON_RIGHT")
if classic_buttons_state is not None:
if (classic_buttons_state & cwiid.CLASSIC_BTN_ZL
or classic_buttons_state & cwiid.CLASSIC_BTN_ZR):
# One of the Z buttons pressed, disable
# motors and set neutral.
self.core.enable_motors(False)
else:
# Neither Z buttons pressed,
# allow motors to move freely.
self.core.enable_motors(True)
time.sleep(0.05)
# Verify Wiimote is connected each loop. If not, set wiimote
# to None and it "should" attempt to reconnect.
if not self.wiimote.wm:
self.stop_threads()
self.wiimote = None
class launcher:
def __init__(self):
self.reading_calibration = True
# Initialise wiimote, will be created at beginning of loop.
self.wiimote = None
# Instantiate CORE / Chassis module and store in the launcher.
self.core = core.Core(VL53L0X.tof_lib)
GPIO.setwarnings(False)
self.GPIO = GPIO
self.challenge = None
self.challenge_thread = None
# Shutting down status
self.shutting_down = False
self.killed = False
# Mode/Challenge Dictionary
self.menu_list = OrderedDict((
(Mode.MODE_POWER, "Power Off"),
(Mode.MODE_RC, "RC"),
(Mode.MODE_WALL, "Wall"),
(Mode.MODE_MAZE, "Maze"),
(Mode.MODE_CALIBRATION, "Calibration")
))
self.current_mode = Mode.MODE_NONE
self.menu_mode = Mode.MODE_RC
# Create oled object, nominating the correct I2C bus, default address
# Note: Set to None if you need to disable screen
self.oled = ssd1306(VL53L0X.i2cbus)
def stop_threads(self):
""" Single point of call to stop any RC or Challenge Threads """
if self.challenge:
if (self.current_mode == Mode.MODE_CALIBRATION):
# Write the config file when exiting the calibration module.
self.challenge.write_config()
self.challenge.stop()
self.challenge = None
self.challenge_thread = None
logging.info("Stopping Challenge Thread")
else:
logging.info("No Challenge Thread")
# Reset current mode index
self.current_mode = Mode.MODE_NONE
# Safety setting
self.core.enable_motors(False)
# Show state on OLED display
self.show_menu()
def get_mode_name(self, mode):
""" Return appropriate mode name """
mode_name = ""
if mode != Mode.MODE_NONE:
mode_name = self.menu_list[mode]
return mode_name
def get_next_mode(self, mode):
""" Find the previous menu item """
mode_index = self.menu_list.keys().index(mode)
next_index = mode_index + 1
if next_index >= len(self.menu_list):
next_index = 0 # Wrapped round to end
return self.menu_list.keys()[next_index]
def get_previous_mode(self, mode):
""" Find the previous menu item """
mode_index = self.menu_list.keys().index(mode)
previous_index = mode_index - 1
if previous_index < 0:
previous_index = len(self.menu_list) - 1 # Wrapped round to end
return self.menu_list.keys()[previous_index]
def show_message(self, message):
""" Show state on OLED display """
if self.oled is not None:
self.oled.cls() # Clear Screen
self.oled.canvas.text((10, 10), message, fill=1)
# Now show the mesasge on the screen
self.oled.display()
def show_mode(self):
""" Display current menu item. """
if self.oled is not None:
# Clear Screen
self.oled.cls()
# Get current mode name and display it.
mode_name = self.get_mode_name(self.current_mode)
self.oled.canvas.text((10, 10), 'Mode: ' + mode_name, fill=1)
# Now show the mesasge on the screen
self.oled.display()
@debounce(0.25)
def menu_item_pressed(self):
""" Current menu item pressed. Do something """
if self.menu_mode == Mode.MODE_POWER:
self.power_off()
elif self.menu_mode == Mode.MODE_RC:
self.start_rc_mode()
elif self.menu_mode == Mode.MODE_WALL:
logging.info("Wall Mode")
elif self.menu_mode == Mode.MODE_MAZE:
logging.info("Maze Mode")
elif self.menu_mode == Mode.MODE_CALIBRATION:
self.start_calibration_mode()
@debounce(0.25)
def menu_up(self):
self.menu_mode = self.get_previous_mode(self.menu_mode)
self.show_menu()
@debounce(0.25)
def menu_down(self):
self.menu_mode = self.get_next_mode(self.menu_mode)
self.show_menu()
def show_menu(self):
""" Display menu. """
# Display current menu item to prompt for when no OLED attached
mode_name = self.get_mode_name(self.menu_mode)
print(mode_name)
# Clear Screen
if self.oled is not None:
self.oled.cls()
# Get next and previous list items
previous_mode = self.get_previous_mode(self.menu_mode)
next_mode = self.get_next_mode(self.menu_mode)
# Get mode names and display them.
current_mode_name = self.get_mode_name(self.current_mode)
mode_name_up = self.get_mode_name(previous_mode)
mode_name_down = self.get_mode_name(next_mode)
header_y = 0
previous_y = 20
current_y = 30
next_y = 40
# Display Bot name and header information
self.oled.canvas.text(
(10, header_y),
'TITO 2: ' + current_mode_name,
fill=1)
# Line underneath header
self.oled.canvas.line(
(0, 9, self.oled.width - 1, 9),
fill=1)
# Draw rect around current selection.
# NOTE: Has to be done BEFORE text below
self.oled.canvas.rectangle(
(10, current_y, self.oled.width - 1, current_y + 10),
outline=1,
fill=0)
# show current mode as well as one mode either side
self.oled.canvas.text(
(15, previous_y),
'Mode: ' + mode_name_up,
fill=1)
self.oled.canvas.text(
(15, current_y),
'Mode: ' + mode_name,
fill=1)
self.oled.canvas.text(
(15, next_y),
'Mode: ' + mode_name_down,
fill=1)
# 2x triangles indicating menu direction
self.oled.canvas.polygon(
((1, previous_y + 9),
(5, previous_y + 1),
(9, previous_y + 9),
(1, previous_y + 9)),
outline=1,
fill=0)
self.oled.canvas.polygon(
((1, next_y + 1),
(5, next_y + 9),
(9, next_y + 1),
(1, next_y + 1)),
outline=1,
fill=0)
# Now show the mesasge on the screen
self.oled.display()
def read_config(self):
# Read the config file when starting up.
if self.reading_calibration:
calibration = Calibration.Calibration(
self.core,
self.wiimote,
self)
calibration.read_config()
def power_off(self):
""" Power down the pi """
self.stop_threads()
if self.oled is not None:
self.oled.cls() # Clear Screen
self.oled.canvas.text((10, 10), 'Powering off...', fill=1)
# Now show the mesasge on the screen
self.oled.display()
# Call system OS to shut down the Pi
logging.info("Shutting Down Pi")
os.system("sudo shutdown -h now")
def start_rc_mode(self):
# Kill any previous Challenge / RC mode
self.stop_threads()
# Set Wiimote LED to RC Mode index
self.current_mode = Mode.MODE_RC
# Inform user we are about to start RC mode
logging.info("Entering into RC Mode")
self.challenge = rc.rc(self.core, self.wiimote, self.oled)
# Create and start a new thread
# running the remote control script
logging.info("Starting RC Thread")
self.challenge_thread = threading.Thread(
target=self.challenge.run)
self.challenge_thread.start()
logging.info("RC Thread Running")
def start_calibration_mode(self):
# Kill any previous Challenge / RC mode
self.stop_threads()
# Set Wiimote LED to RC Mode index
self.current_mode = Mode.MODE_CALIBRATION
# Inform user we are about to start RC mode
logging.info("Entering into Calibration Mode")
self.challenge = \
Calibration.Calibration(self.core, self.wiimote, self.oled)
# Create and start a new thread
# running the remote control script
logging.info("Starting Calibration Thread")
self.challenge_thread = threading.Thread(
target=self.challenge.run)
self.challenge_thread.start()
logging.info("Calibration Thread Running")
def run(self):
""" Main Running loop controling bot mode and menu state """
# Show state on OLED display
self.show_message('Booting...')
# Read config file FIRST
self.read_config()
self.show_message('Initialising Bluetooth...')
# Never stop looking for wiimote.
while not self.killed:
if self.oled is not None:
# Show state on OLED display
self.oled.cls() # Clear screen
self.oled.canvas.text(
(10, 10),
'Waiting for WiiMote...',
fill=1)
self.oled.canvas.text(
(10, 30),
'***Press 1+2 now ***',
fill=1)
self.oled.display()
self.wiimote = None
try:
self.wiimote = Wiimote()
except WiimoteException:
logging.error("Could not connect to wiimote. please try again")
# Show state on OLED display
self.show_menu()
# Constantly check wiimote for button presses
while self.wiimote:
buttons_state = self.wiimote.get_buttons()
classic_buttons_state = self.wiimote.get_classic_buttons()
if buttons_state is not None:
if (buttons_state & cwiid.BTN_A and
self.challenge is None):
# Only works when NOT in a challenge
self.menu_item_pressed()
self.show_menu()
if (buttons_state & cwiid.BTN_B):
# Kill any previous Challenge / RC mode
# NOTE: will ALWAYS work
self.stop_threads()
if (buttons_state & cwiid.BTN_UP and
self.challenge is None):
# Only works when NOT in a challenge
self.menu_up()
if (buttons_state & cwiid.BTN_DOWN and
self.challenge is None):
# Only works when NOT in a challenge
self.menu_down()
if classic_buttons_state is not None:
if (classic_buttons_state & cwiid.CLASSIC_BTN_ZL or
classic_buttons_state & cwiid.CLASSIC_BTN_ZR):
# One of the Z buttons pressed, disable
# motors and set neutral.
# NOTE: will ALWAYS work
self.core.enable_motors(False)
else:
# Neither Z buttons pressed,
# allow motors to move freely.
# NOTE: will ALWAYS work
self.core.enable_motors(True)
time.sleep(0.05)
# Verify Wiimote is connected each loop. If not, set wiimote
# to None and it "should" attempt to reconnect.
if not self.wiimote.wm:
self.stop_threads()
self.wiimote = None
