🎯 Projeto: robô que segue um objeto

#!/usr/bin/env python3
"""Robô que segue um objeto colorido (módulo 3.6). Requer link_esp32.py (módulo 3.5) na mesma pasta."""
import time
import cv2
import numpy as np
from picamera2 import Picamera2
from link_esp32 import Link

LARGURA, ALTURA = 320, 240
HSV_MIN = np.array([100, 120, 70])    # faixa do azul; calibre com o módulo 3.2
HSV_MAX = np.array([130, 255, 255])
AREA_MIN = 300                        # menor que isso é ruído
AREA_ALVO = 6000                      # área (pixels) na distância desejada: meça a sua
KP_GIRO, KP_DIST, VEL_MAX = 180, 160, 180


def acha_objeto(quadro):
    hsv = cv2.cvtColor(quadro, cv2.COLOR_BGR2HSV)
    mascara = cv2.inRange(hsv, HSV_MIN, HSV_MAX)
    mascara = cv2.morphologyEx(mascara, cv2.MORPH_OPEN, np.ones((5, 5), np.uint8))
    contornos, _ = cv2.findContours(mascara, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
    if not contornos:
        return None
    maior = max(contornos, key=cv2.contourArea)
    area = cv2.contourArea(maior)
    if area < AREA_MIN:
        return None
    m = cv2.moments(maior)
    return m["m10"] / m["m00"], area      # centro x e área


def comando(cx, area):
    erro_x = (cx - LARGURA / 2) / (LARGURA / 2)                    # -1 esquerda .. +1 direita
    erro_d = max(-1.0, min(1.0, (AREA_ALVO - area) / AREA_ALVO))   # >0 longe, <0 perto
    giro, avanco = KP_GIRO * erro_x, KP_DIST * erro_d
    esq = max(-VEL_MAX, min(VEL_MAX, avanco + giro))
    drt = max(-VEL_MAX, min(VEL_MAX, avanco - giro))
    return esq, drt


def main():
    cam = Picamera2()
    cam.configure(cam.create_video_configuration(main={"size": (LARGURA, ALTURA), "format": "RGB888"}))
    cam.start()
    link = Link()
    t0, quadros, esq, drt = time.monotonic(), 0, 0, 0
    try:
        while True:
            quadro = cam.capture_array()     # "RGB888" do picamera2 já vem na ordem BGR do OpenCV
            achado = acha_objeto(quadro)
            esq, drt = comando(*achado) if achado else (0, 0)   # perdeu o alvo: parado
            link.motores(esq, drt)           # todo quadro manda um comando: mantém o ESP32 vivo
            quadros += 1
            if time.monotonic() - t0 >= 1.0:
                print(f"{quadros} FPS | alvo={'sim' if achado else 'nao'} | esq={esq:.0f} dir={drt:.0f}")
                t0, quadros = time.monotonic(), 0
    except KeyboardInterrupt:
        pass
    finally:
        link.fechar()                        # manda "S"
        cam.stop()


if __name__ == "__main__":
    main()

#!/usr/bin/env python3
"""Segue uma pessoa com YOLO26 (NCNN) no Pi 5 (módulo 3.6). Requer link_esp32.py e o modelo exportado."""
import time
from picamera2 import Picamera2
from ultralytics import YOLO
from link_esp32 import Link

LARGURA, ALTURA = 640, 480
CLASSE = 0              # 0 = "person" no COCO; 32 = "sports ball"
CONF_MIN = 0.5
ALTURA_ALVO = 0.6       # altura da caixa ÷ altura da imagem na distância desejada
KP_GIRO, KP_DIST, VEL_MAX = 140, 150, 150
modelo = YOLO("yolo26n_ncnn_model")


def maior_caixa(resultado):
    melhor = None
    for caixa in resultado.boxes:
        if int(caixa.cls) != CLASSE or float(caixa.conf) < CONF_MIN:
            continue
        x1, y1, x2, y2 = caixa.xyxy[0].tolist()
        if melhor is None or (y2 - y1) > (melhor[3] - melhor[1]):
            melhor = (x1, y1, x2, y2)
    return melhor


def main():
    cam = Picamera2()
    cam.configure(cam.create_video_configuration(main={"size": (LARGURA, ALTURA), "format": "RGB888"}))
    cam.start()
    link = Link()
    try:
        while True:
            t = time.monotonic()
            quadro = cam.capture_array()
            caixa = maior_caixa(modelo.predict(quadro, imgsz=320, verbose=False)[0])
            esq = drt = 0
            if caixa:
                x1, y1, x2, y2 = caixa
                erro_x = ((x1 + x2) / 2 - LARGURA / 2) / (LARGURA / 2)
                erro_d = max(-1.0, min(1.0, (ALTURA_ALVO - (y2 - y1) / ALTURA) / ALTURA_ALVO))
                giro, avanco = KP_GIRO * erro_x, KP_DIST * erro_d
                esq = max(-VEL_MAX, min(VEL_MAX, avanco + giro))
                drt = max(-VEL_MAX, min(VEL_MAX, avanco - giro))
            link.motores(esq, drt)
            print(f"{(time.monotonic() - t) * 1000:.0f} ms | pessoa={'sim' if caixa else 'nao'} | esq={esq:.0f} dir={drt:.0f}")
    except KeyboardInterrupt:
        pass
    finally:
        link.fechar()
        cam.stop()


if __name__ == "__main__":
    main()

"""PID simples com limite e anti-windup (módulo 3.6)."""
import time


class PID:
    def __init__(self, kp, ki=0.0, kd=0.0, limite=255.0):
        self.kp, self.ki, self.kd, self.limite = kp, ki, kd, limite
        self.reset()

    def reset(self):
        self.integral, self.erro_ant, self.t_ant = 0.0, 0.0, None

    def __call__(self, erro):
        agora = time.monotonic()
        dt = 0.0 if self.t_ant is None else agora - self.t_ant
        deriv = (erro - self.erro_ant) / dt if dt > 0 else 0.0
        self.integral += erro * dt
        saida = self.kp * erro + self.ki * self.integral + self.kd * deriv
        if abs(saida) > self.limite:          # saturou: desfaz a soma (anti-windup)
            self.integral -= erro * dt
            saida = max(-self.limite, min(self.limite, saida))
        self.erro_ant, self.t_ant = erro, agora
        return saida

from pid import PID

pid_giro = PID(kp=180, ki=0.0, kd=20, limite=150)   # comece com kd=0 e suba aos poucos
pid_dist = PID(kp=160, ki=30, kd=0, limite=150)


def comando(cx, area):
    erro_x = (cx - LARGURA / 2) / (LARGURA / 2)
    erro_d = max(-1.0, min(1.0, (AREA_ALVO - area) / AREA_ALVO))
    if abs(erro_x) < 0.05:                    # zona morta: não tremer com o alvo no centro
        erro_x = 0.0
    giro, avanco = pid_giro(erro_x), pid_dist(erro_d)
    esq = max(-VEL_MAX, min(VEL_MAX, avanco + giro))
    drt = max(-VEL_MAX, min(VEL_MAX, avanco - giro))
    return esq, drt