diff --git a/config/devices.properties b/config/devices.properties index b1c45d4..b00572c 100644 --- a/config/devices.properties +++ b/config/devices.properties @@ -7,7 +7,7 @@ mscan_pin=ch.psi.pshell.serial.TcpDevice|mscan-tell6s-pin:2001||| mscan_pin_cmd=ch.psi.pshell.serial.TcpDevice|mscan-tell6s-pin:2003||| mscan_puck=ch.psi.pshell.serial.TcpDevice|mscan-tell6s-puck:2001||| mscan_puck_cmd=ch.psi.pshell.serial.TcpDevice|mscan-tell6s-puck:2003||| -ue=LaserUE|COM4|||false +ue=LaserUE|COM4|||true puck_detection=ch.psi.mxsc.PuckDetection|tell6s-raspberrypi:5556||| wago=ch.psi.pshell.modbus.ModbusTCP|tell6s-wago:502||| led_ok_1=ch.psi.pshell.modbus.DigitalInput|wago 0||1000| diff --git a/config/settings.properties b/config/settings.properties index 381e92c..f14eede 100644 --- a/config/settings.properties +++ b/config/settings.properties @@ -1,5 +1,8 @@ -#Tue Dec 18 15:59:39 CET 2018 +#Mon Jan 14 18:14:03 CET 2019 barcode_reader_scan_pucks=false +dry_mount_counter=0 +dry_timestamp=1.547481588887E9 imaging_enabled=false led_level=0.0 +mounted_sample_position=F57 puck_types=true diff --git a/plugins/Recovery.java b/plugins/Recovery.java index 7b6381b..844c3c1 100644 --- a/plugins/Recovery.java +++ b/plugins/Recovery.java @@ -17,7 +17,7 @@ public class Recovery extends Panel { public Recovery() { initComponents(); - startTimer(5000, 200); + startTimer(2500, 200); } //Overridable callbacks diff --git a/script/devices/RobotSC.py b/script/devices/RobotSC.py index d0289e8..4a2ad5c 100644 --- a/script/devices/RobotSC.py +++ b/script/devices/RobotSC.py @@ -27,7 +27,7 @@ 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.set_known_points(["pPark", "pGonioA", "pDewar", "pGonioG", "pScan", "pHeater", "pHeat", "pHeatB", "pScanStop","pHelium", "pHome", "pCold", "pAux"]) self.setPolling(DEFAULT_ROBOT_POLLING) #self.setSimulated() @@ -204,7 +204,7 @@ class RobotSC(RobotTCP): return self.is_in_point("pHeatB") def is_gonio(self): - return self.is_in_point("pGonio") + return self.is_in_point("pGonioA") def is_helium(self): return self.is_in_point("pHelium") @@ -244,7 +244,7 @@ class RobotSC(RobotTCP): self.assert_in_point("pDewar") def assert_gonio(self): - self.assert_in_point("pGonio") + self.assert_in_point("pGonioA") def assert_helium(self): self.assert_in_point("pHelium") diff --git a/script/devices/RobotTCP.py b/script/devices/RobotTCP.py index c701a48..a34da81 100644 --- a/script/devices/RobotTCP.py +++ b/script/devices/RobotTCP.py @@ -422,7 +422,11 @@ class RobotTCP(TcpDevice, Stoppable): if not is_string(joint_or_point): robot.set_pnt(joint_or_point , "tcp_p") joint_or_point = "tcp_p" - ret = self.eval_int("movej(" + joint_or_point + ", " + tool + ", " + desc +")") + + #TODO: in new robot movel and movej is freezing controller + #ret = self.eval_int("movej(" + joint_or_point + ", " + tool + ", " + desc +")") + ret = int(self.execute('movej',joint_or_point, tool, desc)) + if sync: self.wait_end_of_move() return ret @@ -433,7 +437,10 @@ class RobotTCP(TcpDevice, Stoppable): if not is_string(point): robot.set_pnt(point , "tcp_p") point = "tcp_p" - ret = self.eval_int("movel(" + point + ", " + tool + ", " + desc +")") + #TODO: in new robot movel and movej is freezing controller + #ret = self.eval_int("movel(" + point + ", " + tool + ", " + desc +")") + ret = int(self.execute('movel',point, tool, desc)) + if sync: self.wait_end_of_move() return ret @@ -441,7 +448,11 @@ class RobotTCP(TcpDevice, Stoppable): def movec(self, point_interm, point_target, tool=None, desc=None, sync=False): if desc is None: desc = self.default_desc if tool is None: tool = self.tool - ret = self.eval_int("movec(" + point_interm + ", " + point_target + ", " + tool + ", " + desc +")") + + #TODO: in new robot movel and movej is freezing controller + #ret = self.eval_int("movec(" + point_interm + ", " + point_target + ", " + tool + ", " + desc +")") + ret = int(self.execute('movec', point_interm, point_target, tool, desc)) + if sync: self.wait_end_of_move() return ret @@ -658,9 +669,6 @@ class RobotTCP(TcpDevice, Stoppable): #Cartesian space """ - def get_flange_pos(self): - return self.eval_pnt("jointToPoint(" +FLANGE + ", " + self.frame + ", herej())") - def get_cartesian_pos(self): self.assert_tool() return self.eval_pnt("jointToPoint(" + self.tool + ", " + self.frame + ", herej())") @@ -668,23 +676,35 @@ class RobotTCP(TcpDevice, Stoppable): #return self.joint_to_point(tool, frame) """ - #TODO: in new robot robot.evaluate("tcp_p=jointToPoint(tSuna, world, herej())") is freezing controller (cannot cal herej directly) + + #TODO: in new robot robot.evaluate("tcp_p=jointToPoint(tSuna, world, herej())") is freezing controller (cannot call herej directly) #We can consider atomic because tcp_j is only accessed in comTcp - def get_flange_pos(self): - self.evaluate("tcp_j=herej(); tcp_p=jointToPoint(" +FLANGE + ", " + self.frame + ", tcp_j)") - return self.get_pnt() - - def get_cartesian_pos(self): - self.assert_tool() - self.evaluate("tcp_j=herej(); tcp_p=jointToPoint(" + self.tool + ", " + self.frame + ", tcp_j)") - return self.get_pnt() - #self.set_jnt(self.herej()) - #return self.joint_to_point(tool, frame) + def get_cartesian_pos(self, tool=None, frame=None): + if tool is None: + self.assert_tool() + tool = self.tool + if frame is None: + frame = self.frame + #Do not work + #self.evaluate("tcp_j=herej(); tcp_p=jointToPoint(" + tool + ", " + frame + ", tcp_j)") + #return self.get_pnt() + a = self.execute('get_pos', tool, frame) + ret = [] + for i in range(6): ret.append(float(a[i])) + return ret - def get_cartesian_destination(self): - self.assert_tool() - return self.here(self.tool, self.frame) + def get_flange_pos(self, frame=None): + return get_cartesian_pos(FLANGE, frame) + + + def get_cartesian_destination(self, tool=None, frame=None): + if tool is None: + self.assert_tool() + tool = self.tool + if frame is None: + frame = self.frame + return self.here(tool, frame) def get_distance_to_pnt(self, name): #self.here(self.tool, self.frame) #??? diff --git a/script/motion/recover.py b/script/motion/recover.py index bee6369..b3a5244 100644 --- a/script/motion/recover.py +++ b/script/motion/recover.py @@ -5,33 +5,35 @@ import org.apache.commons.math3.geometry.euclidean.threed.Line as Line3D RECOVER_DESC = "mRecovery" RECOVER_TOOL = TOOL_DEFAULT -known_segments = [ ("pHome", "pPark", 50), \ - ("pHome", "pGonio", 30), \ - ("pHome", "pScan", 25), \ - ("pHome", "pHeater", 75), \ +known_segments = [ ("pHome", "pPark", 100), \ + ("pScan", "pGonioA", 80), \ + ("pPark", "pScan", 25), \ + ("pScan", "pHeater", 75), \ ("pHome", "pDewar", 10), \ - ("pHome", "pHelium", 230), \ - ("pGonio", "pGonioG", 10), \ - ("pPark", "pHeat", 40), \ + ("pGonioA", "pGonioG", 10), \ ("pHeater", "pHeatB", 10), \ - ("pPark", "pAux", 50), \ + ("pHome", "pAux", 50), \ ] + +def get_robot_position(): + return robot.get_cartesian_pos() + + def get_dist_to_line(segment): tolerance = segment[2] p1, p2 = robot.get_pnt(segment[0]), robot.get_pnt(segment[1]) - p = robot.get_cartesian_pos() + p = get_robot_position() v = Vector3D(p[0], p[1], p[2]) v1 = Vector3D(p1[0], p1[1], p1[2]) v2 = Vector3D(p2[0], p2[1], p2[2]) l = Line3D(v1, v2, 0.01) return l.distance(v) - def get_dist_to_segment(segment): tolerance = segment[2] p1, p2 = robot.get_pnt(segment[0]), robot.get_pnt(segment[1]) - p = robot.get_cartesian_pos() + p = get_robot_position() v = Vector3D(p[0], p[1], p[2]) v1 = Vector3D(p1[0], p1[1], p1[2]) v2 = Vector3D(p2[0], p2[1], p2[2]) @@ -62,7 +64,7 @@ def is_on_segment(segment): def get_pojection_at_line(segment): tolerance = segment[2] p1, p2 = robot.get_pnt(segment[0]), robot.get_pnt(segment[1]) - p = robot.get_cartesian_pos() + p = get_robot_position() v = Vector3D(p[0], p[1], p[2]) v1 = Vector3D(p1[0], p1[1], p1[2]) v2 = Vector3D(p2[0], p2[1], p2[2]) @@ -87,7 +89,7 @@ def get_current_distance(): def move_to_segment(segment): tolerance = segment[2] - p = robot.get_cartesian_pos() + p = get_robot_position() v = Vector3D(p[0], p[1], p[2]) lv = get_pojection_at_line(segment) dlv = lv.distance(v) diff --git a/script/test/test_segments.py b/script/test/test_segments.py new file mode 100644 index 0000000..54a8898 --- /dev/null +++ b/script/test/test_segments.py @@ -0,0 +1,16 @@ +p=robot.get_cartesian_pos() +print "Pos: ", p +print "Cache pos:", robot.cartesian_pos +print "Cache dest: ", robot.cartesian_destination + + +print "Points: ", robot.get_current_points() +for segment in known_segments: + if is_on_segment(segment): + print " On : " + str(segment) + " - Dist:" + str(get_dist_to_segment(segment)) + + + +p2 = robot.get_cartesian_pos() +if arrsub( p2, p) != [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]: + print "Pos: ", p2 \ No newline at end of file