class launcher:
def __init__(self):
# Need a state set for this launcher.
self.menu = ["Remote Control"]
self.menu += ["Three Point Turn"]
self.menu += ["Straight Line Speed"]
self.menu += ["Line Following"]
self.menu += ["Proximity"]
self.menu += ["Quit Challenge"]
self.menu += ["Power Off Pi"]
self.menu_quit_challenge = 3
# default menu item is remote control
self.menu_state = 0
self.menu_button_pressed = False
self.drive = None
self.wiimote = None
# Current Challenge
self.challenge = None
self.challenge_name = ""
GPIO.setwarnings(False)
self.GPIO = GPIO
# LCD Display
self.lcd = Adafruit_CharLCD( pin_rs=25, pin_e=24, pins_db=[23, 17, 27, 22], GPIO=self.GPIO )
self.lcd.begin(16, 1)
self.lcd.clear()
self.lcd.message('Initiating...')
self.lcd_loop_skip = 5
# Shutting down status
self.shutting_down = False
def menu_item_selected(self):
"""Select the current menu item"""
# If ANYTHING selected, we gracefully
# kill any challenge threads open
self.stop_threads()
if self.menu[self.menu_state]=="Remote Control":
# Start the remote control
logging.info("Entering into Remote Control Mode")
self.challenge = rc.rc(self.drive, self.wiimote)
# Create and start a new thread running the remote control script
self.challenge_thread = threading.Thread(target=self.challenge.run)
self.challenge_thread.start()
# Ensure we know what challenge is running
if self.challenge:
self.challenge_name = self.menu[self.menu_state]
# Move menu index to quit challenge by default
self.menu_state = self.menu_quit_challenge
elif self.menu[self.menu_state]=="Three Point Turn":
# Start the three point turn challenge
logging.info("Starting Three Point Turn Challenge")
self.challenge = ThreePointTurn(self.drive)
# Create and start a new thread running the remote control script
self.challenge_thread = threading.Thread(target=self.challenge.run)
self.challenge_thread.start()
# Ensure we know what challenge is running
if self.challenge:
self.challenge_name = self.menu[self.menu_state]
# Move menu index to quit challenge by default
self.menu_state = self.menu_quit_challenge
elif self.menu[self.menu_state]=="Straight Line Speed":
# Start the straight line speed challenge
logging.info("Starting Straight Line Speed Challenge")
self.challenge = StraightLineSpeed(self.drive)
# Ensure we know what challenge is running
if self.challenge:
self.challenge_name = self.menu[self.menu_state]
# Move menu index to quit challenge by default
self.menu_state = self.menu_quit_challenge
elif self.menu[self.menu_state]=="Line Following":
# Start the Line Following challenge
logging.info("Starting Line Following Challenge")
self.challenge = LineFollowing(self.drive)
# Ensure we know what challenge is running
if self.challenge:
self.challenge_name = self.menu[self.menu_state]
# Move menu index to quit challenge by default
self.menu_state = self.menu_quit_challenge
elif self.menu[self.menu_state]=="Proximity":
# Start the Proximity challenge
logging.info("Starting Proximity Challenge")
self.challenge = Proximity(self.drive)
# Ensure we know what challenge is running
if self.challenge:
self.challenge_name = self.menu[self.menu_state]
# Move menu index to quit challenge by default
self.menu_state = self.menu_quit_challenge
elif self.menu[self.menu_state]=="Quit Challenge":
# Reset menu item back to top of list
self.menu_state = 0
logging.info("No Challenge Challenge Thread")
elif self.menu[self.menu_state]=="Power Off Pi":
# Power off the raspberry pi safely
# by sending shutdown command to terminal
logging.info("Shutting Down Pi")
os.system("sudo shutdown -h now")
self.shutting_down = True
def set_neutral(self, drive, wiimote):
"""Simple method to ensure motors are disabled"""
if drive:
drive.set_neutral()
drive.disable_drive()
if wiimote is not None:
# turn on leds on wii remote
wiimote.led = 2
def set_drive(self, drive, wiimote):
"""Simple method to highlight that motors are enabled"""
if wiimote is not None:
# turn on leds on wii remote
#turn on led to show connected
drive.enable_drive()
wiimote.led = 1
def stop_threads(self):
"""Method neatly closes any open threads started by this class"""
if self.challenge:
self.challenge.stop()
self.challenge = None
self.challenge_thread = None
logging.info("Stopping Challenge Thread")
else:
logging.info("No Challenge Challenge Thread")
# Safety setting
self.set_neutral(self.drive, self.wiimote)
def run(self):
""" Main Running loop controling bot mode and menu state """
# Tell user how to connect wiimote
self.lcd.clear()
self.lcd.message( 'Press 1+2 \n' )
self.lcd.message( 'On Wiimote' )
# Initiate the drivetrain
self.drive = drivetrain.DriveTrain(pwm_i2c=0x40)
self.wiimote = None
try:
self.wiimote = Wiimote()
except WiimoteException:
logging.error("Could not connect to wiimote. please try again")
if not self.wiimote:
# Tell user how to connect wiimote
self.lcd.clear()
self.lcd.message( 'Wiimote \n' )
self.lcd.message( 'Not Found' + '\n' )
# Constantly check wiimote for button presses
loop_count = 0
while self.wiimote:
buttons_state = self.wiimote.get_buttons()
nunchuk_buttons_state = self.wiimote.get_nunchuk_buttons()
joystick_state = self.wiimote.get_joystick_state()
# logging.info("joystick_state: {0}".format(joystick_state))
# logging.info("button state {0}".format(buttons_state))
# Always show current menu item
# logging.info("Menu: " + self.menu[self.menu_state])
if loop_count >= self.lcd_loop_skip:
# Reset loop count if over
loop_count = 0
self.lcd.clear()
if self.shutting_down:
# How current menu item on LCD
self.lcd.message( 'Shutting Down Pi' + '\n' )
else:
# How current menu item on LCD
self.lcd.message( self.menu[self.menu_state] + '\n' )
# If challenge is running, show it on line 2
if self.challenge:
self.lcd.message( '[' + self.challenge_name + ']' )
# Increment Loop Count
loop_count = loop_count + 1
# Test if B button is pressed
if joystick_state is None or (buttons_state & cwiid.BTN_B) or (nunchuk_buttons_state & cwiid.NUNCHUK_BTN_Z):
# No nunchuk joystick detected or B or Z button
# pressed, must go into neutral for safety
logging.info("Neutral")
self.set_neutral(self.drive, self.wiimote)
else:
# Enable motors
self.set_drive(self.drive, self.wiimote)
if ((buttons_state & cwiid.BTN_A)
or (buttons_state & cwiid.BTN_UP)
or (buttons_state & cwiid.BTN_DOWN)):
# Looking for state change only
if not self.menu_button_pressed and (buttons_state & cwiid.BTN_A):
# User wants to select a menu item
self.menu_item_selected()
elif not self.menu_button_pressed and (buttons_state & cwiid.BTN_UP):
# Decrement menu index
self.menu_state = self.menu_state - 1
if self.menu_state < 0:
# Loop back to end of list
self.menu_state = len(self.menu)-1
logging.info("Menu item: {0}".format(self.menu[self.menu_state]))
elif not self.menu_button_pressed and (buttons_state & cwiid.BTN_DOWN):
# Increment menu index
self.menu_state = self.menu_state + 1
if self.menu_state >= len(self.menu):
# Loop back to start of list
self.menu_state = 0
logging.info("Menu item: {0}".format(self.menu[self.menu_state]))
# Only change button state AFTER we have used it
self.menu_button_pressed = True
else:
# No menu buttons pressed
self.menu_button_pressed = False
time.sleep(0.05)
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
wiimote = None
while not wiimote:
# Loop for ever waiting for the wiimote to connect.
try:
print("Waiting for you to press '1+2' on wiimote")
wiimote = Wiimote()
except WiimoteException:
print("Wiimote error")
wiimote = None
# logging.error("Could not connect to wiimote. please try again")
try:
# Constantly check wiimote for button presses
while wiimote:
buttons_state = wiimote.get_buttons()
nunchuk_buttons_state = wiimote.get_nunchuk_buttons()
joystick_state = wiimote.get_joystick_state()
# Test if B or Z button is pressed
if (
joystick_state is None or
(buttons_state & cwiid.BTN_B) or
(nunchuk_buttons_state & cwiid.NUNCHUK_BTN_Z)
):
# No nunchuk joystick detected or B or Z button
# pressed, must go into neutral for safety
set_neutral(drive, wiimote)
else:
# Enable motors
set_drive(drive, wiimote)
class launcher:
def __init__(self):
# Need a state set for this launcher.
self.menu = ["Remote Control"]
self.menu += ["Three Point Turn"]
self.menu += ["Straight Line Speed"]
self.menu += ["Line Following"]
self.menu += ["Proximity"]
self.menu += ["Quit Challenge"]
self.menu += ["Power Off Pi"]
self.menu_quit_challenge = 3
# default menu item is remote control
self.menu_state = 0
self.menu_button_pressed = False
self.drive = None
self.wiimote = None
# Current Challenge
self.challenge = None
self.challenge_name = ""
GPIO.setwarnings(False)
self.GPIO = GPIO
# LCD Display
self.lcd = Adafruit_CharLCD(pin_rs=25,
pin_e=24,
pins_db=[23, 17, 27, 22],
GPIO=self.GPIO)
self.lcd.begin(16, 1)
self.lcd.clear()
self.lcd.message('Initiating...')
self.lcd_loop_skip = 5
# Shutting down status
self.shutting_down = False
def menu_item_selected(self):
"""Select the current menu item"""
# If ANYTHING selected, we gracefully
# kill any challenge threads open
self.stop_threads()
if self.menu[self.menu_state] == "Remote Control":
# Start the remote control
logging.info("Entering into Remote Control Mode")
self.challenge = rc.rc(self.drive, self.wiimote)
# Create and start a new thread running the remote control script
self.challenge_thread = threading.Thread(target=self.challenge.run)
self.challenge_thread.start()
# Ensure we know what challenge is running
if self.challenge:
self.challenge_name = self.menu[self.menu_state]
# Move menu index to quit challenge by default
self.menu_state = self.menu_quit_challenge
elif self.menu[self.menu_state] == "Three Point Turn":
# Start the three point turn challenge
logging.info("Starting Three Point Turn Challenge")
self.challenge = ThreePointTurn(self.drive)
# Create and start a new thread running the remote control script
self.challenge_thread = threading.Thread(target=self.challenge.run)
self.challenge_thread.start()
# Ensure we know what challenge is running
if self.challenge:
self.challenge_name = self.menu[self.menu_state]
# Move menu index to quit challenge by default
self.menu_state = self.menu_quit_challenge
elif self.menu[self.menu_state] == "Straight Line Speed":
# Start the straight line speed challenge
logging.info("Starting Straight Line Speed Challenge")
self.challenge = StraightLineSpeed(self.drive)
# Ensure we know what challenge is running
if self.challenge:
self.challenge_name = self.menu[self.menu_state]
# Move menu index to quit challenge by default
self.menu_state = self.menu_quit_challenge
elif self.menu[self.menu_state] == "Line Following":
# Start the Line Following challenge
logging.info("Starting Line Following Challenge")
self.challenge = LineFollowing(self.drive)
# Ensure we know what challenge is running
if self.challenge:
self.challenge_name = self.menu[self.menu_state]
# Move menu index to quit challenge by default
self.menu_state = self.menu_quit_challenge
elif self.menu[self.menu_state] == "Proximity":
# Start the Proximity challenge
logging.info("Starting Proximity Challenge")
self.challenge = Proximity(self.drive)
# Ensure we know what challenge is running
if self.challenge:
self.challenge_name = self.menu[self.menu_state]
# Move menu index to quit challenge by default
self.menu_state = self.menu_quit_challenge
elif self.menu[self.menu_state] == "Quit Challenge":
# Reset menu item back to top of list
self.menu_state = 0
logging.info("No Challenge Challenge Thread")
elif self.menu[self.menu_state] == "Power Off Pi":
# Power off the raspberry pi safely
# by sending shutdown command to terminal
logging.info("Shutting Down Pi")
os.system("sudo shutdown -h now")
self.shutting_down = True
def set_neutral(self, drive, wiimote):
"""Simple method to ensure motors are disabled"""
if drive:
drive.set_neutral()
drive.disable_drive()
if wiimote is not None:
# turn on leds on wii remote
wiimote.led = 2
def set_drive(self, drive, wiimote):
"""Simple method to highlight that motors are enabled"""
if wiimote is not None:
# turn on leds on wii remote
#turn on led to show connected
drive.enable_drive()
wiimote.led = 1
def stop_threads(self):
"""Method neatly closes any open threads started by this class"""
if self.challenge:
self.challenge.stop()
self.challenge = None
self.challenge_thread = None
logging.info("Stopping Challenge Thread")
else:
logging.info("No Challenge Challenge Thread")
# Safety setting
self.set_neutral(self.drive, self.wiimote)
def run(self):
""" Main Running loop controling bot mode and menu state """
# Tell user how to connect wiimote
self.lcd.clear()
self.lcd.message('Press 1+2 \n')
self.lcd.message('On Wiimote')
# Initiate the drivetrain
self.drive = drivetrain.DriveTrain(pwm_i2c=0x40)
self.wiimote = None
try:
self.wiimote = Wiimote()
except WiimoteException:
logging.error("Could not connect to wiimote. please try again")
if not self.wiimote:
# Tell user how to connect wiimote
self.lcd.clear()
self.lcd.message('Wiimote \n')
self.lcd.message('Not Found' + '\n')
# Constantly check wiimote for button presses
loop_count = 0
while self.wiimote:
buttons_state = self.wiimote.get_buttons()
nunchuk_buttons_state = self.wiimote.get_nunchuk_buttons()
joystick_state = self.wiimote.get_joystick_state()
# logging.info("joystick_state: {0}".format(joystick_state))
# logging.info("button state {0}".format(buttons_state))
# Always show current menu item
# logging.info("Menu: " + self.menu[self.menu_state])
if loop_count >= self.lcd_loop_skip:
# Reset loop count if over
loop_count = 0
self.lcd.clear()
if self.shutting_down:
# How current menu item on LCD
self.lcd.message('Shutting Down Pi' + '\n')
else:
# How current menu item on LCD
self.lcd.message(self.menu[self.menu_state] + '\n')
# If challenge is running, show it on line 2
if self.challenge:
self.lcd.message('[' + self.challenge_name + ']')
# Increment Loop Count
loop_count = loop_count + 1
# Test if B button is pressed
if joystick_state is None or (buttons_state & cwiid.BTN_B) or (
nunchuk_buttons_state & cwiid.NUNCHUK_BTN_Z):
# No nunchuk joystick detected or B or Z button
# pressed, must go into neutral for safety
logging.info("Neutral")
self.set_neutral(self.drive, self.wiimote)
else:
# Enable motors
self.set_drive(self.drive, self.wiimote)
if ((buttons_state & cwiid.BTN_A) or (buttons_state & cwiid.BTN_UP)
or (buttons_state & cwiid.BTN_DOWN)):
# Looking for state change only
if not self.menu_button_pressed and (buttons_state
& cwiid.BTN_A):
# User wants to select a menu item
self.menu_item_selected()
elif not self.menu_button_pressed and (buttons_state
& cwiid.BTN_UP):
# Decrement menu index
self.menu_state = self.menu_state - 1
if self.menu_state < 0:
# Loop back to end of list
self.menu_state = len(self.menu) - 1
logging.info("Menu item: {0}".format(
self.menu[self.menu_state]))
elif not self.menu_button_pressed and (buttons_state
& cwiid.BTN_DOWN):
# Increment menu index
self.menu_state = self.menu_state + 1
if self.menu_state >= len(self.menu):
# Loop back to start of list
self.menu_state = 0
logging.info("Menu item: {0}".format(
self.menu[self.menu_state]))
# Only change button state AFTER we have used it
self.menu_button_pressed = True
else:
# No menu buttons pressed
self.menu_button_pressed = False
time.sleep(0.05)
