def main():
wi = Wiimote()
def cb_A():
print("RELEASED A !")
wi.on(BTN_A, RELEASED, cb_A)
import time
while True:
# connect the wiimote
if not wi.connected():
try:
print("connecting...")
wi.connect()
time.sleep(0.4)
print("connected")
except WiimoteError:
print("connection fail")
time.sleep(0.2)
else:
time.sleep(0.4)
exit = False
while not exit:
c = sys.stdin.read(1)
if c == "s":
print wi._wm.state
elif c == "a":
print wi.is_pressed(BTN_A)
exit = c == "x"
wi.close()
def activate_wiimote(self):
""" make wiimote available
"""
from wiimote import Wiimote, WiimoteError
self.wiimote = Wiimote()
def check_reconnect():
connected = False
while True:
# connect the wiimote
if not self.wiimote.connected():
try:
self.log.info("Connecting to WiiMote...")
self.wiimote.connect()
self.wait(0.4)
self.log.info("Connected to WiiMote !")
except WiimoteError:
self.log.info("Connection to WiiMote failed !")
self.wait(0.2)
else:
self.wait(0.4)
self._to_spawn.append(check_reconnect)
def activate_wiimote(self):
""" make wiimote available
"""
from wiimote import Wiimote, WiimoteError
self.wiimote = Wiimote()
def check_reconnect():
connected = False
while True:
# connect the wiimote
if not self.wiimote.connected():
try:
print("connecting...")
self.wiimote.connect()
self.wait(0.4)
print("connected")
except WiimoteError:
print("connection fail")
self.wait(0.2)
else:
self.wait(0.4)
self._to_spawn.append(check_reconnect)
def activate_wiimote(self):
""" make wiimote available
"""
from wiimote import Wiimote, WiimoteError
self.wiimote = Wiimote()
def check_reconnect():
connected = False
while True:
# connect the wiimote
if not self.wiimote.connected():
try:
self.log.info("Connecting to WiiMote...")
self.wiimote.connect()
self.wait(0.4)
self.log.info("Connected to WiiMote !")
except WiimoteError:
self.log.info("Connection to WiiMote failed !")
self.wait(0.2)
else:
self.wait(0.4)
self._to_spawn.append(check_reconnect)
import os
from time import sleep
import math
import cwiid
from acc import TiltCalculator
def calculate_tilt(acc1):
X = acc1[cwiid.X]
Y = acc1[cwiid.Y]
Z = acc1[cwiid.Z]
pitch = math.atan(Y/math.sqrt(X*X + Z*Z))
roll = math.atan(X/math.sqrt(Y*Y + Z*Z))
return 30 - math.degrees(pitch), 30 - math.degrees(roll)
mote = Wiimote()
#nunchukTilt = TiltCalculator()
#nunchukTilt.wmplugin_init(mote.wiimote)
moteTilt = TiltCalculator()
moteTilt.wmplugin_init(mote.wiimote)
while(1):
os.system("clear")
print mote.get_status()
status = mote.get_status()
print "mote pitch: %d roll: %d" % moteTilt.wmplugin_exec(status['acc'])
#print "nunchukTilt pitch: %d roll: %d" % nunchukTilt.wmplugin_exec(status['nunchuk']['acc'])
#print "nunchuk stick: X: %d Y: %d" % status['nunchuk']['stick']
print 'Button Report: %.4X' % (int(status['buttons'])&4)
# sleep(0.2)
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 Wiimote(BBLampHardware):
BTNS = [
(cwiid.BTN_2, "2"), # 1
(cwiid.BTN_1, "1"), # 2
(cwiid.BTN_B, "B"), # 4
(cwiid.BTN_A, "A"), # 8
(cwiid.BTN_MINUS, "MINUS"), # 16
(cwiid.BTN_HOME, "HOME"), # 128
(cwiid.BTN_LEFT, "LEFT"), # 256
(cwiid.BTN_RIGHT, "RIGHT"), # 512
(cwiid.BTN_DOWN, "DOWN"), # 1024
(cwiid.BTN_UP, "UP"), # 2048
(cwiid.BTN_PLUS, "PLUS"), # 4096
]
def __init__(self):
super(BBLampHardware, self).__init__()
self._connected = False
self._wm = None
self._led = 0
self._rpt_mode = cwiid.RPT_BTN # by default read only btns
self._last_btn = 0
# init callbacks
self._on = {} # self._on[BTN][PRESSED]
def activate(self):
self.connect()
#TODO: code qui était dans lampapp
def activate_wiimote(self):
""" make wiimote available
"""
from wiimote import Wiimote, WiimoteError
self.wiimote = Wiimote()
def check_reconnect():
connected = False
while True:
# connect the wiimote
if not self.wiimote.connected():
try:
self.log.info("Connecting to WiiMote...")
self.wiimote.connect()
self.wait(0.4)
self.log.info("Connected to WiiMote !")
except WiimoteError:
self.log.info("Connection to WiiMote failed !")
self.wait(0.2)
else:
self.wait(0.4)
self._to_spawn.append(check_reconnect)
def connect(self):
#TODO: add msg throw log ?
try:
self._wm = cwiid.Wiimote()
self._wm.mesg_callback = self._callback
# enable callbacks and (re)set wiimote set
self._wm.enable(cwiid.FLAG_MESG_IFC)
self._wm.rpt_mode = self._rpt_mode
self._wm.led = self._led
self._connected = True
except RuntimeError, ValueError:
# connection fail
raise WiimoteError()
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
def connectWii(self):
if self.connected:
self.disconnectDevice()
return
self.wii = Wiimote()
pBar = PBarDlg(self)
pBar.setModal(True)
pBar.show()
conf = Configuration()
selectedMac = conf.getValueStr("selectedmac")
pBar.reInit(selectedMac)
pool = []
while 1:
thread = self.wii.createConnectThread(selectedMac, pool)
thread.start()
while not thread.wait(30):
QtWidgets.QApplication.processEvents()
if pBar.cancelled == True:
if self.wii.isConnected():
self.wii.close()
self.wii = None
pBar.close()
return
if selectedMac == '*' and len(pool) >= 1:
if Configuration().getValueStr('nowaitdevices') == 'Yes':
selectedMac = pool[0]
else:
pBar.inform(
self.tr('Found ') + str(len(pool)) +
self.tr(' Devices. Press to Choose'))
if self.wii.isConnected():
self.connected = True
self.calibrated = False
self.active = False
self.updateButtons()
self.batteryLevel.setValue(self.wii.battery() * 100)
self.pushButtonConnect.setText(self.tr("Disconnect"))
pBar.close()
self.confDialog.wii = self.wii
self.confDialog.checkButtons()
self.wii.disable()
self.wii.putCallbackBTN(self.makeBTNCallback())
self.wii.putCallbackIR(None)
self.wii.enable()
# Start calibration if configuration says so
conf = Configuration()
if conf.getValueStr("autocalibration") == "Yes":
if conf.getValueStr("automatrix") == "Yes":
self.calibrateWiiFromSettings()
else:
self.calibrateWiiScreen()
return
if self.wii.error:
self.wii = None
msgbox = QtWidgets.QMessageBox(self)
msgbox.setWindowTitle(self.tr('Error'))
msgbox.setText(self.tr("Error. Check your bluetooth driver"))
msgbox.setModal(True)
ret = msgbox.exec_()
pBar.close()
return
if pBar.choice:
if len(pool) == 1:
selectedMac = str(pool[0])
pBar.reInit(selectedMac)
else:
item, ok = QtWidgets.QInputDialog.getItem(
self, self.tr("Warning"), self.tr("Choose device"),
pool, 0, False)
if ok:
selectedMac = str(item)
pBar.reInit(selectedMac)
else:
pBar.close()
return
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)
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)
last_pressed = 0
DOUBLECLICK_THRESHOLD = 0.5
def button_pressed():
global last_pressed
# skip on doubleclick
if time() - last_pressed < DOUBLECLICK_THRESHOLD:
skipped()
last_pressed = time()
if __name__ == '__main__':
wiimote = Wiimote(button_callback=button_pressed)
wiimote.pair()
gait_analyzer = GaitAnalyzer(
step_frequency_observer=step_frequency_changed, skip_observer=skipped)
count = 0
SMOOTHING_WINDOW = 2
# array of SMOOTHING_WINDOW*2 + 1 values.
values = []
while True:
if IS_SMOOTHING:
values.append(wiimote.read_accelerometer())
# print values
if len(values) == SMOOTHING_WINDOW * 2 + 1:
# smooth the values and extract x, y, z
x = sum([x for x, y, z in values]) / len(values)
class LampApp(object):
NBPIXEL = 25
def __init__(self):
#logging
self.debug = False # if true exception aren't catched
self.log = logging.getLogger("LampApp")
# stdout (for self.print)
self._stdout_filemane = None
## events callbacks
self._setup_fct = None
self._to_spawn = []
def activate_lamp(self):
""" make lamp (led pixels) available
"""
self.lamp = LedPixels(self.NBPIXEL)
self.lamp.off()
def activate_wiimote(self):
""" make wiimote available
"""
from wiimote import Wiimote, WiimoteError
self.wiimote = Wiimote()
def check_reconnect():
connected = False
while True:
# connect the wiimote
if not self.wiimote.connected():
try:
print("connecting...")
self.wiimote.connect()
self.wait(0.4)
print("connected")
except WiimoteError:
print("connection fail")
self.wait(0.2)
else:
self.wait(0.4)
self._to_spawn.append(check_reconnect)
def _run_log(self, fn):
""" Run a fct and log exception (if any)
"""
error = None
try:
fn()
except Exception as err:
self.log.exception("uncaught exception:")
if self.debug:
raise
error = err
return error
def setup(self):
""" Function decorator to declare an function to be run at start up
"""
def setup_deco(fn):
self._setup_fct = fn
return fn
return setup_deco
def every(self, wait_time):
""" Function decorator to declare an function to be run every indicaded
"""
def every_deco(fn):
def run_every():
while True:
stime = time.time()
error = self._run_log(fn)
if error:
break
exec_time = time.time() - stime
# wait at least 20ms
self.wait(max(20e-3, wait_time-exec_time))
self._to_spawn.append(run_every)
return fn
return every_deco
def on(self, obj, event):
""" Function decorator to declare an function to be run on an event
"""
#TODO
def on_deco(fn):
return fn
return on_deco
def msg(self, msg):
""" print a message
"""
if self._stdout_filemane:
with open(self._stdout_filemane, "a") as stdout:
stdout.write("%s\n" % msg)
def on_exit(self):
if self.lamp:
self.lamp.turn_off()
sys.exit()
def wait(self, time):
gevent.sleep(time)
def run(self):
""" Run the lapp, ie:
* setup and parse args
* check if already running lapp with same pidfile
* setup logging (for exception, at least)
* setup output (self.msg)
* write the pidfile
and then :
* call self.setup()
* in a infinite loop, call self.loop()
"""
# clean all at exit
signal.signal(signal.SIGTERM, lambda signum, frame: self.on_exit())
# cmd line argument parser
parser = argparse.ArgumentParser(description='bb-lamp App.')
outdir = "./lapp_output/"
parser.add_argument(
'--logfile', dest='logfile', type=str, default=outdir+"/lapp.log",
help='path of the logging file'
)
parser.add_argument(
'--outfile', dest='outfile', type=str, default=outdir+"/lapp.out",
help='path for the output (print) file'
)
parser.add_argument(
'--pidfile', dest='pidfile', type=str, default=outdir+"/lapp.pid",
help='path of the PID file'
)
args = parser.parse_args()
## check PID file : only one lamp app at time
running_lapp = read_lapp_pidfile(args.pidfile)
if running_lapp is not None:
print("Sorry, a lapp is already running !")
for key, value in running_lapp.iteritems():
print("%s: %s" % (key, value))
raise SystemExit
## logging handler
self.log.setLevel(logging.DEBUG)
# create file handler which logs even debug messages
fhandler = logging.FileHandler(args.logfile)
fhandler.setLevel(logging.DEBUG)
# create formatter and add it to the handlers
formatter = logging.Formatter(
'%(name)s:%(asctime)s:%(levelname)s:%(message)s'
)
fhandler.setFormatter(formatter)
self.log.addHandler(fhandler)
try:
## configure hardware
self.activate_lamp()
## fill pid file
write_lapp_pidfile(args.pidfile)
## out file for self.msg
self._stdout_filemane = args.outfile
## run the lapp itself
setup_error = None
if self._setup_fct:
setup_error = self._run_log(self._setup_fct)
if not setup_error:
jobs = [gevent.spawn(fn) for fn in self._to_spawn]
gevent.joinall(jobs)
except Exception:
self.log.exception("uncaught exception:")
raise
finally:
#
pass
def connectWii(self):
if self.connected:
self.disconnectDevice()
return
self.wii = Wiimote()
pBar = PBarDlg(self)
pBar.setModal( True )
pBar.show()
conf = Configuration()
selectedMac = conf.getValueStr("selectedmac")
while 1:
thread = self.wii.createConnectThread(selectedMac)
thread.start()
while not thread.wait(30):
QtGui.QApplication.processEvents()
if pBar.cancelled == True:
if self.wii.isConnected():
self.wii.close()
self.wii = None
pBar.close()
return
if self.wii.isConnected():
self.connected = True
self.calibrated = False
self.active = False
self.updateButtons()
self.batteryLevel.setValue(self.wii.battery()*100)
self.pushButtonConnect.setText(self.tr("Disconnect"))
pBar.close()
self.confDialog.wii = self.wii
self.confDialog.checkButtons()
self.wii.disable()
self.wii.putCallbackBTN(self.makeBTNCallback())
self.wii.putCallbackIR(None)
self.wii.enable()
# Start calibration if configuration says so
conf = Configuration()
if conf.getValueStr("autocalibration") == "Yes":
if conf.getValueStr("automatrix") == "Yes":
self.calibrateWiiFromSettings()
else:
self.calibrateWiiScreen()
return
if self.wii.error:
self.wii = None
msgbox = QtGui.QMessageBox( self )
msgbox.setWindowTitle( self.tr('Error') )
msgbox.setText( self.tr("Error. Check your bluetooth driver") )
msgbox.setModal( True )
ret = msgbox.exec_()
pBar.close()
return
if not self.wii.isConnected() and len(self.wii.wiimotesDetected) > 1:
item, ok = QtGui.QInputDialog.getItem(self,
self.tr("Warning"), self.tr("Choose device"), self.wii.wiimotesDetected, 0, False)
if ok:
selectedMac = unicode(item)
def connectWii(self):
if self.connected:
self.disconnectDevice()
return
self.wii = Wiimote()
pBar = PBarDlg(self)
pBar.setModal( True )
pBar.show()
conf = Configuration()
selectedMac = conf.getValueStr("selectedmac")
pBar.reInit(selectedMac)
pool = []
while 1:
thread = self.wii.createConnectThread(selectedMac,pool)
thread.start()
while not thread.wait(30):
QtWidgets.QApplication.processEvents()
if pBar.cancelled == True:
if self.wii.isConnected():
self.wii.close()
self.wii = None
pBar.close()
return
if selectedMac == '*' and len(pool) >= 1:
if Configuration().getValueStr('nowaitdevices') == 'Yes':
selectedMac = pool[0]
else:
pBar.inform(self.tr('Found ') + str(len(pool)) + self.tr(' Devices. Press to Choose'))
if self.wii.isConnected():
self.connected = True
self.calibrated = False
self.active = False
self.updateButtons()
self.batteryLevel.setValue(self.wii.battery()*100)
self.pushButtonConnect.setText(self.tr("Disconnect"))
pBar.close()
self.confDialog.wii = self.wii
self.confDialog.checkButtons()
self.wii.disable()
self.wii.putCallbackBTN(self.makeBTNCallback())
self.wii.putCallbackIR(None)
self.wii.enable()
# Start calibration if configuration says so
conf = Configuration()
if conf.getValueStr("autocalibration") == "Yes":
if conf.getValueStr("automatrix") == "Yes":
self.calibrateWiiFromSettings()
else:
self.calibrateWiiScreen()
return
if self.wii.error:
self.wii = None
msgbox = QtWidgets.QMessageBox( self )
msgbox.setWindowTitle( self.tr('Error') )
msgbox.setText( self.tr("Error. Check your bluetooth driver") )
msgbox.setModal( True )
ret = msgbox.exec_()
pBar.close()
return
if pBar.choice:
if len(pool) == 1:
selectedMac = str(pool[0])
pBar.reInit(selectedMac)
else:
item, ok = QtWidgets.QInputDialog.getItem(self,
self.tr("Warning"), self.tr("Choose device"), pool, 0, False)
if ok:
selectedMac = str(item)
pBar.reInit(selectedMac)
else:
pBar.close()
return
class MainWindow(QtGui.QMainWindow):
def __init__(self, parent=None):
super(MainWindow, self).__init__(parent)
self.ui = uic.loadUi("mainwindow.ui",self)
self.setWindowTitle("python-whiteboard")
self.setWindowFlags( qt.Qt.CustomizeWindowHint | qt.Qt.WindowMinimizeButtonHint |
qt.Qt.WindowCloseButtonHint )
self.connected = False
self.calibrated = False
self.active = False
self.mustquit = False
self.wii = None
self.cursor = None
self.batteryLevel.reset()
self.batteryLevel.setRange(0,99)
self.batteryLevel.setValue(0)
conf = Configuration()
self.connect(self.ui.pushButtonConnect,
QtCore.SIGNAL("clicked()"), self.connectWii)
self.connect(self.ui.pushButtonCalibrate,
QtCore.SIGNAL("clicked()"), self.calibrateWiiScreen)
self.connect(self.ui.pushButtonActivate,
QtCore.SIGNAL("clicked()"), self.activateWii)
self.connect(self.ui.pushButtonLoadCal,
QtCore.SIGNAL("clicked()"), self.calibrateWiiFromSettings)
self.connect(self.ui.pushButtonSettings,
QtCore.SIGNAL("clicked()"), self.showHideSettings)
self.connect(self.ui.comboProfiles,
QtCore.SIGNAL("currentIndexChanged(int)"), self.changeProfile)
self.updateButtons()
self.connect(self.ui.actionQuit,
QtCore.SIGNAL("activated()"), self.mustQuit)
self.connect(self.ui.actionHelp,
QtCore.SIGNAL("activated()"), self.showAboutDlg)
self.connect(self.ui.actionNew_Profile,
QtCore.SIGNAL("activated()"), self.addProfile)
self.connect(self.ui.actionDelete_Current_Profile,
QtCore.SIGNAL("activated()"), self.delCurrentProfile)
self.ui.moveOnlyCheck.setChecked( conf.getValueStr('moveonly') == 'Yes' )
self.connect(self.ui.moveOnlyCheck,
QtCore.SIGNAL("stateChanged(int)"), self.checkMoveOnly)
if conf.getValueStr("autoconnect") == "Yes":
self.timer = qt.QTimer(self)
self.timer.setInterval(500)
self.connect(self.timer, QtCore.SIGNAL("timeout()"), self.autoConnect)
self.timer.start()
self.timer2 = qt.QTimer(self)
self.timer2.setInterval(4000)
self.connect(self.timer2, QtCore.SIGNAL("timeout()"), self.checkWii)
self.timer2.start()
self.confDialog = ConfigDialog(self, self.wii)
layout = QtGui.QGridLayout()
layout.addWidget(self.confDialog)
self.ui.confContainer.setLayout(layout)
self.ui.confContainer.setVisible(False)
self.refreshProfiles()
self.center()
def changeProfile(self,i):
conf = Configuration()
if i == 0:
conf.setGroup("default")
else:
g = unicode(self.ui.comboProfiles.currentText())
conf.setGroup(hashlib.md5(g.encode('utf-8')).hexdigest())
self.confDialog.refreshWidgets()
self.ui.moveOnlyCheck.setChecked( conf.getValueStr('moveonly') == 'Yes' )
def refreshProfiles(self):
conf = Configuration()
self.ui.comboProfiles.clear()
self.ui.comboProfiles.addItem(self.tr("default"))
for p in conf.getProfileList():
self.ui.comboProfiles.addItem(p)
self.confDialog.refreshWidgets()
self.ui.moveOnlyCheck.setChecked( conf.getValueStr('moveonly') == 'Yes' )
def addProfile(self):
profName, ok = QtGui.QInputDialog.getText(self,
self.tr("New Profile"), self.tr('Name:'))
profName = unicode(profName)
if ok and profName != '':
conf = Configuration()
profiles = conf.getProfileList()
for p in profiles:
if p == profName: return
profiles.append(profName)
conf.setProfileList(profiles)
self.refreshProfiles()
i = self.ui.comboProfiles.findText(profName)
self.ui.comboProfiles.setCurrentIndex(i)
def delCurrentProfile(self):
i = self.ui.comboProfiles.currentIndex()
currentProfile = unicode(self.ui.comboProfiles.currentText())
if i == 0: return
conf = Configuration()
profiles = conf.getProfileList()
profiles = [ p for p in profiles if p != currentProfile ]
conf.setProfileList(profiles)
self.refreshProfiles()
self.ui.comboProfiles.setCurrentIndex(0)
def showHideSettings(self):
self.ui.confContainer.setVisible(not self.ui.confContainer.isVisible())
QtGui.QApplication.processEvents()
if self.ui.confContainer.isVisible():
self.ui.pushButtonSettings.setText(self.tr('Hide settings'))
# Res¡ze to max
self.resize(1000,1000)
else:
self.ui.pushButtonSettings.setText(self.tr('Show settings'))
self.adjustSize()
def checkMoveOnly(self,i):
conf = Configuration()
if self.sender().isChecked():
conf.saveValue('moveonly','Yes')
if self.cursor:
self.cursor.noClicks = True
else:
conf.saveValue('moveonly','No')
if self.cursor:
self.cursor.noClicks = False
def showAboutDlg(self):
about = AboutDlg(self)
about.show()
about.exec_()
def checkWii(self):
if self.wii == None: return
if self.connected == False: return
if self.wii.checkStatus() == False:
# Deactivate cursor
self.deactivateWii()
# Deactivate device
self.connected = False
self.calibrated = False
self.active = False
self.pushButtonConnect.setText(self.tr("Connect"))
self.updateButtons()
self.ui.label_utilization.setText(self.tr("Utilization: 0%"))
self.clearScreenGraphic()
self.batteryLevel.setValue(0)
msgbox = QtGui.QMessageBox( self )
msgbox.setText( self.tr("Wii device disconnected") )
msgbox.setModal( True )
ret = msgbox.exec_()
return
self.batteryLevel.setValue(self.wii.battery()*100)
def autoConnect(self):
if self.isVisible():
self.timer.stop()
self.connectWii()
else:
self.timer.start()
def drawScreenGraphic(self):
max_x = self.wiiScreen.geometry().width()
max_y = self.wiiScreen.geometry().height()
self.scene = qt.QGraphicsScene()
self.scene.setSceneRect(0,0,max_x,max_y)
quad = QtGui.QPolygonF()
for p in self.wii.calibrationPoints:
x = max_x * p[0]/Wiimote.MAX_X
y = max_y * (1-float(p[1])/Wiimote.MAX_Y)
quad.append(qt.QPointF(x,y))
self.scene.addPolygon(quad)
self.wiiScreen.setScene(self.scene)
self.wiiScreen.show()
def clearScreenGraphic(self):
if self.wiiScreen.scene():
self.scene.clear()
def center(self):
screen = QtGui.QDesktopWidget().screenGeometry()
size = self.geometry()
self.move((screen.width()-size.width())/2, (screen.height()-size.height())/2)
def updateButtons(self):
if self.connected == False:
self.ui.pushButtonConnect.setEnabled(1)
self.ui.pushButtonCalibrate.setEnabled(0)
self.ui.pushButtonActivate.setEnabled(0)
self.ui.pushButtonLoadCal.setEnabled(0)
#self.ui.frame_mouseControl.setEnabled(1)
self.statusBar().showMessage("")
return
self.statusBar().showMessage(self.tr("Connected to ") + self.wii.addr)
if self.calibrated == False:
self.ui.pushButtonConnect.setEnabled(1)
self.ui.pushButtonCalibrate.setEnabled(1)
self.ui.pushButtonActivate.setEnabled(0)
self.ui.pushButtonLoadCal.setEnabled(1)
#self.ui.frame_mouseControl.setEnabled(1)
return
if self.active == False:
self.ui.pushButtonConnect.setEnabled(1)
self.ui.pushButtonCalibrate.setEnabled(1)
self.ui.pushButtonActivate.setEnabled(1)
self.ui.pushButtonLoadCal.setEnabled(1)
#self.ui.frame_mouseControl.setEnabled(1)
return
else:
self.ui.pushButtonConnect.setEnabled(0)
self.ui.pushButtonCalibrate.setEnabled(1)
self.ui.pushButtonLoadCal.setEnabled(0)
self.ui.pushButtonActivate.setEnabled(1)
#self.ui.frame_mouseControl.setEnabled(0)
def disconnectDevice(self):
if self.active:
if self.cursor:
self.cursor.finish()
self.active = False
if self.wii:
self.wii.disable()
self.wii.close()
self.wii = None
self.connected = False
self.calibrated = False
self.active = False
self.pushButtonConnect.setText(self.tr("Connect"))
self.updateButtons()
self.ui.label_utilization.setText(self.tr("Utilization: 0%"))
self.clearScreenGraphic()
self.batteryLevel.setValue(0)
self.confDialog.wii = None
self.confDialog.checkButtons()
return
def makeBTNCallback(self):
def func():
# Simulate click to calibrate button
self.ui.pushButtonCalibrate.click()
return func
def connectWii(self):
if self.connected:
self.disconnectDevice()
return
self.wii = Wiimote()
pBar = PBarDlg(self)
pBar.setModal( True )
pBar.show()
conf = Configuration()
selectedMac = conf.getValueStr("selectedmac")
while 1:
thread = self.wii.createConnectThread(selectedMac)
thread.start()
while not thread.wait(30):
QtGui.QApplication.processEvents()
if pBar.cancelled == True:
if self.wii.isConnected():
self.wii.close()
self.wii = None
pBar.close()
return
if self.wii.isConnected():
self.connected = True
self.calibrated = False
self.active = False
self.updateButtons()
self.batteryLevel.setValue(self.wii.battery()*100)
self.pushButtonConnect.setText(self.tr("Disconnect"))
pBar.close()
self.confDialog.wii = self.wii
self.confDialog.checkButtons()
self.wii.disable()
self.wii.putCallbackBTN(self.makeBTNCallback())
self.wii.putCallbackIR(None)
self.wii.enable()
# Start calibration if configuration says so
conf = Configuration()
if conf.getValueStr("autocalibration") == "Yes":
if conf.getValueStr("automatrix") == "Yes":
self.calibrateWiiFromSettings()
else:
self.calibrateWiiScreen()
return
if self.wii.error:
self.wii = None
msgbox = QtGui.QMessageBox( self )
msgbox.setWindowTitle( self.tr('Error') )
msgbox.setText( self.tr("Error. Check your bluetooth driver") )
msgbox.setModal( True )
ret = msgbox.exec_()
pBar.close()
return
if not self.wii.isConnected() and len(self.wii.wiimotesDetected) > 1:
item, ok = QtGui.QInputDialog.getItem(self,
self.tr("Warning"), self.tr("Choose device"), self.wii.wiimotesDetected, 0, False)
if ok:
selectedMac = unicode(item)
# doscreen: if doscreen is true, calibrate by manual pointing
def calibrateWii(self,loadFromSettings=False):
self.deactivateWii()
self.ui.label_utilization.setText(self.tr("Utilization: 0%"))
self.clearScreenGraphic()
self.calibrated = False
self.active = False
self.wii.state = Wiimote.NONCALIBRATED
if loadFromSettings:
# If calibration matrix can't be loaded, calibrate manually
if not self.loadCalibration(self.wii):
doCalibration(self,self.wii)
else:
doCalibration(self,self.wii)
if self.wii.state == Wiimote.CALIBRATED:
self.calibrated = True
self.active = False
self.drawScreenGraphic()
self.updateButtons()
self.ui.label_utilization.setText(self.tr("Utilization: ") + "%d%%" % (100.0*self.wii.utilization))
self.saveCalibrationPars(self.wii)
# Activate cursor after calibration (always)
self.activateWii()
else:
self.updateButtons()
msgbox = QtGui.QMessageBox( self )
msgbox.setText( self.tr("Error during Calibration") )
msgbox.setModal( True )
ret = msgbox.exec_()
# Installs button callback (for calling calibration)
self.wii.disable()
self.wii.putCallbackBTN(self.makeBTNCallback())
self.wii.putCallbackIR(None)
self.wii.enable()
def calibrateWiiScreen(self):
self.calibrateWii()
def calibrateWiiFromSettings(self):
self.calibrateWii(loadFromSettings=True)
def saveCalibrationPars(self,wii):
conf = Configuration()
for i,p in enumerate(wii.screenPoints):
conf.saveValue("screenPoint"+str(i)+"x",str(p[0]))
conf.saveValue("screenPoint"+str(i)+"y",str(p[1]))
for i,p in enumerate(wii.calibrationPoints):
conf.saveValue("wiiPoint"+str(i)+"x",str(p[0]))
conf.saveValue("wiiPoint"+str(i)+"y",str(p[1]))
def loadCalibration(self,wii):
try:
conf = Configuration()
pwii = []
pscr = []
for i in range(0,4):
p = []
p.append(float(conf.getValueStr("screenPoint"+str(i)+"x")))
p.append(float(conf.getValueStr("screenPoint"+str(i)+"y")))
q = []
q.append(float(conf.getValueStr("wiiPoint"+str(i)+"x")))
q.append(float(conf.getValueStr("wiiPoint"+str(i)+"y")))
pwii.append(list(q))
pscr.append(list(p))
wii.calibrate(pscr,pwii)
return True
except:
return False
def deactivateWii(self):
if self.active:
self.cursor.finish()
self.active = False
self.pushButtonActivate.setText(self.tr("Activate"))
self.updateButtons()
def activateWii(self):
if self.active:
# Deactivate
self.deactivateWii()
else:
# Activate
self.cursor = FakeCursor(self.wii)
if self.ui.moveOnlyCheck.isChecked():
self.cursor.noClicks = True
# Installs button callback (for calling calibration)
self.wii.disable()
self.wii.putCallbackBTN(self.makeBTNCallback())
conf = Configuration()
zones = [ conf.getValueStr(z) for z in ("zone1","zone2","zone3","zone4") ]
self.cursor.setZones(zones)
self.cursor.runThread()
self.active = True
self.pushButtonActivate.setText(self.tr("Deactivate"))
self.updateButtons()
# Exit callback
def closeEvent(self,e):
if self.mustquit:
self.disconnectDevice()
e.accept()
else:
msgbox = QtGui.QMessageBox(self)
msgbox.setText(self.tr("The application will remain active (systray).") + "\n" + \
self.tr("To quit, use file->quit menu") )
msgbox.setModal( True )
ret = msgbox.exec_()
self.showHide()
e.ignore()
def showHide(self):
if self.isVisible():
self.hide()
else:
self.show()
def mustQuit(self):
self.mustquit = True
self.close()
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
class MainWindow(QtWidgets.QMainWindow):
def __init__(self, parent=None):
super(MainWindow, self).__init__(parent)
self.ui = uic.loadUi("mainwindow.ui", self)
self.setWindowTitle("python-whiteboard")
self.setWindowFlags(qt.Qt.CustomizeWindowHint
| qt.Qt.WindowMinimizeButtonHint
| qt.Qt.WindowCloseButtonHint)
self.connected = False
self.calibrated = False
self.active = False
self.mustquit = False
self.wii = None
self.cursor = None
self.batteryLevel.reset()
self.batteryLevel.setRange(0, 99)
self.batteryLevel.setValue(0)
conf = Configuration()
self.ui.pushButtonConnect.clicked.connect(self.connectWii)
self.ui.pushButtonCalibrate.clicked.connect(self.calibrateWiiScreen)
self.ui.pushButtonActivate.clicked.connect(self.activateWii)
self.ui.pushButtonLoadCal.clicked.connect(
self.calibrateWiiFromSettings)
self.ui.pushButtonSettings.clicked.connect(self.showHideSettings)
self.ui.comboProfiles.currentIndexChanged.connect(self.changeProfile)
self.updateButtons()
self.ui.actionQuit.triggered.connect(self.mustQuit)
self.ui.actionHelp.triggered.connect(self.showAboutDlg)
self.ui.actionNew_Profile.triggered.connect(self.addProfile)
self.ui.actionDelete_Current_Profile.triggered.connect(
self.delCurrentProfile)
self.ui.actionWipe_configuration.triggered.connect(
self.wipeConfiguration)
self.ui.moveOnlyCheck.setChecked(conf.getValueStr('moveonly') == 'Yes')
self.ui.moveOnlyCheck.stateChanged.connect(self.checkMoveOnly)
if conf.getValueStr("autoconnect") == "Yes":
self.timer = qt.QTimer(self)
self.timer.setInterval(500)
self.timer.timeout.connect(self.autoConnect)
self.timer.start()
self.timer2 = qt.QTimer(self)
self.timer2.setInterval(4000)
self.timer2.timeout.connect(self.checkWii)
self.timer2.start()
self.confDialog = ConfigDialog(self, self.wii)
layout = QtWidgets.QGridLayout()
layout.addWidget(self.confDialog)
self.ui.confContainer.setLayout(layout)
self.ui.confContainer.setVisible(False)
self.refreshProfiles()
self.center()
def changeProfile(self, i):
conf = Configuration()
if i == 0:
conf.setGroup("default")
else:
g = str(self.ui.comboProfiles.currentText())
conf.setGroup(hashlib.md5(g.encode('utf-8')).hexdigest())
self.confDialog.refreshWidgets()
self.ui.moveOnlyCheck.setChecked(conf.getValueStr('moveonly') == 'Yes')
def refreshProfiles(self):
conf = Configuration()
self.ui.comboProfiles.clear()
self.ui.comboProfiles.addItem(self.tr("default"))
for p in conf.getProfileList():
self.ui.comboProfiles.addItem(p)
self.confDialog.refreshWidgets()
self.ui.moveOnlyCheck.setChecked(conf.getValueStr('moveonly') == 'Yes')
def addProfile(self):
profName, ok = QtWidgets.QInputDialog.getText(self,
self.tr("New Profile"),
self.tr('Name:'))
profName = str(profName)
if ok and profName != '':
conf = Configuration()
profiles = conf.getProfileList()
for p in profiles:
if p == profName: return
profiles.append(profName)
conf.setProfileList(profiles)
self.refreshProfiles()
i = self.ui.comboProfiles.findText(profName)
self.ui.comboProfiles.setCurrentIndex(i)
def delCurrentProfile(self):
i = self.ui.comboProfiles.currentIndex()
currentProfile = str(self.ui.comboProfiles.currentText())
if i == 0: return
conf = Configuration()
profiles = conf.getProfileList()
profiles = [p for p in profiles if p != currentProfile]
conf.setProfileList(profiles)
self.refreshProfiles()
self.ui.comboProfiles.setCurrentIndex(0)
def wipeConfiguration(self):
conf = Configuration()
conf.wipe()
msgbox = QtWidgets.QMessageBox(self)
msgbox.setText(
self.tr("The application will close. Please restart manually"))
msgbox.setModal(True)
ret = msgbox.exec_()
self.mustQuit()
def showHideSettings(self):
self.ui.confContainer.setVisible(not self.ui.confContainer.isVisible())
QtWidgets.QApplication.processEvents()
if self.ui.confContainer.isVisible():
self.ui.pushButtonSettings.setText(self.tr('Hide settings'))
# Res¡ze to max
self.resize(1000, 1000)
else:
self.ui.pushButtonSettings.setText(self.tr('Show settings'))
self.adjustSize()
def checkMoveOnly(self, i):
conf = Configuration()
if self.sender().isChecked():
conf.saveValue('moveonly', 'Yes')
if self.cursor:
self.cursor.noClicks = True
else:
conf.saveValue('moveonly', 'No')
if self.cursor:
self.cursor.noClicks = False
def showAboutDlg(self):
about = AboutDlg(self)
about.show()
about.exec_()
def checkWii(self):
if self.wii == None: return
if self.connected == False: return
if self.wii.checkStatus() == False:
# Deactivate cursor
self.deactivateWii()
# Deactivate device
self.connected = False
self.calibrated = False
self.active = False
self.pushButtonConnect.setText(self.tr("Connect"))
self.updateButtons()
self.ui.label_utilization.setText(self.tr("Utilization: 0%"))
self.clearScreenGraphic()
self.batteryLevel.setValue(0)
msgbox = QtWidgets.QMessageBox(self)
msgbox.setText(self.tr("Wii device disconnected"))
msgbox.setModal(True)
ret = msgbox.exec_()
return
self.batteryLevel.setValue(self.wii.battery() * 100)
def autoConnect(self):
if self.isVisible():
self.timer.stop()
self.connectWii()
else:
self.timer.start()
def drawScreenGraphic(self):
max_x = self.wiiScreen.geometry().width()
max_y = self.wiiScreen.geometry().height()
self.scene = qt.QGraphicsScene()
self.scene.setSceneRect(0, 0, max_x, max_y)
quad = QtGui.QPolygonF()
for p in self.wii.calibrationPoints:
x = max_x * p[0] / Wiimote.MAX_X
y = max_y * (1 - old_div(float(p[1]), Wiimote.MAX_Y))
quad.append(qt.QPointF(x, y))
self.scene.addPolygon(quad)
self.wiiScreen.setScene(self.scene)
self.wiiScreen.show()
def clearScreenGraphic(self):
if self.wiiScreen.scene():
self.scene.clear()
def center(self):
screen = QtWidgets.QDesktopWidget().screenGeometry()
size = self.geometry()
self.move(old_div((screen.width() - size.width()), 2),
old_div((screen.height() - size.height()), 2))
def updateButtons(self):
if self.connected == False:
self.ui.pushButtonConnect.setEnabled(1)
self.ui.pushButtonCalibrate.setEnabled(0)
self.ui.pushButtonActivate.setEnabled(0)
self.ui.pushButtonLoadCal.setEnabled(0)
#self.ui.frame_mouseControl.setEnabled(1)
self.statusBar().showMessage("")
return
self.statusBar().showMessage(self.tr("Connected to ") + self.wii.addr)
if self.calibrated == False:
self.ui.pushButtonConnect.setEnabled(1)
self.ui.pushButtonCalibrate.setEnabled(1)
self.ui.pushButtonActivate.setEnabled(0)
self.ui.pushButtonLoadCal.setEnabled(1)
#self.ui.frame_mouseControl.setEnabled(1)
return
if self.active == False:
self.ui.pushButtonConnect.setEnabled(1)
self.ui.pushButtonCalibrate.setEnabled(1)
self.ui.pushButtonActivate.setEnabled(1)
self.ui.pushButtonLoadCal.setEnabled(1)
#self.ui.frame_mouseControl.setEnabled(1)
return
else:
self.ui.pushButtonConnect.setEnabled(0)
self.ui.pushButtonCalibrate.setEnabled(1)
self.ui.pushButtonLoadCal.setEnabled(0)
self.ui.pushButtonActivate.setEnabled(1)
#self.ui.frame_mouseControl.setEnabled(0)
def disconnectDevice(self):
if self.active:
if self.cursor:
self.cursor.finish()
self.active = False
if self.wii:
self.wii.disable()
self.wii.close()
self.wii = None
self.connected = False
self.calibrated = False
self.active = False
self.pushButtonConnect.setText(self.tr("Connect"))
self.updateButtons()
self.ui.label_utilization.setText(self.tr("Utilization: 0%"))
self.clearScreenGraphic()
self.batteryLevel.setValue(0)
self.confDialog.wii = None
self.confDialog.checkButtons()
return
def makeBTNCallback(self):
def func():
# Simulate click to calibrate button
self.ui.pushButtonCalibrate.click()
return func
def connectWii(self):
if self.connected:
self.disconnectDevice()
return
self.wii = Wiimote()
pBar = PBarDlg(self)
pBar.setModal(True)
pBar.show()
conf = Configuration()
selectedMac = conf.getValueStr("selectedmac")
pBar.reInit(selectedMac)
pool = []
while 1:
thread = self.wii.createConnectThread(selectedMac, pool)
thread.start()
while not thread.wait(30):
QtWidgets.QApplication.processEvents()
if pBar.cancelled == True:
if self.wii.isConnected():
self.wii.close()
self.wii = None
pBar.close()
return
if selectedMac == '*' and len(pool) >= 1:
if Configuration().getValueStr('nowaitdevices') == 'Yes':
selectedMac = pool[0]
else:
pBar.inform(
self.tr('Found ') + str(len(pool)) +
self.tr(' Devices. Press to Choose'))
if self.wii.isConnected():
self.connected = True
self.calibrated = False
self.active = False
self.updateButtons()
self.batteryLevel.setValue(self.wii.battery() * 100)
self.pushButtonConnect.setText(self.tr("Disconnect"))
pBar.close()
self.confDialog.wii = self.wii
self.confDialog.checkButtons()
self.wii.disable()
self.wii.putCallbackBTN(self.makeBTNCallback())
self.wii.putCallbackIR(None)
self.wii.enable()
# Start calibration if configuration says so
conf = Configuration()
if conf.getValueStr("autocalibration") == "Yes":
if conf.getValueStr("automatrix") == "Yes":
self.calibrateWiiFromSettings()
else:
self.calibrateWiiScreen()
return
if self.wii.error:
self.wii = None
msgbox = QtWidgets.QMessageBox(self)
msgbox.setWindowTitle(self.tr('Error'))
msgbox.setText(self.tr("Error. Check your bluetooth driver"))
msgbox.setModal(True)
ret = msgbox.exec_()
pBar.close()
return
if pBar.choice:
if len(pool) == 1:
selectedMac = str(pool[0])
pBar.reInit(selectedMac)
else:
item, ok = QtWidgets.QInputDialog.getItem(
self, self.tr("Warning"), self.tr("Choose device"),
pool, 0, False)
if ok:
selectedMac = str(item)
pBar.reInit(selectedMac)
else:
pBar.close()
return
# doscreen: if doscreen is true, calibrate by manual pointing
def calibrateWii(self, loadFromSettings=False):
self.deactivateWii()
self.ui.label_utilization.setText(self.tr("Utilization: 0%"))
self.clearScreenGraphic()
self.calibrated = False
self.active = False
try:
self.wii.state = Wiimote.NONCALIBRATED
if loadFromSettings:
# If calibration matrix can't be loaded, calibrate manually
if not self.loadCalibration(self.wii):
doCalibration(self, self.wii)
else:
doCalibration(self, self.wii)
if self.wii.state == Wiimote.CALIBRATED:
self.calibrated = True
self.active = False
self.drawScreenGraphic()
self.updateButtons()
self.ui.label_utilization.setText(
self.tr("Utilization: ") + "%d%%" %
(100.0 * self.wii.utilization))
self.saveCalibrationPars(self.wii)
# Activate cursor after calibration (always)
self.activateWii()
return
except CalibrationAbort:
# Do nothing (user choice)
pass
except:
print("Error during Calibration")
traceback.print_exc(file=sys.stdout)
self.updateButtons()
msgbox = QtWidgets.QMessageBox(self)
msgbox.setText(self.tr("Error during Calibration"))
msgbox.setModal(True)
ret = msgbox.exec_()
# Installs button callback (for calling calibration)
self.wii.disable()
self.wii.putCallbackBTN(self.makeBTNCallback())
self.wii.putCallbackIR(None)
self.wii.enable()
def calibrateWiiScreen(self):
self.calibrateWii()
def calibrateWiiFromSettings(self):
self.calibrateWii(loadFromSettings=True)
def saveCalibrationPars(self, wii):
conf = Configuration()
for i, p in enumerate(wii.screenPoints):
conf.saveValue("screenPoint" + str(i) + "x", str(p[0]))
conf.saveValue("screenPoint" + str(i) + "y", str(p[1]))
for i, p in enumerate(wii.calibrationPoints):
conf.saveValue("wiiPoint" + str(i) + "x", str(p[0]))
conf.saveValue("wiiPoint" + str(i) + "y", str(p[1]))
def loadCalibration(self, wii):
try:
conf = Configuration()
pwii = []
pscr = []
for i in range(0, 4):
p = []
p.append(float(conf.getValueStr("screenPoint" + str(i) + "x")))
p.append(float(conf.getValueStr("screenPoint" + str(i) + "y")))
q = []
q.append(float(conf.getValueStr("wiiPoint" + str(i) + "x")))
q.append(float(conf.getValueStr("wiiPoint" + str(i) + "y")))
pwii.append(list(q))
pscr.append(list(p))
wii.calibrate(pscr, pwii)
return True
except:
return False
def deactivateWii(self):
if self.active:
self.cursor.finish()
self.active = False
self.pushButtonActivate.setText(self.tr("Activate"))
self.updateButtons()
def activateWii(self):
if self.active:
# Deactivate
self.deactivateWii()
else:
# Activate
self.cursor = FakeCursor(self.wii)
if self.ui.moveOnlyCheck.isChecked():
self.cursor.noClicks = True
# Installs button callback (for calling calibration)
self.wii.disable()
self.wii.putCallbackBTN(self.makeBTNCallback())
conf = Configuration()
zones = [
conf.getValueStr(z)
for z in ("zone1", "zone2", "zone3", "zone4")
]
self.cursor.setZones(zones)
self.cursor.runThread()
self.active = True
self.pushButtonActivate.setText(self.tr("Deactivate"))
self.updateButtons()
# Exit callback
def closeEvent(self, e):
# Unity does not support qt systray anymore.
# So, I'm putting the old code on hold
#if self.mustquit:
#self.disconnectDevice()
#e.accept()
#else:
#msgbox = QtWidgets.QMessageBox(self)
#msgbox.setText(self.tr("The application will remain active (systray).") + "\n" + \
#self.tr("To quit, use file->quit menu") )
#msgbox.setModal( True )
#ret = msgbox.exec_()
#self.showHide()
#e.ignore()
# Instead, we simply ask if the user wants to really quit.
msgbox = QtWidgets.QMessageBox(self)
msgbox.setText(self.tr("Are you sure you want to exit?"))
msgbox.setStandardButtons(QtWidgets.QMessageBox.Ok
| QtWidgets.QMessageBox.Cancel)
msgbox.setModal(True)
ret = msgbox.exec_()
if ret == QtWidgets.QMessageBox.Ok:
# Exit the application
self.disconnectDevice()
e.accept()
else:
e.ignore()
def showHide(self):
if self.isVisible():
self.hide()
else:
self.show()
def mustQuit(self):
self.mustquit = True
self.close()
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
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
def step_frequency_changed(new_frequency):
print "Step frequency is now %d SPM." % new_frequency
last_pressed = 0
DOUBLECLICK_THRESHOLD = 0.5
def button_pressed():
global last_pressed
# skip on doubleclick
if time() - last_pressed < DOUBLECLICK_THRESHOLD:
skipped()
last_pressed = time()
if __name__ == '__main__':
wiimote = Wiimote(button_callback=button_pressed)
wiimote.pair()
gait_analyzer = GaitAnalyzer(step_frequency_observer=step_frequency_changed, skip_observer=skipped)
count = 0
SMOOTHING_WINDOW = 2
# array of SMOOTHING_WINDOW*2 + 1 values.
values = []
while True:
if IS_SMOOTHING:
values.append(wiimote.read_accelerometer())
# print values
if len(values) == SMOOTHING_WINDOW * 2 + 1:
# smooth the values and extract x, y, z
x = sum([x for x,y,z in values]) / len(values)
y = sum([y for x,y,z in values]) / len(values)
def main():
wi = Wiimote()
def cb_A():
print("RELEASED A !")
wi.on(BTN_A, RELEASED, cb_A)
import time
while True:
# connect the wiimote
if not wi.connected():
try:
print("connecting...")
wi.connect()
time.sleep(0.4)
print("connected")
except WiimoteError:
print("connection fail")
time.sleep(0.2)
else:
time.sleep(0.4)
exit = False
while not exit:
c = sys.stdin.read(1)
if c == "s":
print wi._wm.state
elif c == "a":
print wi.is_pressed(BTN_A)
exit = c == "x"
wi.close()
start_pin,
GPIO.FALLING,
callback=start_wiimote_callback,
bouncetime=300
)
# Initiate the drivetrain
drive = drivetrain.DriveTrain(pwm_i2c=pwm_address)
# Initiate the wiimote connection
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 (
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)
from wiimote import Wiimote
import cwiid
import time
button_delay = 1
wii = Wiimote()
wii.connect_wiimote()
wii.connection_fun()
while True:
buttons = wii.wiimote.state['buttons']
# If Plus and Minus buttons pressed
# together then rumble and quit.
if (buttons - cwiid.BTN_PLUS - cwiid.BTN_MINUS == 0):
print '\nClosing connection ...'
exit(wii)
# Check if other buttons are pressed by
# doing a bitwise AND of the buttons number
# and the predefined constant for that button.
if (buttons & cwiid.BTN_LEFT):
print 'Left pressed'
time.sleep(button_delay)
if(buttons & cwiid.BTN_RIGHT):
print 'Right pressed'
time.sleep(button_delay)
def main_loop():
# Get colors based on controller roll
colors = get_colors()
# Get dim modifier based on controller pitch
dim_modifier = get_dim()
set_color([(color * dim_modifier) for color in colors])
# Snd data to dmx
for light in lights:
change_data(light.start_address, light.data)
send_dmx()
def init_lights(amount):
for i in xrange(amount):
lights.append(LED(i * 4, i * 4 + 3))
if __name__ == '__main__':
if len(sys.argv) != 2:
print "Usage: main.py <amount of lights>"
sys.exit(1)
wiimote = Wiimote()
init_lights(int(sys.argv[1]))
while True:
main_loop()
class Wiimote(BBLampHardware):
BTNS = [
(cwiid.BTN_2, "2"), # 1
(cwiid.BTN_1, "1"), # 2
(cwiid.BTN_B, "B"), # 4
(cwiid.BTN_A, "A"), # 8
(cwiid.BTN_MINUS, "MINUS"), # 16
(cwiid.BTN_HOME, "HOME"), # 128
(cwiid.BTN_LEFT, "LEFT"), # 256
(cwiid.BTN_RIGHT, "RIGHT"), # 512
(cwiid.BTN_DOWN, "DOWN"), # 1024
(cwiid.BTN_UP, "UP"), # 2048
(cwiid.BTN_PLUS, "PLUS"), # 4096
]
def __init__(self):
super(BBLampHardware, self).__init__()
self._connected = False
self._wm = None
self._led = 0
self._rpt_mode = cwiid.RPT_BTN # by default read only btns
self._last_btn = 0
# init callbacks
self._on = {} # self._on[BTN][PRESSED]
def activate(self):
self.connect()
#TODO: code qui était dans lampapp
def activate_wiimote(self):
""" make wiimote available
"""
from wiimote import Wiimote, WiimoteError
self.wiimote = Wiimote()
def check_reconnect():
connected = False
while True:
# connect the wiimote
if not self.wiimote.connected():
try:
self.log.info("Connecting to WiiMote...")
self.wiimote.connect()
self.wait(0.4)
self.log.info("Connected to WiiMote !")
except WiimoteError:
self.log.info("Connection to WiiMote failed !")
self.wait(0.2)
else:
self.wait(0.4)
self._to_spawn.append(check_reconnect)
def connect(self):
#TODO: add msg throw log ?
try:
self._wm = cwiid.Wiimote()
self._wm.mesg_callback = self._callback
# enable callbacks and (re)set wiimote set
self._wm.enable(cwiid.FLAG_MESG_IFC)
self._wm.rpt_mode = self._rpt_mode
self._wm.led = self._led
self._connected = True
except RuntimeError, ValueError:
# connection fail
raise WiimoteError()
