Commit 4ce8a673 authored by Sofia Llavayol's avatar Sofia Llavayol
Browse files

luces full anda ahi

parent 947c28c3
......@@ -8,7 +8,12 @@ local v = 0.05
local e_dist = {
{},{},{},{},{},{}
{color = {10,0,0}},
{color = {10,5,0}},
{color = {10,10,0}},
{color = {0,10,0}},
{color = {0,0,10}},
{color = {5,3,10}}
}
for i, e in ipairs(e_dist) do
......@@ -18,12 +23,14 @@ for i, e in ipairs(e_dist) do
e.maxdist = 0
end
--EVENTS
local e_ksearch = { _name = "KEEP_SEARCH" }
local e_back = { _name = "GO_BACK" }
local e_bsearch = { _name = "BACK_SEARCH" }
local e_position = { _name = "GO_POSITION" }
local e_reforward = { _name = "GO_REFORWARD" }
local e_forward = { _name = "GO_FORWARD" }
local e_repos = { _name = "RE_POSITION" }
local e_search = { _name = "SEARCH_AGAIN" }
......@@ -35,41 +42,19 @@ local range = function(num)
return (num > 0 and num < 100)
end
local min = function(num1,num2)
if(num1 < num2) then
return num1
else
return num2
end
end
local max = function(num1,num2)
if(num1 > num2) then
return num1
else
return num2
end
end
local th = function(num,th_dist)
local maximo = min(th_dist + num, 99)
return maximo
end
local s_far
local w2 = 0.3
local cont_direction
local angle
local t_ms = 100 -- period of distance measurements
local t_ms = 80 -- period of distance measurements
nvs.write('laser','period',t_ms)
local dtheta = (math.pi/3)/30 -- partition of pi/3
local w = 1000*dtheta/t_ms -- rad/s
--[[
t_ms ms ____ dtheta rad
1000 ms ____ w = 1000*dtheta/t_ms rad
]]--
local find_sense
--CALLBACKS
local argmax = function(t)
......@@ -80,22 +65,27 @@ local argmax = function(t)
index = i
max = t[i]
end
end
end
return index
end
local callback_search = function()
local norm_d = laser.norm_d
leds.clear()
for i = 1,6 do
if(range(norm_d[i]))then
leds.set_segment(i,e_dist[i].color[1],e_dist[i].color[2],e_dist[i].color[3],true)
end
end
--print(norm_d[1],norm_d[2],norm_d[3],norm_d[4],norm_d[5],norm_d[6])
angle = angle + dtheta
if (angle < math.pi/3) then
for i = 1,6 do
if(range(norm_d[i]) and norm_d[i] > e_dist[i].maxdist) then
if(range(norm_d[i]) and norm_d[i] < e_dist[i].maxdist) then
e_dist[i].maxdist = norm_d[i]
end
end
else
--print('callback else')
omni.drive(0,0,0)
local d = {}
for i = 1,6 do
......@@ -111,18 +101,22 @@ local callback_search = function()
end -- callback_search
local callback_back = function()
local norm_d = laser.norm_d
leds.clear()
for i = 1,6 do
if(range(norm_d[i]))then
leds.set_segment(i,e_dist[i].color[1],e_dist[i].color[2],e_dist[i].color[3],true)
end
end
angle = angle + dtheta
if (angle < math.pi/3) then
local actual = norm_d[s_far]
local max = e_dist[s_far].maxdist
local th_dist = 10
if (math.abs(actual-max) < th_dist) then
robot.hsm.queue_event(e_position) --change state
if (math.abs(actual-max) < th_dist and range(actual)) then
robot.hsm.queue_event(e_forward) --change state
end
else
e_dist[s_far].maxdist = norm_d[s_far]
robot.hsm.queue_event(e_bsearch) --change state
end
end
......@@ -130,37 +124,48 @@ end
local callback_position = function()
local norm_d = laser.norm_d
leds.clear()
for i = 1,6 do
if(range(norm_d[i]))then
leds.set_segment(i,e_dist[i].color[1],e_dist[i].color[2],e_dist[i].color[3],true)
end
end
local actual = norm_d[s_far]
local previous = e_dist[s_far].maxdist
local th_dist = 20
local th_dist = 10
--print (actual, previous)
if ( math.abs(actual - previous) < th_dist and actual ~= 100) then
if (math.abs(actual - previous) < th_dist and range(actual)) then
omni.drive(0,0,0)
robot.hsm.queue_event(e_forward)
robot.hsm.queue_event(e_reforward)
else
cont_direction = cont_direction + 1
end
if (cont_direction > 15) then
cont_direction = -15
w2 = -w2
omni.drive(0,0,w2)
if (cont_direction > 8) then
cont_direction = -8
w = -w
omni.drive(0,0,w)
end
end
local callback_forward = function()
local norm_d = laser.norm_d
leds.clear()
for i = 1,6 do
if(range(norm_d[i]))then
leds.set_segment(i,e_dist[i].color[1],e_dist[i].color[2],e_dist[i].color[3],true)
end
end
local actual = norm_d[s_far]
local previous = e_dist[s_far].maxdist
local th_dist = 5
if (actual > previous + th_dist or actual == 100) then
local th_dist = 20
if (actual > previous + th_dist or not range(actual)) then
robot.hsm.queue_event(e_repos) --change state
elseif (actual == 0) then
elseif (actual < 5 and range(actual)) then
robot.hsm.queue_event(e_search)
else
e_dist[s_far].maxdist = actual
end
end
......@@ -168,20 +173,15 @@ end
local s_search = ahsm.state{
entry = function()
leds.clear()
leds.set_all(37,42, 10, true)
angle = 0
for i,e in ipairs(e_dist) do
e.maxdist = 0
e.maxdist = 100
end
omni.drive(0,0,w)
laser.cb.append(callback_search)
--laser.enable(true)
omni.enable(true)
end,
exit = function()
leds.clear()
-- leds.set_segment(s_far,10,0,10, true)
laser.cb.remove(callback_search)
end
}
......@@ -191,7 +191,6 @@ local s_back = ahsm.state{
angle = 0
omni.drive(0,0,-w)
laser.cb.append(callback_back)
--laser.enable(true)
end,
exit = function()
omni.drive(0,0,0)
......@@ -201,34 +200,26 @@ local s_back = ahsm.state{
local s_position = ahsm.state{
entry = function()
leds.clear()
leds.set_segment(s_far,0,10,10, true)
cont_direction = 0
omni.drive(0,0,w2)
omni.drive(0,0,w)
laser.cb.append(callback_position)
--laser.enable(true)
end,
exit = function()
w = -w
omni.drive(0,0,0)
--laser.enable(false)
laser.cb.remove(callback_position)
end
}
local s_forward = ahsm.state{
entry = function()
leds.clear()
leds.set_segment(s_far,10,0,0, true)
local dirx = e_dist[s_far].dirx
local diry = e_dist[s_far].diry
omni.drive(dirx*v,diry*v,0)
laser.cb.append(callback_forward)
--laser.enable(true)
end,
exit = function()
omni.drive(0,0,0)
--laser.enable(false)
laser.cb.remove(callback_forward)
end
}
......@@ -237,7 +228,7 @@ local s_forward = ahsm.state{
local t_reset = ahsm.transition{
src = s_position, tgt = s_search,
timeout = 10
timeout = 6
}
local t_ksearch = ahsm.transition {
......@@ -255,14 +246,14 @@ local t_bsearch = ahsm.transition {
events = {e_bsearch}
}
local t_pos = ahsm.transition {
src = s_back, tgt = s_position,
events = {e_position}
local t_forw = ahsm.transition {
src = s_back, tgt = s_forward,
events = {e_forward}
}
local t_forw = ahsm.transition {
local t_reforw = ahsm.transition {
src = s_position, tgt = s_forward,
events = {e_forward}
events = {e_reforward}
}
local t_repos = ahsm.transition {
......@@ -283,7 +274,7 @@ local distance = ahsm.state {
keep_search = t_ksearch,
switch_back = t_back,
switch_bsearch = t_bsearch,
switch_pos = t_pos,
switch_reforw = t_reforw,
switch_forw = t_forw,
switch_repos = t_repos,
switch_search = t_search,
......@@ -293,7 +284,7 @@ local distance = ahsm.state {
e_ksearch,
e_back,
e_bsearch,
e_position,
e_reforward,
e_forward,
e_repos,
e_search,
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment