From 846d0b1b77f9010ee42378fc8c19e3de2e704498 Mon Sep 17 00:00:00 2001 From: lukesta182 <33330639+lukesta182@users.noreply.github.com> Date: Tue, 27 Nov 2018 15:40:04 -0700 Subject: [PATCH] vision program from 2018 remember to make this a put program and install all libraries on the rasp pi before you put on the robot. 4388 rasp pies should all be configured as of november 2018 --- big man tyrone.py | 188 ++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 188 insertions(+) create mode 100644 big man tyrone.py diff --git a/big man tyrone.py b/big man tyrone.py new file mode 100644 index 0000000..491705d --- /dev/null +++ b/big man tyrone.py @@ -0,0 +1,188 @@ +# USAGE +# python ball_tracking.py --video ball_tracking_example.mp4 +# python ball_tracking.py + +# import the necessary packages +from collections import deque +from picamera.array import PiRGBArray +from picamera import PiCamera +import time +import numpy as np +import argparse +import serial +# import imutils +import cv2 +import sys + +print("Sending hello message over serial at 115200,n,8,1") +port = serial.Serial("/dev/serial0", baudrate=115200, timeout=0) +port.write("\r\nHi, this is Raspberry Pi\r\n") +port.write("Valid commands are BG?Q\r\n") +PY3 = sys.version_info[0] == 3 + +if PY3: + xrange = range + +# construct the argument parse and parse the arguments +ap = argparse.ArgumentParser() +ap.add_argument("-v", "--video", + help="path to the (optional) video file") +ap.add_argument("-b", "--buffer", type=int, default=64, + help="max buffer size") +args = vars(ap.parse_args()) + +#Horizontal +mode = 'G' + +# define the lower and upper boundaries of the "green" +# ball in the HSV color space, then initialize the +# list of tracked points +# greenLower = (28, 128, 6) +# greenUpper = (34, 255, 255) +greenLower = (59, 93, 61) +greenUpper = (69, 255, 218) +pts = deque(maxlen=args["buffer"]) + +# initialize the camera and grab a reference to the raw camera capture +camera = PiCamera() +camera.resolution = (640, 480) +camera.framerate = 32 +camera.shutter_speed = 461 # microseconds +camera.awb_mode = 'sunlight' + +rawCapture = PiRGBArray(camera, size=(640,480)) +# allow the camera to warmup +time.sleep(0.1) +xOut=str +yOut=str + +center1x=0.0 +center1y=0.0 +center2x=0.0 +center2y=0.0 +centerGearTarget=tuple + +# loop forever (at least, until the q key is pressed) +for frame in camera.capture_continuous(rawCapture, format="bgr", use_video_port=True): + + gearBlobsFound = 0 + xOut = "000" + yOut = "000" + + # grab the raw NumPy array representing the image, then initialize the timestamp + # and occupied/unoccupied text + image = frame.array + + # convert the frame to the HSV color space + # blurred = cv2.GaussianBlur(frame, (11, 11), 0) + hsv = cv2.cvtColor(image, cv2.COLOR_BGR2HSV) + + # construct a mask for the color "green", then perform + # a series of dilations and erosions to remove any small + # blobs left in the mask + mask = cv2.inRange(hsv, greenLower, greenUpper) + mask = cv2.erode(mask, None, iterations=2) + mask = cv2.dilate(mask, None, iterations=2) + + # find contours in the mask + cnts = cv2.findContours(mask.copy(), cv2.RETR_EXTERNAL, + cv2.CHAIN_APPROX_SIMPLE)[-2] + + #Look at all contours + for c in cnts: + + #find rect and aspect ratio + x, y, w, h = cv2.boundingRect(c) + aspect = float(w)/h + M = cv2.moments(c) + center = (int(M["m10"] / M["m00"]),int(M["m01"] / M["m00"])) + centerx = (int(M["m10"] / M["m00"])) + centery = (int(M["m01"] / M["m00"])) + + if mode == 'G' and aspect >= 0.1 and aspect <= 0.6: + cv2.circle(image, center, 5, (0, 0, 255), -1) + cv2.circle(image, (x,y), 5, (0, 0, 255), -1) + cv2.circle(image, (x,y), 5, (0, 0, 255), -1) + cv2.circle(image, (x + w, y + h), 5, (0, 0, 255), -1) + cv2.circle(image, (x + w, y), 5, (0, 0, 255), -1) + cv2.circle(image, (x, y + h), 5, (0, 0, 255), -1) + + if gearBlobsFound == 0: + center1x = centerx + center1y = centery + gearBlobsFound+=1 + elif gearBlobsFound == 1: + center2x = centerx + center2y = centery + gearBlobsFound+=1 + elif gearBlobsFound>=2: + print("Found more than 2 gear blobs") + gearBlobsFound+=1 + + centerGearTarget=(int((center1x + center2x)/2), int(((center1y + center2y)/2))) + cv2.circle(image,centerGearTarget,5,(255,255,255),-1) + + xOut1 = format(centerx, '03d') + yOut1 = format(centery, '03d') + + if mode == 'B' and aspect >= 1 and aspect <= 3: + concaveX = x+(w/2) + concaveY = y+(h*4/5) + inOrOut = cv2.pointPolygonTest(c, (concaveX, concaveY), True) + if inOrOut < 0: + cv2.circle(image, (x,y), 5, (0, 0, 255), -1) + cv2.circle(image, (x + w, y + h), 5, (0, 0, 255), -1) + cv2.circle(image, (x + w, y), 5, (0, 0, 255), -1) + cv2.circle(image, (x, y + h), 5, (0, 0, 255), -1) + # print(x, y, w, h, aspect) + # print(inOrOut) + cv2.circle(image, (concaveX, concaveY),5, (255, 0, 0), -1) + cv2.circle(image, center, 5, (0, 0, 255), -1) + xOut2 = format(centerx, '03d') + yOut2 = format(centery, '03d') + #print (xOut+yOut) + + #print(gearBlobsFound) + + #port.write("\r\nHi, " +str(gearBlobsFound)+ "\r\n") + + # show the frame to our screen + cv2.imshow("Frame", image) + key = cv2.waitKey(1) & 0xFF + + # clear the stream in preparation for the next frame + rawCapture.truncate(0) + + # if the 'q' key is pressed, stop the loop + if key == ord("q"): + break + + read = port.read() + + if read == 'B': + print("Looking for boiler targe") + mode = 'B' + port.write (xOut2+yOut2+"\r\n") + + if read == 'G': + print("Looking for gear target") + mode = 'G' + port.write(xOut1+yOut1+"\r\n") + + if key == ord("B"): + mode = 'B' + print(xOut2 + yOut2 + "\r\n") + + if key == ord("G"): + mode = 'G' + print(xOut1 + yOut1 + "\r\n") + + #if key == ord("A"): + # print(center1y - center2y) +# + # if key == ord("D"): + # print(center1x - (int(xOut1))) + +# clean up camera resources and close any open windows +camera.close() +cv2.destroyAllWindows()