diff --git a/ScriptManager/.DS_Store b/ScriptManager/.DS_Store new file mode 100644 index 0000000..d935ab3 Binary files /dev/null and b/ScriptManager/.DS_Store differ diff --git a/ScriptManager/ball.py b/ScriptManager/ball.py index 32ebb3e..1037021 100644 --- a/ScriptManager/ball.py +++ b/ScriptManager/ball.py @@ -173,11 +173,14 @@ def __find_lines(input): from picamera import PiCamera from picamera.array import PiRGBArray - +from table import Table import io +table = Table(1) + grip = GripPipeline() + def main (stop_message): def connection(stop_message): @@ -227,13 +230,21 @@ def connection(stop_message): cv2.rectangle(grip.mask_output,(x1,y1),(x2,y2),(255,255,255),2) sideX = ((x1-x2)/2)+x2 sideY = ((y1-y2)/2)+y2 + width = x2-x1 cv2.circle(grip.mask_output,(int(sideX),int(sideY)),50,(255,255,255),2) print("Ball found") + + + table.updateNumber((sideX, sideY)) + table.updateNumber(width, key=1) except Exception as e: print(e) print("--------------") print("Ball not found") - cv2.imshow('frame',grip.mask_output) + + table.updateNumber("B") + table.updateNumber("B", key=1) + rawCap.truncate(0) ## xraw.seek(0) diff --git a/ScriptManager/listener.py b/ScriptManager/listener.py new file mode 100644 index 0000000..4b9b8e6 --- /dev/null +++ b/ScriptManager/listener.py @@ -0,0 +1,23 @@ +import time +from networktables import NetworkTables as nt + +nt.initialize(server="10.15.12.2") + +table = nt.getTable("chooser_data") +table2 = nt.getTable("tape_data") +table3 = nt.getTable("ball_data") + +def valueChanged(table, key, value, isNew): + print("Changed:", table, key, value) + +def connectionListener(info, connected): + print("Connected:", info, "Connection:", connected[0],"-",connected[1]) + +nt.addConnectionListener(connectionListener) + +table.addEntryListener(valueChanged) +table2.addEntryListener(valueChanged) +table3.addEntryListener(valueChanged) + +while True: + time.sleep(5) diff --git a/ScriptManager/scriptmanager.py b/ScriptManager/scriptmanager.py index 3d855cb..caf9aef 100644 --- a/ScriptManager/scriptmanager.py +++ b/ScriptManager/scriptmanager.py @@ -3,10 +3,10 @@ from networktables import NetworkTables as nt import tape3 -import thread_example import ball +import thread_example -ip = "10.31.51.43" +ip = "10.15.12.2" nt.initialize(server=ip) table = nt.getTable("chooser_data") @@ -34,7 +34,7 @@ def valueChanged(table, key, value, isNew): if value != 0: time.sleep(1) - + print("[*]Starting thread: {}".format(value)) t = threading.Thread(target=target_list[value]) t.start() diff --git a/ScriptManager/table.py b/ScriptManager/table.py new file mode 100644 index 0000000..8c067ae --- /dev/null +++ b/ScriptManager/table.py @@ -0,0 +1,21 @@ +from networktables import NetworkTables as nt + +table_list = ["tape_data", "ball_data"] +key_list = ["midpoint", "width"] + +class Table: + + def __init__(self, table_number): + nt.initialize(server='10.31.51.43') + self.table = nt.getTable(table_list[table_number]) + + def updateNumber(self, midpoint, key = 0): + table = self.table + + try: + table.putString(key_list[key],str(midpoint)) + except Exception as e: + print(e) + + +#https://robotpy.readthedocs.io/projects/pynetworktables/en/stable/examples.html diff --git a/ScriptManager/tape3.py b/ScriptManager/tape3.py index cd74f16..055abc9 100644 --- a/ScriptManager/tape3.py +++ b/ScriptManager/tape3.py @@ -20,8 +20,8 @@ def __init__(self): self.__hsv_threshold_input = self.blur_output self.__hsv_threshold_hue = [0.0, 180.0] - self.__hsv_threshold_saturation = [0.0, 47.0] - self.__hsv_threshold_value = [252.0, 255.0] + self.__hsv_threshold_saturation = [0.0, 255.0] + self.__hsv_threshold_value = [190.0, 255.0] self.hsv_threshold_output = None @@ -113,9 +113,7 @@ def midpoint(box_list): else: y += y1 - midx, midy = int(x), int(y) - - return (midx, midy) + return (round(x,2),round(y2)) #Defines bounding boxes from contours def find_boxes(contours): @@ -149,13 +147,16 @@ def find_boxes(contours): from picamera.array import PiRGBArray from picamera import PiCamera +from table import Table + +table = Table(0) BlurType = Enum('BlurType', 'Box_Blur Gaussian_Blur Median_Filter Bilateral_Filter') g = GripPipeline() #Minimum area of a bounding box -size_threshold = 4000 +size_threshold = 250 def main(stop_message): @@ -173,6 +174,7 @@ def connection(stop_message): cam = PiCamera() cam.resolution = (640, 480) cam.framerate = 32 + cam.exposure_mode = 'off' rawCap = PiRGBArray(cam, size=(640, 480)) for frame in cam.capture_continuous(rawCap, format="bgr", use_video_port=True): @@ -216,14 +218,22 @@ def connection(stop_message): #Draws midpoint if one exists if mid != 0: - cv2.circle(frame, (mid[0], mid[1]), 5, (0, 0, 255), -1) + cv2.circle(frame, (int(mid[0]),int(mid[1])), 5, (0, 0, 255), -1) + table.updateNumber((mid[0], mid[1])) + #table.updateNumber(-box_list[0][1][0], key=1) + table.updateNumber(-50, key=1) + print("Midpoint:", mid) + + else: + + table.updateNumber("B") + table.updateNumber("B", key=1) if box_list != []: print("************************") - cv2.imshow('frame', frame) rawCap.truncate(0) diff --git a/ScriptManager/tape4.py b/ScriptManager/tape4.py new file mode 100644 index 0000000..62fab04 --- /dev/null +++ b/ScriptManager/tape4.py @@ -0,0 +1,244 @@ +import sys +import cv2 +import numpy as np +import math +from enum import Enum + +class GripPipeline: + """ + An OpenCV pipeline generated by GRIP. + """ + + def __init__(self): + """initializes all values to presets or None if need to be set + """ + + self.__blur_type = BlurType.Box_Blur + self.__blur_radius = 6.306306306306296 + + self.blur_output = None + + self.__hsv_threshold_input = self.blur_output + self.__hsv_threshold_hue = [0.0, 180.0] + self.__hsv_threshold_saturation = [0.0, 255.0] + self.__hsv_threshold_value = [190.0, 255.0] + + self.hsv_threshold_output = None + + + + def process(self, source0): + """ + Runs the pipeline and sets all outputs to new values. + """ + # Step Blur0: + self.__blur_input = source0 + (self.blur_output) = self.__blur(self.__blur_input, self.__blur_type, self.__blur_radius) + + # Step HSV_Threshold0: + self.__hsv_threshold_input = self.blur_output + (self.hsv_threshold_output) = self.__hsv_threshold(self.__hsv_threshold_input, self.__hsv_threshold_hue, self.__hsv_threshold_saturation, self.__hsv_threshold_value) + + + @staticmethod + def __blur(src, type, radius): + """Softens an image using one of several filters. + Args: + src: The source mat (numpy.ndarray). + type: The blurType to perform represented as an int. + radius: The radius for the blur as a float. + Returns: + A numpy.ndarray that has been blurred. + """ + if(type is BlurType.Box_Blur): + ksize = int(2 * round(radius) + 1) + return cv2.blur(src, (ksize, ksize)) + elif(type is BlurType.Gaussian_Blur): + ksize = int(6 * round(radius) + 1) + return cv2.GaussianBlur(src, (ksize, ksize), round(radius)) + elif(type is BlurType.Median_Filter): + ksize = int(2 * round(radius) + 1) + return cv2.medianBlur(src, ksize) + else: + return cv2.bilateralFilter(src, -1, round(radius), round(radius)) + + @staticmethod + def __hsv_threshold(input, hue, sat, val): + """Segment an image based on hue, saturation, and value ranges. + Args: + input: A BGR numpy.ndarray. + hue: A list of two numbers the are the min and max hue. + sat: A list of two numbers the are the min and max saturation. + lum: A list of two numbers the are the min and max value. + Returns: + A black and white numpy.ndarray. + """ + out = cv2.cvtColor(input, cv2.COLOR_BGR2HSV) + return cv2.inRange(out, (hue[0], sat[0], val[0]), (hue[1], sat[1], val[1])) + + class Line: + + def __init__(self, x1, y1, x2, y2): + self.x1 = x1 + self.y1 = y1 + self.x2 = x2 + self.y2 = y2 + + def length(self): + return np.sqrt(pow(self.x2 - self.x1, 2) + pow(self.y2 - self.y1, 2)) + + def angle(self): + return np.degrees(np.arctan2(self.y2 - self.y1, self.x2 - self.x1)) + + +#finds the midpoint between the coordinates of the centers of two boxes +def midpoint(box_list): + + if len(box_list) < 2: + return 0 + + x1, y1 = box_list[0][0] + x2, y2 = box_list[1][0] + + x = abs((x1 - x2)/2) + y = abs((y1 - y2)/2) + + if x1 > x2: + x += x2 + else: + x += x1 + + if y1 > y2: + y += y2 + else: + y += y1 + + midx, midy = int(x), int(y) + + return (midx, midy) + +#Defines bounding boxes from contours +def find_boxes(contours): + + box_list=[] + t = 0 + + for cnt in contours: + rect = cv2.minAreaRect(cnt) + + width = rect[1][0] + length = rect[1][1] + + area = math.floor(width * length) + + if t == 2: + break + + if area > size_threshold: + + if width > 2*length or length > 2*width: + + #adds the area as an element + rect = rect + (area,) + box_list.append(rect) + + t+=1 + + return box_list + + +from picamera.array import PiRGBArray +from picamera import PiCamera +from table import Table + + +table = Table(0) + +BlurType = Enum('BlurType', 'Box_Blur Gaussian_Blur Median_Filter Bilateral_Filter') + +g = GripPipeline() + +#Minimum area of a bounding box +size_threshold = 1000 + +def main(): + + cam = PiCamera() + cam.resolution = (640, 480) + cam.framerate = 32 + cam.exposure_mode = "off" + rawCap = PiRGBArray(cam, size=(640, 480)) + + for frame in cam.capture_continuous(rawCap, format="bgr", use_video_port=True): + + box_list=[] + + frame = frame.array + frame2 = frame + #Runs the frame through the grip pipeline + g.process(frame) + threshed_frame = g.hsv_threshold_output + + ret, thresh = cv2.threshold(threshed_frame, 127, 255, cv2.THRESH_BINARY) + im2, contours, heirarchy = cv2.findContours(thresh, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE) + + try: + box_list = find_boxes(contours) + + except Exception as e: + print(e) + + + for rect in box_list: + + #Converts box into coordinates + box = cv2.boxPoints(rect[:3]) + box = np.int0(box) + + angle = math.floor(rect[2]) + area = rect[3] + + print("Angle:", -angle) + print("Area:", area) + + print("|----------|") + + #Draws bounding boxes + frame = cv2.drawContours(threshed_frame, [box], 0, (255, 255, 255), 2) + + mid = midpoint(box_list) + + #Draws midpoint if one exists + if mid != 0: + + cv2.circle(threshed_frame, (mid[0], mid[1]), 5, (255, 255, 255), -1) + #table.updateNumber(mid) + print("Midpoint:", mid) + + #else: + + #table.updateNumber("B") + + + + if box_list != []: + print("************************") + + + rawCap.truncate(0) + + cv2.imshow("frame", threshed_frame) + cv2.imshow("original", frame2) + + #connection(stop_message) + + if cv2.waitKey(1) & 0xFF == ord('q'): + break + +main() + + + + + +