AverageBot Final Code

#! /usr/bin/python
# Code explanation & breakdown: http://www.averagemanvsraspberrypi.com/2015/12/averagebot-devblog-10.html
# PiWars 2015 python code for 'AverageBot'
# Written by @AverageManVsPi - Lots of code borrowed from the 4Tronix PiRoCon libraries
# This robot used a Model A Raspberry Pi and 4Tronix PiRoCon motor controller (http://4tronix.co.uk/store/index.php?rt=product/product&product_id=182)
# See the full development blog series at http://www.AverageManVsRaspberryPi.com/tag/pi-wars

#======================================================================
# 8x PIROCON GPIO BLOCK AVAILABILITY (RPI-GPIO NOT BCM!)

    # 7  *** FREE ***
    # 11 [USED BY RIGHT LINE SENSOR]
    # 12 [USED BY MIDDLE LINE SENSOR]
    # 13 [USED BY LEFT LINE SENSOR]
    # 15 [USED BY SERVO 1]
    # 16 [USED BY SERVO 2]
    # 18 [USED BY SWITCH 2]
    # 22 [USED BY SWITCH 1]

print "- - - - - SCRIPT STARTED"
print "- - - - - - - - - - - - "

print ""


#======================================================================
# IMPORTS

print "- - - - - START IMPORTS"

# IMPORT OS FOR PYGAME SCREEN FRIG
import os
print "imported os"

# IMPORT SBMUS FOR 4TRONIX DISPLAY BOARD
import smbus
print "imported smbus"

# IMPORT GPIO FOR MOTOR CONTROLLER
import RPi.GPIO as GPIO
print "imported GPIO"

# IMPORT SUBPROCESS FOR OS. COMMANDS
import subprocess
print "imported subprocess"

# IMPORT PYGAME FOR 2.4GHZ KEYBOARD CONTROL
import pygame
print "imported pygame"

# IMPORT TIME FOR THE PAUSES IN OUR CODE
import time
print "imported time"

# IMPORT SYS FOR MOTOR CONTROLLER CODE
import sys
print "imported sys"

# IMPORT THREADING FOR MOTOR CONTROLLER CODE
import threading
print "imported threading"

print ""


#======================================================================
# SET GPIO MODE

print "- - - - - START GPIO SETMODE"
time.sleep(0.5)

GPIO.setmode(GPIO.BOARD) # SET GPIO MODE TO BOARD
print "GPIO setmode complete"

print ""


#======================================================================
# PYGAME SETUP

print "- - - - - START PYGAME SETUP"
time.sleep(0.5)

os.environ["SDL_VIDEODRIVER"] = "dummy" # SET UP FAKE VIDEO DRIVER FOR PYGAME
print "PyGame - fake video driver set up"

pygame.init() # INITIALISE PYGAME
print "PyGame - initialised"

# SET UP FAKE SCREEN SETTINGS FOR PYGAME
width = 320
height = 240
display = pygame.display.set_mode((width,height))
print "PyGame - fake screen settings complete"

print ""


#======================================================================
# SET UP SERVOS

print "- - - - - START SERVO SETUP"
time.sleep(0.5)

# SET UP GPIO NUMBERS
servo1 = 15
servo2 = 16

# SET GPIO DIRECTIONS FOR SERVOS (OUT)
GPIO.setup(servo1, GPIO.OUT) # SET SERVO1 AS A GPIO OUTPUT
GPIO.setup(servo2, GPIO.OUT) # SET SERVO2 AS A GPIO OUTPUT
print "GPIO set up"

servo1 = GPIO.PWM(servo1,50) # SERVO1 PWM FREQUENCY SET TO 5OHZ
servo2 = GPIO.PWM(servo2,50) # SERVO2 PWM FREQUENCY SET TO 5OHZ
time.sleep(0.5) # BREAK TO ALLOW SERVOS TO SET
print "Servo frequency set"

# SET INITIAL SERVO POSITION
servo1.start(4) # SET SERVO 1 INTO INITIAL POSITION
servo2.start(11) # SET SERVO 2 INTO INITIAL POSITION
print "Servos set up"

print ""


#======================================================================
# SET UP SWITCHES

print "- - - - - START SWITCH SETUP"
time.sleep(0.5)

# SET UP GPIO NUMBERS
switch1 = 22
switch2 = 18
print "Switch GPIO numbers set"

# SET GPIO DIRECTIONS FOR SWITCHES (IN)
GPIO.setup(switch1,GPIO.IN, pull_up_down=GPIO.PUD_DOWN)
GPIO.setup(switch2,GPIO.IN, pull_up_down=GPIO.PUD_DOWN)
print "Swich directions set to inputs"

print ""


#======================================================================
# SET UP SMBUS FOR 4TRONIX DISPLAY BOARD

print "- - - - - START SMBUS SETUP"
time.sleep(0.5)

# SET SMBUS NUMBER
bus = smbus.SMBus(1) # 0 for Revision 1 Pi
print "smbus ready"

print ""


#======================================================================
# SET UP 4TRONIX DISPLAY BOARD

print "- - - - - START DISPLAY BOARD SETUP"
time.sleep(0.5)

# SET UP CHARACTERS FOR DISPLAY BOARD (HEX CODES)
N0 = 0x3F
N1 = 0x06
N2 = 0x5B
N3 = 0x4F
N4 = 0x66
N5 = 0x6D
N6 = 0x7D
N7 = 0x07
N8 = 0x7F
N9 = 0x6F
dash = 0x40
underscore = 0x08
print "characters ready"

# SET UP MCP23017 FOR DISPLAY BOARD
addr = 0x20 # I2C address of MCP23017
bus.write_byte_data(addr, 0x00, 0x00) # SET ALL OF BANK 0 TO OUTPUTS
bus.write_byte_data(addr, 0x01, 0x00) # SET ALL OF BANK 1 TO OUTPUTS
print "MCP23017 ready"

# CLEAR DISPLAY
bus.write_byte_data(addr, 0x13, 0xff) # SET ALL OF BANK 1 TO HIGH (OFF)
print "display cleared"

print ""


#======================================================================
# SET UP SR-04 GPIO PINS

print "- - - - - START SR-04 SETUP"
time.sleep(0.5)

# SET SR-04 GPIO PIN
sonar = 8
print "SR-04 GPIO pin set"

print ""


#======================================================================
# SET UP MOTORS

print "- - - - - START MOTOR GPIO SETUP"
time.sleep(0.5)

# INITIALISE THE PWM DEVICE USING DEFAULT ADDRESS
global p, q, a, b
print "PWM address set"

# MOTOR GPIO PINS
R1 = 24
R2 = 26
L1 = 19
L2 = 21
print "Motor GPIO pins set"

# USE PWM TO CONTROL MOTOR SPEED
GPIO.setup(L1, GPIO.OUT) # SET LEFT MOTOR GPIO 1 AS OUTPUT
p = GPIO.PWM(L1, 20) # SET LEFT MOTOR GPIO 1 AS PWM
p.start(0) # START LEFT MOTOR 1 PWM AT ZERO

GPIO.setup(L2, GPIO.OUT) # SET LEFT MOTOR GPIO 2 AS OUTPUT
q = GPIO.PWM(L2, 20) # SET LEFT MOTOR GPIO 2 AS PWM
q.start(0) # START LEFT MOTOR 2 PWM AT ZERO

GPIO.setup(R1, GPIO.OUT) # SET RIGHT MOTOR GPIO 1 AS OUTPUT
a = GPIO.PWM(R1, 20) # SET RIGHT MOTOR GPIO 1 AS PWM
a.start(0) # START RIGHT MOTOR 1 PWM AT ZERO

GPIO.setup(R2, GPIO.OUT) # SET RIGHT MOTOR GPIO 2 AS OUTPUT
b = GPIO.PWM(R2, 20) # SET RIGHT MOTOR GPIO 2 AS PWM
b.start(0) # START RIGHT MOTOR 2 PWM AT ZERO
print "Motor GPIO pins set"

print ""


#======================================================================
# SET UP SPEED LEVELS
#    - This section sets a number of speed levels
#    - Allows us to change the PWM tuning for the entire program
#    - First number is the left motor, second is the right motor
#    - Motors have different values to compensate for inaccuracies

print "- - - - - START SPEED LEVEL SETUP"
time.sleep(0.5)

# TURN SPEED SETTINGS (PWM)
SpeedLow = 60
SpeedMedium = 80
SpeedHigh = 100
print "Turn speeds set"

# FORWARD SPEED SETTINGS (PWM)
ForwardLowL = 18
ForwardLowR = 20
ForwardMediumL = 57
ForwardMediumR = 60
ForwardHighL = 98
ForwardHighR = 99
print "Forward speeds set"

# REVERSE SPEED SETTINGS (PWM)
ReverseLowL = 26
ReverseLowR = 20
ReverseMediumL = 60
ReverseMediumR = 56
ReverseHighL = 98
ReverseHighR = 94
print "Reverse speeds set"

# SET INITIAL MOTOR SPEEDS

TurnSpeed = SpeedHigh         # Start @ SpeedHigh
ForwardSpeedL = ForwardHighL  # Start @ ForwardHighL
ForwardSpeedR = ForwardHighR  # Start @ ForwardHighR
ReverseSpeedL = ReverseHighL  # Start @ ReverseHighL
ReverseSpeedR = ReverseHighR  # Start @ ReverseHighR
print "Initial speeds set"

print ""


#======================================================================
# SET UP LINE SENSORS

print "- - - - - START LINE SENSOR SETUP"
time.sleep(0.5)

# SET GPIO FOR LINE FOLLOWING SENSORS
lineRight = 11  #(BROWN)
lineMiddle = 12 # (WHITE)
lineLeft = 13  #(BLUE)
print "Line sensor GPIO numbers set"

# SET UP DIGITAL LINE DETECTORS AS INPUTS
GPIO.setup(lineRight,GPIO.IN)
GPIO.setup(lineMiddle,GPIO.IN)
GPIO.setup(lineLeft,GPIO.IN)
print "Line sensor GPIO direction set (IN)"

print ""


#======================================================================
# SET COUNT AND INITIAL DISPLAY CHARACTER

print "- - - - - START INITIAL COUNT & DISPLAY CHARACTER SETUP"
time.sleep(0.5)

# SET INITIAL COUNT FOR DISPLAY BOARD MENU CODE
count = 0
print "Count set to zero"

# SET AN UNDERSCORE IN THE MENU TO START WITH (MCP23017 COMMAND)
bus.write_byte_data(addr, 0x13, 7)
bus.write_byte_data(addr, 0x12, underscore)
print "Initial display character set"

print ""


#======================================================================
# PRINT TO CONFIRM START UP PROCESSES COMPLETE

print "- - - - - ALL START UP PROCESSES COMPLETE"
print "- - - - - - - - - - - - - - - - - - - - -"
print ""
time.sleep(0.5)


#======================================================================
#======================================================================
#======================================================================
# START MAIN PROGRAM
#======================================================================
#======================================================================
#======================================================================


print "- - - - - STARTING MAIN PROGRAM"
print "- - - - - - - - - - - - - - - -"
print ""
time.sleep(0.5)


#======================================================================
# TRY BLOCK
# This is the main program
# When we exit this block, the finally/except block will run

try:
    def menu(): # MENU CODE FOR 4TRONIX DISPLAY BOARD AND SWITCH

        global count # IMPORTS 'COUNT' BUT LETS US EDIT IT IN A MODULE
        print "Menu start"

        while 1: # RUN FOREVER UNTIL WE MANUALLY EXIT THIS MODULE

            time.sleep(0.01)

            if GPIO.input(switch1): # IF SWITCH 1 IS CLICKED
                time.sleep(0.8)
                count = count +1 # ADD 1 TO COUNT

                if count == 1: # IF 'COUNT' IS 1
                    print "count is 1"
                    bus.write_byte_data(addr, 0x13, 7)
                    bus.write_byte_data(addr, 0x12, N1) # DISPLAY 1

                if count == 2: # IF 'COUNT' IS 2
                    print "count is 2"
                    bus.write_byte_data(addr, 0x13, 7)
                    bus.write_byte_data(addr, 0x12, N2) # DISPLAY 2

                if count == 3: # IF 'COUNT' IS 3
                    print "count is 3"
                    bus.write_byte_data(addr, 0x13, 7)
                    bus.write_byte_data(addr, 0x12, N3) # DISPLAY 3

                if count == 4: # IF 'COUNT' IS 4
                    print "count is 4"
                    bus.write_byte_data(addr, 0x13, 7)
                    bus.write_byte_data(addr, 0x12, N4) # DISPLAY 4

                if count == 5: # IF 'COUNT' IS 5
                    print "count is 5"
                    bus.write_byte_data(addr, 0x13, 7)
                    bus.write_byte_data(addr, 0x12, N5) # DISPLAY 5

                if count == 6: # IF 'COUNT' IS 6
                    print "count is 6"
                    bus.write_byte_data(addr, 0x13, 7)
                    bus.write_byte_data(addr, 0x12, N6) # DISPLAY 6

                if count == 7: # IF 'COUNT' IS 7
                    print "count is 7"
                    bus.write_byte_data(addr, 0x13, 7)
                    bus.write_byte_data(addr, 0x12, N7) # DISPLAY 7

                if count == 8: # IF 'COUNT' IS 8
                    count = 1 # RETURN COUNT TO 1 TO RESET MENU
                    print "...returning count to 1"
                    bus.write_byte_data(addr, 0x13, 7)
                    bus.write_byte_data(addr, 0x12, N1) # DISPLAY 1


            if GPIO.input(switch2): # IF SWITCH 2 IS CLICKED
                time.sleep(0.8)

                if count == 1: # IF 'COUNT' IS 1
                    bus.write_byte_data(addr, 0x13, 0xff)
                    time.sleep(3)
                    count = 0 # RESET COUNT SO MENU RETURNS TO 1 NEXT TIME
                    generalcontrol() # GO TO THE GENERALCONTROL MODULE

                if count == 2: # IF 'COUNT' IS 2
                    bus.write_byte_data(addr, 0x13, 0xff)
                    time.sleep(3)
                    count = 0 # RESET COUNT SO MENU RETURNS TO 1 NEXT TIME
                    proximity() # GO TO THE PROXIMITY MODULE

                if count == 3: # IF 'COUNT' IS 3
                    bus.write_byte_data(addr, 0x13, 0xff)
                    time.sleep(3)
                    count = 0 # RESET COUNT SO MENU RETURNS TO 1 NEXT TIME
                    threepoint() # GO TO THE THREEPOINT MODULE

                if count == 4: # IF 'COUNT' IS 4
                    bus.write_byte_data(addr, 0x13, 0xff)
                    time.sleep(3)
                    count = 0 # RESET COUNT SO MENU RETURNS TO 1 NEXT TIME
                    linefollow() # GO TO THE LINEFOLLOW MODULE

                if count == 5: # IF 'COUNT' IS 5
                    bus.write_byte_data(addr, 0x13, 0xff)
                    time.sleep(3)
                    count = 0 # RESET COUNT SO MENU RETURNS TO 1 NEXT TIME
                    skittles()  # GO TO THE SKITTLES MODULE

                if count == 6: # IF 'COUNT' IS 6
                    bus.write_byte_data(addr, 0x13, 0xff)
                    time.sleep(3)
                    count = 0 # RESET COUNT SO MENU RETURNS TO 1 NEXT TIME
                    endscript() # GO TO THE ENDSCRIPT MODULE

                if count == 7: # IF 'COUNT' IS 7
                    bus.write_byte_data(addr, 0x13, 0xff)
                    time.sleep(3)
                    count = 0 # RESET COUNT SO MENU RETURNS TO 1 NEXT TIME
                    haltcommand() # GO TO THE HALTCOMMAND MODULE


    #======================================================================
    # DEFINE GENERAL CONTROL MODULE
    # We have separate speed sets for forward and back due to inaccuracies

    def generalcontrol(): ### PROGRAM 1 ###

        # IMPORT GLOBAL VARIABLES SO WE CAN CHANGE THEM IN A MODULE
        global TurnSpeed
        global ForwardSpeedL
        global ForwardSpeedR
        global ReverseSpeedL
        global ReverseSpeedR

        while 1: # RUN FOREVER UNTIL WE MANUALLY EXIT THIS MODULE

            for event in pygame.event.get(): # LOOK FOR A PYGAME EVENT
                print 'General Control PyGame Event'

                #########################################
                # IF A KEY IS BEING PRESSED...          #
                #########################################

                if event.type == pygame.KEYDOWN: # IF A KEY IS PRESSED DOWN


                    #======================================================
                    # KEY DEBUG - PRINTS THE KEY DETECTED IN THE TERMINAL (Optional)

                    #newkey = ""
                    #newkey = pygame.key.name(event.key)
                    #print newkey


                    #======================================================
                    # BASIC MOVEMENT (FWD, REV, RIGHT, LEFT)

                    # FORWARD (UP KEY)
                    if event.key == pygame.K_UP: # UP KEY PRESSED
                        Forward(ForwardSpeedL, ForwardSpeedR) # FORWARD AT SET SPEED
                        print 'Forward speed', ForwardSpeedL, ForwardSpeedR # PRINT SPEED

                    # REVERSE (DOWN KEY)
                    if event.key == pygame.K_DOWN: # DOWN KEY PRESSED
                        Reverse(ReverseSpeedL, ReverseSpeedR) # REVERSE AT SET SPEED
                        print 'Reverse speed:', ReverseSpeedL, ReverseSpeedR # PRINT SPEED

                    # RIGHT (RIGHT KEY)
                    if event.key == pygame.K_RIGHT: # RIGHT KEY PRESSED
                        Right(TurnSpeed) # SPIN RIGHT AT SET SPEED
                        print 'Spin right speed:', TurnSpeed # PRINT SPEED

                    # LEFT (LEFT KEY)
                    if event.key == pygame.K_LEFT: # LEFT KEY PRESSED
                        Left(TurnSpeed) # SPIN LEFT AT SET SPEED
                        print 'Spin left speed', TurnSpeed # PRINT SPEED


                    #======================================================
                    # TURN THE FORWARD/REVERSE SPEED UP
                    # Looks at left AND right speed and changes both at the same time
                    # This maintains PWM left/right balancing to handle inaccuracies

                    if event.key == pygame.K_1: # 1 KEY PRESSED

                        # IF SPEED IS LOW - SET TO MEDIUM
                        if (ForwardSpeedL == ForwardLowL and ForwardSpeedR == ForwardLowR and ReverseSpeedL == ReverseLowL and ReverseSpeedR == ReverseLowR):
                            print 'Increasing to medium speed'
                            print 'Forward:', ForwardMediumL, ForwardMediumR
                            print 'Reverse:', ReverseMediumL, ReverseMediumR
                            ForwardSpeedL = ForwardMediumL
                            ForwardSpeedR = ForwardMediumR
                            ReverseSpeedL = ReverseMediumL
                            ReverseSpeedR = ReverseMediumR


                        # IF SPEED IS MEDIUM - SET TO HIGH
                        elif (ForwardSpeedL == ForwardMediumL and ForwardSpeedR == ForwardMediumR and ReverseSpeedL == ReverseMediumL and ReverseSpeedR == ReverseMediumR):
                            print 'Increasing to high speed'
                            print 'Forward:', ForwardHighL, ForwardHighR
                            print 'Reverse:', ReverseHighL, ReverseHighR
                            ForwardSpeedL = ForwardHighL
                            ForwardSpeedR = ForwardHighR
                            ReverseSpeedL = ReverseHighL
                            ReverseSpeedR = ReverseHighR

                        # IF SPEED ALREADY HIGHEST - DO NOTHING
                        elif (ForwardSpeedL == ForwardHighL and ForwardSpeedR == ForwardHighR and ReverseSpeedL == ReverseHighL and ReverseSpeedR == ReverseHighR):
                            print 'Highest speed reached!'


                    #======================================================
                    # TURN FORWARD/REVERSE SPEED DOWN
                    # Looks at left AND right speed and changes both at the same time
                    # This maintains PWM left/right balancing to handle inaccuracies

                    if event.key == pygame.K_2: # 2 KEY PRESSED

                        # IF SPEED ALREADY LOWEST - DO NOTHING
                        if (ForwardSpeedL == ForwardLowL and ForwardSpeedR == ForwardLowR and ReverseSpeedL == ReverseLowL and ReverseSpeedR == ReverseLowR):
                            print 'Lowest speed reached'

                        # IF SPEED IS MEDIUM - SET TO LOW
                        elif (ForwardSpeedL == ForwardMediumL and ForwardSpeedR == ForwardMediumR and ReverseSpeedL == ReverseMediumL and ReverseSpeedR == ReverseMediumR):
                            print 'Dropping to low speed'
                            print 'Forward:', ForwardLowL, ForwardLowR
                            print 'Reverse:', ReverseLowL, ReverseLowR
                            ForwardSpeedL = ForwardLowL
                            ForwardSpeedR = ForwardLowR
                            ReverseSpeedL = ReverseLowL
                            ReverseSpeedR = ReverseLowR

                        # IF SPEED IS HIGH - SET TO MEDIUM
                        elif (ForwardSpeedL == ForwardHighL and ForwardSpeedR == ForwardHighR and ReverseSpeedL == ReverseHighL and ReverseSpeedR == ReverseHighR):
                            print 'Dropping to medium speed'
                            print 'Forward:', ForwardMediumL, ForwardMediumR
                            print 'Reverse:', ReverseMediumL, ReverseMediumR
                            ForwardSpeedL = ForwardMediumL
                            ForwardSpeedR = ForwardMediumR
                            ReverseSpeedL = ReverseMediumL
                            ReverseSpeedR = ReverseMediumR


                    #======================================================
                    # TURN THE TURNING SPEED UP

                    if event.key == pygame.K_3: # 3 KEY PRESSED

                        # IF SPEED IS LOW - SET TO MEDIUM
                        if TurnSpeed == SpeedLow:
                            print 'Increasing to medium turning speed. Speed:', SpeedMedium
                            TurnSpeed = SpeedMedium

                        # IF SPEED IS MEDIUM - SET TO HIGH
                        elif TurnSpeed == SpeedMedium:
                            print 'Increasing to high turning speed. Speed:', SpeedHigh
                            TurnSpeed = SpeedHigh

                        # IF SPEED ALREADY HIGHEST - DO NOTHING
                        elif TurnSpeed == SpeedHigh:
                            print 'Highest turning speed reached'


                    #======================================================
                    # TURN THE TURNING SPEED DOWN

                    if event.key == pygame.K_4: # 4 KEY PRESSED

                        # IF SPEED ALREADY LOWEST - DO NOTHING
                        if TurnSpeed == SpeedLow:
                            print 'Lowest turning speed reached'

                        # IF SPEED IS MEDIUM - SET TO LOW
                        elif TurnSpeed == SpeedMedium:
                            print 'Dropping to low turning speed. Speed:', SpeedLow
                            TurnSpeed = SpeedLow

                        # IF SPEED IS HIGH - SET TO MEDIUM
                        elif TurnSpeed == SpeedHigh:
                            print 'Dropping to medium turning speed. Speed:', SpeedMedium
                            TurnSpeed = SpeedMedium


                        #======================================================
                        # EXIT THE PROGRAM AND BACK TO MENU

                        if event.key == pygame.K_ESCAPE: # ESCAPE KEY PRESSED

                            # SET SEGMENT BACK TO UNDERSCORE FOR THE MAIN MENU
                            bus.write_byte_data(addr, 0x13, 7)
                            bus.write_byte_data(addr, 0x12, underscore)
                            time.sleep(0.5)

                            # RESET GLOBAL SPEEDS FOR OTHER PROGRAMS THAT HAVE FIXED SPEED
                            TurnSpeed = SpeedHigh
                            ForwardSpeedL = ForwardHighL
                            ForwardSpeedR = ForwardHighR
                            ReverseSpeedL = ReverseHighL
                            ReverseSpeedR = ReverseHighR

                            print "Returning to main menu"
                            time.sleep(0.5)

                            # GO BACK TO MAIN MENU
                            menu()


                #########################################
                # IF A KEY IS NOT BEING PRESSED...      #
                #########################################

                elif event.type == pygame.KEYUP: # IF A KEY IS RELEASED

                    # STOP ALL MOVEMENT (WHEN NO KEY PRESSED)


                    if event.key == pygame.K_UP: # UP KEY RELEASED
                        stop() # STOP MOTORS
                        print 'STOP - No key press'

                    if event.key == pygame.K_DOWN: # DOWN KEY RELEASED
                        stop() # STOP MOTORS
                        print 'STOP - No key press'

                    if event.key == pygame.K_RIGHT: # RIGHT KEY RELEASED
                        stop() # STOP MOTORS
                        print 'STOP - No key press'

                    if event.key == pygame.K_LEFT: # LEFT KEY RELEASED
                        stop() # STOP MOTORS
                        print 'STOP - No key press'

                    if event.key == pygame.K_1: # 1 KEY RELEASED
                        stop() # STOP MOTORS
                        print 'STOP - No key press'

                    if event.key == pygame.K_2: # 2 KEY RELEASED
                        stop() # STOP MOTORS
                        print 'STOP - No key press'

                    if event.key == pygame.K_3: # 3 KEY RELEASED
                        stop() # STOP MOTORS
                        print 'STOP - No key press'

                    if event.key == pygame.K_4: # 4 KEY RELEASED
                        stop() # STOP MOTORS
                        print 'STOP - No key press'

                    if event.key == pygame.K_ESCAPE: # ESCAPE KEY RELEASED
                        stop() # STOP MOTORS
                        print 'STOP - No key press'


    #======================================================================
    # DEFINE PROXIMITY MODULE

    def proximity(): ### PROGRAM 2 ###

        countme = 0

        while True:

            # GET DISTANCE
            dist = getDistance()

            # PRINT DISTANCE
            print "------------------------------"
            print "DISTANCE = ", int(dist)
            print "------------------------------"

            # DISTANCE MOVEMENT CONTROL
            if dist >= 150: # IF DISTANCE EQUAL TO OR GREATER THAN 150
                #print "Distance: ", int(dist)
                #print "Distance greater than 150"
                Forward(ForwardHighL, ForwardHighR) # MOVE FORWARD FOR THE TIME IN TIME.SLEEP BELOW
                time.sleep(1)
                #stop()

            elif 51 <= dist <= 150: # IF DISTANCE BETWEEN 50 AND 150
                #print "Distance: ", int(dist)
                #print "Distance 51 to 150"
                Forward(ForwardMediumL, ForwardMediumR) # MOVE FORWARD FOR THE TIME IN TIME.SLEEP BELOW
                time.sleep(0.3)
                #stop()

            elif 17 <= dist <= 50: # IF DISTANCE BETWEEN 15 AND 50
                #print "Distance: ", int(dist)
                #print "Distance 17 to 50"
                Forward(ForwardLowL, ForwardLowR) # MOVE FORWARD FOR THE TIME IN TIME.SLEEP BELOW
                time.sleep(0.01)
                #stop()

            elif 10 <= dist <= 16: # IF DISTANCE BETWEEN 8 AND 15
                #print "Distance: ", int(dist)
                #print "Distance is 10 to 16"
                Forward(ForwardLowL, ForwardLowR) # MOVE FORWARD FOR THE TIME IN TIME.SLEEP BELOW
                time.sleep(0.001)
                #stop()

            elif 0 <= dist <= 9 and countme <= 7: # IF DISTANCE BETWEEN 0 AND 9
                print "0-8", countme
                stop() # STOP MOVING
                countme = countme+1
                time.sleep(0.5)

            elif 0 <= dist <= 9 and countme == 8: # IF DISTANCE BETWEEN 0 AND 9
                print "countme 5..."
                stop() # STOP MOVING
                #print "PROGRAM COMPLETE - KILLING PROGRAM"
                time.sleep(0.5)

                # SET SEGMENT BACK TO UNDERSCORE FOR THE MAIN MENU
                bus.write_byte_data(addr, 0x13, 7)
                bus.write_byte_data(addr, 0x12, underscore)
                time.sleep(0.5)

                print "Returning to main menu"
                time.sleep(0.5)

                # RETURN TO MAIN MENU
                menu()


    #======================================================================
    # DEFINE THREE POINT TURN MODULE

    def threepoint(): ### PROGRAM 3 ###

        Forward(ForwardHighL, ForwardHighR) # MOVE FORWARD FOR THE TIME IN TIME.SLEEP BELOW
        time.sleep(3.5)

        Left(SpeedMedium) # SPIN LEFT AT SET SPEED
        time.sleep(0.6)

        Forward(ForwardHighL, ForwardHighR) # MOVE FORWARD FOR THE TIME IN TIME.SLEEP BELOW
        time.sleep(0.8)

        Reverse(ReverseHighL, ReverseHighR) # REVERSE FOR THE TIME IN TIME.SLEEP BELOW
        time.sleep(1.7)

        Forward(ForwardHighL, ForwardHighR) # MOVE FORWARD FOR THE TIME IN TIME.SLEEP BELOW
        time.sleep(0.8)

        Left(SpeedMedium) # SPIN LEFT AT SET SPEED
        time.sleep(0.5)

        Forward(ForwardHighL, ForwardHighR) # MOVE FORWARD FOR THE TIME IN TIME.SLEEP BELOW
        time.sleep(3.5)

        # STOP THE ROBOT
        stop()

        # SET SEGMENT BACK TO UNDERSCORE FOR THE MAIN MENU
        bus.write_byte_data(addr, 0x13, 7)
        bus.write_byte_data(addr, 0x12, underscore)
        time.sleep(0.5)

        print "Returning to main menu"
        time.sleep(0.5)

        # RETURN TO MAIN MENU
        menu()


    #======================================================================
    # DEFINE LINE FOLLOWER MODULE

    def linefollow(): ### PROGRAM 4 ###

    ## Line sensor: http://4tronix.co.uk/store/index.php?rt=product/product&product_id=144
    ## Logic:
    ##     When seeing white (off of the line) = Low logic
    ##     When seeing black (on the line) = High logic

        print "LineFollow script started"
        while True:
            print "while started"

            # IF SWITCH PRESSED
            if GPIO.input(switch1):
                print "linefollow script exit"
                stop()
                time.sleep(0.5)
                bus.write_byte_data(addr, 0x13, 7)
                bus.write_byte_data(addr, 0x12, underscore)
                menu()

            # X-0 or X-X-0
            elif (GPIO.input(lineLeft) == 1 and GPIO.input(lineRight) == 0) or (GPIO.input(lineLeft) == 1 and GPIO.input(lineMiddle) == 1 and GPIO.input(lineRight) == 0) :
                print "X-0-0"
                LastDir = 1
                Left(40) # SPIN LEFT AT LOW SPEED SETTING
                time.sleep(0.2)

            # 0-X or 0-X-X
            elif (GPIO.input(lineLeft) == 0 and GPIO.input(lineRight) == 1) or (GPIO.input(lineLeft) == 0 and GPIO.input(lineMiddle) == 1 and GPIO.input(lineRight) == 1):
                print "0-0-X"
                LastDir = 3
                Right(40) # SPIN RIGHT AT LOW SPEED SETTING
                time.sleep(0.2)

            # 0-X-0
            elif GPIO.input(lineLeft) == 0 and GPIO.input(lineMiddle) == 1 and GPIO.input(lineRight) == 0:
                print "0-X-0"
                Forward (30,30) # FORWARD AT LOW SPEED SETTING
                time.sleep(0.1)


    #======================================================================
    # DEFINE SKITTLES MODULE
    # Will always be same speed as default set initially before try block
    # Could add speed control but seems excessive

    def skittles(): ### PROGRAM 5 ###

        global ForwardSpeedL
        global ForwardSpeedR
        global ReverseSpeedL
        global ReverseSpeedR
        global TurnSpeed

        while True:

            for event in pygame.event.get():
                print 'General Control PyGame Event'

                #########################################
                # IF A KEY IS BEING PRESSED...          #
                #########################################

                if event.type == pygame.KEYDOWN: # IF A KEY IS BEING PRESSED

                    #======================================================
                    # BASIC MOVEMENT (FWD, REV, RIGHT, LEFT)

                    # FORWARD (UP KEY)
                    if event.key == pygame.K_UP: # UP KEY PRESSED
                        Forward(ForwardSpeedL, ForwardSpeedR) # FORWARD AT SET SPEED
                        print 'Forward speed', ForwardSpeedL, ForwardSpeedR # PRINT SPEED

                    # REVERSE (DOWN KEY)
                    if event.key == pygame.K_DOWN: # DOWN KEY PRESSED
                        Reverse(ReverseSpeedL, ReverseSpeedR) # REVERSE AT SET SPEED
                        print 'Reverse speed:', ReverseSpeedL, ReverseSpeedR # PRINT SPEED

                    # RIGHT (RIGHT KEY)
                    if event.key == pygame.K_RIGHT: # RIGHT KEY PRESSED
                        Right(TurnSpeed) # SPIN RIGHT AT SET SPEED
                        print 'Spin right speed:', TurnSpeed # PRINT SPEED

                    # LEFT (LEFT KEY)
                    if event.key == pygame.K_LEFT: # LEFT KEY PRESSED
                        Left(TurnSpeed) # SPIN LEFT AT SET SPEED
                        print 'Spin left speed', TurnSpeed # PRINT SPEED

                    #======================================================
                    # SERVO PADDLE CONTROL

                    # FIRE PADDLE
                    if event.key == pygame.K_f: # F KEY PRESSED
                        print "Firing"

                        # FIRE PADDLE BY MOVING SERVOS
                        servo1.ChangeDutyCycle(10) #180
                        servo2.ChangeDutyCycle(5) #0

                        # WAIT 3 SECONDS
                        time.sleep(1)

                        # AUTOMATICALLY RESET PADDLE AFTER FIRING
                        print "Resetting"
                        servo1.ChangeDutyCycle(4) #0
                        servo2.ChangeDutyCycle(11) #180


                    #======================================================
                    # TURN THE FORWARD/REVERSE SPEED UP
                    # Looks at left AND right speed and changes both at the same time
                    # This maintains PWM left/right balancing to handle inaccuracies

                    if event.key == pygame.K_1: # 1 KEY PRESSED

                        # IF SPEED IS LOW - SET TO MEDIUM
                        if (ForwardSpeedL == ForwardLowL and ForwardSpeedR == ForwardLowR and ReverseSpeedL == ReverseLowL and ReverseSpeedR == ReverseLowR):
                            print 'Increasing to medium speed'
                            print 'Forward:', ForwardMediumL, ForwardMediumR
                            print 'Reverse:', ReverseMediumL, ReverseMediumR
                            ForwardSpeedL = ForwardMediumL
                            ForwardSpeedR = ForwardMediumR
                            ReverseSpeedL = ReverseMediumL
                            ReverseSpeedR = ReverseMediumR


                        # IF SPEED IS MEDIUM - SET TO HIGH
                        elif (ForwardSpeedL == ForwardMediumL and ForwardSpeedR == ForwardMediumR and ReverseSpeedL == ReverseMediumL and ReverseSpeedR == ReverseMediumR):
                            print 'Increasing to high speed'
                            print 'Forward:', ForwardHighL, ForwardHighR
                            print 'Reverse:', ReverseHighL, ReverseHighR
                            ForwardSpeedL = ForwardHighL
                            ForwardSpeedR = ForwardHighR
                            ReverseSpeedL = ReverseHighL
                            ReverseSpeedR = ReverseHighR

                        # IF SPEED ALREADY HIGHEST - DO NOTHING
                        elif (ForwardSpeedL == ForwardHighL and ForwardSpeedR == ForwardHighR and ReverseSpeedL == ReverseHighL and ReverseSpeedR == ReverseHighR):
                            print 'Highest speed reached!'


                    #======================================================
                    # TURN FORWARD/REVERSE SPEED DOWN
                    # Looks at left AND right speed and changes both at the same time
                    # This maintains PWM left/right balancing to handle inaccuracies

                    if event.key == pygame.K_2: # 2 KEY PRESSED

                        # IF SPEED ALREADY LOWEST - DO NOTHING
                        if (ForwardSpeedL == ForwardLowL and ForwardSpeedR == ForwardLowR and ReverseSpeedL == ReverseLowL and ReverseSpeedR == ReverseLowR):
                            print 'Lowest speed reached'

                        # IF SPEED IS MEDIUM - SET TO LOW
                        elif (ForwardSpeedL == ForwardMediumL and ForwardSpeedR == ForwardMediumR and ReverseSpeedL == ReverseMediumL and ReverseSpeedR == ReverseMediumR):
                            print 'Dropping to low speed'
                            print 'Forward:', ForwardLowL, ForwardLowR
                            print 'Reverse:', ReverseLowL, ReverseLowR
                            ForwardSpeedL = ForwardLowL
                            ForwardSpeedR = ForwardLowR
                            ReverseSpeedL = ReverseLowL
                            ReverseSpeedR = ReverseLowR

                        # IF SPEED IS HIGH - SET TO MEDIUM
                        elif (ForwardSpeedL == ForwardHighL and ForwardSpeedR == ForwardHighR and ReverseSpeedL == ReverseHighL and ReverseSpeedR == ReverseHighR):
                            print 'Dropping to medium speed'
                            print 'Forward:', ForwardMediumL, ForwardMediumR
                            print 'Reverse:', ReverseMediumL, ReverseMediumR
                            ForwardSpeedL = ForwardMediumL
                            ForwardSpeedR = ForwardMediumR
                            ReverseSpeedL = ReverseMediumL
                            ReverseSpeedR = ReverseMediumR

                    #======================================================
                    # TURN THE TURNING SPEED UP

                    if event.key == pygame.K_3: # 3 KEY PRESSED

                        # IF SPEED IS LOW - SET TO MEDIUM
                        if TurnSpeed == SpeedLow:
                            print 'Increasing to medium turning speed. Speed:', SpeedMedium
                            TurnSpeed = SpeedMedium

                        # IF SPEED IS MEDIUM - SET TO HIGH
                        elif TurnSpeed == SpeedMedium:
                            print 'Increasing to high turning speed. Speed:', SpeedHigh
                            TurnSpeed = SpeedHigh

                        # IF SPEED ALREADY HIGHEST - DO NOTHING
                        elif TurnSpeed == SpeedHigh:
                            print 'Highest turning speed reached'


                    #======================================================
                    # TURN THE TURNING SPEED DOWN

                    if event.key == pygame.K_4: # 4 KEY PRESSED

                        # IF SPEED ALREADY LOWEST - DO NOTHING
                        if TurnSpeed == SpeedLow:
                            print 'Lowest turning speed reached'

                        # IF SPEED IS MEDIUM - SET TO LOW
                        elif TurnSpeed == SpeedMedium:
                            print 'Dropping to low turning speed. Speed:', SpeedLow
                            TurnSpeed = SpeedLow

                        # IF SPEED IS HIGH - SET TO MEDIUM
                        elif TurnSpeed == SpeedHigh:
                            print 'Dropping to medium turning speed. Speed:', SpeedMedium
                            TurnSpeed = SpeedMedium

                    #======================================================
                    # EXIT THE PROGRAM AND BACK TO MENU

                    if event.key == pygame.K_ESCAPE: # ESCAPE KEY PRESSED
                        print "Returning to main menu"
                        time.sleep(0.5)

                        # SET SEGMENT BACK TO UNDERSCORE FOR THE MAIN MENU
                        bus.write_byte_data(addr, 0x13, 7)
                        bus.write_byte_data(addr, 0x12, underscore)
                        time.sleep(0.5)

                        # RESET GLOBAL SPEEDS FOR OTHER PROGRAMS THAT HAVE FIXED SPEED
                        TurnSpeed = SpeedHigh
                        ForwardSpeedL = ForwardHighL
                        ForwardSpeedR = ForwardHighR
                        ReverseSpeedL = ReverseHighL
                        ReverseSpeedR = ReverseHighR

                        print "Returning to main menu"
                        time.sleep(0.5)

                        # GO BACK TO MAIN MENU
                        menu()


                #########################################
                # IF A KEY IS NOT BEING PRESSED...      #
                #########################################

                # STOP ALL MOVEMENT (WHEN NO KEY PRESSED) ################################################ can we just add stop() under the keyup?
                                                          ################################################ or use OR statements?

                elif event.type == pygame.KEYUP: # IF A KEY IS RELEASED

                    if event.key == pygame.K_UP: # UP KEY RELEASED
                        stop() # STOP MOTORS
                        print 'STOP - No key press'

                    if event.key == pygame.K_DOWN: # DOWN KEY RELEASED
                        stop() # STOP MOTORS
                        print 'STOP - No key press'

                    if event.key == pygame.K_RIGHT: # RIGHT KEY RELEASED
                        stop() # STOP MOTORS
                        print 'STOP - No key press'

                    if event.key == pygame.K_LEFT: # LEFT KEY RELEASED
                        stop() # STOP MOTORS
                        print 'STOP - No key press'

                    if event.key == pygame.K_f: # K KEY RELEASED
                        stop() # STOP MOTORS
                        print 'STOP - No key press'

                    if event.key == pygame.K_1: # 1 KEY RELEASED
                        stop() # STOP MOTORS
                        print 'STOP - No key press'

                    if event.key == pygame.K_2: # 2 KEY RELEASED
                        stop() # STOP MOTORS
                        print 'STOP - No key press'

                    if event.key == pygame.K_3: # 3 KEY RELEASED
                        stop() # STOP MOTORS
                        print 'STOP - No key press'

                    if event.key == pygame.K_4: # 4 KEY RELEASED
                        stop() # STOP MOTORS
                        print 'STOP - No key press'

                    if event.key == pygame.K_ESCAPE: # ESCAPE KEY RELEASED
                        stop() # STOP MOTORS
                        print 'STOP - No key press'


    #======================================================================
    # DEFINE CLEAN EXIT MENU OPTION (DISPLAY OFF AND GPIO CLEANUP)

    def endscript(): ### PROGRAM 6 ###

        quit() # WILL RUN THE 'FINALLY' BLOCK AS WELL


    #======================================================================
    # DEFINE HALT COMMAND (SHUT DOWN)

    def haltcommand(): ### PROGRAM 7 ###
        print "Shutting down system"
        time.sleep(1)

        # TURN OFF 4TRONIX DISPLAY BOARD
        bus.write_byte_data(addr, 0x13, 0xff)
        time.sleep(0.5)

        # TURN OFF MOTORS
        print "MOTORS OFF"
        stop()
        time.sleep(0.5)

        # CLEAN UP GPIOS
        GPIO.cleanup()
        time.sleep(0.5)

        # QUIT PYGAME
        pygame.quit()
        time.sleep(0.5)

        # SHUT DOWN
        os.system("sudo halt") # After this it will try and run the finally block but will already be shutting down


    #======================================================================
    # DEFINE MOTOR FUNCTIONS (PWM SETTINGS)

    # STOP ALL MOTORS
    def stop():
        p.ChangeDutyCycle(0)
        q.ChangeDutyCycle(0)
        a.ChangeDutyCycle(0)
        b.ChangeDutyCycle(0)

    # LEFT(SPEED): SETS MOTORS TO TURN OPPOSITE DIRECTIONS AT SPEED. 0 <= SPEED <= 100
    def Left(speed):
        p.ChangeDutyCycle(0)
        q.ChangeDutyCycle(speed)
        a.ChangeDutyCycle(speed)
        b.ChangeDutyCycle(0)

    # RIGHT(SPEED): SETS MOTORS TO TURN OPPOSITE DIRECTIONS AT SPEED. 0 <= SPEED <= 100
    def Right(speed):
        p.ChangeDutyCycle(speed)
        q.ChangeDutyCycle(0)
        a.ChangeDutyCycle(0)
        b.ChangeDutyCycle(speed)

    # FORWARD(LEFTSPEED, RIGHTSPEED): MOVES FORWARDS IN AN ARC BY SETTING DIFFERENT SPEEDS. 0 <= LEFTSPEED,RIGHTSPEED <= 100
    def Forward(leftSpeed, rightSpeed):
        p.ChangeDutyCycle(leftSpeed)
        q.ChangeDutyCycle(0)
        a.ChangeDutyCycle(rightSpeed)
        b.ChangeDutyCycle(0)

    # REVERSE(LEFTSPEED, RIGHTSPEED): MOVES BACKWARDS IN AN ARC BY SETTING DIFFERENT SPEEDS. 0 <= LEFTSPEED,RIGHTSPEED <= 100
    def Reverse(leftSpeed, rightSpeed):
        p.ChangeDutyCycle(0)
        q.ChangeDutyCycle(leftSpeed)
        a.ChangeDutyCycle(0)
        b.ChangeDutyCycle(rightSpeed)


    #======================================================================
    # DEFINE SR-04 FUNCTIONS

    # GETDISTANCE(). RETURNS THE DISTANCE IN CM TO THE NEAREST OBJECT
    def getDistance():
        GPIO.setup(sonar, GPIO.OUT)
        # SEND 10US PULSE TO TRIGGER
        GPIO.output(sonar, True)
        time.sleep(0.00001)
        GPIO.output(sonar, False)
        start = time.time()
        count=time.time()
        GPIO.setup(sonar,GPIO.IN)

        while GPIO.input(sonar)==0 and time.time()-count<0.1:
            start = time.time()
        count=time.time()
        stop=count

        while GPIO.input(sonar)==1 and time.time()-count<0.1:
            stop = time.time()

        # CALCULATE PULSE LENGTH
        elapsed = stop-start

        # DISTANCE PULSE TRAVELLED IN THAT TIME IS TIME
        # MULTIPLIED BY THE SPEED OF SOUND (CM/S)
        distance = elapsed * 34000

        # THAT WAS THE DISTANCE THERE AND BACK SO HALVE THE VALUE
        distance = distance / 2
        return distance


    #======================================================================
    # !! IMPORTANT !! MAKE THE SCRIPT RUN 'MENU' AT THE START !!
    # Without this, noting will happen!

    menu()

#======================================================================
# FINALLY/EXCEPT BLOCK
# This will run if there is an error or we choose to exit the program
# We use Finally in the end code, but have Except ready for debuging

#finally:
except KeyboardInterrupt: # USE THIS OPTION FOR DEBUGGING

    print "EXIT SCRIPT"
    time.sleep(1)

    # TURN OFF 4TRONIX DISPLAY
    print "TURNING OFF DISPLAY"
    time.sleep(0.5)
    bus.write_byte_data(addr, 0x13, 0xff) # Set all of bank 1 to High (Off)

    # TURN OFF MOTORS
    print "MOTORS OFF"
    stop()
    time.sleep(0.5)

    # CLEAN UP GPIOS
    print "PERFORMING GPIO CLEANUP"
    time.sleep(0.5)
    GPIO.cleanup()

    # EXIT PYGAME
    print "EXIT PYGAME"
    time.sleep(0.5)
    pygame.quit()

    # EXIT PROGRAM
    print "--- EXIT NOW ---"
    time.sleep(0.5)
    quit()