114 lines
3.3 KiB
Python
114 lines
3.3 KiB
Python
TOOL_CALIBRATION = "tCalib"
|
|
TOOL_SUNA= "tSuna"
|
|
TOOL_DEFAULT= TOOL_SUNA
|
|
|
|
DESC_FAST = "mFast"
|
|
DESC_SLOW = "mSlow"
|
|
DESC_SCAN = "mScan"
|
|
DESC_DEFAULT = DESC_FAST
|
|
|
|
|
|
run("devices/RobotTCP")
|
|
|
|
|
|
simulation = True
|
|
joint_forces = False
|
|
|
|
|
|
class RobotSC(RobotTCP):
|
|
def __init__(self, name, server, timeout = 1000, retries = 1):
|
|
RobotTCP.__init__(self, name, server, timeout, retries)
|
|
|
|
def mount(self, puck, sample):
|
|
return self.execute('mount',segment, puck, sample)
|
|
|
|
def firstmount(self, puck, sample):
|
|
return self.execute('firstmount', segment, puck, sample)
|
|
|
|
def unmount(self, puck, sample):
|
|
return self.execute('unmount',segment, puck, sample)
|
|
|
|
def on_event(self,ev):
|
|
#print "EVT: " + ev
|
|
pass
|
|
|
|
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
|
|
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")
|
|
|
|
|
|
|
|
if simulation:
|
|
#add_device(RobotSC("robot","129.129.126.92:1000"),force = True)
|
|
add_device(RobotSC("robot","129.129.110.99:1000"),force = True)
|
|
else:
|
|
add_device(RobotSC("robot", "129.129.110.100:1000"), force = True)
|
|
|
|
robot.high_level_tasks = ["mount", "firstmount"]
|
|
robot.set_tool(TOOL_CALIBRATION)
|
|
robot.setPolling(500)
|
|
|
|
#robot.set_monitor_speed(20)
|
|
|
|
|
|
|
|
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)
|
|
|
|
|
|
|
|
""" |