ultrasonic.py

import RPi.GPIO as GPIO
import time


class US():
	def rotate(self, angle):
		duty=float(angle)/9.0+2.5
		self.pwm.ChangeDutyCycle(duty)
#		self.pwm.ChangeDutyCycle(0)

	def distance(self):
		GPIO.output(self.triggerPin,True)
		time.sleep(0.00001)
		GPIO.output(self.triggerPin,False)
		start=time.time()
		stop=time.time()
		while GPIO.input(self.echoPin) == 0:
			start=time.time()
		while GPIO.input(self.echoPin) == 1:
			stop=time.time()
		
		stop=time.time()
		return((stop-start)*34300.0)/2.0

	def findBestWay(self):
		#return the angle where to go next
		func = reversed if self.rev else list
		arr = [0] * self.steps
		for i in func(range(self.steps)):
			print(i*180/(self.steps-1))
			self.rotate(i*180.0/(self.steps-1))
			time.sleep(0.2)
			arr[i]=self.distance()
			#dist > 500cm or dist <5cm means that a timeout occured => value wrong
			if arr[i] > 500 or arr[i] < 5:
				for j in range(3):
					arr[i]=self.distance()
					if arr[i] < 500 and arr[i] > 5:
						break
					else:
						arr[i]=-1

		self.rev = not self.rev
		self.pwm.ChangeDutyCycle(0)
		return arr

	def findWay(self, actualIndex):
		dist=self.distance()

	def __init__(self, servoPin =22, triggerPin=12, echoPin=13):
		GPIO.setwarnings(False)
		GPIO.setmode(GPIO.BCM)
		#save vars
		self.echoPin=echoPin
		self.triggerPin=triggerPin
		GPIO.setup(self.echoPin,GPIO.IN)
		GPIO.setup(self.triggerPin,GPIO.OUT)
		#init servo
		self.servoPin=servoPin
		GPIO.setup(self.servoPin,GPIO.OUT)
		self.pwm = GPIO.PWM(servoPin,100)
		self.pwm.start(5)
		self.steps = 7 # should be odd
		self.rev =False
		#set servo on 90 degree (mid)
		self.rotate(90)
		time.sleep(0.5)
		self.pwm.ChangeDutyCycle(0)
		time.sleep(0.2)