161 lines
4.5 KiB
Python
161 lines
4.5 KiB
Python
"""
|
||
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
|