Advertisement
rabirajkhadka

Obstacle Avoiding Robot using Raspberry Pi

Nov 8th, 2019
165
0
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
Python 2.26 KB | None | 0 0
  1. import RPi.GPIO as GPIO
  2. import time
  3.  
  4. GPIO.setwarnings(False)
  5. GPIO.setmode(GPIO.BCM)
  6.  
  7. TRIG = 17   #11
  8. ECHO = 27   #13
  9. statusLED = 22
  10.  
  11. pin1MotorA=16   #32
  12. pin2MotorA=12   #36
  13. pin1MotorB=21   #40
  14. pin2MotorB=20   #38
  15.  
  16. GPIO.setup(TRIG,GPIO.OUT)
  17. GPIO.setup(ECHO,GPIO.IN)
  18. GPIO.setup(statusLED,GPIO.OUT)
  19.  
  20. GPIO.setup(pin1MotorA,GPIO.OUT)
  21. GPIO.setup(pin2MotorA,GPIO.OUT)
  22. GPIO.setup(pin1MotorB,GPIO.OUT)
  23. GPIO.setup(pin2MotorB,GPIO.OUT)
  24.  
  25. GPIO.output(statusLED, 1)
  26.  
  27. time.sleep(5)
  28.  
  29. def stop():
  30.     GPIO.output(pin1MotorA, 0)
  31.     GPIO.output(pin2MotorA, 0)
  32.     GPIO.output(pin1MotorB, 0)
  33.     GPIO.output(pin2MotorB, 0)
  34.     print "stop"
  35.  
  36. def forward():
  37.     GPIO.output(pin1MotorA, 1)
  38.     GPIO.output(pin2MotorA, 0)
  39.     GPIO.output(pin1MotorB, 1)
  40.     GPIO.output(pin2MotorB, 0)
  41.     print "Forward"
  42.  
  43. def back():
  44.     GPIO.output(pin1MotorA, 0)
  45.     GPIO.output(pin2MotorA, 1)
  46.     GPIO.output(pin1MotorB, 0)
  47.     GPIO.output(pin2MotorB, 1)
  48.     print "back"
  49.  
  50. def left():
  51.     GPIO.output(pin1MotorA, 0)
  52.     GPIO.output(pin2MotorA, 1)
  53.     GPIO.output(pin1MotorB, 1)
  54.     GPIO.output(pin2MotorB, 0)
  55.     print "left"
  56.  
  57. def right():
  58.     GPIO.output(pin1MotorA, 1)
  59.     GPIO.output(pin2MotorA, 0)
  60.     GPIO.output(pin1MotorB, 0)
  61.     GPIO.output(pin2MotorB, 1)
  62.     print "right"
  63.  
  64. stop()
  65. count=0
  66. while True:
  67.  i=0
  68.  avgDistance=0
  69.  for i in range(5):
  70.   GPIO.output(TRIG, False)                
  71.   time.sleep(0.1)                                  
  72.  
  73.   GPIO.output(TRIG, True)
  74.   time.sleep(0.00001)
  75.   GPIO.output(TRIG, False)
  76.  
  77.   while GPIO.input(ECHO)==0:
  78.        GPIO.output(statusLED, False)            
  79.   pulse_start = time.time()
  80.  
  81.   while GPIO.input(ECHO)==1:
  82.        GPIO.output(statusLED, False)
  83.   pulse_end = time.time()
  84.   pulse_duration = pulse_end - pulse_start
  85.  
  86.   distance = pulse_duration * 17150
  87.   distance = round(distance,2)
  88.   avgDistance=avgDistance+distance
  89.  
  90.  #avgDistance=avgDistance/5
  91.  print avgDistance
  92.  flag=0
  93.  if avgDistance < 15:      #Check whether the distance is within 15 cm range
  94.     count=count+1
  95.     stop()
  96.     time.sleep(1)
  97.     back()
  98.     time.sleep(1.5)
  99.     if (count%3 ==1) & (flag==0):
  100.      right()
  101.      flag=1
  102.     else:
  103.      left()
  104.      flag=0
  105.     time.sleep(1.5)
  106.     stop()
  107.     time.sleep(1)
  108.  else:
  109.     forward()
  110.     flag=0
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement