289 lines
8.4 KiB
Python
289 lines
8.4 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 = target # offset from image centre (0,0)
|
|||
|
|
print("target at", x, y)
|
|||
|
|
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
|
|||
|
|
|
|||
|
|
_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
|
|||
|
|
|
|||
|
|
@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)
|
|||
|
|
|
|||
|
|
# --- 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
|
|||
|
|
|
|||
|
|
Returns:
|
|||
|
|
(x, y) 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
|
|||
|
|
if area >= 20000 or radius >= 100 or area == 65535:
|
|||
|
|
return None
|
|||
|
|
return (int(cx) - _CENTER_X, _CENTER_Y - int(cy))
|