From 7760a652a132a8f9e7d6ed3fa247c3834203b9ee Mon Sep 17 00:00:00 2001 From: "EPanepucci (x06sa-cons-701)" Date: Wed, 28 Jul 2021 14:46:40 +0200 Subject: [PATCH] SOFT-227 implement cryostage motions, WIP - ported modifications offline, still need to finish and test --- script/data/pucks.py | 1 - script/data/samples.py | 3 +- script/devices/RobotSC.py | 78 ++++++++++++++++++++++---------------- script/motion/dry.py | 9 +++-- script/motion/get_aux.py | 1 + script/motion/mount.py | 71 +++++++++++++++++++++++++--------- script/motion/move_cold.py | 6 +-- script/motion/unmount.py | 22 +++++++---- 8 files changed, 124 insertions(+), 67 deletions(-) diff --git a/script/data/pucks.py b/script/data/pucks.py index 7a0a4ff..4b82cbc 100644 --- a/script/data/pucks.py +++ b/script/data/pucks.py @@ -48,4 +48,3 @@ def restore_puck_info(): print >> sys.stderr, "Error reading pucks info file: " + str(sys.exc_info()[1]) info = [] set_puck_info(info) - \ No newline at end of file diff --git a/script/data/samples.py b/script/data/samples.py index 3d6549b..7648bd7 100644 --- a/script/data/samples.py +++ b/script/data/samples.py @@ -8,7 +8,6 @@ samples_info = [] def set_samples_info(info): - log("Update samples info: " + str(info)) global samples_info if (is_string(info)): info = json.loads(info) @@ -361,4 +360,4 @@ test_sample_data = [ \ "sampleMountCount": 0, } , \ ] - \ No newline at end of file + diff --git a/script/devices/RobotSC.py b/script/devices/RobotSC.py index 8fc0088..e7373d4 100644 --- a/script/devices/RobotSC.py +++ b/script/devices/RobotSC.py @@ -32,28 +32,41 @@ class RobotSC(RobotTCP): self.setPolling(DEFAULT_ROBOT_POLLING) self.last_command_timestamp = None self.last_command_position = None + self.ongoing_task = None #self.setSimulated() - def move_dewar(self): - self.start_task('moveDewar') + def wait_async_motion(self): + og_position, og_assert = self.ongoing_task self.wait_task_finished(TASK_WAIT_ROBOT_POLLING) - self.assert_dewar() - self.last_command_position = "dewar" + if callable(og_assert): + og_assert() + self.last_command_position = og_position self.last_command_timestamp = time.time() + self.ongoing_task = None + + def move_dewar_async(self): + self.start_task('moveDewar') + self.ongoing_task = ("dewar", self.assert_dewar) + + def move_dewar(self): + self.move_dewar_async() + self.wait_async_motion() + + def move_cold_async(self): + self.start_task('moveCold') + self.ongoing_task = ("cold", self.assert_cold) def move_cold(self): - self.start_task('moveCold') - self.wait_task_finished(TASK_WAIT_ROBOT_POLLING) - self.assert_cold() - self.last_command_position = "cold" - self.last_command_timestamp = time.time() + self.move_cold_async() + self.wait_async_motion() + + def move_home_async(self): + self.start_task('moveHome') + self.ongoing_task = ("home", self.assert_home) def move_home(self): - self.start_task('moveHome') - self.wait_task_finished(TASK_WAIT_ROBOT_POLLING) - self.assert_home() - self.last_command_position = "home" - self.last_command_timestamp = time.time() + self.move_home_async() + self.wait_async_motion() def get_dewar(self, segment, puck, sample): segment = self.toSegmentNumber(segment) @@ -76,7 +89,6 @@ class RobotSC(RobotTCP): def put_gonio(self): pin_offset = get_pin_offset() pin_angle_offset = get_pin_angle_offset() - print "Pin offset = " + str(pin_offset) self.start_task('putGonio', pin_offset) self.wait_task_finished(TASK_WAIT_ROBOT_POLLING) self.assert_gonio() @@ -85,7 +97,6 @@ class RobotSC(RobotTCP): def get_gonio(self): pin_offset = get_pin_offset() - print "Pin offset = " + str(pin_offset) self.start_task('getGonio', pin_offset) self.wait_task_finished(TASK_WAIT_ROBOT_POLLING) self.assert_gonio() @@ -127,20 +138,22 @@ class RobotSC(RobotTCP): # self.wait_task_finished(TASK_WAIT_ROBOT_POLLING) # self.assert_scan_stop() - def move_gonio(self): + def move_gonio_async(self): + assert_detector_safe() self.start_task('moveGonio') - self.wait_task_finished(TASK_WAIT_ROBOT_POLLING) - self.assert_gonio() - self.last_command_position = "gonio" - self.last_command_timestamp = time.time() + self.ongoing_task = ("gonio", self.assert_gonio) + def move_gonio(self): + self.move_gonio_async() + self.wait_async_motion() + + def move_park_async(self): + self.start_task('movePark') + self.ongoing_task = ("park", self.assert_park) def move_park(self): - self.start_task('movePark') - self.wait_task_finished(TASK_WAIT_ROBOT_POLLING) - self.assert_park() - self.last_command_position = "park" - self.last_command_timestamp = time.time() + self.move_park_async() + self.wait_async_motion() def move_heater(self, speed=-1, to_bottom=True): self.start_task('moveHeater', speed, to_bottom) @@ -160,12 +173,13 @@ class RobotSC(RobotTCP): self.last_command_position = "home" self.last_command_timestamp = time.time() - def move_aux(self): + def move_aux_async(self): self.start_task('moveAux') - self.wait_task_finished(TASK_WAIT_ROBOT_POLLING) - self.assert_aux() - self.last_command_position = "aux" - self.last_command_timestamp = time.time() + self.ongoing_task = ("aux", self.assert_aux) + + def move_aux(self): + self.move_aux_async() + self.wait_async_motion() def get_calibration_tool(self): self.start_task('getCalTool') @@ -359,4 +373,4 @@ if joint_forces: add_device(jf4(), force = True) add_device(jf5(), force = True) add_device(jf6(), force = True) - add_device(jfc(), force = True) \ No newline at end of file + add_device(jfc(), force = True) diff --git a/script/motion/dry.py b/script/motion/dry.py index a70e878..383fc61 100644 --- a/script/motion/dry.py +++ b/script/motion/dry.py @@ -9,17 +9,18 @@ def dry(heat_time=None, speed=None, wait_cold = None): wait_cold(float): if negative, move to dewar after drying Else move to cold and wait (in seconds) before returning. """ - print "dry" if heat_time is None: heat_time = DEFAULT_DRY_HEAT_TIME - if speed is None: speed = DEFAULT_DRY_SPEED - if wait_cold is None: wait_cold = DEFAULT_DRY_WAIT_COLD + + print "dry heat_time = {}, speed = {}, wait_cold = {}".format(heat_time, speed, wait_cold) + + if robot.simulated: time.sleep(10.0) return @@ -55,4 +56,4 @@ def dry(heat_time=None, speed=None, wait_cold = None): robot.move_cold() time.sleep(wait_cold) else: - robot.move_park() \ No newline at end of file + robot.move_park() diff --git a/script/motion/get_aux.py b/script/motion/get_aux.py index 67bfc7d..e816c4c 100644 --- a/script/motion/get_aux.py +++ b/script/motion/get_aux.py @@ -3,6 +3,7 @@ def get_aux(sample): """ print "get_aux: ",sample + sample = int(sample) #assert_valid_sample only acceps int as parameters #Initial checks assert_valid_sample(sample) diff --git a/script/motion/mount.py b/script/motion/mount.py index 6593649..39bf85e 100644 --- a/script/motion/mount.py +++ b/script/motion/mount.py @@ -5,19 +5,20 @@ def mount(segment, puck, sample, force=False, read_dm=False, auto_unmount=False) """ """ global mount_sample_id, mount_sample_detected + tg = time.time + t1 = tg() print "mount: ", segment, puck, sample, force - start = time.time() is_aux = (segment == AUX_SEGMENT) is_rt = (segment == RT_SEGMENT) is_normal = not (is_aux or is_rt) - previous_mounted = get_setting("mounted_sample_position") if previous_mounted is not None: previous_segment, previous_puck, previous_sample = previous_mounted[0], int(previous_mounted[1]), int(previous_mounted[2:]) else: previous_segment, previous_puck, previous_sample = "-", 0, 0 + mounting_in_same_segment = (previous_segment == segment) # ZAC was_cold = robot.is_cold() @@ -27,6 +28,8 @@ def mount(segment, puck, sample, force=False, read_dm=False, auto_unmount=False) count_mount = is_normal or (is_aux and was_cold) zero_counts = (is_aux and not was_cold) or (is_rt and was_cold) + pin_offset = get_pin_offset() + puck_address = get_puck_address(puck) if puck_address is None: puck_obj = get_puck_obj_by_id(puck) @@ -37,6 +40,8 @@ def mount(segment, puck, sample, force=False, read_dm=False, auto_unmount=False) segment = puck_address[:1] puck = int(puck_address[1:]) #Initial checks + print "%4.1f s asserting" % (tg() - t1,) + #Initial checks assert_valid_address(segment, puck, sample) assert_puck_detected(segment, puck) @@ -56,6 +61,7 @@ def mount(segment, puck, sample, force=False, read_dm=False, auto_unmount=False) hexiposi.assert_homed() assert_mount_position() do_unmount = False + print "%4.1f s asserted " % (tg() - t1,) # detect sample before moving to cold if smart_magnet.get_supress() == True: @@ -64,16 +70,26 @@ def mount(segment, puck, sample, force=False, read_dm=False, auto_unmount=False) sample_det = smart_magnet.check_mounted(idle_time=0.25, timeout = 1.0) Controller.getInstance().logEvent("Sample Detection", str(sample_det)) + print "%4.1f s SMC checked" % (tg() - t1,) + + + mounting_in_same_segment = False + try: if sample_det == True: if was_warm and previous_segment in COLD_SEGMENTS: - print "was warm previous %s %s $s" % (previous_segment, COLD_SEGMENTS, previous_mounted) + print " was warm previous %s %s %s" % (previous_segment, COLD_SEGMENTS, previous_mounted) robot.move_cold() time.sleep(30.0) if auto_unmount and (get_setting("mounted_sample_position") is not None): + if mounting_in_same_segment: + print " Mounting from the same segment" #auto_unmount set to true so detection remains enabled - sample_det = unmount(force = True, auto_unmount = True, next_segment=segment) + print "%4.1f s unmounting" % (tg() - t1,) + sample_det = unmount(force = True, auto_unmount = True, mounting_in_same_segment=mounting_in_same_segment, next_segment=segment) + print "%4.1f s ....done" % (tg() - t1,) + do_unmount = True if sample_det == True: raise Exception("Pin detected on gonio") @@ -92,8 +108,6 @@ def mount(segment, puck, sample, force=False, read_dm=False, auto_unmount=False) if not do_unmount: enable_motion() - # ZAC - # a room temp pin is being mounted but the gripper is cold if needs_drying: dry(wait_cold=-1) # move to park after dry @@ -103,33 +117,53 @@ def mount(segment, puck, sample, force=False, read_dm=False, auto_unmount=False) robot.get_aux(sample) elif is_rt: + if not robot.is_rt(): + robot.move_rt() + robot.get_rt(puck, sample) # TODO: implement room temperature for X06SA - pass - else: + print "%4.1f s moving lid " % (tg() - t1,) + set_hexiposi(segment) if not force: visual_check_hexiposi(segment) - if not robot.is_dewar(): + if (not robot.is_dewar()) and (not mounting_in_same_segment): robot.move_dewar() - robot.get_dewar(segment, puck, sample) + print "%4.1f s get_dewar" % (tg() - t1,) + robot.get_dewar(segment, puck, sample, mounting_in_same_segment=mounting_in_same_segment) if read_dm: + print "%4.1f s barcode read" % (tg() - t1,) barcode_reader.start_read(10.0) robot.move_scanner() + print "%4.1f s ...done" % (tg() - t1,) #time.sleep(1.0) - + print "%4.1f s move to gonio " % (tg() - t1,) robot.move_gonio() + print "%4.1f s ....done" % (tg() - t1,) if read_dm: + print "%4.1f s reading pin barcode" % (tg() - t1,) mount_sample_id = barcode_reader.get_readout() - print "Datamatrix: " , mount_sample_id + print "%4.1f s ....done --- barcode: %s" % (tg() - t1, mount_sample_id) else: mount_sample_id = None + print "%4.1f s moving cryo out" % (tg() - t1,) + curpos = cryopos.getPosition() + speed = cryopos.getSpeed() + movetime = int(round(5000 + (15. - curpos) / speed)) + cryopos.move(15.0, movetime) + cryopos.waitValueInRange(15.0, 1.0, movetime) + print " ....done\n%4.1f s putting_gonio with pin_offset = %.1f mm" % (tg() - t1, pin_offset) robot.put_gonio() + print "%4.1f s .....done" % (tg() - t1,) + + if is_normal: + print "%4.1f s async moving cryo to 5mm " % (tg() - t1,) + cryopos.moveAsync(5.0) if count_mount: try: @@ -143,16 +177,17 @@ def mount(segment, puck, sample, force=False, read_dm=False, auto_unmount=False) set_setting("dry_mount_counter", 0) if is_rt: - print "mount/moving park in RT" - robot.move_park() + print "%4.1f s mount/moving home in RT" % (tg() - t1,) + robot.move_home() elif is_normal: - print "mount/moving cold in cold" + print "%4.1f mount/moving cold in cold " % (tg() - t1,) robot.move_cold() elif is_aux: if was_cold: - print "mount/moving park in AUX" + print "%4.1f s mount/moving cold after auxtool mount " % (tg() - t1,) robot.move_cold() else: + print "%4.1f s mount/moving park after auxtool mount" % (tg() - t1,) robot.move_park() else: robot.move_cold() @@ -166,13 +201,13 @@ def mount(segment, puck, sample, force=False, read_dm=False, auto_unmount=False) if is_force_dry(): smart_magnet.set_default_current() - print "Auto dry" + print "%4.1f Auto dry async" % (tg() - t1,) log("Starting auto dry", False) set_exec_pars(then = "dry()") set_setting("mounted_sample_position", get_sample_name(segment, puck, sample)) return [mount_sample_detected, mount_sample_id] finally: + print "%4.1f s magnet suppress/default current" % (tg() - t1,) smart_magnet.set_default_current() smart_magnet.set_supress(True) - diff --git a/script/motion/move_cold.py b/script/motion/move_cold.py index 78bdea0..4c02aca 100644 --- a/script/motion/move_cold.py +++ b/script/motion/move_cold.py @@ -2,10 +2,10 @@ def move_cold(reset_timestamp=False): """ """ if reset_timestamp: - print "move_cold and reset ry_timestamp" set_setting("dry_timestamp", time.time()) + print "move_cold and resetting dry_timestamp" else: - print "move_cold" + print "move cold" if robot.simulated: time.sleep(3.0) @@ -23,4 +23,4 @@ def move_cold(reset_timestamp=False): if not robot.is_cold(): - robot.move_cold() \ No newline at end of file + robot.move_cold() diff --git a/script/motion/unmount.py b/script/motion/unmount.py index 7ba4f83..9239fd4 100644 --- a/script/motion/unmount.py +++ b/script/motion/unmount.py @@ -1,15 +1,16 @@ -def unmount(segment = None, puck = None, sample = None, force=False, auto_unmount = False, next_segment="-*-"): +def unmount(segment = None, puck = None, sample = None, force=False, auto_unmount = False, next_segment="-*-", mounting_in_same_segment=False): """ """ - print "unmount: ", segment, puck, sample, force, auto_unmount + print "unmount: ", segment, puck, sample, force + tg = time.time + t1 = tg() if (segment is None) or (puck is None) or (sample is None): pos = get_setting("mounted_sample_position") if pos is None: raise Exception("Mounted sample position is not defined") segment, puck , sample = pos[0:1], int(pos[1]), int(pos[2:]) - print "Mounted sample position: ", segment, puck , sample - + print "%4.1f Mounted sample position: %s%s-%s" % (tg() - t1, segment, puck , sample) # ZAC: these need to happen after defining segment, puck, sample is_aux = (segment == AUX_SEGMENT) is_rt = (segment == RT_SEGMENT) @@ -24,9 +25,10 @@ def unmount(segment = None, puck = None, sample = None, force=False, auto_unmoun #Initial checks if not auto_unmount: + print "%4.1f s asserting" % (tg() - t1,) print "assert valid address" assert_valid_address(segment, puck, sample) - print "asser puck detected" + print " assert puck detected" assert_puck_detected(segment, puck) if robot.simulated: @@ -69,18 +71,18 @@ def unmount(segment = None, puck = None, sample = None, force=False, auto_unmoun #Enabling enable_motion() - if not is_aux: + if is_normal: set_hexiposi(segment) if not force: visual_check_hexiposi(segment) if needs_chilling: - print "umount():78 chilling" robot.move_cold() time.sleep(30.) else: if needs_drying: dry(wait_cold=-1) + #location = robot.get_current_point() if not robot.is_gonio(): @@ -88,6 +90,12 @@ def unmount(segment = None, puck = None, sample = None, force=False, auto_unmoun #smart_magnet.set_unmount_current() + curpos = cryopos.getPosition() + speed = cryopos.getSpeed() + movetime = int(round(5000 + (15. - curpos) / speed)) + cryopos.move(15.0, movetime) + cryopos.waitValueInRange(15.0, 1.0, movetime) + robot.get_gonio() smart_magnet.apply_reverse()