#!/usr/bin/python
#
# ultrasonic_2.py
# Measure distance using an ultrasonic module
# in a loop.
#
# Author : Oliver Dyson
# Date   : 17/12/2013

# -----------------------
# Import required Python libraries
# -----------------------
import time
import RPi.GPIO as GPIO

# -----------------------
# Define some functions
# -----------------------

def measure():
  # This function measures a distance in cm
  #print("Measuring distance:")
  GPIO.output(GPIO_TRIGGER, True)
  time.sleep(0.00001)
  GPIO.output(GPIO_TRIGGER, False)
  #print("Pulse sent")
  start = stop = time.time()

  while GPIO.input(GPIO_ECHO)==0:
    start = time.time()

  while GPIO.input(GPIO_ECHO)==1:
    stop = time.time()
    if stop - start >= 0.6:
      print("No echo received!")

  elapsed = stop - start
  #print("Elapsed time = %f" % elapsed)
  # Speed of sound = 34300cm/s at sea level and 20C
  distance = (elapsed * 34300)/2

  return distance

def measure_average():
  # This function takes 3 measurements and
  # returns the average, in metres.
  distance1=measure()
  time.sleep(0.1)
  distance2=measure()
  time.sleep(0.1)
  distance3=measure()
  distance = distance1 + distance2 + distance3
  distance = distance / 3
  return distance

# -----------------------
# Main Script
# -----------------------

# Use BCM GPIO references
# instead of physical pin numbers
GPIO.setmode(GPIO.BCM)

# Define GPIO to use on Pi
GPIO_TRIGGER = 17 # Pin 11
GPIO_ECHO    = 18 # Pin 12
GPIO_RED     = 27 # Pin 13
GPIO_AMBER   = 22 # Pin 15
GPIO_GREEN   = 23 # Pin 16
GPIO_WAIT    = 24 # Pin 18
GPIO_CROSS   = 25 # Pin 22
GPIO_BEEP    = 4  # Pin 7

# Set pins as output and input
GPIO.setup(GPIO_TRIGGER,GPIO.OUT)  # Trigger
GPIO.setup(GPIO_ECHO,GPIO.IN)      # Echo
GPIO.setup(GPIO_RED,GPIO.OUT)      # Red light
GPIO.setup(GPIO_AMBER,GPIO.OUT)    # Amber light
GPIO.setup(GPIO_GREEN,GPIO.OUT)    # Green light
GPIO.setup(GPIO_WAIT,GPIO.OUT)     # Pedestrian Wait light
GPIO.setup(GPIO_CROSS,GPIO.OUT)    # Pedestrian Cross light
GPIO.setup(GPIO_BEEP,GPIO.OUT)     # Beep

# Set all outputs to False (Low)
GPIO.output(GPIO_TRIGGER, False)
GPIO.output(GPIO_RED, False)
GPIO.output(GPIO_AMBER, False)
GPIO.output(GPIO_GREEN, False)
GPIO.output(GPIO_WAIT, False)
GPIO.output(GPIO_CROSS, False)
GPIO.output(GPIO_BEEP, False)

# Wrap main content in a try block so we can
# catch the user pressing CTRL-C and run the
# GPIO cleanup function. This will also prevent
# the user seeing lots of unnecessary error
# messages.
try:

    # First, test the LEDs and the beeper
    print("Pelican crossing - testing LEDs and beeper:")
    GPIO.output(GPIO_RED, True)
    time.sleep(0.5)
    GPIO.output(GPIO_RED, False)
    GPIO.output(GPIO_AMBER, True)
    time.sleep(0.5)
    GPIO.output(GPIO_AMBER, False)
    GPIO.output(GPIO_GREEN, True)
    time.sleep(0.5)
    GPIO.output(GPIO_GREEN, False)
    GPIO.output(GPIO_WAIT, True)
    time.sleep(0.5)
    GPIO.output(GPIO_WAIT, False)
    GPIO.output(GPIO_CROSS, True)
    time.sleep(0.5)
    GPIO.output(GPIO_CROSS, False)
    GPIO.output(GPIO_BEEP, True)
    time.sleep(0.5)
    GPIO.output(GPIO_BEEP, False)

    # Now test the ultrasonic module
    print("Testing utrasonic module.")
    print("Test will end when distance < 10cm.")
    while True:
        distance = measure_average()
        print("Distance: %d" % distance)

        if distance < 10:
            GPIO.output(GPIO_BEEP, True)
            time.sleep(0.1)
            GPIO.output(GPIO_BEEP, False)
            break
        time.sleep(0.5)

#
# We start with the Green light and the pedestrian Wait light on
# and continually measure distance.
#
# When we get readings of less than 15cm for 3 secs, we:
# Switch off Green and switch on Amber
# Wait 3 secs
# Switch off Amber and switch on Red
# Wait 3 secs
# Switch off Pedestrian Wait and switch on Pedestrian Cross
# Beep the beeper 4 times/sec for 2 secs
# Switch off Red and
#   Flash Pedestrian Cross and Amber twice a second for 3 secs
# Go back to the start
#
    # Now for the pelican crossing.
    # Set Green and Wait lights
    print("Starting pelican crossing")
    GPIO.output(GPIO_GREEN, True)
    GPIO.output(GPIO_WAIT, True)
    while True:
        count = 0
        while True:
            distance = measure_average()
            print("Distance: %d" % distance)
            if distance < 10:
                count = count + 1
                if count == 3:
                    break
            else:
                count = 0
            time.sleep(1)

        # Now we change the crossing
        GPIO.output(GPIO_GREEN, False)
        GPIO.output(GPIO_AMBER, True)
        time.sleep(3)

        GPIO.output(GPIO_AMBER, False)
        GPIO.output(GPIO_RED, True)
        time.sleep(3)

        GPIO.output(GPIO_WAIT, False)
        GPIO.output(GPIO_CROSS, True)

        # count counts units of 0.125 secs, i.e. counts 8 per sec
        count = 0
        beeper = flasher = False
        while count < 64:
            if count < 24:
                beeper = not beeper
                GPIO.output(GPIO_BEEP, beeper)
            if count == 24:
                GPIO.output(GPIO_RED, False)
            if count >= 24:
                if count % 4 == 0:
                    flasher = not flasher
                    GPIO.output(GPIO_CROSS, flasher)
                    GPIO.output(GPIO_AMBER, flasher)
            count = count + 1
            time.sleep(0.125)

        GPIO.output(GPIO_AMBER, False)
        GPIO.output(GPIO_GREEN, True)
        GPIO.output(GPIO_WAIT, True)
        time.sleep(3)

except KeyboardInterrupt:
  # User pressed CTRL-C
  # Reset GPIO settings
  GPIO.cleanup()