diff --git a/.gitignore b/.gitignore index d9bde85..e549179 100644 --- a/.gitignore +++ b/.gitignore @@ -1,3 +1,4 @@ /.vscode/settings.json /Chess Robot/source/__pycache__/ /Chess Robot/source/.vscode/settings.json +/Chess Robot/.vscode/settings.json diff --git a/Chess Robot/source/board.jpg b/Chess Robot/source/board.jpg new file mode 100644 index 0000000..0fde23d Binary files /dev/null and b/Chess Robot/source/board.jpg differ diff --git a/Chess Robot/source/claw.py b/Chess Robot/source/claw.py index 583a752..b5d7fbf 100644 --- a/Chess Robot/source/claw.py +++ b/Chess Robot/source/claw.py @@ -1,36 +1,43 @@ -from ev3dev.ev3 import * +from ev3dev2.motor import * from time import sleep -class Claw: +class Claw(MediumMotor): def __init__(self, port = OUTPUT_B): - self.motor = MediumMotor(port) - self.motor.reset() + MediumMotor.__init__(self,port) + self.reset() #Close the gripper completely convergence_counter = 0 time_delta = 0.01 - tachometer_reading = self.motor.position + tachometer_reading = self.position while convergence_counter < 3: - self.motor.run_forever(speed_sp=-360) + self.on(-24) sleep(time_delta) - if tachometer_reading == self.motor.position: + if tachometer_reading == self.position: convergence_counter = convergence_counter + 1 - tachometer_reading = self.motor.position - self.motor.stop() + tachometer_reading = self.position + self.stop() #Gripper is closed at this point - self.motor.run_to_rel_pos(position_sp=700, speed_sp = 360) + + #self.motor.run_to_rel_pos(position_sp=700, speed_sp = 360) + self.on_for_degrees(24,700) sleep(3) self.isOpen = True def open(self): if not self.isOpen: - self.motor.run_to_rel_pos(position_sp=700, speed_sp = 360) + #self.motor.run_to_rel_pos(position_sp=700, speed_sp = 360) + self.on_for_degrees(24,700) sleep(3) self.isOpen = True def close(self): if self.isOpen: - self.motor.run_to_rel_pos(position_sp=-700, speed_sp = 360) + #self.motor.run_to_rel_pos(position_sp=-700, speed_sp = 360) + self.on_for_degrees(-24,700) sleep(3) self.isOpen = False + def stop(self): + self.reset() + #if __name__ == "__main__": diff --git a/Chess Robot/source/client.py b/Chess Robot/source/client.py index ac20808..e1c349a 100644 --- a/Chess Robot/source/client.py +++ b/Chess Robot/source/client.py @@ -12,7 +12,7 @@ def __init__(self,port): #We need to use the ipv4 address that shows up in ipconfig in the computer for the USB #Ethernet adapter handling the connection to the EV3 - host = "169.254.137.85" + host = "169.254.82.210" print("setting up client, address =", host, "port =", port) self.s = socket.socket(socket.AF_INET, socket.SOCK_STREAM) self.s.connect((host, port)) diff --git a/Chess Robot/source/coordinates.py b/Chess Robot/source/coordinates.py new file mode 100644 index 0000000..9b72956 --- /dev/null +++ b/Chess Robot/source/coordinates.py @@ -0,0 +1,125 @@ +from webcam import Webcam +import cv2 +from datetime import datetime +from time import sleep +import numpy as np + +from queue import Queue +from server import * + +#Thread-safe queue to get data from threads +queue = Queue() +server = Server(9999) + +webcam = Webcam() +webcam.start() + +#image = cv2.imread("board.jpg") +#in milimeters +side_length = 307 + + +def mouse_drawing(event, x, y, flags, params): + if event == cv2.EVENT_LBUTTONDOWN: + print("Left click") + corners.append((x, y)) + +def add_target(event, x, y, flags, params): + if event == cv2.EVENT_LBUTTONDOWN: + print("Left click") + current_points.append((x, y)) + +#As explained in http://paulbourke.net/geometry/pointlineplane/ +def closest_point(line_start, line_end, point): + #First element is delta x, second one is delta y + delta_vector = np.subtract(line_end,line_start) + u = ((point[0]-line_start[0])*(line_end[0]-line_start[0])+(point[1]-line_start[1])*(line_end[1]-line_start[1]))/np.dot(delta_vector, delta_vector) + x = line_start[0] + u*(line_end[0]-line_start[0]) + y = line_start[1] + u*(line_end[1]-line_start[1]) + return (x,y) + + +def calculate_coordinates(x,y): + print("Distance to left corner") + closest_left = closest_point(corners[0],corners[1], (x,y)) + closest_bottom = closest_point(corners[0],corners[3], (x,y)) + closest_right = closest_point(corners[3],corners[2], (x,y)) + closest_top = closest_point(corners[1],corners[2], (x,y)) + print(str(closest_point(corners[0],corners[1], (x,y)))) + current_point = (x,y) + #Get Horizontal Length by Pythagoras Theorem + horizontal_length = ((closest_right[0] - closest_left[0])**2+(closest_right[1] - closest_left[1])**2)**(0.5) + #Get Vertical Length by Pythagoras Theorem + vertical_length = ((closest_top[0] - closest_bottom[0])**2+(closest_top[1] - closest_bottom[1])**2)**(0.5) + + #Horizontal length until the closest point (from left to right) + horizontal_distance_to_point = ((current_point[0] - closest_left[0])**2+(current_point[1] - closest_left[1])**2)**(0.5) + #Vertical length until the closest point (from bottom to top) + vertical_distance_to_point = ((current_point[0] - closest_bottom[0])**2+(current_point[1] - closest_bottom[1])**2)**(0.5) + + #In pixels + print("Current Point: " + str(current_point)) + #Now we get the percentage eg (current position x)/total length of the x line )both in pixels. This will give us a percentage + print("Proportion in X: " + str(horizontal_distance_to_point/horizontal_length) + " Proportion in Y: " + str(vertical_distance_to_point/vertical_length)) + #Then multiply that percentage by 307 to the the milimiter length + proportionX = horizontal_distance_to_point/horizontal_length + propotionY = vertical_distance_to_point/vertical_length + return (side_length*proportionX, side_length*propotionY) + +#[lower_left, upper_left, upper_right, lower_right] +corners = [] + + +cv2.namedWindow("Frame") +cv2.setMouseCallback("Frame", mouse_drawing) + +while len(corners) < 4: + + # get image from webcam + image = webcam.get_current_frame() + for center_position in corners: + cv2.circle(image, center_position, 5, (0, 0, 255), -1) + # display image + cv2.imshow('Frame', image) + + key = cv2.waitKey(1) + +cv2.setMouseCallback("Frame", add_target) + +current_points = [] + +while True: + # get image from webcam + image = webcam.get_current_frame() + for center_position in corners: + cv2.circle(image, center_position, 5, (0, 0, 255), -1) + # display image + for current_point in current_points: + cv2.circle(image, current_point, 5, (0, 0, 0), -1) + + cv2.imshow('Frame', image) + + if (len(current_points) == 2): + #First, take the initial and position end effector over it + #Make sure to account for the 100 mm in offset for the y axis and the 19mm for x and y + (x1,y1) = calculate_coordinates(current_points[0][0], current_points[0][1]) + server.sendDistances(-x1 + 19, -(100 + y1) + 19, queue) + server.sendLowerClaw() + server.sendCloseClaw() + server.sendRaiseClaw() + #After piece is grabbed, we move it to the desired position specified in the second point + (x2,y2) = calculate_coordinates(current_points[1][0], current_points[1][1]) + delta_x = x2 - x1 + delta_y = y2 - y1 + server.sendDistances(-delta_x, -delta_y, queue) + server.sendLowerClaw() + server.sendOpenClaw() + server.sendRaiseClaw() + server.sendHome() + current_points.clear() + key = cv2.waitKey(1) + if 'q' == chr(key & 255): + break + +server.sendTermination() +cv2.destroyAllWindows() \ No newline at end of file diff --git a/Chess Robot/source/distort.py b/Chess Robot/source/distort.py deleted file mode 100644 index e69de29..0000000 diff --git a/Chess Robot/source/move.py b/Chess Robot/source/move.py index d6e6d4b..2b9c1df 100644 --- a/Chess Robot/source/move.py +++ b/Chess Robot/source/move.py @@ -1,97 +1,20 @@ +from queue import Queue +from server import * -from ev3dev.ev3 import * -import socket -from threading import Event, Thread -from time import sleep -from claw import * +#Thread-safe queue to get data from threads +queue = Queue() +server = Server(9999) -#yMotor = LargeMotor(OUTPUT_A) -#xMotor = LargeMotor(OUTPUT_C) -#zMotor = LargeMotor(OUTPUT_D) -#gripperMotor = MediumMotor(OUTPUT_B) +server.sendDistances(-76,-138, queue) +server.sendLowerClaw() +server.sendCloseClaw() +server.sendRaiseClaw() +server.sendDistances(0,-38,queue) +server.sendLowerClaw() +server.sendOpenClaw() +server.sendRaiseClaw() -#Y -#yMotor.run_to_rel_pos(position_sp=720, speed_sp = 230) -#sleep(3) -#yMotor.stop() -#X -#xMotor.run_to_rel_pos(position_sp=960, speed_sp = 230) -#sleep(3) -#xMotor.stop() -#Gripper -#gripperMotor.run_to_rel_pos(position_sp=500, speed_sp = 360) -#sleep(3) -#gripperMotor.stop() +# robot = Robot() -#Gripper -#gripperMotor.run_to_rel_pos(position_sp=-500, speed_sp = 360) -#sleep(3) -#gripperMotor.stop() - -#Z -#Upwards -#zMotor.run_to_rel_pos(position_sp=-120, speed_sp = 100, stop_action="hold") -##sleep(3) -zMotor.stop() - - -#Downwards - - - - -#gripperMotor.run_to_rel_pos(position_sp=-2000, speed_sp = 360) -#sleep(6) -#gripperMotor.stop() - - -#zMotor.run_to_rel_pos(position_sp=-120, speed_sp = 100, stop_action="hold") -#sleep(3) -#zMotor.stop() - -#xMotor.run_to_rel_pos(position_sp=-960, speed_sp = 230) -#sleep(3) -#xMotor.stop() - -#yMotor.run_to_rel_pos(position_sp=-420, speed_sp = 230) -#sleep(3) -#yMotor.stop() - -#zMotor.run_to_rel_pos(position_sp=120, speed_sp = 100, stop_action="coast") -#sleep(3) -#zMotor.stop() - - -#gripperMotor.run_to_rel_pos(position_sp=700, speed_sp = 360) -#sleep(4) -#gripperMotor.stop() - -zMotor.run_to_rel_pos(position_sp=-120, speed_sp = 100, stop_action="hold") -sleep(3) -zMotor.stop() - - -claw = Claw(OUTPUT_B) - -zMotor.run_to_rel_pos(position_sp=120, speed_sp = 100, stop_action="coast") -sleep(3) -zMotor.stop() - -claw.close() - - -zMotor.run_to_rel_pos(position_sp=-120, speed_sp = 100, stop_action="hold") -sleep(3) -zMotor.stop() - -yMotor.run_to_rel_pos(position_sp=-420, speed_sp = 230) -sleep(3) -yMotor.stop() - -zMotor.run_to_rel_pos(position_sp=120, speed_sp = 100, stop_action="coast") -sleep(3) -zMotor.stop() - -claw.open() -#zMotor.reset() \ No newline at end of file +# robot.xMotor.run_to_rel_pos(position_sp=-360,speed_sp=230) \ No newline at end of file diff --git a/Chess Robot/source/parallelogram.py b/Chess Robot/source/parallelogram.py new file mode 100644 index 0000000..7c07d92 --- /dev/null +++ b/Chess Robot/source/parallelogram.py @@ -0,0 +1,13 @@ +class Rectangle: + #Rectangle array is assumed to be in OpenCV standard representation of a rectange + # that is, an array of 4 elements: [x, y, width, height] where each quantity is given in pixels + # and x,y correspond to the coordinate of the top left corner of the rectangle. + def __init__(self,rectangle_array): + self.array_representation = rectangle_array + self.width = int(rectangle_array[2]) + self.height = int(rectangle_array[3]) + self.top_left = (int(rectangle_array[0]), int(rectangle_array[1])) + self.top_right = (int(rectangle_array[0]) + self.width , int(rectangle_array[1])) + self.bottom_left = (int(rectangle_array[0]) , int(rectangle_array[1]) + self.height) + self.bottom_right = (int(rectangle_array[0]) + self.width , int(rectangle_array[1]) + self.height) + self.centre = (self.top_left[0] + int(self.width/2), self.top_left[1] + int(self.height/2)) \ No newline at end of file diff --git a/Chess Robot/source/robot.py b/Chess Robot/source/robot.py index b1a82c6..0ec782e 100644 --- a/Chess Robot/source/robot.py +++ b/Chess Robot/source/robot.py @@ -1,29 +1,39 @@ -from ev3dev.ev3 import * +from ev3dev2.motor import * +from ev3dev2.sensor.lego import * from time import sleep from claw import * class Robot: def __init__(self): - self.yTouchSensor = TouchSensor(INPUT_1) - self.xTouchSensor = TouchSensor(INPUT_2) + self.millimetersPerTickY = 0.14444444 + #self.millimetersPerTickX = 0.05833333 + self.millimetersPerTickX = 0.0642 + self.yOffset = 100 self.yMotor = LargeMotor(OUTPUT_A) + self.yMotor.stop_action = 'brake' self.xMotor = LargeMotor(OUTPUT_C) self.zMotor = LargeMotor(OUTPUT_D) + self.currentY = 0 + self.currentX = 0 self.xMotor.reset() self.yMotor.reset() self.zMotor.reset() + #Lift the claw + self.clawUp() #Home the X axis self.homeX() #Home the Y axis self.homeY() - #Lift the claw - self.clawUp() + #After the claw has been lifted we initialze it self.claw = Claw() - + + def __del__(self): + self.stop() + def openClaw(self): self.claw.open() @@ -31,34 +41,50 @@ def closeClaw(self): self.claw.close() def homeX(self): - while not self.xTouchSensor.is_pressed: - self.xMotor.run_forever(speed_sp=230) + degrees = -(self.currentX/self.millimetersPerTickX) + self.xMotor.on_for_degrees(SpeedPercent(23), abs(degrees)) self.xMotor.stop() + self.currentX = 0 def homeY(self): - while not self.yTouchSensor.is_pressed: - self.yMotor.run_forever(speed_sp=230) + degrees = (self.currentY/self.millimetersPerTickY) + self.yMotor.on_for_degrees(SpeedPercent(23),abs(degrees)) self.yMotor.stop() + self.currentY = 0 - def moveX(self, degrees): - timer = abs(degrees)/230 + 0.1 - self.xMotor.run_to_rel_pos(position_sp=degrees, speed_sp = 230) - sleep(timer) + def moveX(self, milli): + degrees = abs(milli)/self.millimetersPerTickX + if(milli < 0): + self.xMotor.on_for_degrees(SpeedPercent(-23),degrees) + else: + self.xMotor.on_for_degrees(SpeedPercent(23),degrees) self.xMotor.stop() + self.currentX += milli + + def moveY(self, milli): + + degrees = abs(milli)/self.millimetersPerTickY + if(milli < 0): + self.yMotor.on_for_degrees(SpeedPercent(-23),degrees) + else: + self.yMotor.on_for_degrees(SpeedPercent(23),degrees) - def moveY(self, degrees): - timer = abs(degrees)/230 + 0.1 - self.yMotor.run_to_rel_pos(position_sp=degrees, speed_sp = 230) - sleep(timer) self.yMotor.stop() + self.currentY += milli def clawUp(self): - self.zMotor.run_to_rel_pos(position_sp=-120, speed_sp = 100, stop_action="hold") + self.zMotor.run_to_rel_pos(position_sp=-120, speed_sp = 50, stop_action="hold") sleep(1.8) self.zMotor.stop() def clawDown(self): - self.zMotor.run_to_rel_pos(position_sp=120, speed_sp = 100, stop_action="coast") - sleep(1.8) + self.zMotor.run_to_rel_pos(position_sp=120, speed_sp = 50, stop_action="coast") + sleep(3.5) self.zMotor.stop() + def stop(self): + self.yMotor.reset() + self.xMotor.reset() + self.zMotor.reset() + self.claw.stop() + diff --git a/Chess Robot/source/runner.py b/Chess Robot/source/runner.py index 5173527..0ab9823 100644 --- a/Chess Robot/source/runner.py +++ b/Chess Robot/source/runner.py @@ -11,32 +11,48 @@ robot = Robot() #Attempt to connect to server client = Client(9999) +robot.moveY(100) +robot.currentY = 0 while True: #Block until a command/message from the server is received data = str(client.pollData()) if(data == 'EXIT'): #Terminate the routine on the client - #robot.stop() + robot.stop() break elif(data == "LOWER_CLAW"): robot.clawDown() + elif(data == "RAISE_CLAW"): + robot.clawUp() + elif(data == "OPEN_CLAW"): + robot.openClaw() + elif(data == "CLOSE_CLAW"): + robot.closeClaw() + elif(data == "HOME"): + robot.homeX() + robot.homeY() else: - #If the data we got from the server is not one of the predifined messages - #We assume (big assumption) that the message will contain the angles in the - #Agreed format: x_motor_angle,y_motor_angle - #We convert the angles to float after splitting the message on the tab - x_motor_angle, y_motor_angle = map(float,map(str,data.split(','))) - #This will make sure that both angles are at most 360 degrees. In addition, - #it will consider both, the clockwise and counterclockwise rotations to reach - #the desired position for each motor and will pick the smallest one. So that the chances of making - #very large rotations are lessened. - #x_motor_angle = int(x_motor_angle) % 360 - #x_motor_angle = min([x_motor_angle, x_motor_angle - 360], key=lambda x: abs(x)) - #y_motor_angle = int(y_motor_angle) % 360 - #y_motor_angle = min([y_motor_angle, y_motor_angle - 360], key=lambda x: abs(x)) - #After the proper rotation angle has been selected, we send the move command to the motors - print("Moving X: " + str(x_motor_angle)+ " Moving Y: " + str(y_motor_angle)) - robot.moveX(x_motor_angle) - robot.moveY(y_motor_angle) - client.sendDone() \ No newline at end of file + # #If the data we got from the server is not one of the predifined messages + # #We assume (big assumption) that the message will contain the angles in the + # #Agreed format: x_motor_angle,y_motor_angle + # #We convert the angles to float after splitting the message on the tab + #for char in data: + # print(chr(char), end=', ') + #print() + x_distance, y_distance = map(float,map(str,data.split(','))) + # #This will make sure that both angles are at most 360 degrees. In addition, + # #it will consider both, the clockwise and counterclockwise rotations to reach + # #the desired position for each motor and will pick the smallest one. So that the chances of making + # #very large rotations are lessened. + # #x_motor_angle = int(x_motor_angle) % 360 + # #x_motor_angle = min([x_motor_angle, x_motor_angle - 360], key=lambda x: abs(x)) + # #y_motor_angle = int(y_motor_angle) % 360 + # #y_motor_angle = min([y_motor_angle, y_motor_angle - 360], key=lambda x: abs(x)) + # #After the proper rotation angle has been selected, we send the move command to the motors + # print("Moving X: " + str(x_motor_angle)+ " Moving Y: " + str(y_motor_angle)) + # robot.moveX(x_motor_angle) + # robot.homeY() + robot.moveX(x_distance) + robot.moveY(y_distance) + client.sendDone() diff --git a/Chess Robot/source/server.py b/Chess Robot/source/server.py index a2cdac7..e8ead8d 100644 --- a/Chess Robot/source/server.py +++ b/Chess Robot/source/server.py @@ -13,7 +13,7 @@ def __init__(self,port): serversocket = socket.socket(socket.AF_INET, socket.SOCK_STREAM) #We need to use the ip address that shows up in ipconfig for the usb ethernet adapter #That handles the comunication between the PC and the crick - host = "169.254.137.85" + host = "169.254.82.210" print ("host: ", host) port = 9999 serversocket.bind((host, port)) @@ -29,11 +29,11 @@ def __init__(self,port): # joint_angle [Float]: The angle by which we want to joint to move # queue [Thread-safe Queue]: Mutable data structure to store (and return) # the messages received from the client - def sendAngles(self, x_motor_angle, y_motor_angle, queue): + def sendDistances(self, x_distance, y_distance, queue): #Format in which the client expects the data # angle1 angle2 - print(str(x_motor_angle) + " " + str(y_motor_angle)) - data = str(x_motor_angle)+","+str(y_motor_angle) + print(str(x_distance) + " " + str(y_distance)) + data = str(x_distance)+","+str(y_distance) print("Sending Data: (" + data + ") to robot.") self.cs.send(data.encode("UTF-8")) #Waiting for the client (ev3 brick) to let the server know @@ -48,6 +48,24 @@ def sendTermination(self): def sendLowerClaw(self): self.cs.send("LOWER_CLAW".encode("UTF-8")) + reply = self.cs.recv(128).decode("UTF-8") + + def sendRaiseClaw(self): + self.cs.send("RAISE_CLAW".encode("UTF-8")) + reply = self.cs.recv(128).decode("UTF-8") + + def sendOpenClaw(self): + self.cs.send("OPEN_CLAW".encode("UTF-8")) + reply = self.cs.recv(128).decode("UTF-8") + + def sendCloseClaw(self): + self.cs.send("CLOSE_CLAW".encode("UTF-8")) + reply = self.cs.recv(128).decode("UTF-8") + + def sendHome(self): + self.cs.send("HOME".encode("UTF-8")) + reply = self.cs.recv(128).decode("UTF-8") + diff --git a/Chess Robot/source/webcam.py b/Chess Robot/source/webcam.py index 7ac0b45..9c07b33 100644 --- a/Chess Robot/source/webcam.py +++ b/Chess Robot/source/webcam.py @@ -4,17 +4,21 @@ class Webcam: def __init__(self): - self.video_capture = cv2.VideoCapture(1) + self.video_capture = cv2.VideoCapture(0) self.current_frame = self.video_capture.read()[1] + self.__continue = True + + def __del__(self): + self.__continue = False # create thread for capturing images def start(self): - Thread(target=self._update_frame, args=()).start() + self.thread = Thread(target=self._update_frame, args=()).start() def _update_frame(self): - while(True): + while(self.__continue): self.current_frame = self.video_capture.read()[1] # get the current frame def get_current_frame(self): - return self.current_frame \ No newline at end of file + return self.current_frame