Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- import RPi.GPIO as GPIO
- import time
- GPIO.setwarnings(False)
- GPIO.setmode(GPIO.BCM)
- TRIG = 17 #11
- ECHO = 27 #13
- statusLED = 22
- pin1MotorA=16 #32
- pin2MotorA=12 #36
- pin1MotorB=21 #40
- pin2MotorB=20 #38
- GPIO.setup(TRIG,GPIO.OUT)
- GPIO.setup(ECHO,GPIO.IN)
- GPIO.setup(statusLED,GPIO.OUT)
- GPIO.setup(pin1MotorA,GPIO.OUT)
- GPIO.setup(pin2MotorA,GPIO.OUT)
- GPIO.setup(pin1MotorB,GPIO.OUT)
- GPIO.setup(pin2MotorB,GPIO.OUT)
- GPIO.output(statusLED, 1)
- time.sleep(5)
- def stop():
- GPIO.output(pin1MotorA, 0)
- GPIO.output(pin2MotorA, 0)
- GPIO.output(pin1MotorB, 0)
- GPIO.output(pin2MotorB, 0)
- print "stop"
- def forward():
- GPIO.output(pin1MotorA, 1)
- GPIO.output(pin2MotorA, 0)
- GPIO.output(pin1MotorB, 1)
- GPIO.output(pin2MotorB, 0)
- print "Forward"
- def back():
- GPIO.output(pin1MotorA, 0)
- GPIO.output(pin2MotorA, 1)
- GPIO.output(pin1MotorB, 0)
- GPIO.output(pin2MotorB, 1)
- print "back"
- def left():
- GPIO.output(pin1MotorA, 0)
- GPIO.output(pin2MotorA, 1)
- GPIO.output(pin1MotorB, 1)
- GPIO.output(pin2MotorB, 0)
- print "left"
- def right():
- GPIO.output(pin1MotorA, 1)
- GPIO.output(pin2MotorA, 0)
- GPIO.output(pin1MotorB, 0)
- GPIO.output(pin2MotorB, 1)
- print "right"
- stop()
- count=0
- while True:
- i=0
- avgDistance=0
- for i in range(5):
- GPIO.output(TRIG, False)
- time.sleep(0.1)
- GPIO.output(TRIG, True)
- time.sleep(0.00001)
- GPIO.output(TRIG, False)
- while GPIO.input(ECHO)==0:
- GPIO.output(statusLED, False)
- pulse_start = time.time()
- while GPIO.input(ECHO)==1:
- GPIO.output(statusLED, False)
- pulse_end = time.time()
- pulse_duration = pulse_end - pulse_start
- distance = pulse_duration * 17150
- distance = round(distance,2)
- avgDistance=avgDistance+distance
- #avgDistance=avgDistance/5
- print avgDistance
- flag=0
- if avgDistance < 15: #Check whether the distance is within 15 cm range
- count=count+1
- stop()
- time.sleep(1)
- back()
- time.sleep(1.5)
- if (count%3 ==1) & (flag==0):
- right()
- flag=1
- else:
- left()
- flag=0
- time.sleep(1.5)
- stop()
- time.sleep(1)
- else:
- forward()
- flag=0
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement