diff --git a/door_pi_control/__init__.py b/door_pi_control/__init__.py index 0437767..1b80390 100644 --- a/door_pi_control/__init__.py +++ b/door_pi_control/__init__.py @@ -11,11 +11,12 @@ import serial UPDATE_RATE = 2 MAX_UPDATE_RATE = 20 -MIN_SPEED = 5 ERROR_THRESHOLD = 250 OPEN_THRESHOLD = 190 -CLOSED_THRESHOLD = 160 -CLOSED_WANT = 40 +CLOSED_THRESHOLD = 180 +CLOSED_WANT = 50 +MIN_IDLE_TIME = 1 +COMMAND_IDLE_TIME = 1.5 def timestamp(): return datetime.datetime.now().strftime("%Y-%m-%d %H:%M:%S") @@ -69,8 +70,6 @@ class DoorControl: IDLE: "idling", OPEN: "waiting for open door", CLOSE: "waiting for closed door", - OPEN_THEN_CLOSE: "waiting for open door, then closing again", - CLOSE_THEN_OPEN: "waiting for closed door, then opening again", ERROR: "error", } @@ -90,7 +89,7 @@ class DoorControl: # Current door state self.state = MqttValue(self.mqttc, "door/state/value", True, translate = self.state_names) - self.state_target = MqttValue(self.mqttc, "door/state/target", True, translate = self.state_names, max_update = 0) + self.target_state = MqttValue(self.mqttc, "door/state/target", True, DoorControl.CLOSE, translate = self.state_names, max_update = 0) self.state_pos = MqttValue(self.mqttc, "door/position/value", True, 0) self.last_invalid_token = MqttValue(self.mqttc, "door/token/last_invalid", True) self.speed = MqttValue(self.mqttc, "door/position/speed") @@ -105,6 +104,14 @@ class DoorControl: self.last_door_pos_time = datetime.datetime.now() - datetime.timedelta(minutes = 10) self.last_position = 0 self.last_handle_state_timestamp = datetime.datetime.now() + self.idle_start_time = None + self.last_command_time = datetime.datetime.now() - datetime.timedelta(COMMAND_IDLE_TIME) + + def idle_time(self): + if self.idle_start_time != None: + return (datetime.datetime.now() - self.idle_start_time).total_seconds() + else: + return 0 def _send_door_cmd(self, cmd: bytes): """Send a command to the door.""" @@ -195,15 +202,23 @@ class DoorControl: return valid - def _set_position(self, data: int): + def _set_position(self, data: str): """Set a new door position""" - self.state_pos(data) - if data > ERROR_THRESHOLD and self.state() != DoorControl.ERROR: + pos, speed = [ int(x) for x in data.strip().split() ] + self.state_pos(pos) + self.speed(speed) + + if self.speed() != 0 and self.idle_start_time != None: + self.idle_start_time = None + elif self.speed() == 0 and self.idle_start_time == None: + self.idle_start_time = datetime.datetime.now() + + if pos > ERROR_THRESHOLD and self.state() != DoorControl.ERROR: self.logger.error("Invalid position:", state) self.state(DoorControl.ERROR) - elif data > OPEN_THRESHOLD and self.state() != DoorControl.OPEN: + elif pos > OPEN_THRESHOLD and self.state() != DoorControl.OPEN: self.state(DoorControl.OPEN) - elif data < CLOSED_THRESHOLD and self.state() != DoorControl.CLOSE: + elif pos < CLOSED_THRESHOLD and self.state() != DoorControl.CLOSE: self.state(DoorControl.CLOSE) def _check_reporting(self, current: int): @@ -218,7 +233,7 @@ class DoorControl: data = self._read_door_line().strip() handling = { - "pos": (int, self._set_position), + "pos": (str, self._set_position), "pos reporting": (int, self._check_reporting), } @@ -231,9 +246,9 @@ class DoorControl: pass def poll_door_state(self): - """Checks the door state if the last polling was at least 5 seconds ago, and returns the current state""" + """Checks the door state if the last polling was a while ago, and returns the current state""" t = (datetime.datetime.now() - self.last_door_pos_time).total_seconds() - if t >= 5: + if t >= 0.5: self.last_door_pos_time = datetime.datetime.now() self._send_door_cmd(b"R") self.handle_door_line() @@ -252,9 +267,6 @@ class DoorControl: old_state = self.last_handled_state delta_t = (now - self.last_handle_state_timestamp).total_seconds() - if 1. / delta_t > MAX_UPDATE_RATE: - return - # If no serial port, try to open it or return if not self.serial_port: try: @@ -262,14 +274,13 @@ class DoorControl: except: return - # Get new state self.poll_door_state() - self.last_handled_state = self.state() - speed = abs(self.state_pos() - self.last_position) / delta_t - speed = 10 * round(speed / 10) - self.speed(speed) - self.last_position = self.state_pos() + if (self.idle_time() < MIN_IDLE_TIME) and delta_t < 5: + return + + # Get new state + self.last_handled_state = self.state() self.last_handle_state_timestamp = now if self.state() == DoorControl.ERROR: @@ -281,70 +292,71 @@ class DoorControl: if self.action() == DoorControl.IDLE: if self.state() != old_state: self.logger.info(f"Door changed unexpectedly: {DoorControl.state_names[self.state()]}") + self.target_state(self.state()) self.start_time = now - if self.start_time and (now - self.start_time).total_seconds() >= self.config.state_timeout: + if self.state() == DoorControl.CLOSE \ + and self.start_time \ + and (now - self.start_time).total_seconds() >= self.config.state_timeout: self.start_time = None if self.state_pos() <= CLOSED_THRESHOLD and self.state_pos() > CLOSED_WANT: self.logger.info("Closing door a bit more") self._send_door_cmd(target_state_cmd[DoorControl.CLOSE]) return - # Target state, next action, timeout action - actions = { - DoorControl.OPEN: (DoorControl.OPEN, DoorControl.IDLE, DoorControl.CLOSE_THEN_OPEN ), - DoorControl.CLOSE: (DoorControl.CLOSE, DoorControl.IDLE, DoorControl.OPEN_THEN_CLOSE ), - DoorControl.OPEN_THEN_CLOSE: (DoorControl.OPEN, DoorControl.CLOSE, DoorControl.CLOSE ), - DoorControl.CLOSE_THEN_OPEN: (DoorControl.CLOSE, DoorControl.OPEN, DoorControl.OPEN ) - } - if self.start_time is None: self.start_time = now - self._send_door_cmd(target_state_cmd[actions[self.action()][0]]) + self._send_door_cmd(target_state_cmd[self.action()]) # Target state reached - if self.state() == actions[self.action()][0]: - # Select next action, reset start time and repetitions - self.action(actions[self.action()][1]) - self.start_time = now + if self.state() == self.target_state(): + self.action(DoorControl.IDLE) + self.logger.debug(f"Reached target position: {DoorControl.state_names[self.state()]}") self.repeats = 0 - # On idle, we're done - if self.action() == DoorControl.IDLE: - self.logger.debug(f"Reached target position: {DoorControl.state_names[self.state()]}") - return - + self.start_time = now + return + elif self.state() != old_state: + # Changed state, try to go to the target state again + self.start_time = now + self.action(old_state) + self._send_door_cmd(target_state_cmd[self.action()]) + # Execution time t = (now - self.start_time).total_seconds() - if t >= self.config.state_timeout or (t >= self.config.state_timeout_speed and self.speed() < MIN_SPEED and delta_t >= 1. / UPDATE_RATE): + if t >= self.config.state_timeout: # Timeout -> switch to timeout action - self.state(actions[self.action()][0], force = True, no_update = True) - self.action(actions[self.action()][2]) + if self.action() == DoorControl.OPEN: + self.action(DoorControl.CLOSE) + else: + self.action(DoorControl.OPEN) self.start_time = None self.repeats = 0 self.logger.debug(f"Timeout. Switching to {DoorControl.action_names[self.action()]}") elif t >= (1 + self.repeats) * self.config.repeat_time: # Repeat every couple of seconds self.repeats += 1 - self.logger.debug(f"Repeating command: {target_state_cmd[actions[self.action()][0]]}") - self._send_door_cmd(target_state_cmd[actions[self.action()][0]]) + self.logger.debug(f"Repeating command: {target_state_cmd[self.action()]}") + self._send_door_cmd(target_state_cmd[self.action()]) def open_door(self): self.logger.info("Opening the door") - self.state_target(DoorControl.OPEN) - self.action(DoorControl.OPEN) - self.start_time = None + self.target_state(DoorControl.OPEN) + if self.action() == DoorControl.IDLE: + self.action(DoorControl.OPEN) + self.start_time = None self.handle_door_state() def close_door(self): self.logger.info("Closing the door") - self.state_target(DoorControl.CLOSE) - self.action(DoorControl.CLOSE) - self.start_time = None + self.target_state(DoorControl.CLOSE) + if self.action() == DoorControl.IDLE: + self.action(DoorControl.CLOSE) + self.start_time = None self.handle_door_state() def toggle_door_state(self): - if not self.action() == DoorControl.IDLE: + if self.action() != DoorControl.IDLE: return - if self.state_target() == DoorControl.CLOSE: + if self.target_state() == DoorControl.CLOSE: self.open_door() else: self.close_door() @@ -434,7 +446,7 @@ def main(): parser.add_argument("--log_file", default="/tmp/nfc.log") parser.add_argument("--state_timeout", type=float, default=10) parser.add_argument("--state_timeout_speed", type=float, default=3) - parser.add_argument("--repeat_time", type=float, default=5) + parser.add_argument("--repeat_time", type=float, default=COMMAND_IDLE_TIME) parser.add_argument("--mqtt_host", default="10.10.21.2") config = parser.parse_args()