315 lines
9.5 KiB
Python
315 lines
9.5 KiB
Python
"""
|
||
Vision sensor helper for the linefollower ESP32-S3 "eye" (I2C slave).
|
||
|
||
Typical wiring (Raspberry Pi Pico):
|
||
Pico GP0 (SDA) -> ESP GPIO19 (SDA)
|
||
Pico GP1 (SCL) -> ESP GPIO20 (SCL)
|
||
GND -> GND
|
||
External 2k2–4k7 pull-ups on SDA/SCL to 3V3 recommended
|
||
|
||
Example:
|
||
from vision import Vision
|
||
import time
|
||
|
||
eye = Vision(sda=0, scl=1)
|
||
eye.start() # camera on + local detection
|
||
# eye.set_wifi("MyNetwork", "secret") # optional: browser UI at ESP IP
|
||
# eye.wifi_off()
|
||
|
||
while True:
|
||
target = eye.read_target()
|
||
if target is None:
|
||
print("no target")
|
||
else:
|
||
x, y, diameter = target # offset from image centre (0,0)
|
||
print("target at", x, y, "diameter", diameter)
|
||
time.sleep(0.1)
|
||
"""
|
||
|
||
from machine import I2C, Pin
|
||
import struct
|
||
import time
|
||
|
||
_ADDR = 0x42
|
||
_WHOAMI = 0xA5
|
||
# ESP32-S3 slave often prefixes one junk byte (8-bit read address 0x85)
|
||
_READ_ADDR_BYTE = (_ADDR << 1) | 1
|
||
|
||
_REG_WHOAMI = 0x00
|
||
_REG_VERSION = 0x01
|
||
_REG_STATUS = 0x02
|
||
_REG_FLAGS = 0x03
|
||
_REG_CMD = 0x05
|
||
_REG_STRIDE = 0x06
|
||
_REG_MIN_AREA = 0x07
|
||
_REG_H = 0x09
|
||
_REG_H_TOL = 0x0C
|
||
_REG_FOUND = 0x10
|
||
_REG_WIFI_CRED = 0x20
|
||
_REG_IP0 = 0x21
|
||
_REG_RGB = 0x25
|
||
|
||
_FLAG_LOCAL = 1 << 0
|
||
_FLAG_SHOW_MASK = 1 << 1
|
||
_FLAG_MASK_FULL = 1 << 2
|
||
_FLAG_MORPH_ROI = 1 << 3
|
||
|
||
_STATUS_WIFI = 1 << 0
|
||
_STATUS_CAM = 1 << 1
|
||
_STATUS_LOCAL = 1 << 2
|
||
|
||
_CMD_CAM_ON = 5
|
||
_CMD_CAM_OFF = 6
|
||
_CMD_WIFI_OFF = 8
|
||
|
||
# Camera frame (QVGA) — read_target() returns offset from image centre
|
||
_FRAME_W = 320
|
||
_FRAME_H = 240
|
||
_CENTER_X = _FRAME_W // 2
|
||
_CENTER_Y = _FRAME_H // 2
|
||
|
||
|
||
class Vision:
|
||
"""I2C client for the ESP eye. Frame size is typically 320x240."""
|
||
|
||
def __init__(self, i2c=None, sda=0, scl=1, freq=100_000, addr=_ADDR):
|
||
self.addr = addr
|
||
self._sda = sda
|
||
self._scl = scl
|
||
self._freq = freq
|
||
if i2c is None:
|
||
self._bus_recover(sda, scl)
|
||
self.i2c = I2C(0, sda=Pin(sda), scl=Pin(scl), freq=freq)
|
||
else:
|
||
self.i2c = i2c
|
||
# Optional size filters for read_target(); None = no limit
|
||
self._max_diameter = None
|
||
self._max_area = None
|
||
time.sleep(1)
|
||
|
||
@staticmethod
|
||
def _bus_recover(sda_pin, scl_pin):
|
||
sda = Pin(sda_pin, Pin.IN, Pin.PULL_UP)
|
||
scl = Pin(scl_pin, Pin.OUT, value=1)
|
||
time.sleep_us(10)
|
||
if sda.value() == 0:
|
||
for _ in range(9):
|
||
scl.value(0)
|
||
time.sleep_us(5)
|
||
scl.value(1)
|
||
time.sleep_us(5)
|
||
if sda.value():
|
||
break
|
||
sda = Pin(sda_pin, Pin.OPEN_DRAIN, value=0)
|
||
time.sleep_us(5)
|
||
scl.value(1)
|
||
time.sleep_us(5)
|
||
sda.value(1)
|
||
time.sleep_us(5)
|
||
|
||
def _write(self, reg, *data):
|
||
self.i2c.writeto(self.addr, bytes((reg & 0xFF,) + tuple(d & 0xFF for d in data)))
|
||
|
||
def _read(self, reg, n=1):
|
||
self.i2c.writeto(self.addr, bytes([reg & 0xFF]))
|
||
time.sleep_ms(3)
|
||
raw = self.i2c.readfrom(self.addr, n + 1)
|
||
if len(raw) >= n + 1 and raw[0] == _READ_ADDR_BYTE:
|
||
return raw[1 : 1 + n]
|
||
if len(raw) >= n:
|
||
return raw[:n]
|
||
return raw
|
||
|
||
def _u8(self, reg):
|
||
return self._read(reg, 1)[0]
|
||
|
||
def _write_u8(self, reg, value):
|
||
self._write(reg, value & 0xFF)
|
||
time.sleep_ms(30)
|
||
|
||
# --- connection ---
|
||
|
||
def connected(self, retries=5):
|
||
"""Return True if the eye answers on I2C."""
|
||
for _ in range(retries):
|
||
try:
|
||
if self._u8(_REG_WHOAMI) == _WHOAMI:
|
||
return True
|
||
except OSError:
|
||
pass
|
||
self._bus_recover(self._sda, self._scl)
|
||
self.i2c = I2C(0, sda=Pin(self._sda), scl=Pin(self._scl), freq=self._freq)
|
||
time.sleep_ms(200)
|
||
return False
|
||
|
||
def version(self):
|
||
return self._u8(_REG_VERSION)
|
||
|
||
def camera_on(self):
|
||
return bool(self._u8(_REG_STATUS) & _STATUS_CAM)
|
||
|
||
def wifi_on(self):
|
||
"""True when the eye has a Wi‑Fi IP address."""
|
||
return bool(self._u8(_REG_STATUS) & _STATUS_WIFI)
|
||
|
||
def wifi_ip(self):
|
||
"""
|
||
Return the eye's IPv4 address as 'a.b.c.d', or None if not connected.
|
||
"""
|
||
if not self.wifi_on():
|
||
return None
|
||
b = self._read(_REG_IP0, 4)
|
||
if len(b) != 4:
|
||
return None
|
||
if b[0] == 0 and b[1] == 0 and b[2] == 0 and b[3] == 0:
|
||
return None
|
||
return "{}.{}.{}.{}".format(b[0], b[1], b[2], b[3])
|
||
|
||
# --- power / mode ---
|
||
|
||
def set_camera(self, on, wait_ms=2500):
|
||
"""Turn the ESP camera on/off. Returns True if camera is ready when on=True."""
|
||
self._write_u8(_REG_CMD, 0xFF)
|
||
self._write_u8(_REG_CMD, _CMD_CAM_ON if on else _CMD_CAM_OFF)
|
||
if not on or wait_ms <= 0:
|
||
return True
|
||
deadline = time.ticks_add(time.ticks_ms(), wait_ms)
|
||
while time.ticks_diff(deadline, time.ticks_ms()) > 0:
|
||
if self.camera_on():
|
||
return True
|
||
time.sleep_ms(100)
|
||
return self.camera_on()
|
||
|
||
def set_wifi(self, ssid, password, wait_ms=20000):
|
||
"""
|
||
Connect the eye to Wi‑Fi (enables the browser UI).
|
||
|
||
ssid: network name (max 32 chars)
|
||
password: WPA passphrase (max 64 chars)
|
||
"""
|
||
ssid_b = str(ssid).encode("utf-8")
|
||
pass_b = str(password).encode("utf-8")
|
||
if not ssid_b:
|
||
raise ValueError("ssid required")
|
||
if len(ssid_b) > 32:
|
||
raise ValueError("ssid too long (max 32 bytes)")
|
||
if len(pass_b) > 64:
|
||
raise ValueError("password too long (max 64 bytes)")
|
||
payload = (
|
||
bytes([_REG_WIFI_CRED, len(ssid_b)])
|
||
+ ssid_b
|
||
+ bytes([len(pass_b)])
|
||
+ pass_b
|
||
)
|
||
self.i2c.writeto(self.addr, payload)
|
||
time.sleep_ms(50)
|
||
if wait_ms <= 0:
|
||
return True
|
||
deadline = time.ticks_add(time.ticks_ms(), wait_ms)
|
||
while time.ticks_diff(deadline, time.ticks_ms()) > 0:
|
||
if self.wifi_on():
|
||
return True
|
||
time.sleep_ms(200)
|
||
return self.wifi_on()
|
||
|
||
def wifi_off(self):
|
||
"""Disconnect Wi‑Fi / stop the web UI."""
|
||
self._write_u8(_REG_CMD, 0xFF)
|
||
self._write_u8(_REG_CMD, _CMD_WIFI_OFF)
|
||
|
||
def set_detect(self, on=True):
|
||
"""Enable/disable on-device blob detection (local vision)."""
|
||
flags = _FLAG_MORPH_ROI
|
||
if on:
|
||
flags |= _FLAG_LOCAL
|
||
self._write_u8(_REG_FLAGS, flags)
|
||
|
||
def start(self, wait_ms=2500):
|
||
"""
|
||
Ready the eye for tracking: camera on + local detection.
|
||
Colour/tolerance should already be set (PC viewer or set_hsv).
|
||
"""
|
||
ok = self.set_camera(True, wait_ms=wait_ms)
|
||
self.set_detect(True)
|
||
return ok
|
||
|
||
# --- colour tuning (optional; usually done once via PC viewer) ---
|
||
|
||
def set_hsv(self, h, s, v):
|
||
self._write(_REG_H, h & 0xFF, s & 0xFF, v & 0xFF)
|
||
time.sleep_ms(30)
|
||
|
||
def set_tolerance(self, h_tol, s_tol=None, v_tol=None):
|
||
if s_tol is None:
|
||
s_tol = h_tol * 6
|
||
if v_tol is None:
|
||
v_tol = h_tol * 6
|
||
self._write(_REG_H_TOL, h_tol & 0xFF, s_tol & 0xFF, v_tol & 0xFF)
|
||
time.sleep_ms(30)
|
||
|
||
def set_stride(self, stride=4):
|
||
"""Coarse search stride: 2 (finer/slower) or 4 (default)."""
|
||
self._write_u8(_REG_STRIDE, stride)
|
||
|
||
def set_min_area(self, area=80):
|
||
self._write(_REG_MIN_AREA, area & 0xFF, (area >> 8) & 0xFF)
|
||
time.sleep_ms(30)
|
||
|
||
def set_rgb(self, r, g, b):
|
||
"""Set the WS2812 status LED colour (0–255 per channel)."""
|
||
self._write(_REG_RGB, int(r) & 0xFF, int(g) & 0xFF, int(b) & 0xFF)
|
||
time.sleep_ms(10)
|
||
|
||
def set_size_limits(self, max_diameter=None, max_area=None):
|
||
"""
|
||
Optionally reject oversized blobs in read_target().
|
||
|
||
max_diameter: max blob diameter in pixels, or None for no limit
|
||
max_area: max blob area in pixels, or None for no limit
|
||
|
||
Example (old defaults): eye.set_size_limits(max_diameter=200, max_area=20000)
|
||
"""
|
||
self._max_diameter = max_diameter
|
||
self._max_area = max_area
|
||
|
||
# --- tracking ---
|
||
|
||
def read_target(self):
|
||
"""
|
||
Read the latest blob centre, relative to the image centre.
|
||
|
||
Image is 320x240, so centre is (0, 0):
|
||
+x = right of centre, -x = left
|
||
+y = above centre, -y = below
|
||
diameter = blob diameter in pixels (2 * detector radius)
|
||
|
||
Returns:
|
||
(x, y, diameter) if a target was found
|
||
None if no target (or a bad I2C read)
|
||
"""
|
||
try:
|
||
raw = self._read(_REG_FOUND, 13)
|
||
except OSError:
|
||
return None
|
||
if len(raw) != 13:
|
||
return None
|
||
found, cx, cy, radius, area, _total_ms, _detect_ms = struct.unpack(
|
||
"<BhhHHHH", raw
|
||
)
|
||
if found not in (0, 1):
|
||
return None
|
||
if len(set(raw)) == 1:
|
||
return None
|
||
if raw[0] == _READ_ADDR_BYTE and raw.count(bytes([_READ_ADDR_BYTE])) >= 8:
|
||
return None
|
||
if not found:
|
||
return None
|
||
if cx < 0 or cy < 0 or cx >= _FRAME_W or cy >= _FRAME_H:
|
||
return None
|
||
diameter = int(radius) * 2
|
||
if self._max_diameter is not None and diameter > self._max_diameter:
|
||
return None
|
||
if self._max_area is not None and area > self._max_area:
|
||
return None
|
||
return (int(cx) - _CENTER_X, _CENTER_Y - int(cy), diameter)
|