""" 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