forked from M-Labs/artiq
1
0
Fork 0

suservo: support pure-I

This commit is contained in:
Robert Jördens 2018-05-14 20:15:30 +02:00 committed by Robert Jordens
parent ae50196186
commit a100c73dfe
2 changed files with 37 additions and 27 deletions

View File

@ -11,7 +11,7 @@ WE = 1 << COEFF_DEPTH + 1
STATE_SEL = 1 << COEFF_DEPTH
CONFIG_SEL = 1 << COEFF_DEPTH - 1
CONFIG_ADDR = CONFIG_SEL | STATE_SEL
F_CYCLE = 1/((2*(8 + 64) + 2 + 1)*8*ns)
T_CYCLE = (2*(8 + 64) + 2 + 1)*8*ns
COEFF_SHIFT = 11
@ -277,7 +277,7 @@ class Channel:
:param b1: 18 bit signed B1 coefficient (old,
X1 coefficient, feed forward, proportional gain)
:param dly: IIR update suppression time. In units of IIR cycles
(~1.1 µs)
(~1.2 µs, 0-255)
"""
base = (self.servo_channel << 8) | (profile << 3)
self.servo.write(base + 3, adc | (dly << 8))
@ -305,45 +305,55 @@ class Channel:
Where:
* :math:`s = \\sigma + i\\omega` is the complex frequency
* :math:`K` is the proportional gain
* :math:`\\omega_0` is the integrator corner frequency
* :math:`\\omega_0 = 2\\pi f_0` is the integrator corner frequency
* :math:`g` is the integrator gain limit
:param profile: Profile number (0-31)
:param adc: ADC channel to take IIR input from (0-7)
:param gain: Proportional gain (1). This is usually negative (closed
loop, positive ADC voltage, positive setpoint).
loop, positive ADC voltage, positive setpoint). When 0, this
implements a pure I controller with unit gain frequency at
`corner` (use the sign of `corner` for overall gain sign).
:param corner: Integrator corner frequency (Hz). When 0 (the default)
this implements a pure P controller.
:param limit: Integrator gain limit (1). When 0 (the default) the
integrator gain limit is infinite.
:param delay: Delay (in seconds) before allowing IIR updates after
invoking :meth:`set`.
integrator gain limit is infinite. Positive.
:param delay: Delay (in seconds, 0-300 µs) before allowing IIR updates
after invoking :meth:`set`.
"""
B_NORM = 1 << COEFF_SHIFT + 1
A_NORM = 1 << COEFF_SHIFT
PI_TS = 3.1415927/F_CYCLE
PI_TS = 3.1415927*T_CYCLE
COEFF_MAX = 1 << COEFF_WIDTH - 1
k = B_NORM*gain
gain *= B_NORM
corner *= PI_TS
if corner == 0.:
# pure P
a1_ = 0
b0_ = int(round(k))
b1_ = 0
b0_ = int(round(gain))
else:
a1_ = A_NORM
if gain == 0.:
# pure I
b0 = (2*B_NORM)*corner
b1_ = 0
else:
q = PI_TS*corner
kq = k*q
a1_ = A_NORM
b0 = kq + k
b1 = kq - k
# PI
k = gain*corner
b1 = k - gain
b0 = k + gain
if limit != 0.:
ql = q/limit
qlr = 1./(1. + ql)
a1_ = int(round(a1_*(1. - ql)*qlr))
b0 *= qlr
b0 *= qlr
b0_ = int(round(b0))
# PI with limit
q = corner/limit
qr = 1./(1. + q)
a1_ = int(round(a1_*(1. - q)*qr))
b0 *= qr
b1 *= qr
b1_ = int(round(b1))
b0_ = int(round(b0))
if b1_ == -b0_:
raise ValueError("low corner, gain, limit")
@ -352,7 +362,7 @@ class Channel:
b1_ >= COEFF_MAX or b1_ < -COEFF_MAX):
raise ValueError("high corner, gain, limit")
dly = int(round(delay*F_CYCLE))
dly = int(round(delay/T_CYCLE))
self.set_iir_mu(profile, adc, a1_, b0_, b1_, dly)
@kernel

View File

@ -16,7 +16,7 @@ class SUServo(EnvExperiment):
mask = 1 << 18 - 1
for name, val in zip("ftw1 b1 pow cfg offset a1 ftw0 b0".split(), d):
val = -(val & mask) + (val & ~mask)
print("{}: {:x} = {}".format(name, val, val))
print("{}: {:#x} = {}".format(name, val, val))
@rpc(flags={"async"})
def p1(self, adc, asf, st):