def two_points(dx=100, dy=100, nsamples=1):
"""To calibrate, mechanically block the head of the cantilever at least from the bottom and the positive x direction (left). The resulting matrix allows to convert sensor measurements into micromanipulator coordinates."""
with closing(DT3100('169.254.3.100')) as sensorA, closing(
DT3100('169.254.4.100')) as sensorB, closing(MPC385()) as mpc:
sensors = [sensorA, sensorB]
#setting up sensors
for sensor in sensors:
sensor.set_averaging_type(3)
sensor.set_averaging_number(3)
#remember original positions of the sensors and actuator
ab0 = np.mean([[sensor.readOne().m for sensor in sensors]
for i in range(nsamples)], 0)
assert ab0.min() > 0 and ab0.max() < 800
x0, y0, z0 = mpc.update_current_position()[1:]
#move along x (width)
mpc.move_to(x0 + mpc.um2integer_step(dx), y0, z0)
abx = np.mean([[sensor.readOne().m for sensor in sensors]
for i in range(nsamples)], 0)
assert abx.min() > 0 and abx.max() < 800
#move along y (depth)
mpc.move_to(x0, y0 + mpc.um2integer_step(dy), z0)
abz = np.mean([[sensor.readOne().m for sensor in sensors]
for i in range(nsamples)], 0)
assert abz.min() > 0 and abz.max() < 800
#move back to original position
mpc.move_to(x0, y0, z0)
#the transfer matrix from actuator to sensor coordinates is the dispacements
#we just measured as column vectors
xy2ab = ((np.array([abx, abz]) - ab0).T / [dx, dy])
return np.linalg.inv(xy2ab)
def step_response(outname,
AB2XY,
kp,
ki=0,
kd=0,
dx=10.,
dy=10.,
originalsetpoint=None):
"""Perform a single step of (dx,dy) microns and record the PID response to a file named outname."""
if np.isscalar(kp):
kp = [kp, kp]
if np.isscalar(ki):
ki = [ki, ki]
if np.isscalar(kd):
kd = [kd, kd]
with closing(DT3100('169.254.3.100')) as sensorA, closing(
DT3100('169.254.4.100')) as sensorB, closing(MPC385()) as actuator:
sensors = [sensorA, sensorB]
#setting up sensors
for sensor in sensors:
sensor.set_averaging_type(3)
sensor.set_averaging_number(3)
#remember original positions of the sensors and actuator
if originalsetpoint is None:
originalsetpoint = np.array(
[sensor.readOne().m for sensor in sensors])
originalsetpoint = np.matmul(AB2XY, originalsetpoint)
x0, y0, z0 = actuator.update_current_position()[1:]
#setting up PID
pids = []
for p, i, d, s in zip(kp, ki, kd, originalsetpoint):
pid = PID(p, i, d)
pid.setPoint = s
pids.append(pid)
with open(outname, "wb") as fout:
m = MoverPID_XY(sensors, actuator, AB2XY, pids, outputFile=fout)
m.start()
time.sleep(1)
pids[0].setPoint += dx
pids[1].setPoint += dy
time.sleep(20)
#stop PID
m.go = False
m.join()
#move the actuator back to its original position
actuator.move_to(x0, y0, z0)
#read back the data and print some metrics
meas = np.fromfile(outname)
ts, mx, my, ox, oy = meas.reshape((meas.shape[0] // 5, 5)).T
measures = np.column_stack([mx, my])
rms = np.sqrt(np.sum((originalsetpoint + [dx, dy] - measures)**2, -1))
if not np.any(rms < actuator.step2um(1)):
print("does not converge")
else:
print("cverge\teRMS\t%overshoot")
print('%g\t%g\t%g' %
(ts[np.where(rms < actuator.step2um(1))[0][0] - 1],
np.sqrt(np.mean(rms[np.where(ts > 2)[0][0]:]**2)),
(100 *
(measures - originalsetpoint - [dx, dy]) / [dx, dy]).max()))
def measure_state():
with ExitStack() as stack:
sensors = [
stack.enter_context(closing(DT3100(f'169.254.{i+3:d}.100')))
for i in range(3)
]
mpc = stack.enter_context(closing(MPC385()))
return mechtest3sensors.State().read(sensors, mpc, ab2xy)
def sampled_direction(direction='y', samples=None, repeat=10):
"""To calibrate along z (resp y) the 3rd sensor, By default, test 11 dispacements sampled in log scale.
Added the option for direction to check how movement in other direction affects the sensor reading
The resulting coefficients allows to convert micromanipulator coordinates to sensor measurements coordinates.
Returns coefficients and figure testing linearity."""
if samples is None:
samples = 2**np.arange(11)
measures = np.zeros((len(samples)))
with closing(DT3100('169.254.5.100')) as sensorC, closing(
MPC385()) as actuator:
sensors = [sensorC]
for sensor in sensors:
sensor.set_averaging_type(3)
sensor.set_averaging_number(3)
#remember original positions of the sensors and actuator
x0, y0, z0 = actuator.update_current_position()[1:]
try:
initial = np.zeros(1)
for j in range(repeat):
initial += sensorC.readOne().m #why .m
initial /= repeat
#move along z
for i, sample in enumerate(samples):
#manipulator going up
if direction == "y":
actuator.move_to(x0, y0 + sample, z0)
elif direction == "x":
actuator.move_to(x0 + sample, y0, z0)
else:
actuator.move_to(x0, y0, z0 + sample)
for j in range(repeat):
m = sensorC.readOne().m
if np.any(m == 800) or np.any(m == 0):
raise ValueError('Sensor out of range')
measures[i] += m
finally:
actuator.move_to(x0, y0, z0)
measures /= repeat
measures -= initial
np.save(
'calib_sensorC_' + '_' +
datetime.now().strftime('%Y%m%d_%H%M_calib.npy'), measures)
#fit the results to get coefficients
func = lambda x, p: p * x
dxs = actuator.step2um(samples)
fig, axs = plt.subplots(1, 1)
x2c = []
#for ax,m in zip(axs, (measures).T):
axs.plot(dxs, measures, 'o')
param = curve_fit(func, dxs, measures)[0][0]
print(param)
x2c.append(param)
axs.plot(dxs, func(dxs, param), ':')
axs.set_xlabel(direction + ' manip (µm)')
axs.set_ylabel('Sensor c (µm)')
axs.set_title(direction + '_C')
return np.array(x2c), fig
def interactive(samples=None, repeat=10, axs=None):
"""To calibrate, mechanically block the head of the cantilever from the right (looking to the micromanipulator), then from the bottom. By default, test 11 dispacements in each direction, sampled in log scale.
The resulting matrix allows to convert sensor measurements into micromanipulator coordinates."""
if axs is None:
fig, axs = plt.subplots(2, 2, sharex='col', sharey='row')
else:
fig = axs[0, 0].figure
plt.close(fig)
xy2ab = []
for direction, axss, dirname in zip('xy', axs.T, ['lateral', 'depth']):
while True:
for i in range(3):
recover(f'169.254.{3+i}.100')
print(
f"Please block {dirname} direction. Close figure window when OK."
)
with ExitStack() as stack:
sensors = [
stack.enter_context(closing(DT3100(f'169.254.{3+i}.100')))
for i in range(3)
]
for sensor in sensors:
sensor.set_averaging_type(3)
sensor.set_averaging_number(3)
show_measurement(sensors, 1, ymin=-80)
print(f"Performing measurments in {dirname} direction.")
for ax in axss:
ax.clear()
dir2ab = sampled_single_direction(direction,
samples,
repeat,
axs=axss)
dummy = plt.figure()
new_manager = dummy.canvas.manager
new_manager.canvas.figure = fig
fig.set_canvas(new_manager.canvas)
plt.show(block=False)
if (input('Are you happy with the results? (N)')
in ['Y', 'y', 'yes', 'Yes', 'oui']):
xy2ab.append(dir2ab)
break
plt.close(dummy)
return np.linalg.inv(np.column_stack((xy2ab)))
def step_response_Y(outname, kp, ki=0, kd=0, dy=10., originalsetpoint=None):
"""Perform a single depthwise step of dy microns and record the PID response to a file named outname."""
with closing(DT3100('169.254.3.100')) as sensor, closing(
MPC385()) as actuator:
#setting up sensor
sensor.set_averaging_type(3)
sensor.set_averaging_number(3)
#remember original positions of the sensor and actuator
if originalsetpoint is None:
originalsetpoint = sensor.readOne().m
x0, y0, z0 = actuator.update_current_position()[1:]
#setting up PID
pid = PID(kp, ki, kd)
pid.setPoint = originalsetpoint
with open(outname, "wb") as fout:
m = MoverPID_Y(sensor, actuator, pid, outputFile=fout)
m.start()
time.sleep(1)
pid.setPoint += dy
time.sleep(20)
#stop PID
m.go = False
m.join()
#move the actuator back to its original position
actuator.move_to(x0, y0, z0)
#read back the data and print some metrics
meas = np.fromfile(outname)
ts, measured, out = meas.reshape((meas.shape[0] // 3, 3)).T
if not np.any(
np.abs(measured - originalsetpoint - dy) < actuator.step2um(1)):
print("does not converge")
else:
print("cverge\teRMS\t%overshoot")
print('%g\t%g\t%g' % (ts[np.where(
np.abs(measured - originalsetpoint -
dy) < actuator.step2um(1))[0][0] - 1],
np.sqrt(
np.mean((measured[np.where(ts > 2)[0][0]:] -
originalsetpoint - dy)**2)), 100 *
(measured.max() - originalsetpoint - dy) / dy))
def timedep_armX_positionY(ab2xy,
outname,
functions,
duration=None,
kp=0.9,
ki=0.0,
kd=0.0,
moveback=False,
state0=None):
"""Moving the absolute position of the head in depth (Y) and the arm position in Y following two
time-dependent functions around state0.
Need ab2xy calibration matrix.
Duration is in seconds. If None specified, continues until stopped."""
if not hasattr(kp, "__len__"):
kps = [kp, kp]
else:
kps = kp
if not hasattr(ki, "__len__"):
kis = [ki, ki]
else:
kis = ki
if not hasattr(kd, "__len__"):
kds = [kd, kd]
else:
kds = kd
assert len(kps) == 2
assert len(kis) == 2
assert len(kds) == 2
assert len(functions) == 2
with ExitStack() as stack:
sensors = [
stack.enter_context(closing(DT3100(f'169.254.{3+i}.100')))
for i in range(3)
]
actuator = stack.enter_context(closing(MPC385()))
#setting up sensors
for sensor in sensors:
sensor.set_averaging_type(3)
sensor.set_averaging_number(3)
if state0 is None:
#remember original positions of the sensors and actuator
state0 = State().read(sensors, actuator, ab2xy)
#setting up PID, in microns
pids = []
initialposition = [state0.arm_to_ground[0], state0.head_to_ground[1]]
for s, kp, ki, kd in zip(initialposition, kps, kis, kds):
pid = PID(kp, ki, kd)
pid.setPoint = s
pids.append(pid)
try:
with open(outname, "wb") as fout:
m = moverPID.armX_position_Y_3sensors(sensors,
actuator,
ab2xy,
pids,
outputFile=fout)
t0 = time.time()
m.start()
try:
t = time.time() - t0
while (duration is None) or (t < duration):
t = time.time() - t0
for pid, p0, function in zip(pids, initialposition,
functions):
pid.setPoint = p0 + function(t)
time.sleep(0.001) #no use to update above 1kHz
except KeyboardInterrupt:
pass
finally:
#stop PID thread
m.go = False
m.join()
finally:
if moveback:
#move the actuator back to its original position
actuator.move_to(
*actuator.um2integer_step(state0.actuator_pos))
def move_to_constant_positions(ab2xy,
outnames,
dxs,
dys,
durations,
kp=0.9,
ki=0.0,
kd=0.0,
moveback=False,
state0=None):
"""Moving the absolute position of the head by dy (depth) and dx (width)
and stay at that position using PID feedback.
Need ab2xy calibration matrix.
Duration is in seconds. If None specified, continues until stopped."""
if not hasattr(kp, "__len__"):
kps = [kp, kp]
else:
kps = kp
if not hasattr(ki, "__len__"):
kis = [ki, ki]
else:
kis = ki
if not hasattr(kd, "__len__"):
kds = [kd, kd]
else:
kds = kd
assert len(kps) == 2
assert len(kis) == 2
assert len(kds) == 2
with ExitStack() as stack:
sensors = [
stack.enter_context(closing(DT3100(f'169.254.{3+i}.100')))
for i in range(3)
]
actuator = stack.enter_context(closing(MPC385()))
#setting up sensors
for sensor in sensors:
sensor.set_averaging_type(3)
sensor.set_averaging_number(3)
if state0 is None:
#remember original positions of the sensors and actuator
state0 = State().read(sensors, actuator, ab2xy)
#setting up PID, in microns
pids = []
initialposition = state0.head_to_ground
for s, kp, ki, kd in zip(initialposition, kps, kis, kds):
pid = PID(kp, ki, kd)
pid.setPoint = s
pids.append(pid)
try:
for outname, dx, dy, duration in zip(outnames, dxs, dys,
durations):
for s, pid in zip(initialposition + np.array([dx, dy]), pids):
pid.setPoint = s
with open(outname, "wb") as fout:
m = moverPID.constant_position_XY_3sensors(sensors,
actuator,
ab2xy,
pids,
outputFile=fout)
t0 = time.time()
m.start()
try:
while (duration is None) or (time.time() <
t0 + duration):
time.sleep(1)
except KeyboardInterrupt:
pass
finally:
#stop PID thread
m.go = False
m.join()
finally:
if moveback:
#move the actuator back to its original position
actuator.move_to(
*actuator.um2integer_step(state0.actuator_pos))
def add_constant_deflectionX_move_to_constant_positiony(
ab2xy,
outnames,
dXs,
dys,
durations,
kp=0.9,
ki=0.0,
kd=0.0,
moveback=False,
maxYdispl=None,
state0=None):
"""Considering initial deflection is (0,0),
successively add dX to the deflection and dy to absolute position
and stay at this constant X deflection and constant y position during duration.
Stay at this deflection and positon using using PID feedback (constant stress and constant strain respectively).
Iterate on next dX, duration.
Need ab2xy calibration matrix.
Duration is in seconds. If None specified, continues until stopped."""
if not hasattr(kp, "__len__"):
kps = [kp, kp]
else:
kps = kp
if not hasattr(ki, "__len__"):
kis = [ki, ki]
else:
kis = ki
if not hasattr(kd, "__len__"):
kds = [kd, kd]
else:
kds = kd
#remember original positions of the sensors and actuator
with ExitStack() as stack:
sensors = [
stack.enter_context(closing(DT3100(f'169.254.{3+i}.100')))
for i in range(3)
]
actuator = stack.enter_context(closing(MPC385()))
#setting up sensors
for sensor in sensors:
sensor.set_averaging_type(3)
sensor.set_averaging_number(3)
if state0 is None:
#remember original positions of the sensors and actuator
state0 = State().read(sensors, actuator, ab2xy)
#setting up PID
pids = []
for s, kp, ki, kd in zip(
[state0.deflection[0], state0.head_to_ground[1]], kps, kis, kds):
pid = PID(kp, ki, kd)
pid.setPoint = s
pids.append(pid)
try:
for outname, dX, dy, duration in zip(outnames, dXs, dys,
durations):
ip = [state0.deflection[0] + dX, state0.head_to_ground[1] + dy]
for s, pid in zip(ip, pids):
pid.setPoint = s
with open(outname, "wb") as fout:
m = moverPID.constant_deflectionX_positionY_3sensors(
sensors, actuator, ab2xy, pids, outputFile=fout)
t0 = time.time()
m.start()
try:
while (duration is None) or (time.time() <
t0 + duration):
time.sleep(1) #is it too long ?
x, y, z = m.xyz
y0 = actuator.um2integer_step(
state0.actuator_pos[1])
if maxYdispl is not None and abs(
y - y0) > actuator.um2step(maxYdispl):
break
except KeyboardInterrupt:
pass
finally:
#stop PID thread
m.go = False
m.join()
finally:
if moveback:
#move the actuator back to its original position
actuator.move_to(
*actuator.um2integer_step(state0.actuator_pos))
def add_constant_deflection(ab2xy,
outnames,
dXs,
dYs,
durations,
kp=0.9,
ki=0.0,
kd=0.0,
moveback=False,
delay=0,
state0=None):
"""Considering initial deflection is 0 in x
and successively add dX to the deflection during duration
and stay at this deflection using PID feedback (constant stress).
Iterate on next dX, dY, duration.
Need ab2xy calibration matrix.
Duration is in seconds. If None specified, continues until stopped."""
#remember original positions of the sensors and actuator
if not hasattr(kp, "__len__"):
kps = [kp, kp]
else:
kps = kp
if not hasattr(ki, "__len__"):
kis = [ki, ki]
else:
kis = ki
if not hasattr(kd, "__len__"):
kds = [kd, kd]
else:
kds = kd
assert len(kps) == 2
assert len(kis) == 2
assert len(kds) == 2
with ExitStack() as stack:
sensors = [
stack.enter_context(closing(DT3100(f'169.254.{3+i}.100')))
for i in range(3)
]
actuator = stack.enter_context(closing(MPC385()))
#setting up sensors
for sensor in sensors:
sensor.set_averaging_type(3)
sensor.set_averaging_number(3)
if state0 is None:
#remember original positions of the sensors and actuator
state0 = State().read(sensors, actuator, ab2xy)
#setting up PID, in microns
pids = []
initialposition = state0.deflection
for s, kp, ki, kd in zip(initialposition, kps, kis, kds):
pid = PID(kp, ki, kd)
pid.setPoint = s
pids.append(pid)
try:
for dX, dY, duration, outname in zip(dXs, dYs, durations,
outnames):
for s, pid in zip(state0.deflection + np.array([dX, dY]),
pids):
pid.setPoint = s
with open(outname, "wb") as fout:
m = moverPID.constant_deflection_XY_3sensors(
sensors,
actuator,
ab2xy,
pids,
outputFile=fout,
delay=delay,
)
t0 = time.time()
m.start()
try:
while (duration is None) or (time.time() <
t0 + duration):
time.sleep(1)
except KeyboardInterrupt:
pass
finally:
#stop PID
m.go = False
m.join()
finally:
if moveback:
#move the actuator back to its original position
actuator.move_to(
*actuator.um2integer_step(state0.actuator_pos))
def sampled_single_direction(direction='x', samples=None, repeat=10, axs=None):
"""To calibrate along x (resp y), mechanically block the head of the cantilever from the left (resp bottom). By default, test 11 dispacements sampled in log scale.
The resulting coefficients allows to convert micromanipulator coordinates to sensor measurements coordinates. Returns coefficients."""
if samples is None:
samples = 2**np.arange(11)
measures = np.zeros((len(samples), 2))
with closing(DT3100('169.254.3.100')) as sensorA, closing(
DT3100('169.254.4.100')) as sensorB, closing(MPC385()) as actuator:
sensors = [sensorA, sensorB]
for sensor in sensors:
sensor.set_averaging_type(3)
sensor.set_averaging_number(3)
#remember original positions of the sensors and actuator
x0, y0, z0 = actuator.update_current_position()[1:]
try:
initial = np.zeros(2)
for j in range(repeat):
initial += read_both(sensorA, sensorB)
initial /= repeat
#move along z
for i, sample in enumerate(samples):
if direction == "x":
actuator.move_to(x0 + sample, y0, z0)
elif direction == "y":
#manipulator going down by sample
#head going up by sample since blocked
actuator.move_to(x0, y0 - sample, z0)
else:
raise ValueError('direction should be x or y')
for j in range(repeat):
m = np.array(read_both(sensorA, sensorB))
if np.any(m == 800) or np.any(m == 0):
raise ValueError('Sensor out of range')
measures[i] += m
finally:
actuator.move_to(x0, y0, z0)
measures /= repeat
measures -= initial
np.save(
'calib_' + direction + '_' +
datetime.now().strftime('%Y%m%d_%H%M_calib.npy'), measures)
#fit the results to get coefficients
func = lambda x, p: p * x
if direction == "x":
dxs = -actuator.step2um(samples)
elif direction == "y":
dxs = actuator.step2um(samples)
else:
raise ValueError('direction should be x or y')
if axs is None:
fig, axs = plt.subplots(1, 2, sharex=True)
x2ab = []
for ax, m in zip(axs.ravel(), (measures).T):
ax.plot(dxs, m, 'o')
param = curve_fit(func, dxs, m)[0][0]
print(param)
x2ab.append(param)
ax.plot(dxs, func(dxs, param), ':')
#axs[0].set_title(direction+'a')
#axs[1].set_title(direction+'b')
axs[0].set_xlabel(direction)
axs[0].set_ylabel('a')
axs[1].set_xlabel(direction)
axs[1].set_ylabel('b')
return np.array(x2ab)
def find_wall(direction='y',
repeat=10,
precision=1,
verbose=False,
backup=True,
settle_time=None,
k=100 * 16):
"""Find the position at which the head encounters a wall"""
with closing(DT3100('169.254.3.100')) as sensorA, closing(
DT3100('169.254.4.100')) as sensorB, closing(MPC385()) as actuator:
sensors = [sensorA, sensorB]
for sensor in sensors:
sensor.set_averaging_type(3)
sensor.set_averaging_number(3)
#remember original positions of the sensors and actuator
x0, y0, z0 = actuator.update_current_position()[1:]
try:
initial = np.zeros(2)
for j in range(repeat):
initial += read_both(*sensors)
initial /= repeat
#estimate the maximum range
lbound = 0
ubound = actuator.um2integer_step(initial.min()) // 2
#if sensors in the middle of their range, should be 200um
#try to expand upper bound
moved = False
while not moved:
if direction == "x":
if verbose:
print("Looking between %d and %d in width" %
(x0 + lbound, x0 + ubound))
actuator.move_to(x0 + ubound, y0, z0)
elif direction == "y":
if verbose:
print("Looking between %d and %d in depth" %
(y0 - ubound, y0 - lbound))
actuator.move_to(x0, y0 - ubound, z0)
moved = have_moved(initial, sensors, repeat, precision,
settle_time)
if not moved:
lbound = ubound
ubound += actuator.um2integer_step(initial.min()) // 2
#move back to lower bound
if direction == "x":
print("excessive touching")
actuator.move_to(x0 + lbound, y0, z0)
elif direction == "y":
print("excessive touching")
actuator.move_to(x0, y0 - lbound, z0)
#actuator.move_to(x0, y0+lbound, z0)
while ubound - lbound > actuator.um2integer_step(precision):
midrange = (ubound + lbound) // 2
if direction == "x":
if verbose:
print("Looking between %d and %d in width" %
(x0 + lbound, x0 + ubound))
actuator.move_to(x0 + midrange, y0, z0)
elif direction == "y":
if verbose:
print("Looking between %d and %d in depth" %
(y0 - ubound, y0 - lbound))
actuator.movadjustmente_to(x0, y0 - midrange, z0)
moved = have_moved(initial, sensors, repeat, precision,
settle_time)
if moved:
ubound = midrange
else:
lbound = midrange
touching = (ubound + lbound) // 2
if direction == "x":
touching += x0
actuator.move_to(touching, y0, z0)
elif direction == "y":
touching = y0 - touching
actuator.move_to(x0, touching, z0)
#k += touching
#actuator.move_to(x0, k, z0)
print("touching at %d" % touching)
#print("moved actuator to %d"%k)
finally:
if backup:
print("Backing up to original position")
actuator.move_to(x0, y0, z0)
return touching
help='minimum distance')
parser.add_argument('--ymax',
default=None,
type=float,
help='maximum distance')
parser.add_argument('--tw',
default=1.0,
type=float,
help='Width of the time window, in seconds.')
parser.add_argument(
'--fast',
default=False,
type=bool,
help='Whether to use continuous reading of the sensors.')
args = parser.parse_args()
if args.ip is None or len(args.ip) == 0:
args.ip = ['169.254.3.100']
for ip in args.ip:
recover(ip)
with ExitStack() as stack:
sensors = [stack.enter_context(closing(DT3100(ip))) for ip in args.ip]
for sensor in sensors:
sensor.set_averaging_type(args.avt)
sensor.set_averaging_number(args.avn)
show_measurement(sensors,
args.tw,
ymin=args.ymin,
ymax=args.ymax,
names=args.ip,
fast=args.fast)