openvibers/openvibe.link Commits

Plugin: adeept_adr036 (Adeept 4WD Smart Car Kit for Raspberry Pi)

OP OpenVibers committed 29 Sept 2026 · 71c74f7eb2 · 6 files, +1327 −0

Motors on the Robot HAT's PCA9685 (0x5f, 50 Hz; M1 15/14, M2 12/13,
M3 11/10, M4 8/9, SLOW_DECAY) with the wheel table and directions of the
kit's Move.py, ordinary and mecanum mixing, pan/tilt servos 0/1, ultrasonic,
line sensors, ADS7830 battery, TonalBuzzer and WS2812 over SPI. Fake
hardware backend for tests; the camera is published by the core via
rpicam-vid. Never starts the kit's stock server.

Changes

ddiff --git a/plugins/adeept_adr036/README.md b/plugins/adeept_adr036/README.md
nnew file mode 100644
iindex 0000000..a5df907
---- /dev/null
++++ b/plugins/adeept_adr036/README.md
@@@ -0,0 +1,70 @@
++# openvibe-adeept-adr036
++
++OpenVibe Node plugin for the **Adeept 4WD Smart Car Kit for Raspberry Pi (ADR036)**, ordinary or mecanum wheels,
++on Raspberry Pi OS Bookworm. Built on `openvibe_plugin` (deadlines, heartbeat loss, stdin EOF, e-stop and halt all stop
++the wheels). The mapping was checked against Adeept's `Server/Server_*Wheels/Move.py` and `Voltage.py`; none of the
++kit's code is imported.
++
++## Keep the kit's stock server disabled
++
++The kit's own server listens on `0.0.0.0:8888` with the fixed login `admin` / `123456` and streams MJPEG on `:5000`.
++Anyone on the network could drive the car with it. Disable its autostart (the kit installer adds it to the user's
++crontab or `/etc/rc.local`, depending on version) and do not run it next to the Node. This plugin never starts it.
++
++## Wiring (kit defaults)
++
++| Part | Connection |
++|---|---|
++| Motors (PCA9685 at I2C `0x5f`, 50 Hz) | M1 front-left ch 15/14, M2 rear-left 12/13, M3 rear-right 11/10, M4 front-right 8/9 |
++| Pan / tilt servos | PCA9685 ch 0 / ch 1, 500–2400 µs, 180° |
++| Ultrasonic | echo GPIO 24, trigger GPIO 23 |
++| Line tracking | left GPIO 22, middle 27, right 17 |
++| Battery | ADS7830 at I2C `0x48`, channel 0, 3k/1k divider |
++| Buzzer | GPIO 18 (gpiozero `TonalBuzzer`) |
++| WS2812 LEDs | SPI 0.0 (MOSI, GPIO 10) |
++| Camera | published by the core with `rpicam-vid` |
++
++Enable I2C and SPI (`raspi-config`), then `pip install './plugins/sdk' './plugins/adeept_adr036[pi]'`.
++
++## Config (`hello.config`, deep-merged over the defaults)
++
++| Key | Default | Meaning |
++|---|---|---|
++| `backend` | `"auto"` | `auto` = real on a Raspberry Pi, a `hardware` fault elsewhere; `real`; `fake` (records writes, moves nothing) |
++| `i2c_address` | `0x5f` | PCA9685 address |
++| `pwm_frequency` | `50` | PCA9685 frequency (shared by motors and servos) |
++| `reassert_frequency` | `true` | re-set the frequency before each drive update, as the kit does |
++| `wheels` | `"ordinary"` | `ordinary` (differential) or `mecanum` |
++| `max_throttle` | `1.0` | scales every wheel |
++| `motors.<front_left\|rear_left\|rear_right\|front_right>` | `{"channels": [in1, in2], "direction": -1}` | see wiring; direction -1 = kit forward is negative throttle |
++| `servo_pulse` | `{"min_pulse": 500, "max_pulse": 2400, "actuation_range": 180}` | |
++| `servos.pan` | `{"channel": 0, "min_deg": 10, "max_deg": 170, "center_deg": 90, "invert": false}` | |
++| `servos.tilt` | `{"channel": 1, "min_deg": 30, "max_deg": 150, "center_deg": 90, "invert": false}` | |
++| `ultrasonic` | `{"echo": 24, "trigger": 23, "max_distance_m": 2.0}` | |
++| `line` | `{"left": 22, "middle": 27, "right": 17, "invert": false}` | |
++| `battery` | `{"i2c_bus": 1, "address": 0x48, "channel": 0, "adc_vref": 5.2, "r_top": 3000, "r_bottom": 1000, "full_volts": 8.4, "warning_volts": 6.0, "median_window": 5}` | `low_battery` event once below the warning |
++| `buzzer` | `{"pin": 18, "octaves": 1, "max_seconds": 2.0}` | tones A4 ± octaves; auto-off after `max_seconds` |
++| `lights` | `{"count": 8, "spi_bus": 0, "spi_device": 0, "order": "GRB", "speed_hz": 6400000}` | |
++| `camera` | `{"width": 640, "height": 480, "framerate": 20}` | used in the `h264_command` the core runs |
++
++## Commands
++
++* `drive` (the only motion kind): ordinary `{"throttle", "steer"}` (steer + turns right); mecanum
++ `{"x", "y", "rotation"}` (x forward, y left, rotation counter-clockwise) or throttle/steer.
++* `ptz` `{"pan": -1..1, "tilt": -1..1}`: absolute position, 0 = center, ±1 = the configured limits.
++* `actuator`: `pan`, `tilt` (-1..1), `buzzer` (`{"note": "A4"}`, `{"hz": 440}`, `null`/`0` = off),
++ `lights` (`{"r", "g", "b"[, "index"]}`, `null` = off).
++* `stop` / `estop` / `halt`: wheels to 0 and buzzer off; servos hold their position.
++
++Telemetry (2 Hz): `battery {volts, percent, low}`, `sensors {distance_cm, line {left, middle, right}}` and `faults`
++(sensors that failed to open or read; driving keeps working). A PCA9685 failure is a `hardware` fault at setup.
++
++## Test
++
++```
++python3 -m pytest -q plugins/adeept_adr036/tests
++```
++
++The tests use the fake backend (no Pi needed). To try the plugin by hand:
++`PYTHONPATH=plugins/sdk:plugins/adeept_adr036 python3 -m openvibe_adeept_adr036` and send
++`{"op":"hello","config":{"backend":"fake"}}`. Set `OPENVIBE_FAKE_HW_LOG=/path/file.jsonl` to log every fake write.
ddiff --git a/plugins/adeept_adr036/openvibe_adeept_adr036/__init__.py b/plugins/adeept_adr036/openvibe_adeept_adr036/__init__.py
nnew file mode 100644
iindex 0000000..a232fd8
---- /dev/null
++++ b/plugins/adeept_adr036/openvibe_adeept_adr036/__init__.py
@@@ -0,0 +1,460 @@
++"""Driver for the Adeept 4WD Smart Car Kit for Raspberry Pi (ADR036), ordinary or mecanum wheels.
++
++Hardware (from Adeept's own Move.py / Voltage.py, reimplemented here; none of the kit's code is imported):
++
++* four DC motors on a PCA9685 at I2C 0x5f, 50 Hz, driven as adafruit_motor DCMotors (slow decay);
++* pan/tilt servos on the same PCA9685, channels 0 and 1;
++* HC-SR04 ultrasonic sensor (echo 24, trigger 23), three line-tracking inputs (22, 27, 17);
++* ADS7830 ADC at I2C 0x48 reading the 2x18650 pack through a 3k/1k divider;
++* a tonal buzzer on GPIO 18 and WS2812 LEDs driven over SPI 0.0.
++
++The camera is published by the core (``rpicam-vid``); this plugin only describes the command. The kit's stock server
++(0.0.0.0:8888 with admin/123456, MJPEG on :5000) must stay disabled: it is never started or exposed from here.
++
++All hardware access goes through ``hardware.py`` so the driver runs against a fake in tests (config backend "fake").
++"""
++
++import copy
++import collections
++import statistics
++import time
++
++from openvibe_plugin import Fault, Plugin, clamp
++
++from . import hardware
++
++__version__ = "0.1.0"
++
++MOTOR_NAMES = ("front_left", "rear_left", "rear_right", "front_right")
++
++DEFAULT_CONFIG = {
++ "backend": "auto", # auto (real on a Raspberry Pi, fault elsewhere) | real | fake
++ "i2c_address": 0x5F, # PCA9685
++ "pwm_frequency": 50,
++ "reassert_frequency": True, # the kit re-sets the PCA9685 frequency before each motor update
++ "wheels": "ordinary", # ordinary (differential) | mecanum
++ "max_throttle": 1.0,
++ # Kit M1..M4. direction -1: the kit drives forward with throttle = -speed on every motor.
++ "motors": {
++ "front_left": {"channels": [15, 14], "direction": -1}, # M1
++ "rear_left": {"channels": [12, 13], "direction": -1}, # M2
++ "rear_right": {"channels": [11, 10], "direction": -1}, # M3
++ "front_right": {"channels": [8, 9], "direction": -1}, # M4
++ },
++ "servo_pulse": {"min_pulse": 500, "max_pulse": 2400, "actuation_range": 180},
++ "servos": {
++ "pan": {"channel": 0, "min_deg": 10, "max_deg": 170, "center_deg": 90, "invert": False},
++ "tilt": {"channel": 1, "min_deg": 30, "max_deg": 150, "center_deg": 90, "invert": False},
++ },
++ "ultrasonic": {"echo": 24, "trigger": 23, "max_distance_m": 2.0},
++ "line": {"left": 22, "middle": 27, "right": 17, "invert": False},
++ "battery": {"i2c_bus": 1, "address": 0x48, "channel": 0, "adc_vref": 5.2, "r_top": 3000, "r_bottom": 1000,
++ "full_volts": 8.4, "warning_volts": 6.0, "median_window": 5},
++ "buzzer": {"pin": 18, "octaves": 1, "max_seconds": 2.0},
++ "lights": {"count": 8, "spi_bus": 0, "spi_device": 0, "order": "GRB", "speed_hz": 6400000},
++ "camera": {"width": 640, "height": 480, "framerate": 20},
++}
++
++
++def deep_merge(base, override):
++ """Return base with override merged in; nested dicts merge, everything else replaces."""
++ out = copy.deepcopy(base)
++ for k, v in (override or {}).items():
++ if isinstance(v, dict) and isinstance(out.get(k), dict):
++ out[k] = deep_merge(out[k], v)
++ else:
++ out[k] = copy.deepcopy(v)
++ return out
++
++
++# Pure helpers (tested directly).
++
++def normalise(*ws):
++ """Scale wheel speeds down together so that max |w| <= 1."""
++ m = max(1.0, max(abs(w) for w in ws))
++ return tuple(w / m for w in ws)
++
++
++def mix_differential(throttle, steer):
++ """throttle forward +, steer + turns right. Returns speeds by wheel name (+ = forward)."""
++ left, right = normalise(throttle + steer, throttle - steer)
++ return {"front_left": left, "rear_left": left, "rear_right": right, "front_right": right}
++
++
++def mix_mecanum(x, y, r):
++ """x forward +, y left +, r counter-clockwise +. Returns speeds by wheel name (+ = forward)."""
++ fl, fr, rl, rr = normalise(x - y - r, x + y + r, x + y - r, x - y + r)
++ return {"front_left": fl, "rear_left": rl, "rear_right": rr, "front_right": fr}
++
++
++def servo_angle(v, spec):
++ """Map a normalised position -1..1 onto min..center..max degrees (0 is the center)."""
++ v = clamp(v, -1.0, 1.0)
++ if spec.get("invert"):
++ v = -v
++ lo, c, hi = float(spec["min_deg"]), float(spec["center_deg"]), float(spec["max_deg"])
++ return c + v * (hi - c) if v >= 0 else c + v * (c - lo)
++
++
++def ads7830_command(chn):
++ """ADS7830 single-ended command byte for a channel (as in the kit's ADS7830 code)."""
++ return 0x84 | (((chn << 2 | chn >> 1) & 0x07) << 4)
++
++
++def adc_to_volts(adc, cfg):
++ ratio = float(cfg["r_bottom"]) / (float(cfg["r_top"]) + float(cfg["r_bottom"]))
++ return adc / 255.0 * float(cfg["adc_vref"]) / ratio
++
++
++def battery_percent(volts, cfg):
++ lo, hi = float(cfg["warning_volts"]), float(cfg["full_volts"])
++ return max(0.0, min(100.0, (volts - lo) / (hi - lo) * 100.0))
++
++
++def ws2812_encode(colors, order="GRB"):
++ """SPI bytes for WS2812 LEDs: 8 SPI bits per LED bit at 6.4 MHz, MSB first; a 1 is 0xF8, a 0 is 0x80.
++ ``colors`` is a list of (r, g, b)."""
++ tx = []
++ for r, g, b in colors:
++ c = {"R": r, "G": g, "B": b}
++ for ch in order:
++ d = int(c[ch]) & 0xFF
++ byte = [0] * 8
++ for ibit in range(8):
++ byte[7 - ibit] = ((d >> ibit) & 1) * 0x78 + 0x80
++ tx.extend(byte)
++ return tx
++
++
++_NOTE_SEMITONES = {"C": 0, "D": 2, "E": 4, "F": 5, "G": 7, "A": 9, "B": 11}
++
++
++def note_to_hz(note):
++ """'A4' -> 440.0; accepts sharps (#) and flats (b)."""
++ if not isinstance(note, str) or len(note) < 2 or note[0].upper() not in _NOTE_SEMITONES:
++ raise Fault("bad_value", "bad note %r" % (note,))
++ s = _NOTE_SEMITONES[note[0].upper()]
++ rest = note[1:]
++ if rest[:1] == "#":
++ s, rest = s + 1, rest[1:]
++ elif rest[:1] == "b":
++ s, rest = s - 1, rest[1:]
++ try:
++ octave = int(rest)
++ except ValueError:
++ raise Fault("bad_value", "bad note %r" % (note,))
++ midi = 12 * (octave + 1) + s
++ return 440.0 * 2 ** ((midi - 69) / 12.0)
++
++
++class ADR036(Plugin):
++ driver = "adeept_adr036"
++ version = __version__
++ motion_kinds = frozenset({"drive"})
++ # The loop polls often so the buzzer's max duration is enforced within ~0.1 s; telemetry itself goes out at 2 Hz.
++ telemetry_interval_s = 0.1
++ telemetry_period_s = 0.5
++
++ def __init__(self):
++ self.hardware_factory = hardware.make_hardware # tests replace this
++ self.cfg = copy.deepcopy(DEFAULT_CONFIG)
++ self.hw = None
++ self.motors = {} # name -> (motor, direction)
++ self.servos = {} # name -> (servo, spec)
++ self.distance = None
++ self.line = {}
++ self.adc = None
++ self.buzzer = None
++ self.buzzer_until = None
++ self.spi = None
++ self.leds = []
++ self.faults = {} # component -> error text
++ self.volts_window = collections.deque(maxlen=5)
++ self.low_battery_sent = False
++ self.next_telemetry = 0.0
++
++ def now(self):
++ return self.runtime.clock() if self.runtime is not None else time.monotonic()
++
++ # Capabilities.
++ def describe(self, config):
++ cfg = deep_merge(DEFAULT_CONFIG, config)
++ cam = cfg["camera"]
++ ptz = {}
++ for name in ("pan", "tilt"):
++ s = cfg["servos"][name]
++ ptz[name] = {"min": -1, "max": 1, "min_deg": s["min_deg"], "max_deg": s["max_deg"],
++ "center_deg": s["center_deg"]}
++ return {
++ "drive": {"type": "mecanum" if cfg["wheels"] == "mecanum" else "differential"},
++ "ptz": ptz,
++ "actuator": {"names": ["pan", "tilt", "buzzer", "lights"]},
++ "sensors": ["distance_cm", "line"],
++ "battery": {"full_volts": cfg["battery"]["full_volts"],
++ "warning_volts": cfg["battery"]["warning_volts"]},
++ "lights": {"count": cfg["lights"]["count"]},
++ "camera": {
++ "h264_command": ["rpicam-vid", "-t", "0", "-n", "--codec", "h264", "--inline",
++ "--width", str(cam["width"]), "--height", str(cam["height"]),
++ "--framerate", str(cam["framerate"]), "-o", "-"],
++ "jpeg": False,
++ },
++ }
++
++ # Setup.
++ def setup(self, config):
++ cfg = self.cfg = deep_merge(DEFAULT_CONFIG, config)
++ if cfg["wheels"] not in ("ordinary", "mecanum"):
++ raise Fault("bad_config", "wheels must be ordinary or mecanum")
++ for name in MOTOR_NAMES:
++ if name not in cfg["motors"]:
++ raise Fault("bad_config", "motors.%s missing" % name)
++ try:
++ self.hw = self.hardware_factory(cfg["backend"])
++ except hardware.HardwareError as e:
++ raise Fault("hardware", str(e))
++ if self.hw.name == "fake":
++ self.log("adeept_adr036: FAKE hardware backend, nothing will move")
++
++ # PCA9685, motors and servos: without them there is no robot, so any failure is fatal.
++ try:
++ self.hw.open_pwm(int(cfg["i2c_address"]), int(cfg["pwm_frequency"]))
++ for name in MOTOR_NAMES:
++ m = cfg["motors"][name]
++ in1, in2 = m["channels"]
++ self.motors[name] = (self.hw.motor(name, int(in1), int(in2)), float(m.get("direction", -1)))
++ self.stop_motion()
++ sp = cfg["servo_pulse"]
++ for name in ("pan", "tilt"):
++ s = cfg["servos"][name]
++ self.servos[name] = (self.hw.servo(name, int(s["channel"]), int(sp["min_pulse"]),
++ int(sp["max_pulse"]), int(sp["actuation_range"])), s)
++ self.servos[name][0].angle = float(s["center_deg"])
++ except Exception as e:
++ self.motors, self.servos = {}, {}
++ self._release()
++ if isinstance(e, ImportError):
++ raise Fault("hardware", "missing library: %s (pip install 'openvibe-adeept-adr036[pi]')" % e)
++ raise Fault("hardware", "PCA9685 at 0x%02x: %s" % (int(cfg["i2c_address"]), e))
++
++ # Sensors and extras: a failure is reported in telemetry and driving keeps working.
++ us = cfg["ultrasonic"]
++ self._optional("ultrasonic", lambda: setattr(self, "distance", self.hw.distance_sensor(
++ echo=int(us["echo"]), trigger=int(us["trigger"]), max_distance=float(us["max_distance_m"]))))
++
++ def open_line():
++ self.line = {k: self.hw.input_pin(int(cfg["line"][k])) for k in ("left", "middle", "right")}
++ self._optional("line", open_line)
++ b = cfg["battery"]
++ self.volts_window = collections.deque(maxlen=max(1, int(b["median_window"])))
++ self._optional("battery", lambda: setattr(self, "adc", self.hw.adc(int(b["i2c_bus"]), int(b["address"]))))
++ self._optional("buzzer", lambda: setattr(self, "buzzer", self.hw.buzzer(
++ int(cfg["buzzer"]["pin"]), int(cfg["buzzer"]["octaves"]))))
++ lc = cfg["lights"]
++ self.leds = [(0, 0, 0)] * int(lc["count"])
++
++ def open_lights():
++ self.spi = self.hw.spi(int(lc["spi_bus"]), int(lc["spi_device"]), 0)
++ self._show_leds()
++ self._optional("lights", open_lights)
++ self.log("adeept_adr036: ready (%s wheels, %s backend, faults: %s)", cfg["wheels"], self.hw.name,
++ ", ".join(sorted(self.faults)) or "none")
++
++ def _optional(self, component, fn):
++ try:
++ fn()
++ except Exception as e:
++ self.faults[component] = "init failed: %s" % e
++ self.log("adeept_adr036: %s unavailable: %s", component, e)
++
++ # Commands.
++ def handle(self, cmd):
++ v = cmd.value if isinstance(cmd.value, dict) else {}
++ if cmd.kind == "drive":
++ self._drive(v)
++ elif cmd.kind == "ptz":
++ if "pan" not in v and "tilt" not in v:
++ raise Fault("bad_value", "ptz needs pan and/or tilt")
++ for name in ("pan", "tilt"):
++ if name in v:
++ self._servo(name, v[name])
++ elif cmd.kind == "actuator":
++ name = v.get("name")
++ if name in ("pan", "tilt"):
++ self._servo(name, v.get("value"))
++ elif name == "buzzer":
++ self._buzz(v.get("value"))
++ elif name == "lights":
++ self._lights(v.get("value"))
++ else:
++ raise Fault("unsupported", "no actuator %r" % (name,))
++ else:
++ raise Fault("unsupported", "kind %s not supported" % cmd.kind)
++
++ def _drive(self, v):
++ mt = clamp(self.cfg["max_throttle"], 0.0, 1.0)
++ if self.cfg["wheels"] == "mecanum":
++ if any(k in v for k in ("x", "y", "rotation")):
++ x, y, r = (clamp(v.get(k, 0), -1, 1) for k in ("x", "y", "rotation"))
++ else: # throttle/steer: steer + is a right (clockwise) turn, rotation + is counter-clockwise
++ x, y, r = clamp(v.get("throttle", 0), -1, 1), 0.0, -clamp(v.get("steer", 0), -1, 1)
++ speeds = mix_mecanum(x, y, r)
++ else:
++ if "throttle" in v or "steer" in v:
++ t, s = clamp(v.get("throttle", 0), -1, 1), clamp(v.get("steer", 0), -1, 1)
++ else: # x/rotation on a differential base; y (strafe) cannot be done and is ignored
++ t, s = clamp(v.get("x", 0), -1, 1), -clamp(v.get("rotation", 0), -1, 1)
++ speeds = mix_differential(t, s)
++ if self.cfg["reassert_frequency"]:
++ self.hw.set_pwm_frequency(int(self.cfg["pwm_frequency"]))
++ for name, (m, direction) in self.motors.items():
++ m.throttle = max(-1.0, min(1.0, direction * speeds[name] * mt))
++
++ def _servo(self, name, value):
++ servo, spec = self.servos[name]
++ servo.angle = servo_angle(value, spec)
++
++ def _buzz(self, value):
++ if self.buzzer is None:
++ raise Fault("hardware", "buzzer unavailable: %s" % self.faults.get("buzzer", "not set up"))
++ if not value: # None, 0, {} -> off
++ self._buzzer_off()
++ return
++ if not isinstance(value, dict):
++ raise Fault("bad_value", "buzzer value is {note} or {hz} or null")
++ if "note" in value:
++ hz = note_to_hz(value["note"])
++ elif "hz" in value:
++ hz = clamp(value["hz"], 0, 1e6)
++ else:
++ raise Fault("bad_value", "buzzer value is {note} or {hz} or null")
++ if hz == 0:
++ self._buzzer_off()
++ return
++ # gpiozero's TonalBuzzer plays mid_tone (A4) +/- octaves only.
++ octaves = int(self.cfg["buzzer"]["octaves"])
++ lo, hi = 440.0 / 2 ** octaves, 440.0 * 2 ** octaves
++ if not lo - 0.01 <= hz <= hi + 0.01:
++ raise Fault("bad_value", "tone %.1f Hz outside %.0f..%.0f Hz" % (hz, lo, hi))
++ self.buzzer.play_hz(round(max(lo, min(hi, hz)), 2))
++ self.buzzer_until = self.now() + float(self.cfg["buzzer"]["max_seconds"])
++
++ def _buzzer_off(self):
++ self.buzzer_until = None
++ if self.buzzer is not None:
++ self.buzzer.stop()
++
++ def _lights(self, value):
++ if self.spi is None:
++ raise Fault("hardware", "lights unavailable: %s" % self.faults.get("lights", "not set up"))
++ if not value:
++ rgb, index = (0, 0, 0), None
++ elif isinstance(value, dict):
++ rgb = tuple(int(clamp(value.get(k, 0), 0, 255)) for k in ("r", "g", "b"))
++ index = value.get("index")
++ else:
++ raise Fault("bad_value", "lights value is {r,g,b[,index]} or null")
++ if index is None:
++ self.leds = [rgb] * len(self.leds)
++ else:
++ if isinstance(index, bool) or not isinstance(index, int) or not 0 <= index < len(self.leds):
++ raise Fault("bad_value", "index must be 0..%d" % (len(self.leds) - 1))
++ self.leds[index] = rgb
++ self._show_leds()
++
++ def _show_leds(self):
++ lc = self.cfg["lights"]
++ self.spi.xfer(ws2812_encode(self.leds, lc["order"]), int(lc["speed_hz"]))
++
++ # Stopping.
++ def stop_motion(self):
++ """Wheels to throttle 0 (brake). Every motor is tried even if one write fails."""
++ errors = []
++ for name, (m, _) in self.motors.items():
++ try:
++ m.throttle = 0
++ except Exception as e:
++ errors.append("%s: %s" % (name, e))
++ if errors:
++ raise RuntimeError("motor stop failed: " + "; ".join(errors))
++
++ def stop(self):
++ """Wheels stop, buzzer off. Servos hold their position. Safe before setup and when repeated."""
++ err = None
++ try:
++ self.stop_motion()
++ except Exception as e:
++ err = e
++ try:
++ self._buzzer_off()
++ except Exception as e:
++ err = err or e
++ if err is not None:
++ raise err
++
++ # Telemetry.
++ def poll(self):
++ now = self.now()
++ if self.buzzer_until is not None and now >= self.buzzer_until:
++ try:
++ self._buzzer_off()
++ except Exception as e:
++ self.log("adeept_adr036: buzzer off failed: %r", e)
++ if now < self.next_telemetry:
++ return None
++ self.next_telemetry = now + self.telemetry_period_s
++ out = {}
++ sensors = {}
++ if self.distance is not None:
++ sensors["distance_cm"] = self._read("ultrasonic", lambda: round(self.distance.distance * 100.0, 1))
++ if self.line:
++ inv = bool(self.cfg["line"].get("invert"))
++ sensors["line"] = self._read("line", lambda: {k: bool(d.value) != inv for k, d in self.line.items()})
++ if sensors:
++ out["sensors"] = sensors
++ if self.adc is not None:
++ battery = self._read("battery", self._battery)
++ if battery is not None:
++ out["battery"] = battery
++ out["faults"] = [{"component": k, "error": v} for k, v in sorted(self.faults.items())]
++ return out
++
++ def _read(self, component, fn):
++ try:
++ value = fn()
++ except Exception as e:
++ self.faults[component] = "read failed: %s" % e
++ return None
++ if self.faults.get(component, "").startswith("read failed"):
++ del self.faults[component]
++ return value
++
++ def _battery(self):
++ b = self.cfg["battery"]
++ adc = self.adc.read(ads7830_command(int(b["channel"])))
++ self.volts_window.append(adc_to_volts(adc, b))
++ volts = statistics.median(self.volts_window)
++ warn = float(b["warning_volts"])
++ low = volts < warn
++ if low and not self.low_battery_sent:
++ self.low_battery_sent = True
++ self.emit_event("low_battery", volts=round(volts, 2), warning_volts=warn)
++ elif volts > warn + 0.3: # re-arm after a charge
++ self.low_battery_sent = False
++ return {"volts": round(volts, 2), "percent": int(round(battery_percent(volts, b))), "low": low}
++
++ def close(self):
++ try:
++ if self.spi is not None:
++ self.leds = [(0, 0, 0)] * len(self.leds)
++ self._show_leds()
++ except Exception as e:
++ self.log("adeept_adr036: lights off failed: %r", e)
++ self._release()
++
++ def _release(self):
++ if self.hw is not None:
++ try:
++ self.hw.close()
++ except Exception as e:
++ self.log("adeept_adr036: close failed: %r", e)
ddiff --git a/plugins/adeept_adr036/openvibe_adeept_adr036/__main__.py b/plugins/adeept_adr036/openvibe_adeept_adr036/__main__.py
nnew file mode 100644
iindex 0000000..b38ece3
---- /dev/null
++++ b/plugins/adeept_adr036/openvibe_adeept_adr036/__main__.py
@@@ -0,0 +1,6 @@
++import sys
++
++from openvibe_adeept_adr036 import ADR036
++from openvibe_plugin import run
++
++sys.exit(run(ADR036()))
ddiff --git a/plugins/adeept_adr036/openvibe_adeept_adr036/hardware.py b/plugins/adeept_adr036/openvibe_adeept_adr036/hardware.py
nnew file mode 100644
iindex 0000000..b3a584e
---- /dev/null
++++ b/plugins/adeept_adr036/openvibe_adeept_adr036/hardware.py
@@@ -0,0 +1,333 @@
++"""Hardware access for the Adeept ADR036, behind one small interface with two backends.
++
++* ``RealHardware`` talks to the Raspberry Pi: PCA9685 (motors and servos) over I2C with Adafruit's libraries, gpiozero
++ for the ultrasonic sensor, the line-tracking inputs and the buzzer, smbus2 for the ADS7830 battery ADC and spidev for
++ the WS2812 LEDs. Every import happens inside a method, so this module imports on any machine.
++* ``FakeHardware`` records every write (channel throttles, servo angles, buzzer tones, SPI transfers) and lets tests
++ set the distance, the line inputs and the ADC values. When the environment variable ``OPENVIBE_FAKE_HW_LOG`` names a
++ file, each write is also appended to it as one JSON line, so a test can watch a plugin running as a child process.
++
++Both backends expose the same methods; the driver never imports a hardware library itself.
++"""
++
++import json
++import os
++import threading
++
++DEVICE_TREE_MODEL = "/proc/device-tree/model"
++FAKE_LOG_ENV = "OPENVIBE_FAKE_HW_LOG"
++
++
++def is_raspberry_pi(path=None):
++ """True if the device tree says this is a Raspberry Pi."""
++ try:
++ with open(path or DEVICE_TREE_MODEL, "rb") as f:
++ return b"Raspberry Pi" in f.read()
++ except OSError:
++ return False
++
++
++class HardwareError(Exception):
++ """A backend could not be selected or opened."""
++
++
++def make_hardware(backend, model_path=None):
++ """Return the backend for the config value ``backend`` ("auto", "real" or "fake")."""
++ if backend == "fake":
++ return FakeHardware()
++ if backend == "real":
++ return RealHardware()
++ if backend == "auto":
++ if is_raspberry_pi(model_path):
++ return RealHardware()
++ raise HardwareError("not a Raspberry Pi; set backend=fake to try")
++ raise HardwareError("unknown backend %r (use auto, real or fake)" % (backend,))
++
++
++# Real backend.
++
++class _RealBuzzer:
++ def __init__(self, pin, octaves):
++ from gpiozero import TonalBuzzer
++ from gpiozero.tones import Tone
++ self._tone = Tone
++ self.dev = TonalBuzzer(pin, octaves=octaves)
++
++ def play_hz(self, hz):
++ # An int passed to play() would be read as a MIDI note, so always build the Tone from a frequency.
++ self.dev.play(self._tone(frequency=float(hz)))
++
++ def stop(self):
++ self.dev.stop()
++
++ def close(self):
++ self.dev.close()
++
++
++class _RealAdc:
++ def __init__(self, bus, address):
++ from smbus2 import SMBus
++ self.bus = SMBus(bus)
++ self.address = address
++
++ def read(self, command):
++ return self.bus.read_byte_data(self.address, command)
++
++ def close(self):
++ self.bus.close()
++
++
++class _RealSpi:
++ def __init__(self, bus, device, mode):
++ import spidev
++ self.dev = spidev.SpiDev()
++ self.dev.open(bus, device)
++ self.dev.mode = mode
++
++ def xfer(self, data, speed_hz):
++ self.dev.xfer(list(data), int(speed_hz))
++
++ def close(self):
++ self.dev.close()
++
++
++class RealHardware:
++ name = "real"
++
++ def __init__(self):
++ self.pwm = None
++ self._closables = []
++
++ def open_pwm(self, address, frequency):
++ import board
++ import busio
++ from adafruit_pca9685 import PCA9685
++ i2c = busio.I2C(board.SCL, board.SDA)
++ self.pwm = PCA9685(i2c, address=address)
++ self.pwm.frequency = frequency
++ self._closables.append(self.pwm.deinit)
++
++ def set_pwm_frequency(self, frequency):
++ self.pwm.frequency = frequency
++
++ def motor(self, name, in1, in2):
++ from adafruit_motor import motor
++ m = motor.DCMotor(self.pwm.channels[in1], self.pwm.channels[in2])
++ m.decay_mode = motor.SLOW_DECAY
++ return m # .throttle = -1..1 (0 brakes)
++
++ def servo(self, name, channel, min_pulse, max_pulse, actuation_range):
++ from adafruit_motor import servo
++ return servo.Servo(self.pwm.channels[channel], min_pulse=min_pulse, max_pulse=max_pulse,
++ actuation_range=actuation_range) # .angle = 0..actuation_range
++
++ def distance_sensor(self, echo, trigger, max_distance):
++ from gpiozero import DistanceSensor
++ d = DistanceSensor(echo=echo, trigger=trigger, max_distance=max_distance)
++ self._closables.append(d.close)
++ return d # .distance in metres, 0..max_distance
++
++ def input_pin(self, pin):
++ from gpiozero import InputDevice
++ d = InputDevice(pin)
++ self._closables.append(d.close)
++ return d # .value 0/1
++
++ def adc(self, bus, address):
++ a = _RealAdc(bus, address)
++ self._closables.append(a.close)
++ return a
++
++ def buzzer(self, pin, octaves):
++ b = _RealBuzzer(pin, octaves)
++ self._closables.append(b.close)
++ return b
++
++ def spi(self, bus, device, mode):
++ s = _RealSpi(bus, device, mode)
++ self._closables.append(s.close)
++ return s
++
++ def close(self):
++ while self._closables:
++ try:
++ self._closables.pop()()
++ except Exception:
++ pass
++
++
++# Fake backend.
++
++class _FakeMotor:
++ def __init__(self, hw, name, in1, in2):
++ self.hw, self.name, self.channels = hw, name, (in1, in2)
++ self.decay_mode = "slow"
++ self._throttle = None
++
++ @property
++ def throttle(self):
++ return self._throttle
++
++ @throttle.setter
++ def throttle(self, value):
++ self._throttle = value
++ self.hw.record(dev="motor", name=self.name, channels=list(self.channels), throttle=value)
++
++
++class _FakeServo:
++ def __init__(self, hw, name, channel, min_pulse, max_pulse, actuation_range):
++ self.hw, self.name, self.channel = hw, name, channel
++ self.min_pulse, self.max_pulse, self.actuation_range = min_pulse, max_pulse, actuation_range
++ self._angle = None
++
++ @property
++ def angle(self):
++ return self._angle
++
++ @angle.setter
++ def angle(self, value):
++ if value is not None and not 0 <= value <= self.actuation_range:
++ raise ValueError("angle out of range") # as adafruit_motor.servo does
++ self._angle = value
++ self.hw.record(dev="servo", name=self.name, channel=self.channel, angle=value)
++
++
++class _FakeDistance:
++ def __init__(self, hw, echo, trigger, max_distance):
++ self.hw, self.echo, self.trigger, self.max_distance = hw, echo, trigger, max_distance
++
++ @property
++ def distance(self):
++ return min(self.hw.distance_m, self.max_distance)
++
++
++class _FakeInput:
++ def __init__(self, hw, pin):
++ self.hw, self.pin = hw, pin
++
++ @property
++ def value(self):
++ return int(bool(self.hw.inputs.get(self.pin, 0)))
++
++
++class _FakeAdc:
++ def __init__(self, hw, bus, address):
++ self.hw, self.bus, self.address = hw, bus, address
++
++ def read(self, command):
++ self.hw.adc_reads.append((self.address, command))
++ return self.hw.adc_values.get(command, 0)
++
++
++class _FakeBuzzer:
++ def __init__(self, hw, pin, octaves):
++ self.hw, self.pin, self.octaves = hw, pin, octaves
++
++ def play_hz(self, hz):
++ self.hw.record(dev="buzzer", pin=self.pin, hz=hz)
++
++ def stop(self):
++ self.hw.record(dev="buzzer", pin=self.pin, hz=None)
++
++
++class _FakeSpi:
++ def __init__(self, hw, bus, device, mode):
++ self.hw, self.bus, self.device, self.mode = hw, bus, device, mode
++
++ def xfer(self, data, speed_hz):
++ self.hw.record(dev="spi", bus=self.bus, device=self.device, speed_hz=int(speed_hz), data=list(data))
++
++
++class FakeHardware:
++ """Records what the driver does. Tests read ``writes``/``motors``/``servos``... and set ``distance_m``,
++ ``inputs[pin]`` and ``adc_values[command]``. Put a component name in ``fail`` ("pwm", "distance", "input",
++ "adc", "buzzer", "spi") to make opening it raise."""
++
++ name = "fake"
++
++ def __init__(self, log_path=None):
++ self.log_path = log_path if log_path is not None else os.environ.get(FAKE_LOG_ENV)
++ self._lock = threading.Lock()
++ self.writes = []
++ self.fail = set()
++ self.pwm = None # {"address":..., "frequency":...}
++ self.motors = {} # name -> _FakeMotor
++ self.servos = {}
++ self.distance = None
++ self.inputs_opened = []
++ self.adc_dev = None
++ self.buzzer_dev = None
++ self.spi_dev = None
++ self.closed = False
++ # Inputs a test controls.
++ self.distance_m = 2.0
++ self.inputs = {}
++ self.adc_values = {}
++ self.adc_reads = []
++
++ def record(self, **entry):
++ with self._lock:
++ self.writes.append(entry)
++ if self.log_path:
++ with open(self.log_path, "a") as f:
++ f.write(json.dumps(entry) + "\n")
++
++ def _check(self, what):
++ if what in self.fail:
++ raise OSError("fake %s failure" % what)
++
++ def open_pwm(self, address, frequency):
++ self._check("pwm")
++ self.pwm = {"address": address, "frequency": frequency}
++ self.record(dev="pwm", address=address, frequency=frequency)
++
++ def set_pwm_frequency(self, frequency):
++ self.pwm["frequency"] = frequency
++ self.record(dev="pwm", address=self.pwm["address"], frequency=frequency)
++
++ def motor(self, name, in1, in2):
++ m = self.motors[name] = _FakeMotor(self, name, in1, in2)
++ return m
++
++ def servo(self, name, channel, min_pulse, max_pulse, actuation_range):
++ s = self.servos[name] = _FakeServo(self, name, channel, min_pulse, max_pulse, actuation_range)
++ return s
++
++ def distance_sensor(self, echo, trigger, max_distance):
++ self._check("distance")
++ self.distance = _FakeDistance(self, echo, trigger, max_distance)
++ return self.distance
++
++ def input_pin(self, pin):
++ self._check("input")
++ self.inputs_opened.append(pin)
++ return _FakeInput(self, pin)
++
++ def adc(self, bus, address):
++ self._check("adc")
++ self.adc_dev = _FakeAdc(self, bus, address)
++ return self.adc_dev
++
++ def buzzer(self, pin, octaves):
++ self._check("buzzer")
++ self.buzzer_dev = _FakeBuzzer(self, pin, octaves)
++ return self.buzzer_dev
++
++ def spi(self, bus, device, mode):
++ self._check("spi")
++ self.spi_dev = _FakeSpi(self, bus, device, mode)
++ return self.spi_dev
++
++ def close(self):
++ self.closed = True
++ self.record(dev="close")
++
++ # Helpers for tests.
++ def throttles(self):
++ return {name: m.throttle for name, m in self.motors.items()}
++
++ def last(self, dev):
++ for w in reversed(self.writes):
++ if w.get("dev") == dev:
++ return w
++ return None
ddiff --git a/plugins/adeept_adr036/pyproject.toml b/plugins/adeept_adr036/pyproject.toml
nnew file mode 100644
iindex 0000000..5fba498
---- /dev/null
++++ b/plugins/adeept_adr036/pyproject.toml
@@@ -0,0 +1,23 @@
++[build-system]
++requires = ["setuptools>=61"]
++build-backend = "setuptools.build_meta"
++
++[project]
++name = "openvibe-adeept-adr036"
++version = "0.1.0"
++description = "OpenVibe Node plugin for the Adeept 4WD Smart Car Kit for Raspberry Pi (ADR036)"
++requires-python = ">=3.9"
++license = { text = "AGPL-3.0-or-later" }
++dependencies = ["openvibe-plugin"]
++
++[project.optional-dependencies]
++pi = [
++ "adafruit-circuitpython-pca9685",
++ "adafruit-circuitpython-motor",
++ "gpiozero",
++ "spidev",
++ "smbus2",
++]
++
++[tool.setuptools]
++packages = ["openvibe_adeept_adr036"]
ddiff --git a/plugins/adeept_adr036/tests/test_adr036.py b/plugins/adeept_adr036/tests/test_adr036.py
nnew file mode 100644
iindex 0000000..5abf714
---- /dev/null
++++ b/plugins/adeept_adr036/tests/test_adr036.py
@@@ -0,0 +1,435 @@
++import io
++import json
++import os
++import subprocess
++import sys
++import tempfile
++import unittest
++
++HERE = os.path.dirname(os.path.abspath(__file__))
++PLUGIN_DIR = os.path.dirname(HERE)
++SDK_DIR = os.path.join(os.path.dirname(PLUGIN_DIR), "sdk")
++sys.path.insert(0, PLUGIN_DIR)
++sys.path.insert(0, SDK_DIR)
++
++from openvibe_adeept_adr036 import (ADR036, adc_to_volts, ads7830_command, DEFAULT_CONFIG, mix_mecanum, # noqa: E402
++ note_to_hz, servo_angle, ws2812_encode)
++from openvibe_adeept_adr036 import hardware # noqa: E402
++from openvibe_plugin import Fault, Runtime # noqa: E402
++
++FORWARD = {"front_left": 1, "rear_left": 1, "rear_right": 1, "front_right": 1}
++
++
++class Clock:
++ t = 10.0
++
++ def __call__(self):
++ return self.t
++
++
++class Base(unittest.TestCase):
++ config = {"backend": "fake"}
++ fail = ()
++
++ def setUp(self):
++ self.out = io.StringIO()
++ self.clock = Clock()
++ self.p = ADR036()
++ self.p.log = lambda *a: None
++ fail = set(self.fail)
++
++ def factory(backend):
++ hw = hardware.make_hardware(backend)
++ hw.fail = fail
++ return hw
++ self.p.hardware_factory = factory
++ self.rt = Runtime(self.p, infile=io.StringIO(""), outfile=self.out, clock=self.clock)
++ self.send({"op": "hello", "config": self.config})
++ self.hw = self.p.hw
++
++ def send(self, m):
++ self.rt.feed_line(json.dumps(m))
++
++ def msgs(self):
++ return [json.loads(line) for line in self.out.getvalue().splitlines()]
++
++ def last(self):
++ return self.msgs()[-1]
++
++ def cmd(self, kind, value, id="c", deadline_ms=300):
++ self.send({"op": "command", "id": id, "kind": kind, "value": value, "deadline_ms": deadline_ms})
++ return self.last()
++
++ def advance(self, dt, heartbeat=True):
++ self.clock.t += dt
++ if heartbeat:
++ self.send({"op": "heartbeat", "t": 1})
++ self.rt.tick()
++
++ def speeds(self):
++ """Wheel speeds in the kit's sense (+ = forward): throttle * direction(-1)."""
++ return {k: -v for k, v in self.hw.throttles().items()}
++
++ def assertStopped(self):
++ self.assertEqual(self.hw.throttles(), {"front_left": 0, "rear_left": 0, "rear_right": 0, "front_right": 0})
++
++
++class WiringTest(Base):
++ def test_ready(self):
++ self.assertEqual(self.last(), {"op": "ready"})
++
++ def test_pca9685_and_motor_channels(self):
++ self.assertEqual(self.hw.pwm["address"], 0x5F)
++ self.assertEqual(self.hw.writes[0], {"dev": "pwm", "address": 0x5F, "frequency": 50})
++ ch = {name: m.channels for name, m in self.hw.motors.items()}
++ self.assertEqual(ch, {"front_left": (15, 14), # M1
++ "rear_left": (12, 13), # M2
++ "rear_right": (11, 10), # M3
++ "front_right": (8, 9)}) # M4
++ self.assertStopped()
++
++ def test_servos(self):
++ for name, channel in (("pan", 0), ("tilt", 1)):
++ s = self.hw.servos[name]
++ self.assertEqual((s.channel, s.min_pulse, s.max_pulse, s.actuation_range), (channel, 500, 2400, 180))
++ self.assertEqual(s.angle, 90) # centred at setup
++
++ def test_sensors_buzzer_spi(self):
++ d = self.hw.distance
++ self.assertEqual((d.echo, d.trigger, d.max_distance), (24, 23, 2.0))
++ self.assertEqual(self.hw.inputs_opened, [22, 27, 17])
++ self.assertEqual(self.hw.buzzer_dev.pin, 18)
++ s = self.hw.spi_dev
++ self.assertEqual((s.bus, s.device, s.mode), (0, 0, 0))
++ self.assertEqual(self.hw.adc_dev.address, 0x48)
++ self.assertEqual(self.hw.adc_dev.bus, 1)
++ self.assertEqual(ads7830_command(0), 0x84)
++ self.advance(0.1)
++ self.assertEqual(self.hw.adc_reads[-1], (0x48, 0x84))
++
++ def test_describe(self):
++ d = self.msgs()[0]
++ self.assertEqual(d["op"], "describe")
++ self.assertEqual(d["driver"], "adeept_adr036")
++ self.assertEqual(d["motion_kinds"], ["drive"])
++ caps = d["capabilities"]
++ self.assertEqual(caps["drive"]["type"], "differential")
++ self.assertEqual(caps["sensors"], ["distance_cm", "line"])
++ self.assertEqual(caps["lights"]["count"], 8)
++ self.assertEqual(caps["camera"]["h264_command"][:3], ["rpicam-vid", "-t", "0"])
++ self.assertIn("640", caps["camera"]["h264_command"])
++ self.assertFalse(caps["camera"]["jpeg"])
++
++
++class DifferentialTest(Base):
++ def test_forward_is_negative_throttle(self):
++ self.assertEqual(self.cmd("drive", {"throttle": 0.5, "steer": 0})["op"], "ack")
++ for name, t in self.hw.throttles().items():
++ self.assertLess(t, 0, name)
++ self.assertAlmostEqual(t, -0.5)
++ self.assertEqual(self.hw.writes[-5], {"dev": "pwm", "address": 0x5F, "frequency": 50}) # re-asserted
++
++ def test_rotate_left_matches_kit(self):
++ # Kit 'rotate-left': M1, M2 backward; M3, M4 forward.
++ self.cmd("drive", {"throttle": 0, "steer": -1})
++ self.assertEqual(self.speeds(), {"front_left": -1, "rear_left": -1, "rear_right": 1, "front_right": 1})
++
++ def test_steer_right_normalised(self):
++ self.cmd("drive", {"throttle": 1, "steer": 1})
++ self.assertEqual(self.speeds(), {"front_left": 1, "rear_left": 1, "rear_right": 0, "front_right": 0})
++
++ def test_bad_value_and_clamp(self):
++ self.assertEqual(self.cmd("drive", {"throttle": "fast"})["fault_code"], "bad_value")
++ self.cmd("drive", {"throttle": 5})
++ self.assertEqual(self.speeds(), FORWARD)
++
++ def test_unsupported(self):
++ for kind, value in (("say", {"text": "hi"}), ("display", {"text": "hi"}),
++ ("actuator", {"name": "lift", "value": 1})):
++ self.assertEqual(self.cmd(kind, value)["fault_code"], "unsupported", kind)
++
++
++class MaxThrottleTest(Base):
++ config = {"backend": "fake", "max_throttle": 0.5}
++
++ def test_scaled(self):
++ self.cmd("drive", {"throttle": 1})
++ self.assertEqual(set(self.speeds().values()), {0.5})
++
++
++class MecanumTest(Base):
++ config = {"backend": "fake", "wheels": "mecanum"}
++
++ def test_kit_cases(self):
++ # (value, kit M1 front_left, M2 rear_left, M3 rear_right, M4 front_right) from Server_MecanumWheels/Move.py.
++ cases = [
++ ({"x": 1}, (1, 1, 1, 1)), # forward
++ ({"y": 1}, (-1, 1, -1, 1)), # strafe left: M1 back, M2 fwd, M3 back, M4 fwd
++ ({"y": -1}, (1, -1, 1, -1)), # strafe right
++ ({"x": 1, "y": 1}, (0, 1, 0, 1)), # forward-left: M1 0, M2 fwd, M3 0, M4 fwd
++ ({"x": 1, "y": -1}, (1, 0, 1, 0)), # forward-right
++ ({"rotation": 1}, (-1, -1, 1, 1)), # rotate-left: M1, M2 back, M3, M4 fwd
++ ]
++ for value, want in cases:
++ self.assertEqual(self.cmd("drive", value)["op"], "ack")
++ got = self.speeds()
++ self.assertEqual((got["front_left"], got["rear_left"], got["rear_right"], got["front_right"]),
++ want, value)
++ self.assertEqual(self.msgs()[0]["capabilities"]["drive"]["type"], "mecanum")
++
++ def test_pure_mix_normalised(self):
++ w = mix_mecanum(1, 1, 1)
++ self.assertEqual(max(abs(x) for x in w.values()), 1.0)
++
++ def test_throttle_steer_on_mecanum(self):
++ self.cmd("drive", {"throttle": 0, "steer": -1}) # left turn == counter-clockwise rotation
++ self.assertEqual(self.speeds(), {"front_left": -1, "rear_left": -1, "rear_right": 1, "front_right": 1})
++
++
++class SafetyTest(Base):
++ def test_deadline_stop(self):
++ self.cmd("drive", {"throttle": 1}, deadline_ms=300)
++ self.assertEqual(self.speeds(), FORWARD)
++ self.advance(0.29)
++ self.assertEqual(self.speeds(), FORWARD)
++ self.advance(0.02)
++ self.assertStopped()
++ self.assertIn("deadline", self.rt.stop_reasons)
++
++ def test_heartbeat_loss(self):
++ self.cmd("drive", {"throttle": 1}, deadline_ms=5000)
++ self.advance(1.05, heartbeat=False)
++ self.assertStopped()
++ self.assertTrue(any(m.get("name") == "heartbeat_lost" for m in self.msgs()))
++ self.assertEqual(self.cmd("drive", {"throttle": 1})["fault_code"], "no_heartbeat")
++ self.assertStopped()
++
++ def test_estop_until_resume(self):
++ self.cmd("drive", {"throttle": 1}, deadline_ms=5000)
++ self.send({"op": "estop"})
++ self.assertStopped()
++ self.assertEqual(self.cmd("drive", {"throttle": 1})["fault_code"], "estopped")
++ self.assertEqual(self.cmd("actuator", {"name": "buzzer", "value": {"hz": 440}})["fault_code"], "estopped")
++ self.assertStopped()
++ self.send({"op": "resume"})
++ self.assertEqual(self.cmd("drive", {"throttle": 1})["op"], "ack")
++ self.assertEqual(self.speeds(), FORWARD)
++
++ def test_halt_and_stop_kill_buzzer(self):
++ self.cmd("drive", {"throttle": 1}, deadline_ms=5000)
++ self.cmd("actuator", {"name": "buzzer", "value": {"note": "A4"}})
++ self.assertEqual(self.hw.last("buzzer")["hz"], 440.0)
++ self.assertEqual(self.cmd("halt", {}), {"op": "ack", "id": "c"})
++ self.assertStopped()
++ self.assertIsNone(self.hw.last("buzzer")["hz"])
++
++ def test_deadline_keeps_buzzer(self):
++ self.cmd("actuator", {"name": "buzzer", "value": {"hz": 523.25}})
++ self.cmd("drive", {"throttle": 1})
++ self.advance(0.4)
++ self.assertStopped()
++ self.assertEqual(self.hw.last("buzzer")["hz"], 523.25)
++
++ def test_buzzer_max_duration(self):
++ self.cmd("actuator", {"name": "buzzer", "value": {"hz": 440}})
++ self.advance(1.9)
++ self.assertEqual(self.hw.last("buzzer")["hz"], 440)
++ self.advance(0.15)
++ self.assertIsNone(self.hw.last("buzzer")["hz"])
++
++ def test_buzzer_values(self):
++ self.cmd("actuator", {"name": "buzzer", "value": {"hz": 440}})
++ self.cmd("actuator", {"name": "buzzer", "value": None})
++ self.assertIsNone(self.hw.last("buzzer")["hz"])
++ self.assertEqual(self.cmd("actuator", {"name": "buzzer", "value": {"hz": 5000}})["fault_code"], "bad_value")
++ self.assertEqual(self.cmd("actuator", {"name": "buzzer", "value": {"note": "H2"}})["fault_code"],
++ "bad_value")
++ self.assertAlmostEqual(note_to_hz("C5"), 523.25, places=2)
++ self.assertAlmostEqual(note_to_hz("Bb4"), note_to_hz("A#4"))
++
++ def test_stop_before_setup_and_idempotent(self):
++ p = ADR036()
++ p.stop()
++ p.stop()
++ p.close()
++ self.p.stop()
++ self.p.stop()
++ self.assertStopped()
++
++
++class ServoTest(Base):
++ def test_mapping(self):
++ pan, tilt = DEFAULT_CONFIG["servos"]["pan"], DEFAULT_CONFIG["servos"]["tilt"]
++ self.assertEqual(servo_angle(0, pan), 90)
++ self.assertEqual(servo_angle(1, pan), 170)
++ self.assertEqual(servo_angle(-1, pan), 10)
++ self.assertEqual(servo_angle(5, pan), 170)
++ self.assertEqual(servo_angle(-5, tilt), 30)
++ self.assertEqual(servo_angle(0.5, tilt), 120)
++ self.assertRaises(Fault, servo_angle, "up", pan)
++
++ def test_ptz_and_actuator(self):
++ self.assertEqual(self.cmd("ptz", {"pan": 1, "tilt": -1})["op"], "ack")
++ self.assertEqual((self.hw.servos["pan"].angle, self.hw.servos["tilt"].angle), (170, 30))
++ self.cmd("actuator", {"name": "pan", "value": -1})
++ self.assertEqual(self.hw.servos["pan"].angle, 10)
++ self.cmd("actuator", {"name": "tilt", "value": 0})
++ self.assertEqual(self.hw.servos["tilt"].angle, 90)
++ self.assertEqual(self.cmd("ptz", {})["fault_code"], "bad_value")
++
++ def test_servos_hold_on_stop(self):
++ self.cmd("ptz", {"pan": 1})
++ self.send({"op": "stop"})
++ self.assertEqual(self.hw.servos["pan"].angle, 170)
++
++
++class BatteryTest(Base):
++ def test_conversion(self):
++ b = DEFAULT_CONFIG["battery"]
++ self.assertAlmostEqual(adc_to_volts(200, b), 200 / 255 * 5.2 / 0.25)
++ self.assertAlmostEqual(adc_to_volts(255, b), 20.8)
++ self.hw.adc_values[0x84] = 91 # 91/255*5.2*4 = 7.42 V
++ self.advance(0.1)
++ t = self.last()
++ self.assertEqual(t["op"], "telemetry")
++ self.assertEqual(t["battery"], {"volts": 7.42, "percent": 59, "low": False})
++
++ def test_low_battery_once_with_median(self):
++ self.hw.adc_values[0x84] = 91
++ self.advance(0.1)
++ self.hw.adc_values[0x84] = 70 # 5.71 V
++ for _ in range(8):
++ self.advance(0.5)
++ events = [m for m in self.msgs() if m.get("op") == "event" and m["name"] == "low_battery"]
++ self.assertEqual(len(events), 1)
++ self.assertEqual(events[0]["volts"], 5.71)
++ tel = [m for m in self.msgs() if m.get("op") == "telemetry"]
++ self.assertEqual(tel[1]["battery"]["volts"], 6.57) # median of 7.42 and 5.71: one bad reading is smoothed
++ self.assertEqual(tel[-1]["battery"]["percent"], 0)
++
++ def test_sensors_telemetry(self):
++ self.hw.distance_m = 0.345
++ self.hw.inputs[27] = 1
++ self.advance(0.1)
++ t = self.last()
++ self.assertEqual(t["sensors"], {"distance_cm": 34.5, "line": {"left": False, "middle": True, "right": False}})
++ self.assertEqual(t["faults"], [])
++
++
++class SensorFailureTest(Base):
++ fail = ("distance", "adc", "spi")
++
++ def test_drive_still_works(self):
++ self.assertEqual(self.msgs()[1], {"op": "ready"})
++ self.assertEqual(self.cmd("drive", {"throttle": 1})["op"], "ack")
++ self.assertEqual(self.speeds(), FORWARD)
++ self.assertEqual(self.cmd("actuator", {"name": "lights", "value": {"r": 1}})["fault_code"], "hardware")
++ self.advance(0.1)
++ t = self.last()
++ self.assertEqual([f["component"] for f in t["faults"]], ["battery", "lights", "ultrasonic"])
++ self.assertIn("line", t["sensors"])
++
++
++class PcaFailureTest(Base):
++ fail = ("pwm",)
++
++ def test_fault(self):
++ m = self.last()
++ self.assertEqual((m["op"], m["fault_code"]), ("fault", "hardware"))
++ self.assertIn("0x5f", m["message"])
++ self.assertEqual(self.cmd("drive", {"throttle": 1})["fault_code"], "not_ready")
++
++
++class LightsTest(Base):
++ def test_encoding(self):
++ # Pure red: GRB order -> G=0x00, R=0xFF, B=0x00; bit 1 = 0xF8, bit 0 = 0x80, MSB first.
++ self.assertEqual(ws2812_encode([(255, 0, 0)]), [0x80] * 8 + [0xF8] * 8 + [0x80] * 8)
++ # 0xA5 in green: 1010 0101.
++ self.assertEqual(ws2812_encode([(0, 0xA5, 0)])[:8], [0xF8, 0x80, 0xF8, 0x80, 0x80, 0xF8, 0x80, 0xF8])
++
++ def test_command(self):
++ self.assertEqual(self.hw.last("spi")["data"], [0x80] * 8 * 24) # all off at setup
++ self.cmd("actuator", {"name": "lights", "value": {"r": 0, "g": 0, "b": 255, "index": 2}})
++ w = self.hw.last("spi")
++ self.assertEqual(w["speed_hz"], 6400000)
++ self.assertEqual(len(w["data"]), 8 * 24)
++ self.assertEqual(w["data"][2 * 24:3 * 24], [0x80] * 16 + [0xF8] * 8)
++ self.assertEqual(w["data"][:24], [0x80] * 24)
++ self.assertEqual(self.cmd("actuator", {"name": "lights", "value": {"r": 1, "index": 8}})["fault_code"],
++ "bad_value")
++
++
++class ProbeTest(unittest.TestCase):
++ def test_probe_touches_no_hardware(self):
++ p = ADR036()
++ p.log = lambda *a: None
++
++ def boom(backend):
++ raise AssertionError("hardware opened in probe mode")
++ p.hardware_factory = boom
++ out = io.StringIO()
++ rt = Runtime(p, infile=io.StringIO(""), outfile=out, clock=Clock())
++ rt.feed_line(json.dumps({"op": "hello", "probe": True, "config": {"backend": "real", "wheels": "mecanum"}}))
++ msgs = [json.loads(line) for line in out.getvalue().splitlines()]
++ self.assertEqual([m["op"] for m in msgs], ["describe"])
++ self.assertEqual(msgs[0]["capabilities"]["drive"]["type"], "mecanum")
++ rt.shutdown()
++ self.assertIsNone(p.hw)
++
++
++class AutoBackendTest(unittest.TestCase):
++ def test_auto_off_pi_faults(self):
++ with tempfile.NamedTemporaryFile("wb", delete=False) as f:
++ f.write(b"Generic x86 PC\0")
++ try:
++ with self.assertRaises(hardware.HardwareError) as e:
++ hardware.make_hardware("auto", model_path=f.name)
++ self.assertIn("backend=fake", str(e.exception))
++ self.assertFalse(hardware.is_raspberry_pi("/nonexistent"))
++ with open(f.name, "wb") as g:
++ g.write(b"Raspberry Pi 5 Model B Rev 1.0\0")
++ self.assertTrue(hardware.is_raspberry_pi(f.name))
++ finally:
++ os.unlink(f.name)
++
++ def test_auto_setup_fault_on_this_machine(self):
++ if hardware.is_raspberry_pi():
++ self.skipTest("running on a Raspberry Pi")
++ p = ADR036()
++ p.log = lambda *a: None
++ out = io.StringIO()
++ rt = Runtime(p, infile=io.StringIO(""), outfile=out, clock=Clock())
++ rt.feed_line(json.dumps({"op": "hello", "config": {}}))
++ m = json.loads(out.getvalue().splitlines()[-1])
++ self.assertEqual((m["op"], m["fault_code"]), ("fault", "hardware"))
++ self.assertIn("not a Raspberry Pi", m["message"])
++
++
++class ChildProcessTest(unittest.TestCase):
++ def test_stdin_eof_stops_motors(self):
++ with tempfile.TemporaryDirectory() as d:
++ log = os.path.join(d, "hw.jsonl")
++ env = dict(os.environ, PYTHONPATH=os.pathsep.join([SDK_DIR, PLUGIN_DIR]))
++ env[hardware.FAKE_LOG_ENV] = log
++ inp = "\n".join(json.dumps(m) for m in [
++ {"op": "hello", "config": {"backend": "fake"}},
++ {"op": "command", "id": "d1", "kind": "drive", "value": {"throttle": 0.8}, "deadline_ms": 5000},
++ ]) + "\n"
++ p = subprocess.run([sys.executable, "-m", "openvibe_adeept_adr036"], input=inp, env=env, cwd=d,
++ capture_output=True, text=True, timeout=20)
++ self.assertEqual(p.returncode, 0, p.stderr)
++ out = [json.loads(line) for line in p.stdout.splitlines()]
++ self.assertIn({"op": "ack", "id": "d1"}, out)
++ with open(log) as f:
++ writes = [json.loads(line) for line in f]
++ motor = [w for w in writes if w["dev"] == "motor"]
++ self.assertIn(-0.8, [w["throttle"] for w in motor])
++ last = {}
++ for w in motor:
++ last[tuple(w["channels"])] = w["throttle"]
++ self.assertEqual(last, {(15, 14): 0, (12, 13): 0, (11, 10): 0, (8, 9): 0})
++ self.assertEqual(writes[-1]["dev"], "close")
++
++
++if __name__ == "__main__":
++ unittest.main()