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

View File

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