script/motion/recover.py

@@ -6,14 +6,14 @@ RECOVER_DESC = "mRecovery"
 RECOVER_TOOL = TOOL_DEFAULT
 
 known_segments = [ ("pHome",         "pPark",         50), \
-                   ("pHome",         "pDewarHome",    30), \
                    ("pHome",         "pGonioHome",    30), \
                    ("pHome",         "pScanHome",     25), \
-                   ("pHome",         "pHeaterHome",   25), \
-                   ("pDewarHome",    "pDewarWait",    10), \
+                   ("pHome",         "pHeaterHome",   75), \
+                   ("pHome",         "pDewarWait",    10), \
+                   ("pHome",         "pHelium",      230), \
                    ("pGonioHome",    "pGonioGet",     10), \
-                   ("pHeaterHome",   "pHeater",       10), \
-                   ("pHeater",       "pHeaterBottom", 10), \
+                   ("pPark",         "pHeater",       40), \
+                   ("pHeaterHome",   "pHeaterBottom", 10), \
                  ]
 
 def get_dist_to_line(segment):
@@ -85,6 +85,7 @@ def get_current_distance():
 
 
 def move_to_segment(segment):
+    tolerance = segment[2]
     p =  robot.get_cartesian_pos()
     v = Vector3D(p[0], p[1], p[2])
     lv = get_pojection_at_line(segment)
@@ -103,7 +104,8 @@ def move_to_segment(segment):
 #Moves to first point of the segment ehich is safer, unless in the vicinity of the second
 def move_to_safest_point(segment, vicinity_tolerance = 100):
     d1, d2 = robot.get_distance_to_pnt(segment[0]),  robot.get_distance_to_pnt(segment[1])
-    if (d2<=d1) and (d2 <= vicinity_tolerance):
+    #Always moving to primary point
+    if False : #(d2<=d1) and (d2 <= vicinity_tolerance):
         print "Moving to secondary point " + str(segment[1] + " - d1=" + str(d1) + " d2=" + str(d2) )
         robot.movel(segment[1], tool=RECOVER_TOOL, desc=RECOVER_DESC, sync=True) 
     else: