commit b49d27615b059f5c48e0bac5a5187418144e1230 Author: Maciej Grela Date: Fri Feb 24 00:20:27 2017 +0100 Initial revision diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..4acd06b --- /dev/null +++ b/.gitignore @@ -0,0 +1 @@ +config.py diff --git a/README.md b/README.md new file mode 100644 index 0000000..f33e46f --- /dev/null +++ b/README.md @@ -0,0 +1,5 @@ +This is the code for driving the [Fred the Ripper](https://vimeo.com/204440304] robot. + +In the junk/ directory you can find some opencv code that I used to track the CD position and place it precisely on the +drive tray. This did not work as expected but I left the code in case someone might find such an example useful. + diff --git a/config.py.dist b/config.py.dist new file mode 100644 index 0000000..f286cb7 --- /dev/null +++ b/config.py.dist @@ -0,0 +1,12 @@ +# The USB device path used to control the robot +USB_DEVICE = '/dev/serial/by-id/usb-FTDI_FT232R_USB_UART_A6031WVV-if00-port0' +# The reader/burner block device +CD_DEVICE = '/dev/disk/by-id/usb-Optiarc_DVD_RW_AD-5260S_000000000000-0:0' + +# This must be calibrated manually depending on the physical layout of the +# trays and CD drive +TRAY_CD_POS = (-68, 215, 81) +SRC_TRAY_POS = (-147, 50, 125) +DUMP_TRAY_POS = (-300, 52, 100) + +TRIES = 5 diff --git a/junk/find_cd_image.py b/junk/find_cd_image.py new file mode 100644 index 0000000..909debe --- /dev/null +++ b/junk/find_cd_image.py @@ -0,0 +1,104 @@ +#!/usr/bin/env python2 + +import cv2, math +import numpy as np + +#capture from camera at location 0 +cap = cv2.VideoCapture(0) +#set the width and height, and UNSUCCESSFULLY set the exposure time +cap.set(3,1280) +cap.set(4,1024) +cap.set(15, 0.1) + +def dist(a,b): + return math.sqrt( (a[0] - b[0]) * (a[0] - b[0]) + (a[1] - b[1]) * (a[1] - b[1]) ) + +# Needs calibration +ideal_pos = (334,309) +# Needs calibration +ideal_r = (309-214) + +while True: + + best_circles = [] + samples_n = 5 + + for c in range(samples_n): + + ret, img = cap.read() + center = (img.shape[1] / 2, img.shape[0] / 2) + # cv2.line(img, (center[0],0), (center[0], img.shape[1]), (200,0,0)) + # img = img[0:img.shape[0], 0:img.shape[1]/2] + img = cv2.medianBlur(img, 3) + # print("Captured image is %d x %d pixels, center is %d x %d" % (img.shape[1], img.shape[0], center[0], center[1])) + cv2.imshow("input", img) + #cv2.imshow("thresholded", imgray*thresh2) + edges = cv2.Canny(img,100,200) + + circles = cv2.HoughCircles(edges,cv2.cv.CV_HOUGH_GRADIENT,1,20,param1=50,param2=30,minRadius=np.uint16(ideal_r*0.9),maxRadius=np.uint16(ideal_r*1.1)) + # print(circles) + + if circles is not None: + + best_c_p = None + best_c_r = None + best_c_score = 1000 + + circles = np.uint16(np.around(circles)) + for i in circles[0,:]: + if dist(center, (i[0], i[1])) + abs(i[2] - ideal_r) < best_c_score: + best_c_p = (i[0], i[1]) + best_c_r = i[2] + best_c_dist = dist(center, best_c_p) + + best_circles.append( + { + "p": best_c_p, + "r": best_c_r, + "score": best_c_score, + } + ) + + # circles = np.uint16(np.around(circles)) + # for i in circles[0,:]: + # # draw the outer circle + # cv2.circle(edges,(i[0],i[1]),i[2],(100,0,0),2) + # # draw the center of the circle + # cv2.circle(edges,(i[0],i[1]),2,(200,0,0),3) + + # # draw the outer circle + # cv2.circle(edges,tuple(best_c_p),best_c_r,(100,0,0),2) + # # draw the center of the circle + # cv2.circle(edges,tuple(best_c_p),2,(200,0,0),3) + + # cv2.imshow("edges", edges) + + avg_best_c_p = [0,0] + avg_best_c_r = 0 + + if len(best_circles) > 0: + for c in best_circles: + avg_best_c_p[0] = avg_best_c_p[0] + c["p"][0] + avg_best_c_p[1] = avg_best_c_p[1] + c["p"][1] + avg_best_c_r = avg_best_c_r + c["r"] + + avg_best_c_p[0] = np.uint16(avg_best_c_p[0] / len(best_circles)) + avg_best_c_p[1] = np.uint16(avg_best_c_p[1] / len(best_circles)) + avg_best_c_r = np.uint16(avg_best_c_r / len(best_circles)) + + print("Found best avg circle (samples=%d) p=%s r=%s" % (len(best_circles), avg_best_c_p, avg_best_c_r)) + # draw the outer circle + cv2.circle(edges,tuple(avg_best_c_p),avg_best_c_r,(100,0,0),2) + # draw the center of the circle + cv2.circle(edges,tuple(avg_best_c_p),2,(200,0,0),3) + + cv2.imshow("edges", edges) + + key = cv2.waitKey(10) + if key == 27: + break + + +cv2.destroyAllWindows() +cv2.VideoCapture(0).release() + diff --git a/robot.py b/robot.py new file mode 100755 index 0000000..6b7d281 --- /dev/null +++ b/robot.py @@ -0,0 +1,174 @@ +#!/usr/bin/env python3 + +import serial, time, sys, re, os, argparse, logging +import config + +logging.basicConfig(level=logging.INFO) +log = logging.getLogger(__name__) + +f = serial.Serial(config.USB_DEVICE, 115200) +banner = f.readline() +log.info("Banner is '%s'" % (banner)) + +def command(cmd): + i=1 + f.write(("#%d %s\n" % (i, cmd)).encode("ascii") ) + response = f.readline().decode('ascii').rstrip() + m = re.match("\$(?P\d+) (?POK|E\d+) ?(?P.*)", response).groupdict() + log.debug("Command '%s', response token '%s' result_code '%s' value '%s'" % (cmd, m['token'], m['result_code'], m.get('value', '*NONE*'))) + return m + + +# Reference: https://cdn.sparkfun.com/datasheets/Robotics/uArm_Vacuum_System_User_Guide.pdf +def pump(state): + command("M231 V%d" % (state)) + +def valve(state): + command("M240 N5 V%d" % (state)) + +def grab(): + valve(0) + pump(1) + +def release(): + pump(0) + valve(1) + + +def getdigital(pin): + m = command("P240 N%d" % (pin)) + if m['value'] == 'V0': + return True + else: + return False + +def limitsw(): + return getdigital(2) + +def armpos(x,y,z,v=10): + command("G0 X%.1f Y%.1f Z%.1f F%d" % (x,y,z,v)) + wait() + +def armposrel(dx,dy,dz,v=0): + command("G204 X%.1f Y%.1f Z%.1f F%d" % (dx,dy,dz,v)) + wait() + +def device_name(): + command("P201") + command("P202") + command("P203") + command("P204") + command("P205") + +def arm_reset(): + armposrel(0, 0, 100) + armpos(0, 150, 100) + +def wait(): + log.info("Waiting for move to end") + while True: + moving = command("M200") + if moving['value'] == "V0": + break + time.sleep(0.1) + +def grab_cd(): + log.infot("Grabbing CD") + + z = 125 + while True: + armposrel(0, 0, -0.5) + + z = z - 0.5 + if z < -20: + arm_reset() + log.fatal("Something is very wrong") + + if limitsw() is False: + continue + + log.info("We got something") + grab() + time.sleep(1) + + armposrel(0,0,100) + + break + +def pickup_new_cd(): + log.info("Moving to source tray") + armpos(*config.SRC_TRAY_POS) + grab_cd() + +def return_cd(): + log.info("Returning CD to dump tray") + armpos(*config.DUMP_TRAY_POS) + + release() + time.sleep(2) + +def drop_cd_in_drive(): + armpos(*config.TRAY_CD_POS) + release(); + time.sleep(2) + +def pickup_cd_from_drive(): + armpos(*config.TRAY_CD_POS) + grab_cd() + time.sleep(1) + +if __name__ == "__main__": + + device_name() + + for i in range(int( sys.argv[1] )): + + os.system("eject %s" % (config.CD_DEVICE)) + + pickup_new_cd() + drop_cd_in_drive() + + tries = config.TRIES + while tries > 0: + if os.system("eject -t %s" % (config.CD_DEVICE)) > 0: + log.warn("Tray close not successful, trying again") + os.system("eject %s" % (config.CD_DEVICE)) + tries = tries - 1 + + if tries == 0: + armpos(*tray_cd_pos) + grab_cd() + + # Drop cd into *SOURCE* tray to pick it up again + log.info("Moving to source tray") + armpos(*config.SRC_TRAY_POS) + release() + + arm_reset() + + pickup_new_cd() + drop_cd_in_drive() + + tries = config.TRIES + + continue + + else: + + log.info("Now we are reading the CD") + # os.system("cdrdao --device=%s disk-info") + # os.system("dd if=%s of=dump.bin bs=10000000 count=1" % (config.CD_DEVICE)) + time.sleep(3) + + log.info("Finished reading CD") + break + + os.system("eject %s" % (config.CD_DEVICE)) + + pickup_cd_from_drive() + return_cd() + + arm_reset() + os.system("eject -t %s" % (config.CD_DEVICE)) + os.system("beep -f 1000 -n -f 2000 -n -f 1500") +