openvibers/openvibe.link Commits
Plugin: adeept_adr036 (Adeept 4WD Smart Car Kit for Raspberry Pi)
Changes
nnew file mode 100644@@@ -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.nnew file mode 100644@@@ -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)nnew file mode 100644@@@ -0,0 +1,6 @@++import sys++++from openvibe_adeept_adr036 import ADR036++from openvibe_plugin import run++++sys.exit(run(ADR036()))nnew file mode 100644@@@ -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 Nonennew file mode 100644@@@ -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"]nnew file mode 100644@@@ -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()
new file mode 100644
new file mode 100644
@@ -0,0 +1,70 @@
@@ -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.
new file mode 100644
new file mode 100644
@@ -0,0 +1,460 @@
@@ -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)
new file mode 100644
new file mode 100644
@@ -0,0 +1,6 @@
@@ -0,0 +1,6 @@
+import sys
+
+from openvibe_adeept_adr036 import ADR036
+from openvibe_plugin import run
+
+sys.exit(run(ADR036()))
new file mode 100644
new file mode 100644
@@ -0,0 +1,333 @@
@@ -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
new file mode 100644
new file mode 100644
@@ -0,0 +1,23 @@
@@ -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"]
new file mode 100644
new file mode 100644
@@ -0,0 +1,435 @@
@@ -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()