Autonomous Rover and Wiimote Control for ExplorerHat

rover.py

import explorerhat, time, random
import RPi.GPIO as GPIO
import cwiid

mode  = 0
back  = explorerhat.analog.three
front = explorerhat.analog.four
speed = 50
wii_connected = False

def connect_wii():

    global wii_connected
    global wii

    print "Wii not connected please connect now (Press 1+2)"
    time.sleep(1)
    try:
        wii=cwiid.Wiimote()
    except RuntimeError:
        print "No Wii found.. try reconnect again.."
        return False

    print "Wii found.."
    wii.rumble = True
    time.sleep(0.5)
    wii.rumble = False
    wii_connected = True
    time.sleep(3)
    wii.rpt_mode = cwiid.RPT_BTN

def distance(volts):
    distance = 27.242 * pow(volts, -1.1904)
    return distance

def left():
    explorerhat.motor.one.forward()
    explorerhat.motor.two.backward()

def right():
    explorerhat.motor.two.forward()
    explorerhat.motor.one.backward()

def read_command():
    states = ""
    buttons = wii.state['buttons']
    if buttons & cwiid.BTN_A:
        states = "A"
    if buttons & cwiid.BTN_B:
        states = "B"
    if buttons & cwiid.BTN_LEFT:
        states = "L"
    if buttons & cwiid.BTN_RIGHT:
        states= "R"
    if buttons & cwiid.BTN_UP:
        states = "U"
    if buttons & cwiid.BTN_DOWN:
        states = "D"
    if buttons & cwiid.BTN_HOME:
        states = "HOME"
    return states

def wiimode():
    if(wii_connected is False):
        connect_wii()
    if(wii_connected is True):
        if (read_command() == "L"):
            right()
        if (read_command() == "R"):
            left()
        if (read_command() == "U"):
            explorerhat.motor.forward(speed)
        if (read_command() == "D"):
            explorerhat.motor.backwards(speed)
        time.sleep(0.25)
        explorerhat.motor.stop()

def autonomous():
    direction = [left, right]
    explorerhat.motor.forwards(speed)
    if(distance(front.read()) < 50):
        explorerhat.motor.stop()
        while(distance(front.read()) < 50):
            if(distance(back.read()) > 50):
                explorerhat.motor.backwards(speed)
            else:
                break
        explorerhat.motor.stop()
        random.choice(direction)()
        time.sleep(0.2)
    time.sleep(0.1)

def change_mode(channel, event):
    explorerhat.motor.stop()
    if channel > 4:
        return
    if event == 'press':
        explorerhat.motor.stop()
        explorerhat.light.off()
        global mode
        if (channel == mode):
            mode = 0
        else:
            mode = channel
            explorerhat.light[channel-1].on()

explorerhat.touch.pressed(change_mode)

try:
    while True:
        if mode == 2:
            autonomous()
        if mode == 3:
            wiimode()

## Catches control-c.

except KeyboardInterrupt:
        pass

## Catches any other exceptions.

except Exception:
        pass

## Clean up GPIO before exit.

finally:
        GPIO.cleanup()