# Fido Catcher v1.0
# Michael Goldman
# Hannah Brown
# 3/6/2015

"""
CURRRENT STATUS:
    Systems starts up, calibrates M1, M2, M4.
    Determines max, neutral positions of each.
    Adds e-stop capabilities only after calibration.
    (Calibration disables e-stop trigger to prevent infinite loop.)
    Functions defined to:
        Move DC, Stepper motors forward, backward, stop.
        Move Servo to any angle within range of motion.
        Move DC, Stepper motors to any position within range of motion
    Functions alphabetized for ease of reading.
    Functions ask for M1, M2, M4 input.
    Offers randomization for M1 (lateral angle)
    Moves system into position. 
"""

"""
STILL TO DO:
    Gosh Darn IR sensor.
"""

# Important! Servo needs timing functions only available on GPIO18 (Board 12).
# Motor 1 pin 3 is moved to pin 38 (free pin).
# Motor 5 enable is moved to pin 12.
# Pin 35 becomes free pin.


"""
Flowchart
    INITIALIZATION. Set up board, in/out pins,
    libraries, referenced variables, arrays.
    CALIBRATION. Test M1, M2, M4. Determine
    steps for M1 and seconds for M2 and M4.
    READY. Bring M1 and M2 to neutral positions.
    Remove edge detect to M4(?) Careful with
    edge detect on M4, since safest position
    for spring is at edge with no pull.
    AIM. Randomize angle (within limits). Move
    into position. Run clearance check.
    Pullback pinball handle to specified level.
    FIRE. Check for clear path. Release handle.
    Return to neutral.
"""
####SUBROUTINES REFERENCED IN MAIN PROGRAM####

def CalcPointSlope(x1, y1, m, x):
    """
    Given point, slope, and x-value, calculates y-value of line as float.
    """
    return float((m * (x - x1)) + y1)

def CalcSlope(x1, y1, x2, y2):
    """
    Given two points, calculates slope between them.
    """
    return (float(y2-y1) / (x2-x1))

def CalibrateDC(motor):
    """
    Calibrates given DC motor.
    Returns time for that motor's full range of motion.
    """
    lower = 0
    upper = 0
    
    # Removing event detect from limit switches.
    # These will be reinstated after calibration.
    # If the e-stops are hit after the startup calibration,
    # The EStop function will re-run calibration, and hitting
    # the limit switches in the calibration feature could trigger
    # the EStop function from the event detection again, causing
    # an infinite loop. L5 had no event detect (safest place for
    # spring pullback is at the limit switch), so no event is removed.
    GPIO.remove_event_detect(L3)
    GPIO.remove_event_detect(L4)
    GPIO.remove_event_detect(L6)
      
    if (motor == M2):
        lower = L3
        upper = L4
    if (motor == M4):
        lower = L5
        upper = L6

    #Move to an extreme
    while (GPIO.input(lower) == 0 and GPIO.input(upper) == 0):
        DCForward(motor)
    DCStop(motor)

    #Stay at the extreme, but not on the e-stop switch
    while (GPIO.input(lower) == 1 or GPIO.input(upper) == 1):
        DCBackward(motor)
    DCStop(motor)

    #Time moving to the other extreme
    start1 = time.time() #Start timer
    while (GPIO.input(lower) == 0 and GPIO.input(upper) == 0):
        DCBackward(motor)

    #Stay at the extreme, but not on the e-stop switch
    #Reduce time accordingly
    end1 = time.time() #End timer
    start2 = time.time() #Start timer for reduction
    while (GPIO.input(lower) == 1 or GPIO.input(upper) == 1):
        DCForward(motor)
    DCStop(motor)

    end2 = time.time() #End timer for reduction

    totaltime = (end1-start1)-(end2-start2) #Time for full movement

    #Set Neutral Position for Motor
    global M2NEUTRAL
    global M4NEUTRAL

    neut = (1.0*totaltime / 2)

    if (motor == M2):
        M2NEUTRAL = neut
    if (motor == M4):
        M4NEUTRAL = neut

    #Set Present Position for Motor
    global M2PRESENT
    global M4PRESENT
    
    if (motor == M2):
        M2PRESENT = (1.0*totaltime)
    if (motor == M4):
        M4PRESENT = (1.0*totaltime)
    
    #Move Motor to Neutral Position
    MoveDC(motor, neut)

    #Reduce time by limiting factor.
    return (totaltime*LIMFACT)

def CalibrateStepper(motor):
    """
    Calibrates given stepper motor.
    Returns ticks for that motor's range of motion.
    """
    count = 0
    
    # Removing event detect from limit switches.
    # These will be reinstated after calibration.
    # If the e-stops are hit after the startup calibration,
    # The EStop function will re-run calibration, and hitting
    # the limit switches in the calibration feature could trigger
    # the EStop function from the event detection again, causing
    # an infinite loop.
    GPIO.remove_event_detect(L1)
    GPIO.remove_event_detect(L2)
    
    #Move to an extreme
    while (GPIO.input(L1) == 0 and GPIO.input(L2) == 0):
        StepForward(motor)
    StepStop(motor)

    #Stay at the extreme, but not on the e-stop switch
    while (GPIO.input(L1) == 1 or GPIO.input(L2) == 1):
        StepBackward(motor)
    StepStop(motor)

    #Count ticks moving to the other extreme
    while (GPIO.input(L1) == 0 and GPIO.input(L2) == 0):
        StepBackward(motor)
        count = count + 1

    #Stay at the extreme, but not on the e-stop switch
    #Reduce count accordingly
    while (GPIO.input(L1) == 1 or GPIO.input(L2) == 1):
        StepForward(motor)
        count = count - 1
    StepStop(motor)

    #Set Neutral Position for Motor 1
    global M1NEUTRAL
    M1NEUTRAL = int((1.0*count / 2))
    
    #Set Present Position for Motor 1
    global M1PRESENT
    M1PRESENT = int(count)
    
    #Move Motor 1 to Neutral Position
    MoveStep(motor, M1NEUTRAL)
    
    #Reduce count by limiting factor.
    return int(1.0*count*LIMFACT)

def CountBalls():
    """
    Compares current thrown ball count against set limit.
    """
    
    if ballCount >= ballLimit:
        return True
    else:
        return False
        
def CountTime():
    """"
    Compares elapsed time against set limit.
    """
       
    if (time.time()-timeStart) >= timeLimit:
        return True
    else:
        return False

def DCBackward(motor):
    """
    Turns DC motor 'backward.'
    """
    GPIO.output(motor[0], True)
    GPIO.output(motor[1], False)
    GPIO.output(motor[2], True)

def DCForward(motor):
    """
    Turns DC motor 'forward.'
    """
    GPIO.output(motor[0], True)
    GPIO.output(motor[1], True)
    GPIO.output(motor[2], False)

def DCStop(motor):
    """
    Turns DC motor off.
    """
    GPIO.output(motor[0], False)
    GPIO.output(motor[1], False)
    GPIO.output(motor[2], False)

def EStop():
    """
    With GPIO event, safely stops all motors, re-runs calibration.
    """
    global M1STEPS
    global M2SCNDS
    global M4SCNDS    
    
    StepStop(M1)
    DCStop(M2)
    DCStop(M3)
    # Removes tension on spring before stopping spring.
    MoveDC(M4, M4SCNDS)
    DCStop(M4)
    
    M1STEPS = CalibrateStepper(M1)
    M2SCNDS = CalibrateDC(M2)
    M4SCNDS = CalibrateDC(M4)

def InputPinsDOWN(pin, name):
    """
    Sets pin as pull-down resistor input for given pin.
    """
    GPIO.setup(pin, GPIO.IN, pull_up_down=GPIO.PUD_DOWN)
    print "Switch " + name + " set up."

def isClear():
    """
    Checks to see if the path of the ball is clear using the IR sensor.
    """
    if GPIO.input(P1) == 1:
        print "Path Blocked"
        return False
    else:
        return True
    
def MoveDC(motor, position):
    """
    Moves DC motor to specified position.
    """
    global M2PRESENT
    global M4PRESENT
    
    if (motor == M2):
        diff = position - M2PRESENT
    if (motor == M4):
        diff = position - M4PRESENT
    
    start = time.time()
    if diff > 0:
        while time.time()-start < diff:
            DCBackward(motor)
        DCStop(motor)
    elif diff < 0:
        while time.time()-start < abs(diff):
            DCForward(motor)
        DCStop(motor)
    else:
        DCStop(motor)
    
    if (motor == M2):
        M2PRESENT = position
    if (motor == M4):
        M4PRESENT = position

def MoveServo(angle):
    """
    Moves servo motor to specified angle if angle within range of servo motion.
    """
    if angle >= 0 and angle <= 180:
        duty = float(angle) / 10.0 + 2.5
        # I don't understand the above code, but I found it on a website
        # and it worked in testing, so I kept it.
        # Original Code Credit: Simon Monk (RazzPiSampler.OReilly.com)
        pwm.ChangeDutyCycle(duty)

def MoveStep(motor, position):
    """
    Moves stepper motor to specified position.
    """
    global M1PRESENT
    diff = position - M1PRESENT
    
    if diff > 0:
        for i in range(0,diff):
            StepBackward(motor)
        StepStop(motor)
    elif diff < 0:
        for i in range(diff,0):
            StepForward(motor)
        StepStop(motor)
    else:
        StepStop(motor)
    
    M1PRESENT = position

def OutputPins(motor):
    """
    Sets pins as outputs for given array of pins.
    """
    for i in range(0,len(motor)-1):
        GPIO.setup(motor[i], GPIO.OUT)
        GPIO.output(motor[i], False)
    print "Motor " + motor[len(motor)-1] + " set up."

def SetAuger():
    """
    Sets the treat dispenser to be enabled or disabled.
    """
    
    while True:
        try:
            t = int(raw_input("Enable treat dispenser? 1 for 'yes', 0 for 'no': "))
            if t == 1 or t == 0:
                break
            print "Invalid entry. Please enter either 1 for 'yes', or 0 for 'no'."
        except ValueError:
            print "Invalid entry. Please enter either 1 for 'yes', or 0 for 'no'."
    
    # Above forces user entry of either 1 or 0.
    # If 1, enables auger. If 0, disables auger.
    
    if t:
        return True
    else:
        return False

def SetElevation():
    """
    Sets the value for the seconds for the user-given elevation angle.
    """
    
    while True:
            try:
                degree = float(raw_input("What angle should M2 be set to? "))
                if degree >= M2MIN and degree <= M2MAX:
                    break
                print "Angle must be between %d and %d degrees!" % (M2MIN, M2MAX)
            except ValueError:
                print "Invalid entry. Numerical entry required."
    
    # Verification
    print "Elevation angle set to %d degrees." % degree
    
    # Define points for the equation of the form y = m(x-x1) + y1
    # This will relate x (user input degrees) and y (seconds)
    pt1x = float(M2MIN + M2MAX) / 2
    pt1y = M2NEUTRAL
    pt2x = M2MAX
    pt2y = M2SCNDS
    
    m = CalcSlope(pt1x, pt1y, pt2x, pt2y)

    time = CalcPointSlope(pt1x, pt1y, m, degree)
    
    return time

def SetFinish():
    """
    Defines conditions for full run of program.
    """
    global ballLimit
    global timeLimit
    
    # Queries user for end after X balls thrown or Y minutes elapsed.
    print "Program cannot run indefinitely. End condition must be set."
    print "Throw balls until ball count reached or until time limit elapsed?"
    
    while True:
        try:
            c = int(raw_input("End condition choice. 1 for 'ball', 0 for 'time': "))
            if c == 1 or c == 0:
                break
            print "Invalid entry. Please enter either 1 for 'ball', or 0 for 'time'."
        except ValueError:
            print "Invalid entry. Please enter either 1 for 'ball', or 0 for 'time'."
    
    # Above forces either 1 or 0. If 1, user chooses ball limit.
    # If 0, user chooses time limit.
    
    if c:
        while True:
            try:
                ballLimit = int(raw_input("Total number of balls to throw: "))
                if ballLimit > 0:
                    break
                print "Number of balls must be greater than zero."
            except ValueError:
                print "Invalid entry. Numerical entry required."
        print "Ball limit set to %d balls." % ballLimit

    # Sets time in minutes
    else:
        while True:
            try:
                t = int(raw_input("Total time (in minutes) to run machine: "))
                if t > 0:
                    break
                print "Time must be greater than zero."
            except ValueError:
                print "Invalid entry. Numerical entry required."
        print "Time limit set to %d minutes." % t
        # Time functions handle seconds, not minutes. Conversion required.
        timeLimit = t*60

def SetPullback():
    """
    Sets the value for the seconds for the user-given power level.
    """
    
    while True:
            try:
                power = float(raw_input("What percent of spring power should be used? "))
                if power >= M4MIN and power <= M4MAX:
                    break
                print "Percentage must be between %d% and %d%!" % (M4MIN, M4MAX)
            except ValueError:
                print "Invalid entry. Numerical entry required."
    
    # Verification
    print "Spring power set to %d degrees." % power
    
    # Define points for the equation of the form y = m(x-x1) + y1
    # This will relate x (user input percentage) and y (seconds)
    pt1x = float(M4MIN + M4MAX) / 2
    pt1y = M4NEUTRAL
    pt2x = M4MAX
    pt2y = M4SCNDS
    
    m = CalcSlope(pt1x, pt1y, pt2x, pt2y)
    
    time = CalcPointSlope(pt1x, pt1y, m, power)
    
    return time

def SetRotation():
    """
    Sets the value for the steps for the user-given rotation angle.
    """
    
    # Offers randomization of lateral angle
    while True:
        try:
            r = int(raw_input("Randomize lateral angle? 1 for 'yes', 0 for 'no': "))
            if r == 1 or r == 0:
                break
            print "Invalid entry. Please enter either 1 for 'yes', or 0 for 'no'."
        except ValueError:
            print "Invalid entry. Please enter either 1 for 'yes', or 0 for 'no'."
    
    # Above forces user entry of either 1 or 0. If 1, randomly assigns angle
    # from within user-defined range. If 0, user chooses angle.
    
    if r:
        
        # Forces lower bound for randomization
        # within possible bounds of device.
        while True:
            try:
                low = int(raw_input("Randomizing within range. Minimum angle is %d. Lower bound: " % M1MIN))
                if low >= M1MIN and low <= M1MAX:
                    break
                print "Angle must be between %d and %d degrees!" % (M1MIN, M1MAX)
            except ValueError:
                print "Invalid entry. Numerical entry required."
        
        # Forces upper bound for randomization
        # within possible bounds of device and lower bound.
        while True:
            try:
                high = int(raw_input("Randomizing within range. Maximum angle is %d. Upper bound: " % M1MAX))
                if high >= low and high <= M1MAX:
                    break
                print "Angle must be between %d and %d degrees!" % (low, M1MAX)
            except ValueError:
                print "Invalid entry. Numerical entry required."
            
        degree = random.randint(low, high)
    else:
        while True:
            try:
                degree = float(raw_input("What angle should M1 be set to? "))
                if degree >= M1MIN and degree <= M1MAX:
                    break
                print "Angle must be between %d and %d degrees!" % (M1MIN, M1MAX)
            except ValueError:
                print "Invalid entry. Numerical entry required."
    
    # Verification
    print "Rotation angle set to %d degrees." % degree
    
    # Define points for the equation of the form y = m(x-x1) + y1
    # This will relate x (user input degrees) and y (steps)
    pt1x = float(M1MIN + M1MAX) / 2
    pt1y = M1NEUTRAL
    pt2x = M1MAX
    pt2y = M1STEPS
    
    m = CalcSlope(pt1x, pt1y, pt2x, pt2y)
    
    # M1 needs integer count of steps.
    step = int(CalcPointSlope(pt1x, pt1y, m, degree))
    
    return step

def SetStep(motor, w1, w2, w3, w4):
    """
    Sets the step for a given stepper motor array and wire setup.
    """
    GPIO.output(motor[1], w1)
    GPIO.output(motor[2], w2)
    GPIO.output(motor[3], w3)
    GPIO.output(motor[4], w4)

def StepBackward(motor):
    """
    Moves stepper motor one cycle backward.
    """
    GPIO.output(motor[0], True)
    global SPCOUNT
    beat = SPCOUNT % 4

    if beat == 0:
        SetStep(motor,0,0,0,1)
        time.sleep(STEPDLY)
    if beat == 1:
        SetStep(motor,0,1,0,0)
        time.sleep(STEPDLY)
    if beat == 2:
        SetStep(motor,0,0,1,0)
        time.sleep(STEPDLY)
    if beat == 3:
        SetStep(motor,1,0,0,0)
        time.sleep(STEPDLY)

    SPCOUNT = SPCOUNT + 1

def StepForward(motor):
    """
    Moves stepper motor one cycle forward.
    """
    GPIO.output(motor[0], True)
    global SPCOUNT
    beat = SPCOUNT % 4

    if beat == 0:
        SetStep(motor,1,0,0,0)
        time.sleep(STEPDLY)
    if beat == 1:
        SetStep(motor,0,0,1,0)
        time.sleep(STEPDLY)
    if beat == 2:
        SetStep(motor,0,1,0,0)
        time.sleep(STEPDLY)
    if beat == 3:
        SetStep(motor,0,0,0,1)
        time.sleep(STEPDLY)

    SPCOUNT = SPCOUNT + 1

def StepStop(motor):
    """
    Stops stepper motor.
    """
    GPIO.output(motor[0], False)
    SetStep(motor,0,0,0,0)

####END SUBROUTINES. MAIN PROGRAM BELOW.####

####INITIALIZATION####

# Library Imports
import RPi.GPIO as GPIO
import random
import time

# Board Setup
GPIO.setmode(GPIO.BOARD)
GPIO.setwarnings(False)

# GPIO Pin Allocation                       #Variable Name
# 01: Reserved - constant 3.3V source 
# 02: Reserved - constant 5.0V source
# 03: USED - Limit Switch 1                 #L1
# 04: Reserved - constant 5.0V source
# 05: USED - Limit Switch 2                 #L2
# 06: Reserved - ground
# 07: USED - Limit Switch 3                 #L3
# 08: USED - Stepper Motor 1 pin 1          #M11
# 09: Reserved - ground
# 10: USED - Stepper Motor 1 pin 2          #M12
# 11: USED - Limit Switch 4                 #L4
######### 12: USED - Stepper Motor 1 pin 3              #M13 NO LONGER TRUE.
# 12: USED - Servo Motor 5 enable           #M5e
# 13: USED - Limit Switch 5                 #L5
# 14: Reserved - ground
# 15: USED - Limit Switch 6                 #L6
# 16: USED - Stepper Motor 1 pin 4          #M14
# 17: Reserved - constant 3.3V source
# 18: USED - DC Motor 2 pin 1               #M21
# 19: USED - Force Sensor                   #F1
# 20: Reserved - ground
# 21: USED - PIR Sensor                     #P1
# 22: USED - DC Motor 2 pin 2               #M22
# 23: USED - Stepper Motor 1 enable         #M1e
# 24: USED - DC Motor 3 pin 1               #M31
# 25: Reserved - ground
# 26: USED - DC Motor 3 pin 2               #M32
# 27: Reserved - ID_SD I2C ID EEPROM
# 28: Reserved - ID_SC I2C ID EEPROM
# 29: USED - DC Motor 2 enable              #M2e
# 30: Reserved - ground
# 31: USED - DC Motor 3 enable              #M3e
# 32: USED - DC Motor 4 pin 1               #M41
# 33: USED - DC Motor 4 enable              #M4e
# 34: Reserved - ground
######### 35: USED - Servo Motor 5 enable               #M5e NO LONGER TRUE.
# 35: <free>
# 36: USED - DC Motor 4 pin 2               #M42
# 37: <free>
######### 38: <free>                                         NO LONGER TRUE.
# 38: USED - Stepper Motor 1 pin 3          #M13
# 39: Reserved - ground
# 40: <free>

#SWITCHES
# Limit switch 1. L1. Rotation (lower limit). Static Base. Pin 3.
# Limit switch 2. L2. Rotation (upper limit). Static Base. Pin 5.
# Limit switch 3. L3. Elevation (lower limit). Rotating platform. Pin 7.
# Limit switch 4. L4. Elevation (upper limit). Rotating platform. Pin 11.
# Limit switch 5. L5. Pullback (min power). Angled platform. Pin 13.
# Limit switch 6. L6. Pullback (max power). Angled platform. Pin 15.
# Force sensor 1. F1. Launching (weight). Launch Tube. Pin 19.
# PIR/prox sw. 1. P1. Launching (path). Launch Tube. Pin 21.

#MOTORS
# Motor 1. M1. Rotation. Static Base. Stepper. Pins 23[e],8,10,38,16.
# Motor 2. M2. Elevation. Rotating platform. DC. Pins 29[e],18,22.
# Motor 3. M3. Auger. Rotating platform. DC. Pins 31[e],24,26.
# Motor 4. M4. Pullback. Angled platform. DC. Pins 33[e],32,36.
# Motor 5. M5. Grabber. Angled platform. Servo. Pin 12[e].

# Variable Initialization
# Pin values. Motors pins condensed to arrays for ease of reference.
L1 = 3
L2 = 5
L3 = 7
L4 = 11
L5 = 13
L6 = 15
F1 = 19
P1 = 21
M1e = 23
M2e = 29
M3e = 31
M4e = 33
M5e = 12
M11 = 8
M12 = 10
M13 = 38
M14 = 16
M21 = 18
M22 = 22
M31 = 24
M32 = 26
M41 = 32
M42 = 36
M1 = [M1e, M11, M12, M13, M14,'M1']
M2 = [M2e, M21, M22,'M2']
M3 = [M3e, M31, M32,'M3']
M4 = [M4e, M41, M42,'M4']
M5 = [M5e,'M5']
# To prevent accidental input to the system, free pins designated as outputs.
free = [37, 35, 40,'FREE PINS']

# Motor 1 Variables
M1STEPS = 0             # Steps count for M1 motion
M1NEUTRAL = 0           # Neutral Position for M1.
M1PRESENT = 0           # Present Position for M1.
M1MIN = -15             # Minimum Value for M1. (deg)
M1MAX = 15              # Maximum Value for M1. (deg)

# Motor 2 Variables
M2SCNDS = 0             # Travel time for M2 motion
M2NEUTRAL = 0           # Neutral Position for M2.
M2PRESENT = 0           # Present Position for M2.
M2MIN = 5               # Minimum Value for M2. (deg)
M2MAX = 45              # Maximum Value for M2. (deg)

# Motor 3 Variables
TREATSCNDS = 0          # Time to turn auger to dispense one treat.

# Motor 4 Variables
M4SCNDS = 0             # Travel time for M4 motion
M4NEUTRAL = 0           # Neutral Position for M4.
M4PRESENT = 0           # Present Position for M4.
M4MIN = 1               # Minimum Value for M4. (%)
M4MAX = 100             # Maximum Value for M4. (%)

# Other variables
STEPDLY = 0.01          # Delay between steps of stepper motor.
SPCOUNT = 0             # Counter for stepper motion
LIMFACT = 0.95          # Percentage of total range of motion allowed.
ballCount = 0           # Number of balls successfully thrown by FIDO.
ballLimit = 0           # Max balls thrown before FIDO stops.
timeStart = time.time() # Start time of FIDO operation.
timeLimit = 0           # Time limit before FIDO stops.
Current_State  = 0      # Current State of the IR sensor.
Previous_State = 0      # Previous State of the IR sensor.
DELTA = 0.1             # Time between checks of IR sensor (s).
CHECKTIME = 3/DELTA     # Total number of checks. 3 seconds delay = 30 checks.

# Pin Setup
# Set GPIO Pins as Outputs
OutputPins(M1)
OutputPins(M2)
OutputPins(M3)
OutputPins(M4)
OutputPins(M5)
OutputPins(free)

# Set GPIO Pins as Inputs - Pull Down Resistors
InputPinsDOWN(L1,'L1')
InputPinsDOWN(L2,'L2')
InputPinsDOWN(L3,'L3')
InputPinsDOWN(L4,'L4')
InputPinsDOWN(L5,'L5')
InputPinsDOWN(L6,'L6')
InputPinsDOWN(F1,'F1')
InputPinsDOWN(P1,'P1')

####CALIBRATION####

M1STEPS = CalibrateStepper(M1)
M2SCNDS = CalibrateDC(M2)
M4SCNDS = CalibrateDC(M4)

#After calibration, setup servo.
pwm = GPIO.PWM(M5[0], 100) # Sends a pulse at 100Hz, every 10ms.
pwm.start(5)
MoveServo(0) # Assuming 0deg holds ball, and 90deg releases ball
#### ABOVE SUBJECT TO CHANGE PENDING MOUNTED SERVO TEST.
#### SERVO SHOULD ENGAGE PINBALL AT THIS POINT.

# GPIO Events
GPIO.add_event_detect(L1, GPIO.RISING, callback=EStop, bouncetime=300)
GPIO.add_event_detect(L2, GPIO.RISING, callback=EStop, bouncetime=300)
GPIO.add_event_detect(L3, GPIO.RISING, callback=EStop, bouncetime=300)
GPIO.add_event_detect(L4, GPIO.RISING, callback=EStop, bouncetime=300)
GPIO.add_event_detect(L6, GPIO.RISING, callback=EStop, bouncetime=300)

####MAIN PROGRAM####
                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                    
SetFinish()

while not (CountBalls() and CountTime()):
    
    # Ready - gets user input.
    Rot_steps = SetRotation()
    Elev_secs = SetElevation()
    Pull_secs = SetPullback()
    Aug_onoff = SetAuger()
    
    # Ready - does not proceed past this line until ball in place.
    GPIO.wait_for_edge(F1, GPIO.RISING)
    
    # Aim - moves into position
    MoveStep(M1, Rot_steps)
    MoveDC(M2, Elev_secs)
    
    # Engages spring:
    MoveDC(M4, Pull_secs)
    
    # Aim - waits for clearance
    for dt in range(0, int(CHECKTIME)):
        if isClear() != True:
            dt = 0
        time.sleep(DELTA)
    
    # Fires
    MoveServo(90)

    # Finish - Dispenses treat, increases ball count
    if Aug_onoff:
        MoveDC(M3, TREATSCNDS)
    ballCount += 1

GPIO.cleanup()