From bd2f2e993893b41ec11915d11d9051a3279760c4 Mon Sep 17 00:00:00 2001
From: gac-x04sa <gac-x04sa@x04sa-cons-3.psi.ch>
Date: Thu, 25 Jan 2018 11:37:15 +0100
Subject: [PATCH] Script execution

---
 script/scans/SampleSearch.py | 61 +++++++++++++++++++++++++-----------
 1 file changed, 43 insertions(+), 18 deletions(-)

diff --git a/script/scans/SampleSearch.py b/script/scans/SampleSearch.py
index c7e458c..da85234 100644
--- a/script/scans/SampleSearch.py
+++ b/script/scans/SampleSearch.py
@@ -11,7 +11,7 @@ Y_MAX = scy.maxValue / 2
 MAX_SAMPLE_SIZE = 0.4
 MIN_SAMPLE_SIZE = 0.25
 SCAN_RESOLUTION = 0.01
-
+OMEGA_START = 57.0
 
 y_step = MIN_SAMPLE_SIZE * 0.5
 sensor = scd_sim if SIMULATION else scd
@@ -19,6 +19,13 @@ sensor = scd_sim if SIMULATION else scd
 
 X_MIN, X_MAX, Y_MIN, Y_MAX = float(X_MIN), float(X_MAX), float(Y_MIN), float(Y_MAX)
 
+
+def plot_fit(normalization, mean_val, sigma, xdata):
+    gaussian = mathutils.Gaussian(normalization, mean_val, sigma)
+    plotutils.plot_function(get_plots(None)[0], gaussian, "Fit", xdata)
+
+scr.move(OMEGA_START)
+
 step=0
 for y in frange(Y_MIN, Y_MAX, y_step):   
     scy.move(y)
@@ -26,18 +33,11 @@ for y in frange(Y_MIN, Y_MAX, y_step):
     r = cscan(scx, sensor, scan_range[0], scan_range[1], SCAN_RESOLUTION, latency = DIODE_SETTLING_TIME)
 
     try:
-        xdata = enforce_monotonic(r.getPositions(0))
-        ydata = r.getReadable(0)
+        xdata,ydata = enforce_monotonic(r.getPositions(0)), r.getReadable(0)        
         (offset, normalization, mean_val, sigma) = mathutils.fit_gaussian_offset(ydata, xdata)        
-
-        if (normalization-offset) < offset * 1.20:
+        if ((normalization-offset) < offset * 1.20) or (sigma > 0.5):
             raise Exception("Invalid x fit")
-        if sigma > 0.5:
-            raise Exception("Invalid x fit")
-        #Plotting the gaussian
-        gaussian = mathutils.Gaussian(normalization, mean_val, sigma)
-        plotutils.plot_function(get_plots(None)[0], gaussian, "Fit", xdata)
-
+        plot_fit(normalization, mean_val, sigma, xdata)        
         print "Found x center at ", mean_val
         scx.move(mean_val)
         break        
@@ -47,18 +47,43 @@ for y in frange(Y_MIN, Y_MAX, y_step):
     step += 1
 
 r = cscan(scy, sensor, y-MAX_SAMPLE_SIZE/2, Y_MAX+MAX_SAMPLE_SIZE/2, SCAN_RESOLUTION, latency = DIODE_SETTLING_TIME)
-xdata = enforce_monotonic(r.getPositions(0))
-ydata = r.getReadable(0)
+xdata,ydata = enforce_monotonic(r.getPositions(0)), r.getReadable(0)        
 (offset, normalization, mean_val, sigma) = mathutils.fit_gaussian_offset(ydata, xdata)        
-if (normalization-offset) < offset * 1.20:
-    raise Exception("Invalid y fit")
-if sigma > 0.5:
-    raise Exception("Invalid y fit")
+if ((normalization-offset) < offset * 1.20) or (sigma > 0.5):
+    raise Exception("Invalid x fit")
+plot_fit(normalization, mean_val, sigma, xdata)    
 print "Found y center at ", mean_val   
 scy.move(mean_val)
 
 print "Success finding center at x=", scx.read(), " y=", scy.read()
-    
+
+
+scr.move(OMEGA_START-15.0)    
+r = cscan(scx, sensor, X_MIN, X_MAX, SCAN_RESOLUTION, latency = DIODE_SETTLING_TIME)
+xdata,ydata = enforce_monotonic(r.getPositions(0)), r.getReadable(0)        
+(offset, normalization, mean_minus_15, sigma) = mathutils.fit_gaussian_offset(ydata, xdata)        
+if ((normalization-offset) < offset * 1.20) or (sigma > 0.5):
+    raise Exception("Invalid x fit on z search -15deg")
+plot_fit(normalization, mean_minus_15, sigma, xdata)    
+print "Found x center at -15deg at ", mean_minus_15   
+
+
+scr.move(OMEGA_START+15.0)    
+r = cscan(scx, sensor, X_MIN, X_MAX, SCAN_RESOLUTION, latency = DIODE_SETTLING_TIME)
+xdata,ydata = enforce_monotonic(r.getPositions(0)), r.getReadable(0)        
+(offset, normalization, mean_plus_15, sigma) = mathutils.fit_gaussian_offset(ydata, xdata)        
+if ((normalization-offset) < offset * 1.20) or (sigma > 0.5):
+    raise Exception("Invalid x fit on z search +15deg")
+plot_fit(normalization, mean_minus_15, sigma, xdata)    
+print "Found x center at +15deg at ", mean_plus_15
+
+scr.move(OMEGA_START)    
+
+z_offset = 0.5 * (mean_minus_15 - mean_plus_15) / math.tan(math.radians(15.0))
+print "Calculated z offset = ", z_offset
+scz.moveRel(z_offset)
+
+print "Centering successfull"
 
 
     
\ No newline at end of file