221 lines
8.4 KiB
Python
221 lines
8.4 KiB
Python
|
from datetime import datetime
|
||
|
|
import os
|
||
|
|
from threading import Lock, RLock, Condition, Thread
|
||
|
|
|
||
|
|
from .communication import Communication
|
||
|
|
from .. import util
|
||
|
|
from .. import mqtt
|
||
|
|
from .constants import state_names, state
|
||
|
|
from . import constants
|
||
|
|
|
||
|
|
class Control(util.Loggable):
|
||
|
|
def __init__(self, config, mqtt_client=None):
|
||
|
|
self._config = config
|
||
|
|
|
||
|
|
self._mutex = RLock()
|
||
|
|
self._control_update = Condition(self._mutex)
|
||
|
|
|
||
|
|
self._comms = Communication(config.serial_port)
|
||
|
|
self._position_task: Thread = None
|
||
|
|
self._control_task: Thread = None
|
||
|
|
|
||
|
|
self.target = mqtt.Value(mqtt_client, "door/state/target",
|
||
|
|
persistent=True,
|
||
|
|
translate=state_names)
|
||
|
|
self.state = mqtt.Value(mqtt_client, "door/state/value",
|
||
|
|
persistent=True,
|
||
|
|
translate=state_names)
|
||
|
|
self.position = mqtt.Value(mqtt_client, "door/position/value",
|
||
|
|
persistent=True)
|
||
|
|
self._speed = 0
|
||
|
|
|
||
|
|
self._started = False
|
||
|
|
self._stop = False
|
||
|
|
self._idle = True
|
||
|
|
|
||
|
|
def _run_position(self):
|
||
|
|
cond = Condition(self._mutex)
|
||
|
|
warned = False
|
||
|
|
|
||
|
|
def update_position(_, line):
|
||
|
|
p, s = map(int, line.split())
|
||
|
|
with cond:
|
||
|
|
if p != self.position():
|
||
|
|
self._logger().debug(f"Position: {p}, {s}")
|
||
|
|
self.position(p)
|
||
|
|
self._speed = s
|
||
|
|
cond.notify()
|
||
|
|
|
||
|
|
self._comms.subscribe("pos", update_position)
|
||
|
|
last_update = datetime.now()
|
||
|
|
last_movement = datetime.now()
|
||
|
|
while True:
|
||
|
|
with cond:
|
||
|
|
if self._stop:
|
||
|
|
return
|
||
|
|
if not cond.wait(5):
|
||
|
|
self._comms.cmd_report()
|
||
|
|
dt = (datetime.now() - last_update).total_seconds()
|
||
|
|
if dt > 10 and not warned:
|
||
|
|
warned = True
|
||
|
|
self._logger().warn(f"No position for {dt} seconds")
|
||
|
|
else:
|
||
|
|
last_update = datetime.now()
|
||
|
|
warned = False
|
||
|
|
|
||
|
|
if self._speed != 0:
|
||
|
|
last_movement = datetime.now()
|
||
|
|
|
||
|
|
mov_dt = (datetime.now() - last_movement).total_seconds()
|
||
|
|
if mov_dt > constants.MIN_IDLE_TIME:
|
||
|
|
if self.position() < constants.CLOSED_THRESHOLD:
|
||
|
|
self.state(state.CLOSE)
|
||
|
|
self._control_update.notify()
|
||
|
|
elif self.position() > constants.ERROR_THRESHOLD:
|
||
|
|
self.state(state.ERROR)
|
||
|
|
self._control_update.notify()
|
||
|
|
elif self.position() > constants.OPEN_THRESHOLD:
|
||
|
|
self.state(state.OPEN)
|
||
|
|
self._control_update.notify()
|
||
|
|
elif not self._idle:
|
||
|
|
self._control_update.notify()
|
||
|
|
self._idle = True
|
||
|
|
|
||
|
|
else:
|
||
|
|
self._idle = False
|
||
|
|
|
||
|
|
def _run_control(self):
|
||
|
|
# Last known state
|
||
|
|
st = state.IDLE
|
||
|
|
# Current action
|
||
|
|
action = state.IDLE
|
||
|
|
cmd = {
|
||
|
|
None: lambda: None,
|
||
|
|
state.IDLE: lambda: None,
|
||
|
|
state.ERROR: lambda: None,
|
||
|
|
state.RESTART: self._comms.cmd_restart,
|
||
|
|
state.OPEN: self._comms.cmd_open,
|
||
|
|
state.OPEN_THEN_CLOSE: self._comms.cmd_open,
|
||
|
|
state.CLOSE: self._comms.cmd_close,
|
||
|
|
state.CLOSE_THEN_OPEN: self._comms.cmd_close,
|
||
|
|
}
|
||
|
|
|
||
|
|
with self._control_update:
|
||
|
|
last_target = state.IDLE
|
||
|
|
# When starting, reset the MCU once
|
||
|
|
self.target(state.CLOSE)
|
||
|
|
# Starting time of the current action
|
||
|
|
start_time = datetime.now()
|
||
|
|
timeouts = 0
|
||
|
|
while not self._stop:
|
||
|
|
# Wait for an update
|
||
|
|
self._control_update.wait(1)
|
||
|
|
if not self._idle:
|
||
|
|
# If still moving, continue
|
||
|
|
self._logger().debug(f"Not idle")
|
||
|
|
continue
|
||
|
|
|
||
|
|
# Update was that the target has changed
|
||
|
|
if self.target() != last_target:
|
||
|
|
self._logger().debug(f"Target update: {state_names[self.target()]}")
|
||
|
|
self._logger().debug(f"{cmd[self.target()]}, {cmd[last_target]}")
|
||
|
|
if cmd.get(action, None) != cmd[self.target()]:
|
||
|
|
# We need to send a different command for this
|
||
|
|
self._logger().debug(f"Calling {cmd[self.target()]}")
|
||
|
|
cmd[self.target()]()
|
||
|
|
else:
|
||
|
|
self._logger().debug(f"Same command as {state_names[last_target]}")
|
||
|
|
# Update last known target and starting time
|
||
|
|
last_target = self.target()
|
||
|
|
start_time = datetime.now()
|
||
|
|
|
||
|
|
# Update current target
|
||
|
|
target = last_target
|
||
|
|
|
||
|
|
if self.state() != st:
|
||
|
|
# State from position handling differs from last known state
|
||
|
|
self._logger().debug(f"State update, target is {state_names[target]}")
|
||
|
|
st = self.state()
|
||
|
|
self._logger().info("Reached state "
|
||
|
|
f"{state_names.get(st, st)}")
|
||
|
|
if st == target:
|
||
|
|
# Reached target
|
||
|
|
timeouts = 0
|
||
|
|
if target == state.CLOSE \
|
||
|
|
and self.position() > constants.CLOSED_WANT:
|
||
|
|
self._logger().info(
|
||
|
|
f"Position is {self.position()}, "
|
||
|
|
"closing some more")
|
||
|
|
self._comms.cmd_close()
|
||
|
|
action = state.IDLE
|
||
|
|
elif self.state() == state.ERROR:
|
||
|
|
# Position too high, restart
|
||
|
|
self._comm.cmd_restart()
|
||
|
|
self.target(state.CLOSE)
|
||
|
|
else:
|
||
|
|
if timeouts < 3:
|
||
|
|
timeouts += 1
|
||
|
|
if action == target:
|
||
|
|
# Initially, switch to the other one
|
||
|
|
# and execute that
|
||
|
|
action = {
|
||
|
|
state.CLOSE: state.OPEN_THEN_CLOSE,
|
||
|
|
state.OPEN: state.CLOSE_THEN_OPEN
|
||
|
|
}.get(target, state.RESTART)
|
||
|
|
cmd[action]()
|
||
|
|
else:
|
||
|
|
# Then go back
|
||
|
|
action = target
|
||
|
|
cmd[action]()
|
||
|
|
else:
|
||
|
|
# Tried too often, restart
|
||
|
|
self.target(state.RESTART)
|
||
|
|
|
||
|
|
if action == state.IDLE:
|
||
|
|
continue
|
||
|
|
|
||
|
|
def start(self):
|
||
|
|
with self._mutex:
|
||
|
|
if not self._started:
|
||
|
|
self._started = True
|
||
|
|
self._stop = False
|
||
|
|
self._comms.start()
|
||
|
|
self._position_task = Thread(target=self._run_position,
|
||
|
|
daemon=True)
|
||
|
|
self._control_task = Thread(target=self._run_control,
|
||
|
|
daemon=True)
|
||
|
|
self._position_task.start()
|
||
|
|
self._control_task.start()
|
||
|
|
|
||
|
|
def stop(self):
|
||
|
|
with self._mutex:
|
||
|
|
if self._started:
|
||
|
|
self._started = False
|
||
|
|
self._stop = True
|
||
|
|
self._comms.stop()
|
||
|
|
self._mutex.release()
|
||
|
|
self._position_task.join()
|
||
|
|
self._control_task.join()
|
||
|
|
|
||
|
|
def open(self):
|
||
|
|
with self._mutex:
|
||
|
|
self.target(state.OPEN)
|
||
|
|
self._control_update.notify()
|
||
|
|
|
||
|
|
def close(self):
|
||
|
|
with self._mutex:
|
||
|
|
self.target(state.CLOSE)
|
||
|
|
self._control_update.notify()
|
||
|
|
|
||
|
|
def toggle(self):
|
||
|
|
with self._mutex:
|
||
|
|
self._logger().debug("Asked to toggle")
|
||
|
|
if self.target() == state.OPEN:
|
||
|
|
self.close()
|
||
|
|
else:
|
||
|
|
self.open()
|
||
|
|
|
||
|
|
def state(self):
|
||
|
|
with self._mutex:
|
||
|
|
return self._state
|