script/HillClimbingXZ.py

@@ -0,0 +1,45 @@
+import plotutils
+import math
+
+
+#STRATEGY = "Normal" 
+STRATEGY = "Boundary" 
+#STRATEGY = "FullNeighborhood" 
+RANGE = [-5.0, 5.0]
+INITIAL_STEP = 1.0
+STEP_SIZE = 0.05
+LATENCY = 0.05
+NOISE_FILTER = 1
+
+
+robot.enable()
+move_to_laser()
+
+
+robot.set_motors_enabled(True)
+
+current_x = robot_x.getPosition()
+current_z = robot_z.getPosition()
+
+
+
+
+class Distance(Readable):
+    def read(self):
+        ret = ue.readable.read()
+        ret = 0.0 if math.isnan(ret) else ret
+        return ret
+        
+laser_distance=Distance()   
+start = time.time()        
+r = hsearch([robot_x, robot_z],laser_distance, [RANGE[0], RANGE[0]], [RANGE[1], RANGE[1]], [INITIAL_STEP,INITIAL_STEP], [STEP_SIZE,STEP_SIZE], NOISE_FILTER, relative = True, maximum=True, latency = LATENCY, title = "Hill Climbing XZ")
+
+
+
+print r.print()
+opt_x, opt_z= r.getOptimalPosition()
+offset_x, offset_z = opt_x - current_x, opt_z - current_z
+
+print "offset_x: ", offset_x, " offset_z: ", offset_z
+
+

script/LevelMonitoring.scd

@@ -0,0 +1,12 @@
+[ 
+  [ [ true, "phase_separator_level", "Device", 1, 1, "255,0,0" ],
+    [ true, "dewar_level", "Device", 1, 1, "0,0,255" ] ],
+  [ [ "1", 0.0, 100.0, null, null, 100000.0, true ],
+    [ "2", null, null, null, null, null, null ],
+    [ "3", null, null, null, null, null, null ],
+    [ "4", null, null, null, null, null, null ],
+    [ "5", null, null, null, null, null, null ] ],
+  [ [ ] ],
+  [ [ "", 20000, 100 ],
+    [ "", "" ] ]  
+]

script/calibration/BinarySearchXZ.py

@@ -0,0 +1,29 @@
+import plotutils
+import math
+
+
+#STRATEGY = "Normal" 
+#STRATEGY = "Boundary" 
+#STRATEGY = "FullNeighborhood" 
+RANGE = [-5.0, 5.0]
+STEP_SIZE = 0.1
+LATENCY = 0.05
+
+
+robot.enable()
+move_to_laser()
+
+
+robot.set_motors_enabled(True)
+current_x = robot_x.getPosition()
+current_z = robot_z.getPosition()
+
+
+r = bsearch([robot_x, robot_z], laser_distance,[RANGE[0], RANGE[0]], [RANGE[1], RANGE[1]], [STEP_SIZE,STEP_SIZE], relative = True, maximum=True, strategy = STRATEGY, latency = LATENCY, title = "Binary Search XZ")
+
+
+print r.print()
+opt_x, opt_z= r.getOptimalPosition()
+offset_x, offset_z = opt_x - current_x, opt_z - current_z
+
+print "offset_x: ", offset_x, " offset_z: ", offset_z

script/calibration/ScanXZ.py

@@ -9,13 +9,17 @@ else:
 STEP_Z_FINAL = 0.1
     
 RANGE = [-5.0, 5.0]
-LATENCY = 0.005
+LATENCY = 0.05
 
 Z_FINAL_OFFSET = 0.0
 
 
+SINGLE_PASS = True
+STEP_SIZE = 0.1
+
+
 robot.enable()
-#move_to_laser()
+move_to_laser()
 
 step_x = STEP_SIZE
 step_z = STEP_SIZE
@@ -27,7 +31,7 @@ current_x = robot_x.getPosition()
 current_z = robot_z.getPosition()
 
 print "Current pos x,z" , current_x, ",", current_z
-ret = ascan([robot_x, robot_z], ue.readable, [range_x[0], range_z[0]], [range_x[1], range_z[1]], [step_x,step_z], latency = LATENCY, relative = True , zigzag=False, title = "2d Scan")
+ret = ascan([robot_x, robot_z], ue.readable, [range_x[0], range_z[0]], [range_x[1], range_z[1]], [step_x,step_z], latency = LATENCY, relative = True , zigzag=False, title = "Scan XY")
 data = ret.getData(0)[0]
 #plot(Convert.transpose(data), title="Data")
 

script/devices/BarcodeReader.py

@@ -0,0 +1,11 @@
+def enable_barcode_reader():
+    microscan_cmd.write("<H>")
+
+def disable_barcode_reader():    
+    microscan_cmd.write("<I>")
+
+def read_barcode(timeout):    
+    try:
+        return microscan.waitString(int(timeout * 1000))    
+    except:
+        return None

script/devices/LaserDistance.py

@@ -0,0 +1,17 @@
+class LaserDistance(ReadonlyRegisterBase):
+    def __init__(self):
+        ReadonlyRegisterBase.__init__(self, "laser_distance")
+    def doRead(self):
+        ret = ue.readable.read()
+        ret = 0.0 if math.isnan(ret) else ret
+        return ret
+
+class ListenerAI (DeviceListener):
+    def onValueChanged(self, device, value, former):
+        laser_distance.setCache( 0.0 if math.isnan(value) else value, None)
+
+listenerAI = ListenerAI()
+ue.addListener(listenerAI)
+        
+laser_distance=LaserDistance()   
+add_device(laser_distance, True)

script/devices/LedCtrl.py

@@ -0,0 +1,25 @@
+import ch.psi.pshell.device.DiscretePositionerBase as DiscretePositionerBase
+
+
+class LedPositioner(DiscretePositionerBase):
+    def __init__(self):
+        DiscretePositionerBase.__init__(self, "led_ctrl", ["On", "Off"])
+        self.setState(State.Ready)
+        self.val = self.doReadReadback()
+        
+    def doRead(self):
+        return self.val
+    
+    def doReadReadback(self):
+        return "On" if get_led_state() else "Off"
+
+    def doWrite(self, val):
+        self.val = val
+        if self.val == "On":
+            set_led_state(True)
+        else:
+            set_led_state(False)
+
+
+add_device(LedPositioner(), True)
+led_ctrl.polling = 1000

script/devices/RobotMotors.py

@@ -17,12 +17,12 @@ class RobotCartesianMotor (PositionerBase):
         self.setCache(self.doRead(), None)
                   
     def doRead(self):
-        return float("nan") if self.robot.cartesian_destination is None else float(robot.cartesian_destination[self.index])
+        return float("nan") if self.robot.cartesian_destination is None else float(self.robot.cartesian_destination[self.index])
 
     def doWrite(self, value):        
         if self.robot.cartesian_destination is not None:
             print "Move " + ROBOT_MOTORS[self.index] + " to " + str(value)
-            robot.cartesian_destination[self.index] = float(value)  
+            self.robot.cartesian_destination[self.index] = float(value)  
             self.robot.set_pnt(robot.cartesian_destination , "tcp_p")        
             self.robot.movel("tcp_p", self.robot.tool , DESC_SCAN)        
         

script/devices/RobotSC.py

@@ -70,6 +70,9 @@ robot.high_level_tasks = ["mount", "firstmount"]
 robot.set_tool(TOOL_CALIBRATION)
 robot.setPolling(500)
 
+robot.set_motors_enabled(True)
+robot.set_joint_motors_enabled(True)
+
 #robot.set_monitor_speed(20)
 
 
@@ -110,5 +113,6 @@ if joint_forces:
     add_device(jfc(), force = True)   
 
 
+
     
 """

script/devices/RobotTCP.py

@@ -48,11 +48,10 @@ class RobotTCP(TcpDevice, Stoppable):
     def set_tool(self,tool):
         self.tool = tool
         #self.tool_trsf = self.get_tool_trsf()
+        self.evaluate("tcp_curtool=" + tool)         
         if self.cartesian_motors_enabled:            
-            self.update()               
-            for m in self.cartesian_motors:
-                m.initialize()
-                m.update()                
+            self.update()
+            self.set_motors_enabled(True)        
     
     
     def get_tool(self):    
@@ -449,7 +448,8 @@ class RobotTCP(TcpDevice, Stoppable):
         try:
             start = time.time()
             #sts = self._sendReceive("EVT").strip().split(self.array_separator)        
-            sts = self.execute("get_status", self.current_task, self.tool, self.frame)
+            #sts = self.execute("get_status", self.current_task, self.tool, self.frame)
+            sts = self.execute("get_status", self.current_task)
             self._update_working_mode(int(sts[0]), int(sts[1]))
             self.powered = sts[2] == "1"
             self.speed = int(sts[3])

script/devices/Wago.py

@@ -0,0 +1,111 @@
+
+
+    
+"""
+wago=ch.psi.pshell.modbus.ModbusTCP|wago-mxsc-1:502|||
+relays=ch.psi.pshell.modbus.DigitalOutputArray|wago 0 16||1000|
+release_local_safety=ch.psi.pshell.modbus.DigitalOutput|wago 0|||
+release_psys_safety=ch.psi.pshell.modbus.DigitalOutput|wago 1|||
+ln2_main_power=ch.psi.pshell.modbus.DigitalOutput|wago 2|||
+rim_heater=ch.psi.pshell.modbus.DigitalOutput|wago 3|||
+phase_separator_ln2=ch.psi.pshell.modbus.DigitalOutput|wago 4|||
+dewar_ln2=ch.psi.pshell.modbus.DigitalOutput|wago 5|||false
+valve_he_chamber=ch.psi.pshell.modbus.DigitalOutput|wago 6|||
+gripper_dryer=ch.psi.pshell.modbus.DigitalOutput|wago 7|||
+valve_1=ch.psi.pshell.modbus.DigitalOutput|wago 8|||
+valve_2=ch.psi.pshell.modbus.DigitalOutput|wago 9|||
+valve_3=ch.psi.pshell.modbus.DigitalOutput|wago 10|||
+valve_4=ch.psi.pshell.modbus.DigitalOutput|wago 11|||
+#spare_do_1=ch.psi.pshell.modbus.DigitalOutput|wago 12|||
+#spare_do_2=ch.psi.pshell.modbus.DigitalOutput|wago 13|||
+#spare_do_3=ch.psi.pshell.modbus.DigitalOutput|wago 14|||
+#spare_do_4=ch.psi.pshell.modbus.DigitalOutput|wago 15|||
+phase_separator_level=ch.psi.pshell.modbus.ReadonlyProcessVariable|wago 0||1000|
+dewar_level=ch.psi.pshell.modbus.ReadonlyProcessVariable|wago 1||1000|
+rim_heater_temp=ch.psi.pshell.modbus.ReadonlyProcessVariable|wago 2||1000|
+air_pressure=ch.psi.pshell.modbus.ReadonlyProcessVariable|wago 3||1000|
+n2_pressure=ch.psi.pshell.modbus.ReadonlyProcessVariable|wago 4||1000|
+#spare_ai_1=ch.psi.pshell.modbus.AnalogInput|wago 5|||
+#spare_ai_2=ch.psi.pshell.modbus.AnalogInput|wago 6|||
+#spare_ai_3=ch.psi.pshell.modbus.AnalogInput|wago 7|||
+led_ctrl_1=ch.psi.pshell.modbus.ProcessVariable|wago 0|||
+led_ctrl_2=ch.psi.pshell.modbus.ProcessVariable|wago 1|||
+led_ctrl_3=ch.psi.pshell.modbus.ProcessVariable|wago 2|||
+"""
+###################################################################################################
+#  Leds
+###################################################################################################
+
+def set_led_state(value):
+    """
+    Turn leds on and off
+    """
+    if value:
+        led_ctrl_1.write(led_ctrl_1.config.maxValue)
+        led_ctrl_2.write(led_ctrl_2.config.maxValue)
+        led_ctrl_3.write(led_ctrl_3.config.maxValue)    
+    else:
+        led_ctrl_1.write(0.0)
+        led_ctrl_2.write(0.0)
+        led_ctrl_3.write(0.0)
+        
+    
+
+def get_led_state():
+    """
+    Returns led state (on/off)
+    """
+    return led_ctrl_1.read() > 0 
+
+#TODO: The range should be set automatically reading LN2 sensor.
+def set_led_range(room_temp = True):
+    """
+    Led range should be limitted in room temperature
+    """
+    max_val = 0.40 if room_temp else 1.20
+    led_ctrl_1.config.maxValue = max_val
+    led_ctrl_1.config.save()
+    led_ctrl_2.config.maxValue = max_val
+    led_ctrl_2.config.save()
+    led_ctrl_3.config.maxValue = max_val
+    led_ctrl_3.config.save()
+    led_ctrl_1.initialize()
+    led_ctrl_2.initialize()
+    led_ctrl_3.initialize()
+    if led_ctrl_1.read() > max_val:
+        led_ctrl_1.write(max_val)
+    if led_ctrl_2.read() > max_val:
+        led_ctrl_2.write(max_val)
+    if led_ctrl_3.read() > max_val:
+        led_ctrl_3.write(max_val)
+
+
+def is_led_room_temp():
+    return led_ctrl_1.config.maxValue <= 0.50
+
+
+
+###################################################################################################
+#  Safety release
+###################################################################################################
+
+def release_local():
+    """
+    Release local safety
+    """        
+    release_local_safety.write(False)
+    time.sleep(0.01)
+    release_local_safety.write(True)
+    time.sleep(0.01)
+    release_local_safety.write(False)
+
+def release_psys():
+    """
+    Release psys safety
+    """    
+    release_psys_safety.write(False)
+    time.sleep(0.01)
+    release_psys_safety.write(True)
+    time.sleep(0.01)
+    release_psys_safety.write(False)
+

script/imgproc/LedDetectionProc.py

@@ -15,8 +15,7 @@ exclude_edges = True
 set_led_range(room_temp)
 
 img.backgroundEnabled=False
-led_ctrl1.write(0.0)
-led_ctrl2.write(0.0)
+set_led_state(False)
 time.sleep(0.1)
 img.waitNext(100)
 
@@ -27,14 +26,12 @@ background = average_frames(number_backgrounds)
 #show_panel(img.backgroundImage)
 #img.backgroundEnabled = True
 
-led_ctrl1.write(led_ctrl1.config.maxValue)
-led_ctrl2.write(led_ctrl2.config.maxValue)
+set_led_state(True)
 time.sleep(0.1)
 img.waitNext(100)
 image = average_frames(number_frames)
 
-led_ctrl1.write(0.0)
-led_ctrl2.write(0.0)
+set_led_state(False)
 
 op_image(image, background, "subtract", float_result=True, in_place=True)
 

script/imgproc/Utils.py

@@ -0,0 +1,87 @@
+
+###################################################################################################
+#  Image processing utilities
+###################################################################################################
+
+
+from ijutils import *
+from ch.psi.pshell.imaging.Overlays import *
+import ch.psi.pshell.imaging.Pen as Pen
+
+def in_roi(x,y):
+    return math.hypot(x-roi_radius, y-roi_radius) < roi_radius
+
+
+def integrate(ips):    
+    roi =  get_roi()
+    aux = None
+    for i in range(len(ips)):
+        if i==0:
+            aux = new_image(roi[2], roi[3], image_type="float", title = "sum", fill_color = None)
+        op_image(aux, ips[i], "add", float_result=True, in_place=True)    
+    return aux
+
+def average (ips):   
+    aux = integrate(ips)     
+    op_const(aux, "divide", len(ips), in_place=True)    
+    return aux
+
+def grab_frames(samples):
+    frames = []            
+    for i in range(samples):
+        aux = get_image()        
+        frames.append(aux)        
+    return frames
+    
+def average_frames(samples = 1):
+    return average(grab_frames(samples))
+
+def integrate_frames(samples = 1):
+    return integrate(grab_frames(samples))
+
+
+roi_center = (800, 600)
+roi_radius = 600
+
+def get_roi():
+    return (roi_center[0] - roi_radius, roi_center[1] - roi_radius, 2* roi_radius, 2*roi_radius)
+
+
+def get_image():
+    roi =  get_roi()
+    ip = load_image(img.image)                            
+    ret = sub_image(ip, roi[0], roi[1], roi[2], roi[3])
+    grayscale(ret, do_scaling=True)
+    return ret
+
+#def detect_pucks(ip):
+#    """
+#    """
+#    aux = grayscale(ip, in_place=False)
+#    threshold(aux,0,50)
+#    binary_fill_holes(aux)
+#    return analyse_particles(aux, 10000,50000,
+#            fill_holes = False, exclude_edges = True,print_table=True,
+#            output_image = "outlines", minCirc = 0.4, maxCirc = 1.0)
+#
+#def detect_samples(ip):      
+#    """
+#    """  
+#    aux = grayscale(ip, in_place=False)
+#    invert(aux)
+#    subtract_background(aux)
+#    auto_threshold(aux)
+#    binary_open(aux)
+#    return analyse_particles(aux, 250,1000, 
+#            fill_holes = False, exclude_edges = True,print_table=True,
+
+
+r,g,b = [0]*256,[0]*256,[0]*256
+b[0]=0xFF
+b[1]=0xFF ; g[1] = 0x80; r[1] = 0x80
+outline_lut1 = (r,g,b)
+
+r,g,b = [0]*256,[0]*256,[0]*256
+g[0]=0x80;r[0]=0x80;
+g[1]=0xFF ; r[1] = 0x80; b[1] = 0x80
+outline_lut2 = (r,g,b)

script/local.py

@@ -1,214 +1,66 @@
 ###################################################################################################
 #  Deployment specific global definitions - executed after startup.py
 ###################################################################################################
-
+import traceback
 from ch.psi.pshell.serial import TcpDevice
 from ch.psi.pshell.modbus import ModbusTCP
 
+
 run("setup/Layout")
 
 
 ###################################################################################################
-#  Scripted devices
+#  Scripted devices and pseudo-devices
 ###################################################################################################
 
-
-run("devices/RobotSC")
-#run("devices/RobotModbus")
-#run("devices/OneWire")
-
-
-#Raspberry login: usr=pi  pwd=Buntschu
+for script in ["devices/RobotSC", "devices/Wago", "devices/BarcodeReader", "devices/LaserDistance",  \
+               "devices/LedCtrl"]:
+    try:
+        run(script)
+    except:
+        print  >> sys.stderr, traceback.format_exc()    
 
 add_device(img.getContrast(), force = True)
 add_device(img.getCamera(), force = True)
 
 
-#TODO: The range should be set automatically reading LN2 sensor.
-def set_led_range(room_temp = True):
-    led_ctrl1.config.maxValue = 0.40 if room_temp else 1.20
-    led_ctrl1.config.save()
-    led_ctrl2.config.maxValue = 0.40 if room_temp else 1.20
-    led_ctrl2.config.save()
-
-
 ###################################################################################################
-#  Motion modules
+#  Utility modules
 ###################################################################################################
 
 run("motion/tools")
+run("imgproc/Utils")
+run("tools/Math")
 
 
 ###################################################################################################
-#  Barcode reader
+#  Device initialization
 ###################################################################################################
 
-def enable_barcode_reader():
-    microscan_cmd.write("<H>")
+import ch.psi.pshell.device.Camera as Camera
+img.camera.setColorMode(Camera.ColorMode.Mono)
+img.camera.setDataType(Camera.DataType.UInt8)
+img.camera.setGrabMode(Camera.GrabMode.Continuous)
+img.camera.setTriggerMode(Camera.TriggerMode.Fixed_Rate)
+img.camera.setExposure(50.00)
+img.camera.setAcquirePeriod(200.00)
+img.camera.setGain(0.0)
 
-def disable_barcode_reader():    
-    microscan_cmd.write("<I>")
 
-def read_barcode(timeout):    
-    try:
-        return microscan.waitString(int(timeout * 1000))    
-    except:
-        return None
+release_local_safety.write(False)
+release_psys_safety.write(False)
 
 
 ###################################################################################################
-#  Image processing utilities
+#  Global variables
 ###################################################################################################
 
 
-from ijutils import *
-from ch.psi.pshell.imaging.Overlays import *
-import ch.psi.pshell.imaging.Pen as Pen
-
-def in_roi(x,y):
-    return math.hypot(x-roi_radius, y-roi_radius) < roi_radius
-
-
-def integrate(ips):    
-    roi =  get_roi()
-    aux = None
-    for i in range(len(ips)):
-        if i==0:
-            aux = new_image(roi[2], roi[3], image_type="float", title = "sum", fill_color = None)
-        op_image(aux, ips[i], "add", float_result=True, in_place=True)    
-    return aux
-
-def average (ips):   
-    aux = integrate(ips)     
-    op_const(aux, "divide", len(ips), in_place=True)    
-    return aux
-
-def grab_frames(samples):
-    frames = []            
-    for i in range(samples):
-        aux = get_image()        
-        frames.append(aux)        
-    return frames
-    
-def average_frames(samples = 1):
-    return average(grab_frames(samples))
-
-def integrate_frames(samples = 1):
-    return integrate(grab_frames(samples))
-
-
-roi_center = (800, 600)
-roi_radius = 600
-
-def get_roi():
-    return (roi_center[0] - roi_radius, roi_center[1] - roi_radius, 2* roi_radius, 2*roi_radius)
-
-
-def get_image():
-    roi =  get_roi()
-    ip = load_image(img.image)                            
-    ret = sub_image(ip, roi[0], roi[1], roi[2], roi[3])
-    grayscale(ret, do_scaling=True)
-    return ret
-
-#def detect_pucks(ip):
-#    """
-#    """
-#    aux = grayscale(ip, in_place=False)
-#    threshold(aux,0,50)
-#    binary_fill_holes(aux)
-#    return analyse_particles(aux, 10000,50000,
-#            fill_holes = False, exclude_edges = True,print_table=True,
-#            output_image = "outlines", minCirc = 0.4, maxCirc = 1.0)
-#
-#def detect_samples(ip):      
-#    """
-#    """  
-#    aux = grayscale(ip, in_place=False)
-#    invert(aux)
-#    subtract_background(aux)
-#    auto_threshold(aux)
-#    binary_open(aux)
-#    return analyse_particles(aux, 250,1000, 
-#            fill_holes = False, exclude_edges = True,print_table=True,
-
-
-r,g,b = [0]*256,[0]*256,[0]*256
-b[0]=0xFF
-b[1]=0xFF ; g[1] = 0x80; r[1] = 0x80
-outline_lut1 = (r,g,b)
-
-r,g,b = [0]*256,[0]*256,[0]*256
-g[0]=0x80;r[0]=0x80;
-g[1]=0xFF ; r[1] = 0x80; b[1] = 0x80
-outline_lut2 = (r,g,b)
-
-
-
-###################################################################################################
-#  Math utilities
-###################################################################################################
-
-
-from mathutils import estimate_peak_indexes, fit_gaussians, create_fit_point_list, Gaussian
-import java.awt.Color as Color
- 
-import mathutils
-mathutils.MAX_ITERATIONS = 100000
-
-def fit(ydata, xdata = None, draw_plot = True):
-    if xdata is None:
-        xdata = frange(0, len(ydata), 1)
-    max_y= max(ydata)    
-    index_max = ydata.index(max_y)    
-    max_x= xdata[index_max]
-    #print "Max index:" + str(index_max),
-    #print " x:" + str(max_x),
-    #print " y:" + str(max_y)
-    
-    if draw_plot:
-        plots = plot([ydata],["data"],[xdata], title="Fit" )
-        p = None if plots is None else plots[0]
-
-    gaussians = fit_gaussians(ydata, xdata, [index_max,])
-    if gaussians[0] is None:
-        if draw_plot and (p is not None):
-            p.addMarker(max_x, None, "Max="+str(round(max_x,4)), Color.GRAY)    
-        print "Fitting error"
-        return (None, None, None)
-        
-    (norm, mean, sigma) = gaussians[0]
-    if draw_plot:
-        fitted_gaussian_function = Gaussian(norm, mean, sigma)    
-        scale_x = [float(min(xdata)), float(max(xdata)) ]
-        points = max((len(xdata)+1), 100)
-        resolution = (scale_x[1]-scale_x[0]) / points    
-        fit_y = []
-        fit_x = frange(scale_x[0],scale_x[1],resolution, True)
-        for x in fit_x:
-            fit_y.append(fitted_gaussian_function.value(x))            
-        #Server
-        if p is None:
-            plot([ydata,fit_y],["data","fit"],[xdata,fit_x], title="Fit")             
-            draw_plot = False
-        else:
-            p.addSeries(LinePlotSeries("fit"))
-            p.getSeries(1).setData(fit_x, fit_y)            
-
-    if abs(mean - xdata[index_max]) < abs((scale_x[0] + scale_x[1])/2):        
-        if draw_plot:
-            p.addMarker(mean, None, "Mean="+str(round(mean,4)), Color.MAGENTA.darker())
-        #print "Mean -> " +  str(mean)
-        return (norm, mean, sigma)
-    else:
-        if draw_plot:
-            p.addMarker(max_x, None, "Max="+str(round(max_x,4)), Color.GRAY)
-        #print "Invalid gaussian fit: " +  str(mean)
-        return (None, None, None)
-
 context = get_context()
 
 
 
+update()
 
+print "Initialization complete"
     

script/motion/tools.py

@@ -1,5 +1,23 @@
 POSITION_TOLERANCE = 50
 
+def enable_power():
+    """
+    Check safety and enable arm power
+    """
+    release_psys()  
+    time.sleep(0.1)   
+    if feedback_psys_safety.read() == False:
+        raise Exception("Cannot enable power: check doors")
+    release_local()
+    if feedback_local_safety.read() == False:
+        raise Exception("Cannot enable power: check sample changer emergency stop button")
+    time.sleep(0.25)    
+    if not robot.state.isNormal():
+        raise Exception("Cannot enable power: robot state is " + str(robot.state))
+    robot.enable()
+    
+
+
 def wait_end_of_move():
     robot.update()
     while (not robot.settled) or (not robot.empty) or (not robot.isReady()) :

script/tasks/LedMonitoring.py

@@ -0,0 +1,26 @@
+DEWAR_LEVEL_LED = 25.0
+
+try:
+    _level = dewar_level.read()
+except:
+    log("Cannot read Dewar level", False)
+    _level = 0.0
+
+try:      
+    _led_room_temp = is_led_room_temp()
+except:
+    log("Cannot get LED configuration", False)
+    _led_room_temp = False
+
+if _level <= DEWAR_LEVEL_LED:
+    if not _led_room_temp:      
+        log_str="LED Monitoring: Setting LEDs to room temperature range" 
+        log(log_str, False)
+        print (log_str)
+        set_led_range(room_temp = True)
+else:        
+    if _led_room_temp:       
+        log_str="LED Monitoring: Setting LEDs to LN2 range" 
+        log(log_str, False)
+        print (log_str)
+        set_led_range(room_temp = False)

script/tools/Math.py

@@ -0,0 +1,60 @@
+###################################################################################################
+#  Math utilities
+###################################################################################################
+
+
+from mathutils import estimate_peak_indexes, fit_gaussians, create_fit_point_list, Gaussian
+import java.awt.Color as Color
+ 
+import mathutils
+mathutils.MAX_ITERATIONS = 100000
+
+def fit(ydata, xdata = None, draw_plot = True):
+    if xdata is None:
+        xdata = frange(0, len(ydata), 1)
+    max_y= max(ydata)    
+    index_max = ydata.index(max_y)    
+    max_x= xdata[index_max]
+    #print "Max index:" + str(index_max),
+    #print " x:" + str(max_x),
+    #print " y:" + str(max_y)
+    
+    if draw_plot:
+        plots = plot([ydata],["data"],[xdata], title="Fit" )
+        p = None if plots is None else plots[0]
+
+    gaussians = fit_gaussians(ydata, xdata, [index_max,])
+    if gaussians[0] is None:
+        if draw_plot and (p is not None):
+            p.addMarker(max_x, None, "Max="+str(round(max_x,4)), Color.GRAY)    
+        print "Fitting error"
+        return (None, None, None)
+        
+    (norm, mean, sigma) = gaussians[0]
+    if draw_plot:
+        fitted_gaussian_function = Gaussian(norm, mean, sigma)    
+        scale_x = [float(min(xdata)), float(max(xdata)) ]
+        points = max((len(xdata)+1), 100)
+        resolution = (scale_x[1]-scale_x[0]) / points    
+        fit_y = []
+        fit_x = frange(scale_x[0],scale_x[1],resolution, True)
+        for x in fit_x:
+            fit_y.append(fitted_gaussian_function.value(x))            
+        #Server
+        if p is None:
+            plot([ydata,fit_y],["data","fit"],[xdata,fit_x], title="Fit")             
+            draw_plot = False
+        else:
+            p.addSeries(LinePlotSeries("fit"))
+            p.getSeries(1).setData(fit_x, fit_y)            
+
+    if abs(mean - xdata[index_max]) < abs((scale_x[0] + scale_x[1])/2):        
+        if draw_plot:
+            p.addMarker(mean, None, "Mean="+str(round(mean,4)), Color.MAGENTA.darker())
+        #print "Mean -> " +  str(mean)
+        return (norm, mean, sigma)
+    else:
+        if draw_plot:
+            p.addMarker(max_x, None, "Max="+str(round(max_x,4)), Color.GRAY)
+        #print "Invalid gaussian fit: " +  str(mean)
+        return (None, None, None)