#Obstacle avoider robot porgram for Roundbot Pi from Robokits

try:
	from time import sleep
	import time
	import os
	import RPi.GPIO as GPIO
	
	GPIO.setmode(GPIO.BCM)
	GPIO_TRIGGER = 23
	GPIO_ECHO = 22
	GPIO.setup(GPIO_TRIGGER,GPIO.OUT)	# Trigger for Ultrasonic Sensor
	GPIO.setup(GPIO_ECHO,GPIO.IN)		# Echo for Ultrasonic Sensor
	GPIO.setup(7, GPIO.OUT)				#Left Motor Backward
	GPIO.setup(8, GPIO.OUT)				#Left Motor Forward
	GPIO.setup(24, GPIO.OUT)			#Right Motor Forward
	GPIO.setup(25, GPIO.OUT)			#Right Motor Backward
	GPIO.setup(11, GPIO.IN, pull_up_down=GPIO.PUD_UP)		#Shutdown trigger
	
	def measure_distance():
		GPIO.output(GPIO_TRIGGER, False)
		# Set trigger to False (Low)
		GPIO.output(GPIO_TRIGGER, False)

		# Send 10us pulse to trigger
		GPIO.output(GPIO_TRIGGER, True)
		time.sleep(0.00001)
		GPIO.output(GPIO_TRIGGER, False)
		start = time.time()
		while GPIO.input(GPIO_ECHO)==0:
		  start = time.time()

		while GPIO.input(GPIO_ECHO)==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) devided by 2
		# because the distance is double as sound wave goes
		# to the object and comes back. - 34000/2
		
		distance = elapsed * 17000

		#print "%.1f" % distance
		return distance
	
	def forward():
		GPIO.output(7,False)       
		GPIO.output(8,True)       
		GPIO.output(24,True)       
		GPIO.output(25,False)       
	
	def backward():
		GPIO.output(7,True)       
		GPIO.output(8,False)       
		GPIO.output(24,False)       
		GPIO.output(25,True)       
	
	def right():
		GPIO.output(7,False)       
		GPIO.output(8,True)       
		GPIO.output(24,False)       
		GPIO.output(25,True)       
	
	def left():
		GPIO.output(7,True)       
		GPIO.output(8,False)       
		GPIO.output(24,True)       
		GPIO.output(25,False)       
	
	def stop():
		GPIO.output(7,False)       
		GPIO.output(8,False)       
		GPIO.output(24,False)       
		GPIO.output(25,False)       

	def shutdownpi(foo):
		stop();
		os.system("shutdown -h now")
	
	GPIO.add_event_detect(11, GPIO.FALLING, callback=shutdownpi, bouncetime=800) 
	
	while True:
		d = measure_distance()
		if d<15:
			left()
			sleep(.3);
		else:
			forward()
			sleep(.01);


except KeyboardInterrupt:
	GPIO.cleanup()