53 lines
2.7 KiB
Python
53 lines
2.7 KiB
Python
"""Speed acquisition and measured hold time; never infers motion from a command."""
|
|
from .protocol import SPEED_LIMITS
|
|
|
|
|
|
class SpeedHold:
|
|
def __init__(self, erpm, duration, started):
|
|
self.erpm, self.duration, self.started = erpm, duration, started
|
|
self.previous = started
|
|
self.tachometer = None
|
|
self.motion_at = None
|
|
self.stable_at = None
|
|
self.hold_started = None
|
|
self.lost_at = None
|
|
self.rotation_s = 0.0
|
|
self.phase = "accelerating"
|
|
self.previous_good = False
|
|
|
|
def update(self, now, value):
|
|
delta = now - self.previous
|
|
self.previous = now
|
|
if self.tachometer is not None and value["tachometer"] != self.tachometer:
|
|
self.motion_at = now
|
|
self.tachometer = value["tachometer"]
|
|
good = (abs(value["erpm"] - self.erpm) <= abs(self.erpm) * SPEED_LIMITS["speed_tolerance"]
|
|
and self.motion_at is not None and now - self.motion_at <= 0.25)
|
|
error = None
|
|
if self.hold_started is None:
|
|
if good:
|
|
if self.stable_at is None: self.stable_at = now
|
|
if now - self.stable_at >= SPEED_LIMITS["settle_s"]:
|
|
self.hold_started = now
|
|
self.phase = "holding"
|
|
else:
|
|
self.stable_at = None
|
|
if self.hold_started is None and now - self.started >= SPEED_LIMITS["startup_timeout_s"]:
|
|
error = "Мотор не вышел на заданную скорость за 15 секунд. Отсчёт вращения не начался."
|
|
else:
|
|
if good:
|
|
# Both endpoints must be observed in band. Do not count a gap,
|
|
# USB delay, stationary tachometer or an unobserved last interval.
|
|
if self.previous_good and delta <= 0.25: self.rotation_s += delta
|
|
self.lost_at = None
|
|
else:
|
|
if self.lost_at is None: self.lost_at = now
|
|
if now - self.lost_at >= SPEED_LIMITS["lost_speed_timeout_s"]:
|
|
error = "Мотор перестал удерживать заданную скорость. Проверка завершена досрочно."
|
|
self.previous_good = good
|
|
if now - self.started > SPEED_LIMITS["startup_timeout_s"] + self.duration + 5:
|
|
error = "Истёк общий срок проверки; заданное время вращения не набрано."
|
|
done = self.rotation_s >= self.duration
|
|
setpoint = (1 if self.erpm > 0 else -1) * min(abs(self.erpm), max(1, (now - self.started) * SPEED_LIMITS["ramp_erpm_per_s"]))
|
|
return setpoint, done, error
|