175 lines
3.9 KiB
Python
Executable File
175 lines
3.9 KiB
Python
Executable File
#!/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.info("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")
|
|
|