# Este script é um "rascunho" para o robô que irá competir na OBR.
# Ele utiliza sensores de luminosidade (LDR), sensores de distância
# (ultrassônico) e controle de motores via PWM.

# Importações Necessárias Para o Código.
from machine import Pin, PWM, time_pulse_us, ADC, I2C
from time import sleep_msfrom machine import Pin, ADC
from time import sleep_ms

# === Motores Esquerdo ===
velo_esq = PWM(Pin(9), freq=100)
velo_esq2 = PWM(Pin(10), freq=100)

# === Motores Direito ===
velo_dir = PWM(Pin(20), freq=100)
velo_dir2 = PWM(Pin(19), freq=100)

# === Configuração de Sensores e Atuadores ===
trg = Pin(4, Pin.OUT)        # Pino Trigger do sensor ultrassônico.
ech = Pin(5, Pin.IN)         # Pino Echo do sensor ultrassônico.
ldrdir = ADC(Pin(26))           # Entrada 0
ldrmeio = ADC(Pin(27))         # Entrada 1
ldresq = ADC(Pin(28))          # Entrada 2
botao = Pin(6, Pin.IN, Pin.PULL_DOWN)
servo_motor = PWM(Pin(10), freq=50)
bus = I2C(0,sda=Pin(0), scl=Pin(1))
sensor_cor = tcs3472(bus)

# Definição de Variáveis Importantes para o Código ===
vezes = 0
estado = True
limiar = 30000
ligado = True
robo=False

# === Funções Para Classificar o Nível de Luminosidade ===
def mede_luz(valor_lux):
    return 'Está Claro' if valor_lux >= 30000 else 'Está Escuro'

# === Função Para o Nível de Velocidade ===
def velMotor(v_esq, v_dir):
    duty_esq = 65535 - int(65535 * v_esq / 100)
    duty_dir = 65535 - int(65535 * v_dir / 100)
    velo_dir.duty_u16(duty_dir)
    velo_esq.duty_u16(duty_esq)
    velo_dir2.duty_u16(duty_dir)
    velo_esq2.duty_u16(duty_esq)

# === Função Para a Medição dos Sensores ===
def ler_sensores():
    # Medição de distância com o sensor ultrassônico.
    dist = mede_hc_sr04(trg, ech)

    # Medição de luz com LDR.
    valor_lux1 = ldresq.read_u16()   # Sensor esquerdo
    valor_lux2 = ldrmeio.read_u16()  # Sensor do meio
    valor_lux3 = ldrdir.read_u16()   # Sensor direito

    return dist, valor_lux1, valor_lux2, valor_lux3

# === Funções dos Motores ===
def paraFrente():
    velo_dir.duty_u16(0)       # Motor direito liga.
    velo_dir2.duty_u16(65535)   # Motor direito desliga.
    velo_esq.duty_u16(0)        # Motor esquerdo liga.
    velo_esq2.duty_u16(65535)        # Motor esquerdo desliga.

def paraTras():
    velo_dir.duty_u16(65535)       # Motor direito liga.
    velo_dir2.duty_u16(0)   # Motor direito desliga.
    velo_esq.duty_u16(65535)        # Motor esquerdo liga.
    velo_esq2.duty_u16(0)        # Motor esquerdo desliga.
  
def parar():
    velo_dir.duty_u16(0)       # Motor direito liga.
    velo_dir2.duty_u16(0)   # Motor direito desliga.
    velo_esq.duty_u16(0)        # Motor esquerdo liga.
    velo_esq2.duty_u16(0)        # Motor esquerdo desliga.

def paraEsq(p):
    v = int(p/100 * 65535)
    velo_dir.duty_u16(0)
    velo_esq.duty_u16(v)

def paraDir(p):
    v = int(p/100 * 65535)
    velo_dir.duty_u16(v)
    velo_esq.duty_u16(0)

def giroLouco():
    pass # faz dps zé piriquito

def lentoFrente():
    velo_dir.duty_u16(32768)       # Motor direito liga.
    velo_dir2.duty_u16(65535)   # Motor direito desliga.
    velo_esq.duty_u16(32768)        # Motor esquerdo liga.
    velo_esq2.duty_u16(65535)        # Motor esquerdo desliga.

def lentoTras():
    velo_dir.duty_u16(32768)       # Motor direito liga.
    velo_dir2.duty_u16(65535)   # Motor direito desliga.
    velo_esq.duty_u16(32768)        # Motor esquerdo liga.
    velo_esq2.duty_u16(65535)        # Motor esquerdo desliga.

# === Loop Principal e Lógica do Robô ===
while True:
    if botao.value() == 1:
        if ligado:
            
            while ligado:
                if botao.value() == 0:
                    ligado = False
                    robo = True
                    sleep_ms(100)
        else:
            while ligado == False:
                if botao.value() == 0:
                    ligado=True
                    robo=False
                    sleep_ms(100)
    elif robo:
        dist, valor_lux1, valor_lux2, valor_lux3 = ler_sensores()
        esq_preto = valor_lux1 < limiar
        meio_preto = valor_lux2 < limiar
        dir_preto = valor_lux3 < limiar

        if not esq_preto and not meio_preto and not dir_preto:
            paraFrente()

        elif esq_preto and meio_preto and dir_preto:
            parar()

        elif meio_preto and esq_preto and not dir_preto:
            paraEsq(50)
            sleep_ms(500)
            parar()

        elif meio_preto and dir_preto and not esq_preto:
            paraDir(50)
            sleep_ms(500)
            parar()

        elif meio_preto and not esq_preto and not dir_preto:
            paraFrente()

        elif esq_preto and not meio_preto and not dir_preto:
            paraEsq(50)
            sleep_ms(500)
            parar()

        elif dir_preto and not meio_preto and not esq_preto:
            paraDir(50)
            sleep_ms(500)
            parar()

        if meio_preto and dist <= 5:
            paraDir(50)
            sleep_ms(500)
            paraEsq(50)
            sleep_ms(500)
            paraEsq(50)
            sleep_ms(500)
            paraDir(50)
            sleep_ms(500)
            parar()