239 lines
7.8 KiB
Python
239 lines
7.8 KiB
Python
"""
|
||
gesture.py — Lectura del sensor de gestos PAJ7620U2 via I2C raw (smbus2).
|
||
Equivalent a gesture.cpp del codi Arduino/ESP32.
|
||
|
||
No hi ha pin INT disponible al PCB → polling cada 50ms en un thread.
|
||
Bus I2C 3 (GPIO2=SDA, GPIO1=SCL, compartit amb VL53L0X i ADS1115).
|
||
|
||
Llibreria C++ equivalent: RevEng_PAJ7620 (Aaron S. Crandall)
|
||
"""
|
||
|
||
import time
|
||
import threading
|
||
import smbus2
|
||
|
||
# ==================
|
||
# IDs de gest
|
||
# ==================
|
||
|
||
GS_NONE = 0
|
||
GS_FORWARD = 1
|
||
GS_LEFT = 2
|
||
GS_RIGHT = 3
|
||
GS_UP = 4
|
||
GS_DOWN = 5
|
||
GS_CLOCKWISE = 6
|
||
GS_ANTICLOCKWISE = 7
|
||
GS_WAVE = 8
|
||
|
||
# Àlies per al quibot.py
|
||
GS_CW = GS_CLOCKWISE
|
||
GS_CCW = GS_ANTICLOCKWISE
|
||
|
||
# ==================
|
||
# Registres PAJ7620U2
|
||
# ==================
|
||
|
||
_PAJ7620_ADDR = 0x73
|
||
_REG_BANK_SEL = 0xEF # Registre de selecció de banc (0x00=banc0, 0x01=banc1)
|
||
_REG_PART_ID_LSB = 0x00 # Ha de retornar 0x20
|
||
_REG_PART_ID_MSB = 0x01 # Ha de retornar 0x76
|
||
_REG_GESTURE_0 = 0x43 # Bits 0–7: Right, Left, Up, Down, Forward, Backward, CW, CCW
|
||
_REG_GESTURE_1 = 0x44 # Bit 0: Wave
|
||
|
||
# Bits de gest al registre 0x43
|
||
_BIT_RIGHT = 0x01
|
||
_BIT_LEFT = 0x02
|
||
_BIT_UP = 0x04
|
||
_BIT_DOWN = 0x08
|
||
_BIT_FORWARD = 0x10
|
||
_BIT_BACKWARD = 0x20
|
||
_BIT_CW = 0x40
|
||
_BIT_CCW = 0x80
|
||
|
||
# Bit de gest al registre 0x44
|
||
_BIT_WAVE = 0x01
|
||
|
||
# ==================
|
||
# Seqüències d'inicialització
|
||
# ==================
|
||
|
||
_INIT_BANK0 = [
|
||
(0x32, 0x29), (0x33, 0x01), (0x34, 0x00), (0x35, 0x01), (0x36, 0x00),
|
||
(0x37, 0x07), (0x38, 0x17), (0x39, 0x06), (0x3A, 0x12), (0x3F, 0x00),
|
||
(0x40, 0x02), (0x41, 0xFF), (0x42, 0x01), (0x46, 0x2D), (0x47, 0x0F),
|
||
(0x48, 0x3C), (0x49, 0x00), (0x4A, 0x1E), (0x4B, 0x00), (0x4C, 0x20),
|
||
(0x4D, 0x00), (0x4E, 0x1A), (0x4F, 0x14), (0x50, 0x00), (0x51, 0x10),
|
||
(0x52, 0x00), (0x5C, 0x02), (0x5D, 0x00), (0x5E, 0x10), (0x5F, 0x3F),
|
||
(0x60, 0x27), (0x61, 0x28), (0x62, 0x00), (0x63, 0x03), (0x64, 0xF7),
|
||
(0x65, 0x03), (0x66, 0xD9), (0x67, 0x03), (0x68, 0x01), (0x69, 0xC8),
|
||
(0x6A, 0x40), (0x6D, 0x04), (0x6E, 0x00), (0x6F, 0x00), (0x70, 0x80),
|
||
(0x71, 0x00), (0x72, 0x00), (0x73, 0x00), (0x74, 0xF0), (0x75, 0x00),
|
||
(0x80, 0x42), (0x81, 0x44), (0x82, 0x04), (0x83, 0x20), (0x84, 0x20),
|
||
(0x85, 0x00), (0x86, 0x10), (0x87, 0x00), (0x88, 0x05), (0x89, 0x18),
|
||
(0x8A, 0x10), (0x8B, 0x01), (0x8C, 0x37), (0x8D, 0x00), (0x8E, 0xF0),
|
||
(0x8F, 0x81), (0x90, 0x06), (0x91, 0x06), (0x92, 0x1E), (0x93, 0x0D),
|
||
(0x94, 0x0A), (0x95, 0x0A), (0x96, 0x0C), (0x97, 0x05), (0x98, 0x0A),
|
||
(0x99, 0x41), (0x9A, 0x14), (0x9B, 0x0A), (0x9C, 0x3F), (0x9D, 0x33),
|
||
(0x9E, 0xAE), (0x9F, 0xF9), (0xA0, 0x48), (0xA1, 0x13), (0xA2, 0x10),
|
||
(0xA3, 0x08), (0xA4, 0x30), (0xA5, 0x19), (0xA6, 0x10), (0xA7, 0x08),
|
||
(0xA8, 0x24), (0xA9, 0x04), (0xAA, 0x1E), (0xAB, 0x1E), (0xCC, 0x19),
|
||
(0xCD, 0x0B), (0xCE, 0x13), (0xCF, 0x64), (0xD0, 0x21), (0xD1, 0x0F),
|
||
(0xD2, 0x88), (0xE0, 0x01), (0xE1, 0x04), (0xE2, 0x41), (0xE3, 0xD6),
|
||
(0xE4, 0x00), (0xE5, 0x0C), (0xE6, 0x0A), (0xE7, 0x00), (0xE8, 0x00),
|
||
(0xE9, 0x00), (0xEE, 0x07),
|
||
]
|
||
|
||
_INIT_BANK1 = [
|
||
(0x00, 0x1E), (0x01, 0x1E), (0x02, 0x0F), (0x03, 0x10), (0x04, 0x02),
|
||
(0x05, 0x00), (0x06, 0xB0), (0x07, 0x04), (0x08, 0x0D), (0x09, 0x0E),
|
||
(0x0A, 0x9C), (0x0B, 0x04), (0x0C, 0x05), (0x0D, 0x0F), (0x0E, 0x02),
|
||
(0x0F, 0x12), (0x10, 0x02), (0x11, 0x02), (0x12, 0x00), (0x13, 0x01),
|
||
(0x14, 0x05), (0x15, 0x07), (0x16, 0x05), (0x17, 0x07), (0x18, 0x01),
|
||
(0x19, 0x04), (0x1A, 0x05), (0x1B, 0x0C), (0x1C, 0x2A), (0x1D, 0x01),
|
||
(0x1E, 0x00), (0x21, 0x00), (0x22, 0x00), (0x23, 0x00), (0x25, 0x01),
|
||
(0x26, 0x00), (0x27, 0x39), (0x28, 0x7F), (0x29, 0x08), (0x30, 0x03),
|
||
(0x31, 0x00), (0x32, 0x1A), (0x33, 0x1A), (0x34, 0x07), (0x35, 0x07),
|
||
(0x36, 0x01), (0x37, 0xFF), (0x38, 0x36), (0x39, 0x07), (0x3A, 0x00),
|
||
(0x3E, 0xFF), (0x3F, 0x00), (0x40, 0x77), (0x41, 0x40), (0x42, 0x00),
|
||
(0x43, 0x30), (0x44, 0xA0), (0x45, 0x5C), (0x46, 0x00), (0x47, 0x00),
|
||
(0x48, 0x58), (0x4A, 0x1E), (0x4B, 0x1E), (0x4C, 0x00), (0x4D, 0x00),
|
||
(0x4E, 0xA0), (0x4F, 0x80), (0x50, 0x00), (0x51, 0x00), (0x52, 0x00),
|
||
(0x53, 0x00), (0x54, 0x00), (0x57, 0x80), (0x59, 0x10), (0x5A, 0x08),
|
||
(0x5B, 0x94), (0x5C, 0xE8), (0x5D, 0x08), (0x5E, 0x3D), (0x5F, 0x99),
|
||
(0x60, 0x45), (0x61, 0x40), (0x63, 0x2D), (0x64, 0x02), (0x65, 0x96),
|
||
(0x66, 0x00), (0x67, 0x97), (0x68, 0x01), (0x69, 0xCD), (0x6A, 0x01),
|
||
(0x6B, 0xB0), (0x6C, 0x04), (0x6D, 0x2C), (0x6E, 0x01), (0x6F, 0x32),
|
||
(0x71, 0x00), (0x72, 0x01), (0x73, 0x35), (0x74, 0x00), (0x75, 0x33),
|
||
(0x76, 0x31), (0x77, 0x01), (0x7C, 0x84), (0x7D, 0x03), (0x7E, 0x01),
|
||
]
|
||
|
||
# ==================
|
||
# Estat global
|
||
# ==================
|
||
|
||
_bus: smbus2.SMBus = None
|
||
_gesture: int = GS_NONE
|
||
_gesture_lock = threading.Lock()
|
||
_poll_stop = threading.Event()
|
||
|
||
|
||
# ==================
|
||
# Helpers I2C
|
||
# ==================
|
||
|
||
def _select_bank(bank: int):
|
||
_bus.write_byte_data(_PAJ7620_ADDR, _REG_BANK_SEL, bank)
|
||
|
||
def _write(reg: int, val: int):
|
||
_bus.write_byte_data(_PAJ7620_ADDR, reg, val)
|
||
|
||
def _read(reg: int) -> int:
|
||
return _bus.read_byte_data(_PAJ7620_ADDR, reg)
|
||
|
||
|
||
# ==================
|
||
# Setup
|
||
# ==================
|
||
|
||
def gesture_setup():
|
||
"""
|
||
Inicialitza el PAJ7620U2 via I2C raw (smbus2, bus 3).
|
||
Arrenca el thread de polling (equivalent al ISR del C++).
|
||
"""
|
||
global _bus
|
||
|
||
_bus = smbus2.SMBus(3)
|
||
|
||
# Desperta el sensor (primer accés I2C)
|
||
try:
|
||
_bus.write_byte(_PAJ7620_ADDR, 0)
|
||
except Exception:
|
||
pass
|
||
time.sleep(0.001) # 700µs d'espera per wake-up
|
||
|
||
# Verifica ID del dispositiu
|
||
_select_bank(0)
|
||
id_lsb = _read(_REG_PART_ID_LSB)
|
||
id_msb = _read(_REG_PART_ID_MSB)
|
||
if id_lsb != 0x20 or id_msb != 0x76:
|
||
print(f"ERROR: PAJ7620U2 NOT FOUND (ID: {id_msb:02X}{id_lsb:02X})")
|
||
else:
|
||
print("Gesture sensor init OK")
|
||
|
||
# Escriu registres d'inicialització (banc 0)
|
||
_select_bank(0)
|
||
for reg, val in _INIT_BANK0:
|
||
_write(reg, val)
|
||
|
||
# Escriu registres d'inicialització (banc 1)
|
||
_select_bank(1)
|
||
for reg, val in _INIT_BANK1:
|
||
_write(reg, val)
|
||
|
||
# Torna al banc 0 per a la lectura de gestos
|
||
_select_bank(0)
|
||
|
||
# Arrenca el thread de polling
|
||
_poll_stop.clear()
|
||
threading.Thread(target=_poll_loop, daemon=True, name="gesture").start()
|
||
|
||
|
||
def gesture_cleanup():
|
||
"""Atura el polling i tanca el bus I2C."""
|
||
_poll_stop.set()
|
||
if _bus:
|
||
_bus.close()
|
||
|
||
|
||
# ==================
|
||
# Thread de polling (equivalent al ISR + flag del C++)
|
||
# ==================
|
||
|
||
def _poll_loop():
|
||
"""
|
||
Llegeix els registres de gest cada 50ms.
|
||
Equivalent a on_gesture_interrupt() + gesture_available flag del C++.
|
||
"""
|
||
global _gesture
|
||
|
||
while not _poll_stop.is_set():
|
||
try:
|
||
g0 = _read(_REG_GESTURE_0)
|
||
g1 = _read(_REG_GESTURE_1)
|
||
|
||
detected = GS_NONE
|
||
if g0 & _BIT_FORWARD: detected = GS_FORWARD
|
||
elif g0 & _BIT_LEFT: detected = GS_LEFT
|
||
elif g0 & _BIT_RIGHT: detected = GS_RIGHT
|
||
elif g0 & _BIT_UP: detected = GS_UP
|
||
elif g0 & _BIT_DOWN: detected = GS_DOWN
|
||
elif g0 & _BIT_CW: detected = GS_CLOCKWISE
|
||
elif g0 & _BIT_CCW: detected = GS_ANTICLOCKWISE
|
||
elif g1 & _BIT_WAVE: detected = GS_WAVE
|
||
|
||
if detected != GS_NONE:
|
||
with _gesture_lock:
|
||
_gesture = detected
|
||
|
||
except Exception:
|
||
pass # error I2C puntual → ignora i continua
|
||
|
||
time.sleep(0.05) # 50ms de polling (20Hz)
|
||
|
||
|
||
# ==================
|
||
# Lectura de gest
|
||
# ==================
|
||
|
||
def read_gesture() -> int:
|
||
"""
|
||
Retorna l'últim gest detectat i el reseteja a GS_NONE.
|
||
No bloquejant — equivalent a read_gesture() del C++.
|
||
"""
|
||
global _gesture
|
||
with _gesture_lock:
|
||
gest = _gesture
|
||
_gesture = GS_NONE
|
||
return gest
|