Initial revision
Maciej Grela
commit
b49d27615b
|
|
@ -0,0 +1 @@
|
|||
config.py
|
||||
|
|
@ -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.
|
||||
|
||||
|
|
@ -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
|
||||
|
|
@ -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()
|
||||
|
||||
|
|
@ -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<token>\d+) (?P<result_code>OK|E\d+) ?(?P<value>.*)", 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")
|
||||
|
||||
Loading…
Reference in New Issue