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 __init__(self,
sensors,
actuator,
AB2XY,
pids=[PID(), PID()],
outputFile=False):
"""AB2XY is the transfer matrix between sensor coordinates and actuator coordinates."""
Thread.__init__(self)
self.sensors = sensors
self.actuator = actuator
self.AB2XY = AB2XY
self.pids = pids
if outputFile:
self.recorder = Recorder(outputFile)
else:
self.recorder = False
self.go = True
def __init__(self, sensor, actuator, pid=PID(), outputFile=False):
Thread.__init__(self)
self.sensor = sensor
self.actuator = actuator
self.pid = pid
if outputFile:
self.recorder = Recorder(outputFile)
else:
self.recorder = False
self.go = True
def __init__(self,
sensors,
actuator,
AB2XY,
pids=[PID(), PID()],
outputFile=False,
delay=0):
"""AB2XY is the transfer matrix between sensor coordinates and actuator coordinates."""
Thread.__init__(self)
self.sensors = sensors
self.actuator = actuator
self.AB2XY = AB2XY
assert len(pids) == 2, "Two PIDs are needed"
self.pids = pids
self.delay = delay
if outputFile:
self.recorder = Recorder(outputFile)
else:
self.recorder = False
self.go = True
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))