318 lines
9.8 KiB
Python
318 lines
9.8 KiB
Python
TOOL_CALIBRATION = "tCalib"
|
|
TOOL_SUNA= "tSuna"
|
|
TOOL_DEFAULT= TOOL_SUNA
|
|
|
|
FRAME_TABLE = "fTable"
|
|
|
|
DESC_FAST = "mFast"
|
|
DESC_SLOW = "mSlow"
|
|
DESC_SCAN = "mScan"
|
|
DESC_DEFAULT = DESC_FAST
|
|
|
|
AUX_SEGMENT = "X"
|
|
|
|
|
|
DEFAULT_ROBOT_POLLING = 500
|
|
TASK_WAIT_ROBOT_POLLING = 50
|
|
|
|
|
|
run("devices/RobotTCP")
|
|
|
|
|
|
simulation = False
|
|
|
|
joint_forces = False
|
|
|
|
class RobotSC(RobotTCP):
|
|
def __init__(self, name, server, timeout = 1000, retries = 1):
|
|
RobotTCP.__init__(self, name, server, timeout, retries)
|
|
self.set_tasks(["getDewar", "putDewar", "putGonio", "getGonio", "recover", "moveDewar", "moveCold", "movePark", "moveGonio","moveHeater", "moveScanner","moveHome", "moveAux"])
|
|
self.set_known_points(["pPark", "pGonio", "pDewar", "pGonioG", "pScan", "pHeater", "pHeat", "pHeatB", "pScanStop","pHelium", "pHome", "pCold", "pAux"])
|
|
self.setPolling(DEFAULT_ROBOT_POLLING)
|
|
#self.setSimulated()
|
|
|
|
def move_dewar(self):
|
|
self.start_task('moveDewar')
|
|
self.wait_task_finished(TASK_WAIT_ROBOT_POLLING)
|
|
self.assert_dewar()
|
|
|
|
def move_cold(self):
|
|
self.start_task('moveCold')
|
|
self.wait_task_finished(TASK_WAIT_ROBOT_POLLING)
|
|
self.assert_cold()
|
|
|
|
def move_home(self):
|
|
self.start_task('moveHome')
|
|
self.wait_task_finished(TASK_WAIT_ROBOT_POLLING)
|
|
self.assert_home()
|
|
|
|
def get_dewar(self, segment, puck, sample):
|
|
segment = self.toSegmentNumber(segment)
|
|
self.start_task('getDewar',segment, puck, sample, is_room_temp())
|
|
self.wait_task_finished(TASK_WAIT_ROBOT_POLLING)
|
|
self.assert_dewar()
|
|
|
|
def put_dewar(self, segment, puck, sample):
|
|
segment = self.toSegmentNumber(segment)
|
|
self.assert_dewar()
|
|
self.start_task('putDewar',segment, puck, sample, is_room_temp())
|
|
self.wait_task_finished(TASK_WAIT_ROBOT_POLLING)
|
|
#self.assert_dewar()
|
|
self.assert_cold()
|
|
|
|
def put_gonio(self):
|
|
pin_offset = get_pin_offset()
|
|
pin_angle_offset = get_pin_angle_offset()
|
|
self.start_task('putGonio', pin_offset)
|
|
self.wait_task_finished(TASK_WAIT_ROBOT_POLLING)
|
|
self.assert_gonio()
|
|
|
|
def get_gonio(self):
|
|
pin_offset = get_pin_offset()
|
|
self.start_task('getGonio', pin_offset)
|
|
self.wait_task_finished(TASK_WAIT_ROBOT_POLLING)
|
|
self.assert_gonio()
|
|
|
|
def get_aux(self, sample):
|
|
self.assert_aux()
|
|
self.start_task('getAuxiliary', sample)
|
|
self.wait_task_finished(TASK_WAIT_ROBOT_POLLING)
|
|
self.assert_aux()
|
|
|
|
def put_aux(self, sample):
|
|
self.assert_aux()
|
|
self.start_task('putAuxiliary', sample)
|
|
self.wait_task_finished(TASK_WAIT_ROBOT_POLLING)
|
|
self.assert_aux()
|
|
|
|
def move_scanner(self):
|
|
self.start_task('moveScanner')
|
|
self.wait_task_finished(TASK_WAIT_ROBOT_POLLING)
|
|
self.assert_scanner()
|
|
|
|
def move_scanner(self):
|
|
self.start_task('moveScanner')
|
|
self.wait_task_finished(TASK_WAIT_ROBOT_POLLING)
|
|
self.assert_scanner()
|
|
|
|
#def do_scan(self):
|
|
# self.start_task('doScan')
|
|
# self.wait_task_finished(TASK_WAIT_ROBOT_POLLING)
|
|
# self.assert_scan_stop()
|
|
|
|
def move_gonio(self):
|
|
self.start_task('moveGonio')
|
|
self.wait_task_finished(TASK_WAIT_ROBOT_POLLING)
|
|
self.assert_gonio()
|
|
|
|
|
|
def move_park(self):
|
|
self.start_task('movePark')
|
|
self.wait_task_finished(TASK_WAIT_ROBOT_POLLING)
|
|
self.assert_park()
|
|
|
|
def move_heater(self, speed=-1, to_bottom=True):
|
|
self.start_task('moveHeater', speed, to_bottom)
|
|
self.wait_task_finished(TASK_WAIT_ROBOT_POLLING)
|
|
if to_bottom:
|
|
self.assert_heater_bottom()
|
|
else:
|
|
self.assert_heater()
|
|
|
|
|
|
def robot_recover(self):
|
|
self.start_task('recover')
|
|
self.wait_task_finished(TASK_WAIT_ROBOT_POLLING)
|
|
self.assert_home()
|
|
|
|
def move_aux(self):
|
|
self.start_task('moveAux')
|
|
self.wait_task_finished(TASK_WAIT_ROBOT_POLLING)
|
|
self.assert_aux()
|
|
|
|
def get_calibration_tool(self):
|
|
self.start_task('getCalTool')
|
|
self.wait_task_finished(TASK_WAIT_ROBOT_POLLING)
|
|
self.assert_scanner()
|
|
|
|
def put_calibration_tool(self):
|
|
self.start_task('putCalTool')
|
|
self.wait_task_finished(TASK_WAIT_ROBOT_POLLING)
|
|
self.assert_scanner()
|
|
|
|
|
|
|
|
def toSegmentNumber(self, segment):
|
|
if is_string(segment):
|
|
segment = ord(segment.upper()) - ord('A') +1
|
|
return segment
|
|
|
|
|
|
def on_event(self,ev):
|
|
#print "EVT: " + ev
|
|
pass
|
|
def on_change_working_mode(self, working_mode):
|
|
if get_device("hexiposi") is not None:
|
|
hexiposi.moved = True #Force image processing on first sample
|
|
|
|
def doUpdate(self):
|
|
#start = time.time()
|
|
RobotTCP.doUpdate(self)
|
|
global simulation
|
|
if not simulation:
|
|
if joint_forces:
|
|
if self.state != State.Offline:
|
|
self.get_joint_forces()
|
|
for dev in [jf1, jf2, jf3, jf4,jf5, jf6, jfc]:
|
|
dev.update()
|
|
#print time.time() -start
|
|
|
|
def start_task(self, program, *args, **kwargs):
|
|
#TODO: Check safe position
|
|
return RobotTCP.start_task(self, program, *args, **kwargs)
|
|
|
|
def stop_task(self):
|
|
RobotTCP.stop_task(self)
|
|
#TODO: Restore safe position
|
|
|
|
|
|
def set_remote_mode(self):
|
|
robot.set_profile("remote")
|
|
|
|
def set_local(self):
|
|
robot.set_profile("default")
|
|
|
|
def is_park(self):
|
|
return self.is_in_point("pPark")
|
|
|
|
def is_cold(self):
|
|
return self.is_in_point("pCold")
|
|
|
|
def is_home(self):
|
|
return self.is_in_point("pHome")
|
|
|
|
def is_dewar(self):
|
|
return self.is_in_point("pDewar")
|
|
|
|
def is_heater(self):
|
|
return self.is_in_point("pHeat")
|
|
|
|
def is_heater_home(self):
|
|
return self.is_in_point("pHeater")
|
|
|
|
def is_heater_bottom(self):
|
|
return self.is_in_point("pHeatB")
|
|
|
|
def is_gonio(self):
|
|
return self.is_in_point("pGonio")
|
|
|
|
def is_helium(self):
|
|
return self.is_in_point("pHelium")
|
|
|
|
def is_scanner(self):
|
|
return self.is_in_point("pScan")
|
|
|
|
def is_aux(self):
|
|
return self.is_in_point("pAux")
|
|
|
|
#def is_scan_stop(self):
|
|
# return self.is_in_point("pScanStop")
|
|
|
|
def is_cleared(self):
|
|
#return self.is_home() or self.is_park() or self.is_dewar() or self.is_dewar_home()
|
|
return self.get_current_point() is not None
|
|
|
|
def assert_heater_home(self):
|
|
self.assert_in_point("pHeater")
|
|
|
|
def assert_cold(self):
|
|
self.assert_in_point("pCold")
|
|
|
|
def assert_heater(self):
|
|
self.assert_in_point("pHeat")
|
|
|
|
def assert_heater_bottom(self):
|
|
self.assert_in_point("pHeatB")
|
|
|
|
def assert_park(self):
|
|
self.assert_in_point("pPark")
|
|
|
|
def assert_home(self):
|
|
self.assert_in_point("pHome")
|
|
|
|
def assert_dewar(self):
|
|
self.assert_in_point("pDewar")
|
|
|
|
def assert_gonio(self):
|
|
self.assert_in_point("pGonio")
|
|
|
|
def assert_helium(self):
|
|
self.assert_in_point("pHelium")
|
|
|
|
def assert_scanner(self):
|
|
self.assert_in_point("pScan")
|
|
|
|
def assert_aux(self):
|
|
self.assert_in_point("pAux")
|
|
|
|
#def assert_scan_stop(self):
|
|
# self.assert_in_point("pScanStop")
|
|
|
|
def assert_cleared(self):
|
|
if not self.is_cleared():
|
|
raise Exception("Robot not in cleared position")
|
|
|
|
def wait_ready(self):
|
|
robot.waitState(State.Ready, 1000) #robot.state.assertReady()
|
|
|
|
if simulation:
|
|
add_device(RobotSC("robot","localhost:1000"),force = True)
|
|
else:
|
|
#add_device(RobotSC("robot", "129.129.243.120:1000"), force = True)
|
|
#add_device(RobotSC("robot", "pcp068129.psi.ch:1000"), force = True)
|
|
#add_device(RobotSC("robot", "saresb-cons-06.psi.ch:1000"), force = True)
|
|
add_device(RobotSC("robot", "129.129.243.90:1000"), force = True)
|
|
|
|
|
|
robot.set_default_desc(DESC_DEFAULT)
|
|
robot.default_speed = 20
|
|
robot.set_frame(FRAME_DEFAULT)
|
|
robot.set_tool(TOOL_DEFAULT)
|
|
robot.setPolling(DEFAULT_ROBOT_POLLING)
|
|
|
|
robot.get_current_point() #TODO: REMOVE WHEN CURRENT POINT REPORTED BY POLLING MESSAGE
|
|
|
|
class jf1(ReadonlyRegisterBase):
|
|
def doRead(self):
|
|
return None if robot.joint_forces == None else robot.joint_forces[0]
|
|
class jf2(ReadonlyRegisterBase):
|
|
def doRead(self):
|
|
return None if robot.joint_forces == None else robot.joint_forces[1]
|
|
class jf3(ReadonlyRegisterBase):
|
|
def doRead(self):
|
|
return None if robot.joint_forces == None else robot.joint_forces[2]
|
|
class jf4(ReadonlyRegisterBase):
|
|
def doRead(self):
|
|
return None if robot.joint_forces == None else robot.joint_forces[3]
|
|
class jf5(ReadonlyRegisterBase):
|
|
def doRead(self):
|
|
return None if robot.joint_forces == None else robot.joint_forces[4]
|
|
class jf6(ReadonlyRegisterBase):
|
|
def doRead(self):
|
|
return None if robot.joint_forces == None else robot.joint_forces[5]
|
|
class jfc(ReadonlyRegisterBase):
|
|
def doRead(self):
|
|
if robot.joint_forces == None:
|
|
return float('NaN')
|
|
if robot.powered == False:
|
|
return float('NaN')
|
|
return (robot.joint_forces[1]+74)/4 + (robot.joint_forces[2]+30)/4 + (robot.joint_forces[4]-0.8)/0.2
|
|
|
|
if joint_forces:
|
|
add_device(jf1(), force = True)
|
|
add_device(jf2(), force = True)
|
|
add_device(jf3(), force = True)
|
|
add_device(jf4(), force = True)
|
|
add_device(jf5(), force = True)
|
|
add_device(jf6(), force = True)
|
|
add_device(jfc(), force = True) |