gac-x04sa
106 lines
3.9 KiB
Python
106 lines
3.9 KiB
Python
import mathutils
|
|
import plotutils
|
|
|
|
if get_exec_pars().source == CommandSource.ui:
|
|
SIMULATION = True
|
|
X_MIN = scx.minValue / 2
|
|
X_MAX = scx.maxValue / 2
|
|
Y_MIN = scy.minValue / 2
|
|
Y_MAX = scy.maxValue / 2
|
|
MIN_SAMPLE_SIZE = 0.25
|
|
SCAN_RESOLUTION = 0.01
|
|
OMEGA_START = 57.0
|
|
Z_START = 0.0
|
|
|
|
MAX_SAMPLE_SIZE = 0.4
|
|
|
|
|
|
y_step = MIN_SAMPLE_SIZE * 0.5
|
|
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)
|
|
scz.move(Z_START)
|
|
|
|
step=0
|
|
for y in frange(Y_MIN, Y_MAX, y_step):
|
|
scan_range = [X_MIN, X_MAX] if (step % 2==0) else [X_MAX, X_MIN]
|
|
scx.moveAsync(scan_range[0])
|
|
scy.move(y)
|
|
r = cscan(scx, sensor, scan_range[0], scan_range[1], SCAN_RESOLUTION, latency = DIODE_SETTLING_TIME)
|
|
|
|
try:
|
|
xdata,ydata = enforce_monotonic(r.getPositions(0)), r.getReadable(0)
|
|
(offset, normalization, mean_val_x, sigma) = mathutils.fit_gaussian_offset(ydata, xdata)
|
|
if ((normalization-offset) < offset * 1.20) or (sigma > 0.5):
|
|
raise Exception("Invalid x fit")
|
|
plot_fit(normalization, mean_val_x, sigma, xdata)
|
|
print "Found x center at ", mean_val_x
|
|
scx.move(mean_val_x)
|
|
break
|
|
except:
|
|
print "Invalid x fit for y=", y, ": ", sys.exc_info()[0]
|
|
|
|
step += 1
|
|
|
|
r = cscan(scy, sensor, max(y-MAX_SAMPLE_SIZE/2, scy.minValue), min(Y_MAX+MAX_SAMPLE_SIZE/2, scy.maxValue), SCAN_RESOLUTION, latency = DIODE_SETTLING_TIME)
|
|
xdata,ydata = enforce_monotonic(r.getPositions(0)), r.getReadable(0)
|
|
try:
|
|
(offset, normalization, mean_val_y, sigma) = mathutils.fit_gaussian_offset(ydata, xdata)
|
|
except:
|
|
raise Exception ("Invalid y fit: ", sys.exc_info()[0])
|
|
if ((normalization-offset) < offset * 1.20) or (sigma > 0.5):
|
|
raise Exception("Invalid y fit")
|
|
plot_fit(normalization, mean_val_y, sigma, xdata)
|
|
print "Found y center at ", mean_val_y
|
|
scy.move(mean_val_y)
|
|
|
|
print "Success finding center at x=", mean_val_x, " y=", mean_val_y
|
|
|
|
|
|
try:
|
|
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)
|
|
try:
|
|
(offset, normalization, mean_minus_15, sigma) = mathutils.fit_gaussian_offset(ydata, xdata)
|
|
except:
|
|
raise Exception ("Invalid x fit on z search -15deg: ", sys.exc_info()[0])
|
|
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_MAX, X_MIN, SCAN_RESOLUTION, latency = DIODE_SETTLING_TIME)
|
|
xdata,ydata = enforce_monotonic(r.getPositions(0)), r.getReadable(0)
|
|
try:
|
|
(offset, normalization, mean_plus_15, sigma) = mathutils.fit_gaussian_offset(ydata, xdata)
|
|
except:
|
|
raise Exception ("Invalid x fit on z search +15deg: ", sys.exc_info()[0])
|
|
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)
|
|
finally:
|
|
scx.move(mean_val_x)
|
|
scy.move(mean_val_y)
|
|
|
|
print "Centering successfull"
|
|
|
|
|
|
|