Commit fbeb0812 authored by Santiago's avatar Santiago
Browse files

version basica semi estable y version multi-hilo inestable

parent c131994c
--- Autorun script.
-- The first program to run, used to boot the whole system. Configuration
-- is loaded using `nvs.read("autorun", parameter)` calls, where the
-- available parameters are:
--
--* `"runonce"` A program to execute only once. This parameter is set to nil
-- just before running. This can be used to run calibration tools, like
-- `"calibrate_color.lua"`
--
--* `"main"` The program to run. Some examples are `"main_ahsm.lua"`,
-- `"test_omni.lua"`, `"test_wifi.lua"`, etc.
-- @script autorun
print("Booting robotito")
--Version Semi estable, dos hilos, el main y el de sonido.
--No tiene pausa, hay que apagar y prender para pasar un nuevo programa.
--Al inicio se toma un segundo para detectar si está "patas arriba" o no, si está patas arriba
--se queda trancado esperando el comando de ejecución de la app, si está sobre el suelo arranca a ejecutar de primera y luego ya no se puede conectar al robot.
patas_arriba = true
local proximity = require('proximity')
local dump_dist = function(b)
print('close:', b)
patas_arriba = not b
end
proximity.cb.append(dump_dist)
proximity.enable(true)
tmr.sleepms(500)
proximity.enable(false)
dofile("recibe_comandos.lua")
print("Looking for a run-once program into nvs('autorun', 'runonce')")
local runonce = nvs.read("autorun", "runonce", nil)
if runonce then
print("Run-once program:", runonce)
nvs.write("autorun", "runonce", nil)
dofile(runonce)
else
print ("No run-once program set.")
print("Looking for a main program into nvs('autorun', 'main')")
local main = nvs.read("autorun", "main", nil)
if main then
print("Main program:", main)
dofile(main)
else
print ("No main program set.")
end
end
--dofile("test_uart_console.lua")
--Versión para permitir pausar desde la app, 3 hilos (comandos, ejecucion, sonido):
--Inestable, se cuelgan los threads a menudo, el sensor de color demora como 20 segundos en arrancar,
--tira "lua: out of memory" a menudo al darle pausa
--local wifi = require('wifi_net')
--wifi.init()
--local sm = require('sound_module')
--sm.init()
--dofile("cmd_receive.lua")
\ No newline at end of file
--Implementación de los comandos de la app
local M = {}
M.run_program = function()
local exec_mgr = require('vpl_exec_mgr')
exec_mgr.start_exec()
end
M.stop_exec = function()
local exec_mgr = require('vpl_exec_mgr')
exec_mgr.stop_exec()
end
M.get_code = function(clientGetCode)
local file = io.open("vplProgram.lua", "w+")
io.output(file)
while true do
local data, err, partial = clientGetCode:receive('*l')
print('data: ', data, 'error: ', err)
if(err == 'closed') then
break
end
if(data == 'END_CODE') then
io.close(file)
break
else
io.write(data)
io.write("\n")
end
end
print('Fin de recepcion de codigo')
end
M.calibrate_color = function(color_calibrar)
local h_color = ''
local s_color = ''
local v_color = ''
local dump_rgb = function(r, g, b, a, h, s, v)
print('hsv:', h, s, v)
h_color = h
s_color = s
v_color = v
end
local color = require('color')
color.rgb_cb.append(dump_rgb)
color.enable(true)
thread.sleep(1)
nvs.write('color', color_calibrar .. '_h', h_color)
nvs.write('color', color_calibrar .. '_s', s_color)
nvs.write('color', color_calibrar .. '_v', v_color)
color.enable(false)
color.rgb_cb.remove(dump_rgb)
end
return M
--Recibe comandos a través de un socket tcp y los ejecuta utilizando el módulo cmd_exec
local M = {}
local cmd_exec = require('cmd_exec')
local socket = require ('socket')
local servidor = socket.tcp()
servidor:bind('192.168.4.1', 2018)
servidor:listen(1)
local CMD_STOP_EXEC = 'CMD_STOP_EXEC'
local CMD_GET_CODE = 'CMD_GET_CODE'
local CMD_RUN_PROGRAM = 'CMD_RUN_PROGRAM'
local CMD_CALIBRATE_COLOR = 'CMD_CALIBRATE_COLOR'
while true do
local client = servidor:accept()
print('Cliente conectado')
local cmd, er, part = client:receive('*l')
print('data main: ', cmd)
print('error main: ', er)
print('partial main: ', part)
if(cmd == CMD_RUN_PROGRAM) then
cmd_exec.run_program()
elseif (cmd == CMD_GET_CODE) then
cmd_exec.get_code(client)
elseif (cmd == CMD_CALIBRATE_COLOR) then
local color_cal, er_cal, part_cal = client:receive('*l')
print('data cal: ', color_cal)
print('error cal: ', er_cal)
print('partial cal: ', part_cal)
cmd_exec.calibrate_color(color_cal)
client:send('OK\n')
elseif (cmd == CMD_STOP_EXEC) then
print('cmd stop recibido')
cmd_exec.stop_exec()
end
end
return M
......@@ -18,10 +18,55 @@
-- @module color
-- @alias M
local M = {}
-- Configure apds9960 color sensor
local apds9960r = require('apds9960')
assert(apds9960r.init())
local colors
local function split(s, delimiter)
local result = {};
for match in (s..delimiter):gmatch("(.-)"..delimiter) do
table.insert(result, match)
end
return result
end
local defaultColors = {}
defaultColors["red"] = {348,170,135}
defaultColors["yellow"] = {70,226,228}
defaultColors["green"] = {181,250,175}
defaultColors["blue"] = {214,312,180}
defaultColors["purple"] = {300,100,50}
defaultColors["orange"] = {39,100,100}
defaultColors["skyblue"] = {197,42,92}
defaultColors["white"] = {0,0,100}
M.get_color_table = function()
return colors
end
M.init_color_table = function()
local coloresVpl = nvs.read("color","vpl_colors", 'red|yellow|green|blue|purple|orange|skyblue|white')
local coloresTabla = split(coloresVpl, '|')
colors = {};
if(coloresTabla[1] ~= '') then
for k, v in pairs(coloresTabla) do
table.insert(colors,
{coloresTabla[k],
nvs.read("color",coloresTabla[k].."_h", defaultColors[coloresTabla[k]][1]),
nvs.read("color",coloresTabla[k].."_s", defaultColors[coloresTabla[k]][2]),
nvs.read("color",coloresTabla[k].."_v", defaultColors[coloresTabla[k]][3])})
end
end
assert(apds9960r.color.set_color_table(colors))
end
do
--local min_sat = nvs.read("color","min_sat", 24)
local min_val = nvs.read("color","min_val", 40)
......@@ -30,41 +75,15 @@ do
local ds = nvs.read("color","delta_s", 50)
local dv = nvs.read("color","delta_v", 50)
local colors = {
{"red",
nvs.read("color","red_h", 348),
nvs.read("color","red_s", 170),
nvs.read("color","red_v", 135),
},
{"yellow",
nvs.read("color","yellow_h", 70),
nvs.read("color","yellow_s", 226),
nvs.read("color","yellow_v", 228),
},
{"green",
nvs.read("color","green_h", 181),
nvs.read("color","green_s", 250),
nvs.read("color","green_v", 175),
},
{"blue",
nvs.read("color","blue_h", 214),
nvs.read("color","blue_s", 312),
nvs.read("color","blue_v", 180),
},
{"magenta",
nvs.read("color","magenta_h", 260),
nvs.read("color","magenta_s", 170),
nvs.read("color","magenta_v", 135),
},
}
M.init_color_table()
local gain = nvs.read("color","gain", 1) -- default 2x
assert(apds9960r.color.set_color_table(colors))
assert(apds9960r.color.set_sv_limits(dh, ds, dv, min_val,max_val))
assert(apds9960r.color.set_ambient_gain(gain))
end
local color_rgb = {
['red'] = {100,0,0},
['yellow'] = {50,50,0},
......@@ -123,10 +142,12 @@ local enables = 0
-- @tparam[opt=100] integer period Sampling period in ms, if omitted is
-- read from `nvs.read("color","period")`.
M.enable = function (on, period)
print('dentro del color.lua')
if on and enables==0 then
pio.pin.sethigh(led_pin)
period = period or nvs.read("color","period", 100) or 100
apds9960r.color.enable(period)
print('COLOR enabled')
elseif not on and enables==1 then
apds9960r.color.enable(false)
pio.pin.setlow(led_pin)
......
local utils = require('utils')
local thr_sound
local frecuencia_sonido = 0
local velocidad
local velocidad_actual_movimiento
local movimiento_actual
local direccion_actual
estado_actual = ''
local luz_actual
local arco_delantero
local arco_derecho
local arco_trasero
local arco_izquierdo
local color_sensado
local ledRing
--local omni
local laser
local device
udp = nil
--Funcion de sensar color:
local h_color = 0
local s_color = 0
local v_color = 0
local registros_color = {}
local contador = 0
local dh = nvs.read("color","delta_h", 10)
local ds = nvs.read("color","delta_s", 50)
local dv = nvs.read("color","delta_v", 50)
local tablaColores
local dump_rgb = function(r, g, b, a, h, s, v)
contador = contador + 1
registros_color[contador] = {h,s,v}
if(contador == 3) then
for j=1,3 do
h_color = h_color + registros_color[j][1]
s_color = s_color + registros_color[j][2]
v_color = v_color + registros_color[j][3]
end
h_color = math.floor((h_color)/3)
s_color = math.floor((s_color)/3)
v_color = math.floor((v_color)/3)
print('promedios' .. ' : h=' .. h_color .. ' s=' .. s_color .. ' v=' .. v_color)
local hayMatch = false
for k, v in pairs(tablaColores) do
local restaHue = math.abs(h_color - v[2])
local restaSat = math.abs(s_color - v[3])
local restaVal = math.abs(v_color - v[4])
if(restaHue < dh and restaSat < ds and restaVal < dv) then
hayMatch = true
color_sensado = v[1]
break
end
end
if(not hayMatch) then
color_sensado = ''
end
--print('color sensado = ' .. color_sensado)
h_color = 0
s_color = 0
v_color = 0
contador = 0
registros_color = {}
end
end
init_sound_thread = function()
thr_sound = thread.start(sound_thread, 2548, nil, nil, 'thread_sound')
end
init_interprete = function()
--Inicializo el socket udp que notifica para resaltado de bloques y estado actual
local socket = require("__socket")
udp = assert(socket.udp())
udp:setoption('broadcast', true)
assert(udp:setsockname("192.168.4.1", 2016))
--Valores iniciales
velocidad = 0.15
velocidad_actual_movimiento = 'LENTA'
movimiento_actual = ''
direccion_actual = ''
estado_actual = 'INICIAL'
luz_actual = ''
arco_delantero = ''
arco_derecho = ''
arco_trasero = ''
arco_izquierdo = ''
color_sensado = ''
frecuencia_sonido = 0
--Inicio dispositivos
ledRing = require('led_ring')
--omni = require('omni')
--omni.enable(true)
laser= require('laser_ring')
laser.cb.append(laser.get_reading_cb())
laser.enable(true)
if(device == nil) then
device = pwm.attach(pio.GPIO4, 2000, 0.5)
end
--Inicio sensor de color:
local colorSensor = require('color')
colorSensor.init_color_table()
tablaColores = colorSensor.get_color_table()
if(tablaColores[1] ~= '') then
colorSensor.rgb_cb.append(dump_rgb)
colorSensor.enable(true)
end
end
--Thread ejecutor de sonidos:
sound_thread = function ()
while true do
if(frecuencia_sonido ~= 0) then
device:setfreq(frecuencia_sonido)
device:start()
tmr.sleep(2)
device:stop()
end
thread.suspend(thread.self())
end
end
APAGAR_LUCES = function()
ledRing.clear()
luz_actual = ''
arco_delantero = ''
arco_derecho = ''
arco_trasero = ''
arco_izquierdo = ''
end
DESPLAZAMIENTO = function(direccion)
if(direccion_actual ~= direccion or velocidad_actual_movimiento ~= velocidad) then
omni.drive(0,0,0)
if (direccion == 'ADELANTE') then
omni.drive(0,velocidad,0)
elseif (direccion == 'DERECHA') then
omni.drive(velocidad,0,0)
elseif (direccion == 'ATRAS') then
omni.drive(0,-velocidad,0)
elseif (direccion == 'IZQUIERDA') then
omni.drive(-velocidad,0,0)
end
direccion_actual = direccion
velocidad_actual_movimiento = velocidad
movimiento_actual = "DESPLAZAMIENTO('" .. direccion .. "')"
end
end
DETENERSE = function()
omni.drive(0,0,0)
movimiento_actual = ''
direccion_actual = ''
end
GIRAR = function()
if(movimiento_actual ~= 'GIRAR()' or velocidad_actual_movimiento ~= velocidad) then
omni.drive(0,0,velocidad*4)
movimiento_actual = 'GIRAR()'
direccion_actual = ''
end
end
IS_ESTADO = function(estado)
return estado_actual == estado
end
LUCES_POR_ARCOS = function(delantero, derecho, trasero, izquierdo)
if(delantero ~= arco_delantero or derecho ~= arco_derecho or trasero ~= arco_trasero or izquierdo ~= arco_izquierdo) then
--los apago, luego los prendo por arcos
ledRing.clear()
if(delantero ~= 'APAGADO') then
local r, g, b = utils.pasar_a_porcentaje(utils.color_to_rgb(delantero))
ledRing.set_arc(4,5,r,g,b,true)
end
if(derecho ~= 'APAGADO') then
local r, g, b = utils.pasar_a_porcentaje(utils.color_to_rgb(derecho))
ledRing.set_arc(22,5,r,g,b,true)
end
if(trasero ~= 'APAGADO') then
local r, g, b = utils.pasar_a_porcentaje(utils.color_to_rgb(trasero))
ledRing.set_arc(16,5,r,g,b,true)
end
if(izquierdo ~= 'APAGADO') then
local r, g, b = utils.pasar_a_porcentaje(utils.color_to_rgb(izquierdo))
ledRing.set_arc(10,5,r,g,b,true)
end
arco_delantero = delantero
arco_derecho = derecho
arco_trasero = trasero
arco_izquierdo = izquierdo
luz_actual = ''
end
end
NOTIFICAR = function(idBloque)
udp:sendto(idEjecucion .. '*' .. idBloque, "192.168.4.2", 2016)
end
PRENDER_LUCES = function(color)
if(luz_actual ~= color) then
local r, g, b = utils.color_to_rgb(color)
ledRing.set_all(r,g,b,true)
luz_actual = color
end
end
SENSOR_COLOR = function (color)
if(color_sensado == color) then
print('ESTA SENSANDO COLOR ' .. color)
end
return (color_sensado == color)
end
SENSOR_DISTANCIA = function (sensor, distancia)
local sensor_robot = utils.correspondencia_sensor_distancia(sensor)
local d = laser.norm_d
if (distancia == 'CORTA') then
if(d[sensor_robot] > 0 and d[sensor_robot] < 10) then
print('EVENTO: SENSOR ', sensor, ', DISTANCIA: ', distancia)
return true
else
return false
end
elseif (distancia == 'MEDIA') then
if(d[sensor_robot] > 30 and d[sensor_robot] < 60) then
print('EVENTO: SENSOR ', sensor, ', DISTANCIA: ', distancia)
return true
else
return false
end
elseif (distancia == 'LARGA') then
if(d[sensor_robot] > 60 and d[sensor_robot] < 100) then
print('EVENTO: SENSOR ', sensor, ', DISTANCIA: ', distancia)
return true
else
return false
end
end
end
SONIDO = function(emocion)
print('Dentro de sonido ', emocion)
if (emocion == 'FELIZ') then
frecuencia_sonido = 900
thread.resume(thr_sound)
elseif (emocion == 'TRISTE') then
frecuencia_sonido = 100
thread.resume(thr_sound)
elseif (emocion == 'ENOJADO') then
frecuencia_sonido = 1500
thread.resume(thr_sound)
elseif (emocion == 'ASUSTADO') then
frecuencia_sonido = 4000
thread.resume(thr_sound)
end
end
SWITCH_ESTADO = function(estado)
estado_actual = estado
end
TIEMPO = function(segundos)
thread.sleep(segundos)
end
VELOCIDAD = function(vel)
if (vel == 'LENTA') then
velocidad = 0.15
elseif (vel == 'MEDIA') then
velocidad = 0.30
elseif (vel == 'RAPIDA') then
velocidad = 0.50
end
--Ejecuto el movimiento actual con la nueva velocidad:
if(movimiento_actual ~= '') then
load(movimiento_actual)()
end
end
......@@ -18,11 +18,11 @@
local omni = {}
local device = require('omni_hbridge')
do
local WHEEL_DIAMETER = 0.038 --m
local ROBOT_RADIUS = 0.0675 --m
--WHEEL_PERIMETER = WHEEL_DIAMETER*3.141592654
local ENC_CPR = 3--12 --counts per revolution
local MOTOR_REDUCTION = 50
......@@ -74,7 +74,7 @@ end
-- @param number X_0
-- @param number Y_0
-- @param number Phi_0
device.set_odometry_callback(odom_publicator,10,0,0,0)
--device.set_odometry_callback(odom_publicator,10,0,0,0)
local cont_enables = 0
......
--Inicio wifi
local wifi = require('wifi_net')
wifi.init()
idEjecucion = 1
local colorSensor = require('color')
omni = require('omni')
omni.enable(true)
--Inicio thread de sonido
require('interprete_vpl')