API tools faq
paste
Advertisement
safwan092

Untitled

safwan092
May 2nd, 2017
206
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
Python 9.74 KB | None | 0 0
raw download clone embed print report
  1. # import the necessary packages
  2. from picamera.array import PiRGBArray     #As there is a resolution problem in raspberry pi, will not be able to capture frames by VideoCapture
  3. from picamera import PiCamera
  4. import RPi.GPIO as GPIO
  5. import time
  6. import cv2
  7. import cv2.cv as cv
  8. import numpy as np
  9.  
  10. #hardware work
  11. GPIO.setmode(GPIO.BOARD)
  12.  
  13. GPIO_TRIGGER1 = 29      #Left ultrasonic sensor
  14. GPIO_ECHO1 = 31
  15.  
  16. GPIO_TRIGGER2 = 36      #Front ultrasonic sensor
  17. GPIO_ECHO2 = 37
  18.  
  19. GPIO_TRIGGER3 = 33      #Right ultrasonic sensor
  20. GPIO_ECHO3 = 35
  21.  
  22. MOTOR1B=18  #Left Motor
  23. MOTOR1E=22
  24.  
  25. MOTOR2B=21  #Right Motor
  26. MOTOR2E=19
  27.  
  28. LED_PIN=13  #If it finds the ball, then it will light up the led
  29.  
  30. # Set pins as output and input
  31. GPIO.setup(GPIO_TRIGGER1,GPIO.OUT)  # Trigger
  32. GPIO.setup(GPIO_ECHO1,GPIO.IN)      # Echo
  33. GPIO.setup(GPIO_TRIGGER2,GPIO.OUT)  # Trigger
  34. GPIO.setup(GPIO_ECHO2,GPIO.IN)
  35. GPIO.setup(GPIO_TRIGGER3,GPIO.OUT)  # Trigger
  36. GPIO.setup(GPIO_ECHO3,GPIO.IN)
  37. GPIO.setup(LED_PIN,GPIO.OUT)
  38.  
  39. # Set trigger to False (Low)
  40. GPIO.output(GPIO_TRIGGER1, False)
  41. GPIO.output(GPIO_TRIGGER2, False)
  42. GPIO.output(GPIO_TRIGGER3, False)
  43.  
  44. # Allow module to settle
  45. def sonar(GPIO_TRIGGER,GPIO_ECHO):
  46.       start=0
  47.       stop=0
  48.       # Set pins as output and input
  49.       GPIO.setup(GPIO_TRIGGER,GPIO.OUT)  # Trigger
  50.       GPIO.setup(GPIO_ECHO,GPIO.IN)      # Echo
  51.      
  52.       # Set trigger to False (Low)
  53.       GPIO.output(GPIO_TRIGGER, False)
  54.      
  55.       # Allow module to settle
  56.       time.sleep(0.01)
  57.            
  58.       #while distance > 5:
  59.       #Send 10us pulse to trigger
  60.       GPIO.output(GPIO_TRIGGER, True)
  61.       time.sleep(0.00001)
  62.       GPIO.output(GPIO_TRIGGER, False)
  63.       begin = time.time()
  64.       while GPIO.input(GPIO_ECHO)==0 and time.time()<begin+0.05:
  65.             start = time.time()
  66.      
  67.       while GPIO.input(GPIO_ECHO)==1 and time.time()<begin+0.1:
  68.             stop = time.time()
  69.      
  70.       # Calculate pulse length
  71.       elapsed = stop-start
  72.       # Distance pulse travelled in that time is time
  73.       # multiplied by the speed of sound (cm/s)
  74.       distance = elapsed * 34000
  75.      
  76.       # That was the distance there and back so halve the value
  77.       distance = distance / 2
  78.      
  79.       print "Distance : %.1f" % distance
  80.       # Reset GPIO settings
  81.       return distance
  82.  
  83. GPIO.setup(MOTOR1B, GPIO.OUT)
  84. GPIO.setup(MOTOR1E, GPIO.OUT)
  85.  
  86. GPIO.setup(MOTOR2B, GPIO.OUT)
  87. GPIO.setup(MOTOR2E, GPIO.OUT)
  88.  
  89. def forward():
  90.       GPIO.output(MOTOR1B, GPIO.HIGH)
  91.       GPIO.output(MOTOR1E, GPIO.LOW)
  92.       GPIO.output(MOTOR2B, GPIO.HIGH)
  93.       GPIO.output(MOTOR2E, GPIO.LOW)
  94.      
  95. def reverse():
  96.       GPIO.output(MOTOR1B, GPIO.LOW)
  97.       GPIO.output(MOTOR1E, GPIO.HIGH)
  98.       GPIO.output(MOTOR2B, GPIO.LOW)
  99.       GPIO.output(MOTOR2E, GPIO.HIGH)
  100.      
  101. def rightturn():
  102.       GPIO.output(MOTOR1B,GPIO.LOW)
  103.       GPIO.output(MOTOR1E,GPIO.HIGH)
  104.       GPIO.output(MOTOR2B,GPIO.HIGH)
  105.       GPIO.output(MOTOR2E,GPIO.LOW)
  106.      
  107. def leftturn():
  108.       GPIO.output(MOTOR1B,GPIO.HIGH)
  109.       GPIO.output(MOTOR1E,GPIO.LOW)
  110.       GPIO.output(MOTOR2B,GPIO.LOW)
  111.       GPIO.output(MOTOR2E,GPIO.HIGH)
  112.  
  113. def stop():
  114.       GPIO.output(MOTOR1E,GPIO.LOW)
  115.       GPIO.output(MOTOR1B,GPIO.LOW)
  116.       GPIO.output(MOTOR2E,GPIO.LOW)
  117.       GPIO.output(MOTOR2B,GPIO.LOW)
  118.      
  119. #Image analysis work
  120. def segment_colour(frame):    #returns only the red colors in the frame
  121.     hsv_roi =  cv2.cvtColor(frame, cv2.cv.CV_BGR2HSV)
  122.     mask_1 = cv2.inRange(hsv_roi, np.array([160, 160,10]), np.array([190,255,255]))
  123.     ycr_roi=cv2.cvtColor(frame,cv2.cv.CV_BGR2YCrCb)
  124.     mask_2=cv2.inRange(ycr_roi, np.array((0.,165.,0.)), np.array((255.,255.,255.)))
  125.  
  126.     mask = mask_1 | mask_2
  127.     kern_dilate = np.ones((8,8),np.uint8)
  128.     kern_erode  = np.ones((3,3),np.uint8)
  129.     mask= cv2.erode(mask,kern_erode)      #Eroding
  130.     mask=cv2.dilate(mask,kern_dilate)     #Dilating
  131.     #cv2.imshow('mask',mask)
  132.     return mask
  133.  
  134. def find_blob(blob): #returns the red colored circle
  135.     largest_contour=0
  136.     cont_index=0
  137.     contours, hierarchy = cv2.findContours(blob, cv2.RETR_CCOMP, cv2.CHAIN_APPROX_SIMPLE)
  138.     for idx, contour in enumerate(contours):
  139.         area=cv2.contourArea(contour)
  140.         if (area >largest_contour) :
  141.             largest_contour=area
  142.            
  143.             cont_index=idx
  144.             #if res>15 and res<18:
  145.             #    cont_index=idx
  146.                              
  147.     r=(0,0,2,2)
  148.     if len(contours) > 0:
  149.         r = cv2.boundingRect(contours[cont_index])
  150.        
  151.     return r,largest_contour
  152.  
  153. def target_hist(frame):
  154.     hsv_img=cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)
  155.    
  156.     hist=cv2.calcHist([hsv_img],[0],None,[50],[0,255])
  157.     return hist
  158.  
  159. #CAMERA CAPTURE
  160. #initialize the camera and grab a reference to the raw camera capture
  161. camera = PiCamera()
  162. camera.resolution = (160, 120)
  163. camera.framerate = 16
  164. rawCapture = PiRGBArray(camera, size=(160, 120))
  165.  
  166. # allow the camera to warmup
  167. time.sleep(0.001)
  168.  
  169. # capture frames from the camera
  170. for image in camera.capture_continuous(rawCapture, format="bgr", use_video_port=True):
  171.       #grab the raw NumPy array representing the image, then initialize the timestamp and occupied/unoccupied text
  172.       frame = image.array
  173.       frame=cv2.flip(frame,1)
  174.       global centre_x
  175.       global centre_y
  176.       centre_x=0.
  177.       centre_y=0.
  178.       hsv1 = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)
  179.       mask_red=segment_colour(frame)      #masking red the frame
  180.       loct,area=find_blob(mask_red)
  181.       x,y,w,h=loct
  182.      
  183.       #distance coming from front ultrasonic sensor
  184.       distanceC = sonar(GPIO_TRIGGER2,GPIO_ECHO2)
  185.       #distance coming from right ultrasonic sensor
  186.       distanceR = sonar(GPIO_TRIGGER3,GPIO_ECHO3)
  187.       #distance coming from left ultrasonic sensor
  188.       distanceL = sonar(GPIO_TRIGGER1,GPIO_ECHO1)
  189.              
  190.       if (w*h) < 10:
  191.             found=0
  192.       else:
  193.             found=1
  194.             simg2 = cv2.rectangle(frame, (x,y), (x+w,y+h), 255,2)
  195.             centre_x=x+((w)/2)
  196.             centre_y=y+((h)/2)
  197.             cv2.circle(frame,(int(centre_x),int(centre_y)),3,(0,110,255),-1)
  198.             centre_x-=80
  199.             centre_y=6--centre_y
  200.             print centre_x,centre_y
  201.       initial=400
  202.       flag=0
  203.       GPIO.output(LED_PIN,GPIO.LOW)          
  204.       if(found==0):
  205.             #if the ball is not found and the last time it sees ball in which direction, it will start to rotate in that direction
  206.             if flag==0:
  207.                   rightturn()
  208.                   time.sleep(0.05)
  209.             else:
  210.                   leftturn()
  211.                   time.sleep(0.05)
  212.             stop()
  213.             time.sleep(0.0125)
  214.      
  215.       elif(found==1):
  216.             if(area<initial):
  217.                   if(distanceC<10):
  218.                         #if ball is too far but it detects something in front of it,then it avoid it and reaches the ball.
  219.                         if distanceR>=8:
  220.                               rightturn()
  221.                               time.sleep(0.00625)
  222.                               stop()
  223.                               time.sleep(0.0125)
  224.                               forward()
  225.                               time.sleep(0.00625)
  226.                               stop()
  227.                               time.sleep(0.0125)
  228.                               #while found==0:
  229.                               leftturn()
  230.                               time.sleep(0.00625)
  231.                         elif distanceL>=8:
  232.                               leftturn()
  233.                               time.sleep(0.00625)
  234.                               stop()
  235.                               time.sleep(0.0125)
  236.                               forward()
  237.                               time.sleep(0.00625)
  238.                               stop()
  239.                               time.sleep(0.0125)
  240.                               rightturn()
  241.                               time.sleep(0.00625)
  242.                               stop()
  243.                               time.sleep(0.0125)
  244.                         else:
  245.                               stop()
  246.                               time.sleep(0.01)
  247.                   else:
  248.                         #otherwise it move forward
  249.                         forward()
  250.                         time.sleep(0.00625)
  251.             elif(area>=initial):
  252.                   initial2=6700
  253.                   if(area<initial2):
  254.                         if(distanceC>10):
  255.                               #it brings coordinates of ball to center of camera's imaginary axis.
  256.                               if(centre_x<=-20 or centre_x>=20):
  257.                                     if(centre_x<0):
  258.                                           flag=0
  259.                                           rightturn()
  260.                                           time.sleep(0.025)
  261.                                     elif(centre_x>0):
  262.                                           flag=1
  263.                                           leftturn()
  264.                                           time.sleep(0.025)
  265.                               forward()
  266.                               time.sleep(0.00003125)
  267.                               stop()
  268.                               time.sleep(0.00625)
  269.                         else:
  270.                               stop()
  271.                               time.sleep(0.01)
  272.  
  273.                   else:
  274.                         #if it founds the ball and it is too close it lights up the led.
  275.                         GPIO.output(LED_PIN,GPIO.HIGH)
  276.                         time.sleep(0.1)
  277.                         stop()
  278.                         time.sleep(0.1)
  279.       #cv2.imshow("draw",frame)    
  280.       rawCapture.truncate(0)  # clear the stream in preparation for the next frame
  281.          
  282.       if(cv2.waitKey(1) & 0xff == ord('q')):
  283.             break
  284.  
  285. GPIO.cleanup() #free all the GPIO pins
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement
Public Pastes
Advertisement
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy.  OK, I Understand
Not a member of Pastebin yet?
Sign Up, it unlocks many cool features!