TTs whisper

raspi/blocks.py

@@ -0,0 +1,160 @@
+"""
+blocks.py — Lectura del sensor de color TCS34725 i servo d'expulsió de blocs.
+Equivalent a blocks.cpp del codi Arduino/ESP32.
+
+Requereix /boot/config.txt:
+    dtoverlay=i2c-gpio,bus=4,i2c_gpio_sda=22,i2c_gpio_scl=27
+"""
+
+import time
+import pigpio
+import adafruit_extended_bus
+import adafruit_tcs34725
+
+from pins import SERVO_PWM
+
+# ==================
+# IDs de color
+# ==================
+
+BK = 0   # Negre (no reconegut)
+RD = 1   # Vermell  → avançar
+GN = 2   # Verd     → girar dreta
+BU = 3   # Blau     → girar esquerra
+YE = 4   # Groc     → xuclar líquid
+OG = 5   # Taronja  → buidar líquid
+VT = 6   # Violeta  → sorpresa
+
+NUM_COLORS = 7
+
+# Taula de colors de referència (valors RGB 0–255, calibrats amb el sensor)
+_COLORS = [
+    {"name": "BK", "r":  80, "g":  80, "b":  80},
+    {"name": "RD", "r": 202, "g":  32, "b":  34},
+    {"name": "GN", "r": 107, "g":  90, "b":  57},
+    {"name": "BU", "r": 104, "g":  83, "b":  66},
+    {"name": "YE", "r": 150, "g":  69, "b":  33},
+    {"name": "OG", "r": 185, "g":  44, "b":  32},
+    {"name": "VT", "r": 129, "g":  70, "b":  55},
+]
+
+# ==================
+# Paràmetres del servo
+# ==================
+
+# Valors en µs de pulse width (pigpio set_servo_pulsewidth).
+# Conversió des de l'ESP32 (16 bits, 50Hz): valor/65535 * 20000µs
+# MIN  = 3277/65535 * 20000 ≈ 1000µs
+# MAX  = 8000/65535 * 20000 ≈ 2440µs
+# EJECT= 6450/65535 * 20000 ≈ 1968µs
+MIN_SERVO_US   = 1000   # µs → ~0°
+MAX_SERVO_US   = 2440   # µs → ~180°
+OPEN_POSITION  = 2440   # µs (equivalent a 8000 ESP32)
+EJECT_POSITION = 1968   # µs (equivalent a 6450 ESP32)
+_INCREMENT_US  = 3      # µs per iteració de 1ms (equivalent a increment de 10 ESP32)
+
+# ==================
+# Instàncies globals
+# ==================
+
+_pi: pigpio.pi           = None
+_color_sensor            = None
+_current_servo_pos: int  = OPEN_POSITION
+
+
+# ==================
+# Setup
+# ==================
+
+def blocks_setup(pi: pigpio.pi):
+    """
+    Inicialitza el servo i el sensor de color TCS34725.
+    El servo arranca en OPEN_POSITION.
+    """
+    global _pi, _color_sensor, _current_servo_pos
+
+    _pi = pi
+
+    # Servo
+    pi.set_mode(SERVO_PWM, pigpio.OUTPUT)
+    _current_servo_pos = OPEN_POSITION
+    pi.set_servo_pulsewidth(SERVO_PWM, _current_servo_pos)
+
+    # Sensor de color TCS34725 via bus I2C 4 (bit-bang GPIO22=SDA, GPIO27=SCL)
+    i2c = adafruit_extended_bus.ExtendedI2C(4)
+    _color_sensor = adafruit_tcs34725.TCS34725(i2c)
+    _color_sensor.integration_time = 50    # ms
+    _color_sensor.gain = 4                 # 4x (equivalent a TCS34725::Gain::X04)
+
+
+# ==================
+# Servo
+# ==================
+
+def servo_move_to(target_us: int):
+    """
+    Mou el servo fins a target_us de forma suau.
+    Incrementa/decrementa _INCREMENT_US cada mil·lisegon.
+    """
+    global _current_servo_pos
+
+    if not (MIN_SERVO_US <= target_us <= MAX_SERVO_US):
+        return
+
+    while True:
+        if _current_servo_pos < target_us - _INCREMENT_US:
+            _current_servo_pos += _INCREMENT_US
+        elif _current_servo_pos > target_us + _INCREMENT_US:
+            _current_servo_pos -= _INCREMENT_US
+        else:
+            _current_servo_pos = target_us
+
+        _pi.set_servo_pulsewidth(SERVO_PWM, _current_servo_pos)
+        time.sleep(0.001)
+
+        if _current_servo_pos == target_us:
+            return
+
+
+# ==================
+# Sensor de color
+# ==================
+
+def _calc_colors_difference(measured: tuple, reference: dict) -> int:
+    """Distància Manhattan entre el color mesurat i un color de referència."""
+    return (abs(measured[0] - reference["r"]) +
+            abs(measured[1] - reference["g"]) +
+            abs(measured[2] - reference["b"]))
+
+
+def read_color_raw() -> tuple:
+    """Retorna (r, g, b) en bytes 0-255 sense classificació. Útil per calibrar."""
+    try:
+        return _color_sensor.color_rgb_bytes
+    except Exception:
+        return (0, 0, 0)
+
+
+def read_block_color() -> int:
+    """
+    Llegeix el color del bloc des del sensor TCS34725.
+    Compara contra la taula de referència per distància Manhattan.
+    Retorna l'ID del color més proper, o BK si cap supera el llindar.
+    """
+    MAX_DIFFERENCE = 15
+
+    try:
+        r, g, b = _color_sensor.color_rgb_bytes
+    except Exception:
+        return BK
+
+    min_difference   = MAX_DIFFERENCE
+    min_diff_color   = BK
+
+    for color_id, ref in enumerate(_COLORS):
+        diff = _calc_colors_difference((r, g, b), ref)
+        if diff < min_difference:
+            min_difference = diff
+            min_diff_color = color_id
+
+    return min_diff_color