Diferencia entre revisiones de «Butialo: Ejemplo 3 - Seguidor de líneas con obstáculos, sin eventos, utilizando un sensor de grises y uno de distancia - Código»

local COLOR_NEGRO = 20000
local MOTOR_VEL = 500
local MOTOR_GIRO_VEL = 500
local TIEMPO_CENTRADO = 0.1
local INTERVALO_CHEQUEO = 0.1
local DISTANCIA_OBSTACULO = 34000
local TIEMPO_RETROCESO = 0.1
local TIEMPO_AVANCE = 1.7
local TIEMPO_GIRO_90_GRADOS = 1.7
local sentido = "D"
local tiempo

local function invertir_sentido()
    if sentido == "I" then
	sentido = "D"
    else
	sentido = "I"
    end
end

local function avanzar()
    Motors.setvel2mtr(0, MOTOR_VEL, 0, MOTOR_VEL)
end

local function retroceder()
    Motors.setvel2mtr(1, MOTOR_VEL, 1, MOTOR_VEL)
end

local function es_negro()
    local color = Grey_1.getValue()
    return (color > COLOR_NEGRO)
end

local function girar()
    if sentido == "I" then
	Motors.setvel2mtr(0,MOTOR_GIRO_VEL,1,MOTOR_GIRO_VEL)
    else
	Motors.setvel2mtr(1,MOTOR_GIRO_VEL,0,MOTOR_GIRO_VEL)
    end
end

local function buscar_negro()
    -- Se inicializa el tiempo en 0.5 segundo
    tiempo = 0.5
    local tiempo_girando
    while true do
	tiempo_girando = 0
	girar()
	invertir_sentido()
	-- El robot gira hasta encontrar el color negro o si no por x tiempo
	while (not es_negro() and tiempo_girando <= tiempo) do
	    util.wait(0.1)
	    tiempo_girando = tiempo_girando + 0.1
	end
	-- Si es negro no se debe seguir iterando
	if es_negro() then
	    break
	else
	    tiempo = tiempo * 2
	end
    end
end

local function hay_obstaculo()
    local distancia = Distanc_2.getValue()
    return (distancia > DISTANCIA_OBSTACULO)
end

local function esquivar_obstaculo()
    local sentido_previo = sentido
    -- Retroceder unos centímetros
    retroceder()
    util.wait(TIEMPO_RETROCESO)
    -- Girar 90 grados hacia la derecha
    sentido = "D"
    girar()
    util.wait(TIEMPO_GIRO_90_GRADOS)
    -- Avanzar unos centímetros
    avanzar()
    util.wait(TIEMPO_AVANCE)
    -- Girar 90 grados a la izquierda
    sentido = "I"
    girar()
    util.wait(TIEMPO_GIRO_90_GRADOS)
    -- Avanzar el doble de centímetros
    avanzar()
    util.wait(TIEMPO_AVANCE * 3)
    -- Girar 120 grados a la izquierda
    sentido = "I"
    girar()
    util.wait(TIEMPO_GIRO_90_GRADOS * 4 / 3)
    -- Avanzar hasta encontrar el negro
    avanzar()
    while (not es_negro()) do
	util.wait(INTERVALO_CHEQUEO)
    end
    -- Fin: restaurar el sentido al estado previo
    sentido = sentido_previo
end

while true do
    avanzar()
    while (es_negro() and not hay_obstaculo()) do
	util.wait(INTERVALO_CHEQUEO)
    end
    if hay_obstaculo() then
        esquivar_obstaculo()
    else
	buscar_negro()
	util.wait(TIEMPO_CENTRADO)
	invertir_sentido()
    end
end