A robotic telescope on a mountain is steered from a control room at its foot. The azimuth motor's position loop is a P controller, and it used to run beside the motor; now the encoder readings travel over a radio link and arrive delay seconds late. Will the telescope start to hunt back and forth?
The loop (controller times motor) has the frequency response
L(jw) = K * e^(-jw * delay) / (jw * (1 + jw * T))
where K (per second) is the loop gain and T (seconds) the motor's time constant: the jw in the denominator is the motor's speed adding up into an angle. Its size and its angle (in degrees) are
size(w) = K / (w * sqrt(1 + (w * T) ** 2)) (the delay has size 1)
angle(w) = -90 - degrees(atan(w * T)) - degrees(w * delay)
The gain crossover wc is where the size is exactly 1. Squaring size = 1 gives T**2 * x**2 + x - K**2 = 0 for x = wc**2; take its positive root, and wc = sqrt(x) (when T = 0, wc = K). Because the delay does not change the size, wc is the same for every delay. The phase margin is 180 + angle(wc): without the delay PM0 = 90 - degrees(atan(wc * T)), and every second of delay takes away degrees(wc) more. The loop oscillates once the margin reaches 0, which happens at the delay margin radians(PM0) / wc seconds.
Write delay_margin(K, T, delays) that returns a tuple (wc, PM0, margins, max_delay): the gain crossover (rad/s), the phase margin without delay (degrees), the list of phase margins for each delay in delays (degrees, negative for a loop that would oscillate), and the delay margin (seconds).
Examples
Input: K = 2, T = 0.5, delays = [0, 0.1, 0.3, 0.6]
Output: (1.57230276, 51.82729237, [51.82729237, 42.81866117, 24.80139877, -2.22449483], 0.57530707)
Explanation: x = (-1 + sqrt(1 + 4)) / 0.5 = 2.472, so wc = 1.572 rad/s. At wc the motor lags
38.2 degrees, leaving 51.8. Each 0.1 s of delay costs 9.0 degrees; 0.6 s is too much.
Input: K = 3, T = 0, delays = [0.2]
Output: (3.0, 90.0, [55.62253229], 0.52359878)
Constraints
- answers are compared with a tolerance of
1e-6; do not round
Goals
- Find a servo loop's gain crossover frequency from its size formula
- See that a delay leaves the loop's size alone and only adds lag, w * delay
- Compute the phase margin left for each delay and the largest delay the loop survives