#!/usr/bin/env python3 """Counted, absolute-angle pan/tilt control for NanoPi NEO2. Importing this module does not touch hardware. Use ``with PanTilt() as head`` to return to center/front on exit, including exceptions and Ctrl+C. """ import atexit import fcntl import json import math import os import signal import sys import threading import time from pathlib import Path DEFAULT_CONFIG = { "enable_pin": 18, "pan": {"step_pin": 11, "dir_pin": 13, "positive_dir": 1, "steps_at_max": 3000, "max_degree": 90, "min_degree": -90}, "tilt": {"step_pin": 15, "dir_pin": 16, "positive_dir": 1, "steps_at_max": 5000, "max_degree": 90, "min_degree": 0}, } class UnknownPositionError(RuntimeError): """An interrupted/unclean session requires manual home calibration.""" class PanTilt: """Angles: pan +right/-left, tilt +up; speed is requested STEP Hz. Pan: -90..+90 degrees = -3000..+3000 steps. Tilt: 0..90 degrees = 0..5000 steps (top angle is configurable). ``positive_dir`` must match the actual wiring for each axis. These are open-loop counts. """ def __init__(self, config_path=None, state_path=None, home_speed=1000, assume_home=False, handle_signals=True, gpio=None, acceleration=50000): self._mutex = threading.RLock() self._closed = True self._closing = False self._in_motion = False self._cancel_signal = None self._handlers = {} self._initialized = False self._enabled = False self._position_known = True self._session_open = False self._steps = {"pan": 0, "tilt": 0} self._lock_file = None self._gpio = gpio self.home_speed = self._speed(home_speed) self.acceleration = self._number(acceleration, "acceleration") if self.acceleration <= 0: raise ValueError("acceleration must be positive") base = Path(__file__).resolve().parent cfg_path = Path(config_path) if config_path else base / "pan_tilt_config.json" if cfg_path.exists(): self.config = json.loads(cfg_path.read_text()) elif config_path: raise FileNotFoundError(cfg_path) else: self.config = json.loads(json.dumps(DEFAULT_CONFIG)) self._validate_config() self._pins = [self.config["enable_pin"]] for axis in ("pan", "tilt"): self._pins += [self.config[axis]["step_pin"], self.config[axis]["dir_pin"]] self.state_path = Path(state_path) if state_path else base / "pan_tilt_state.json" self.state_path.parent.mkdir(parents=True, exist_ok=True) try: self._lock_file = open(str(self.state_path) + ".lock", "a") try: fcntl.flock(self._lock_file, fcntl.LOCK_EX | fcntl.LOCK_NB) except BlockingIOError: raise RuntimeError("Another PanTilt controller already owns this state file") if self.state_path.exists() and not assume_home: state = json.loads(self.state_path.read_text()) if (state.get("version") != 1 or state.get("session_open", True) or not state.get("position_known", False)): raise UnknownPositionError( "Previous shutdown was unclean: manually place pan at center and " "tilt at front, then start with assume_home=True / --assume-home") if state.get("config") != self.config: raise UnknownPositionError("Calibration changed; physically home and use assume_home=True") for axis in ("pan", "tilt"): value = state["steps"][axis] if type(value) is not int: raise UnknownPositionError("Saved step count is invalid") self._target_steps(axis, value * self._degrees_per_step(axis)) self._steps[axis] = value if self._gpio is None: import NPi.GPIO as GPIO self._gpio = GPIO g = self._gpio g.setmode(g.BOARD) g.setwarnings(False) g.setup(self.config["enable_pin"], g.OUT, initial=g.HIGH) for axis in ("pan", "tilt"): g.setup(self.config[axis]["step_pin"], g.OUT, initial=g.LOW) g.setup(self.config[axis]["dir_pin"], g.OUT, initial=g.LOW) self._initialized = True self._session_open = True self._save() self._closed = False # Keep both axes held: the drivers share one active-low EN pin. g.output(self.config["enable_pin"], g.LOW) self._enabled = True if handle_signals and threading.current_thread() is threading.main_thread(): for sig in (signal.SIGINT, signal.SIGTERM, signal.SIGHUP): self._handlers[sig] = signal.getsignal(sig) signal.signal(sig, self._signal_handler) atexit.register(self._exit_cleanup) except BaseException: if self._initialized: self._gpio.output(self.config["enable_pin"], self._gpio.HIGH) self._cleanup_pins() self._release_lock() raise @staticmethod def _number(value, name): if isinstance(value, bool): raise ValueError(f"{name} must be a finite number") number = float(value) if not math.isfinite(number): raise ValueError(f"{name} must be a finite number") return number @classmethod def _speed(cls, value): speed = cls._number(value, "speed") if not 1000 <= speed <= 10000: raise ValueError("speed must be between 1000 and 10000 Hz") return speed def _validate_config(self): pins = [self.config["enable_pin"]] for name in ("pan", "tilt"): axis = self.config[name] pins += [axis["step_pin"], axis["dir_pin"]] if type(axis["positive_dir"]) is not int or axis["positive_dir"] not in (0, 1): raise ValueError(f"{name}.positive_dir must be 0 or 1") if type(axis["steps_at_max"]) is not int or axis["steps_at_max"] <= 0: raise ValueError(f"{name}.steps_at_max must be a positive integer") maximum = self._number(axis["max_degree"], f"{name}.max_degree") minimum = self._number(axis["min_degree"], f"{name}.min_degree") if not minimum <= 0 < maximum: raise ValueError(f"{name} limits must include home (0 degrees)") if any(type(p) is not int or p <= 0 for p in pins) or len(set(pins)) != len(pins): raise ValueError("EN, STEP and DIR pins must be distinct physical BOARD pins") def _degrees_per_step(self, axis): cfg = self.config[axis] return cfg["max_degree"] / cfg["steps_at_max"] def _target_steps(self, axis, degree): degree = self._number(degree, "degree") cfg = self.config[axis] if not cfg["min_degree"] <= degree <= cfg["max_degree"]: raise ValueError(f"{axis} degree must be {cfg['min_degree']}..{cfg['max_degree']}") return round(degree / self._degrees_per_step(axis)) @property def position(self): with self._mutex: return {axis: {"steps": self._steps[axis], "degree": self._steps[axis] * self._degrees_per_step(axis)} for axis in ("pan", "tilt")} def _save(self): state = {"version": 1, "steps": self._steps.copy(), "config": self.config, "session_open": self._session_open, "position_known": self._position_known, "moving": self._in_motion} temporary = str(self.state_path) + f".tmp.{os.getpid()}" with open(temporary, "w") as stream: json.dump(state, stream, indent=2) stream.flush() os.fsync(stream.fileno()) os.replace(temporary, self.state_path) @staticmethod def _wait_until(deadline): # Short pulses use a monotonic busy wait; ordinary time.sleep alone # cannot accurately produce 10 kHz GPIO pulses on Linux. remaining = deadline - time.perf_counter() if remaining > 0.0004: time.sleep(remaining - 0.0002) while time.perf_counter() < deadline: pass def _move_axis(self, axis, target, speed): if self._closed: raise RuntimeError("Controller is closed") if not self._position_known: raise UnknownPositionError("Position unknown; movement is disabled") delta = target - self._steps[axis] if not delta: return sign = 1 if delta > 0 else -1 cfg = self.config[axis] g = self._gpio direction = cfg["positive_dir"] if sign > 0 else 1 - cfg["positive_dir"] count = abs(delta) self._in_motion = True try: self._save() # An unclean termination is detectable on next startup. g.output(cfg["dir_pin"], g.HIGH if direction else g.LOW) time.sleep(0.00002) # DIR setup before the first rising STEP edge. for index in range(count): if self._cancel_signal is not None and not self._closing: break ramp_speed = math.sqrt(1000 ** 2 + 2 * self.acceleration * min(index, count - 1 - index)) half_period = 0.5 / min(speed, ramp_speed) # Count the actual commanded rising edge, including partial moves. g.output(cfg["step_pin"], g.HIGH) self._steps[axis] += sign self._wait_until(time.perf_counter() + half_period) g.output(cfg["step_pin"], g.LOW) self._wait_until(time.perf_counter() + half_period) except BaseException: # A GPIO failure or arbitrary interruption may leave an ambiguous # edge; never attempt blind homing from an unknown position. self._position_known = False raise finally: g.output(cfg["step_pin"], g.LOW) self._in_motion = False self._save() if self._cancel_signal is not None and not self._closing: sig = self._cancel_signal self.close() if sig == signal.SIGINT: raise KeyboardInterrupt raise SystemExit(128 + sig) def pan(self, degree, speed=3000): """Move to an absolute pan angle: -left, 0 center, +right.""" with self._mutex: target, hz = self._target_steps("pan", degree), self._speed(speed) self._move_axis("pan", target, hz) return self.position def tilt(self, degree, speed=3000): """Move to an absolute tilt angle: 0 front, positive upward.""" with self._mutex: target, hz = self._target_steps("tilt", degree), self._speed(speed) self._move_axis("tilt", target, hz) return self.position def move(self, pan=None, tilt=None, pan_speed=3000, tilt_speed=3000): """Move pan, then tilt; validate both requests before any motion.""" with self._mutex: requests = [] for axis, degree, speed in (("pan", pan, pan_speed), ("tilt", tilt, tilt_speed)): if degree is not None: requests.append((axis, self._target_steps(axis, degree), self._speed(speed))) for axis, target, speed in requests: self._move_axis(axis, target, speed) return self.position def home(self, speed=None): """Return tilt to front first, then pan to center.""" with self._mutex: hz = self.home_speed if speed is None else self._speed(speed) self._move_axis("tilt", 0, hz) self._move_axis("pan", 0, hz) return self.position def _signal_handler(self, sig, frame): self._cancel_signal = sig # Defer shutdown until the current pulse has been lowered and counted. if self._in_motion or self._closing: return self.close() if sig == signal.SIGINT: raise KeyboardInterrupt raise SystemExit(128 + sig) def _release_lock(self): if self._lock_file is not None: self._lock_file.close() self._lock_file = None def _cleanup_pins(self): # NPi.GPIO accepts individual channels, unlike RPi.GPIO's list form. for pin in self._pins: self._gpio.cleanup(pin) def close(self): """Home, save zero counts, disable both drivers and release own pins.""" with self._mutex: if self._closed: return self._closing = True homed = False try: self.home() homed = True finally: try: for axis in ("pan", "tilt"): self._gpio.output(self.config[axis]["step_pin"], self._gpio.LOW) self._gpio.output(self.config["enable_pin"], self._gpio.HIGH) self._enabled = False self._session_open = not homed self._save() finally: try: self._cleanup_pins() finally: self._closed = True self._closing = False self._release_lock() if threading.current_thread() is threading.main_thread(): for sig, handler in self._handlers.items(): signal.signal(sig, handler) self._handlers.clear() atexit.unregister(self._exit_cleanup) def _exit_cleanup(self): try: self.close() except Exception as error: print(f"PanTilt could not home during exit: {error}", file=sys.stderr) def __enter__(self): return self def __exit__(self, exc_type, exc, traceback): self.close() return False