Commit 4c7f352d authored by gamorin's avatar gamorin
Browse files

cambios guille

parent 7434fd7f
--- Debug formatter that generates readable output.
-- @usage local ahsm = require 'ahsm'
-- @usage local ahsm = require 'ahsm'
-- local debug_plain = require 'tools.debug_plain'
-- ahsm.debug = debug_plain.out
local M = {}
--robot = require 'robot' -- global
local debug_names = {}
local function pick_debug_name(v, nv)
if debug_names[v] then return debug_names[v] end
if type(nv)=='string' then
if type(nv)=='string' then
debug_names[v] = nv
else
debug_names[v] = tostring(v._name or v)
else
debug_names[v] = tostring(v._name or v)
end
if v.container and v.container._name ~= '.' then -- build path for states
debug_names[v] = debug_names[v.container] ..'.' ..debug_names[v]
......@@ -25,6 +27,11 @@ end
-- Defaults to `print`
M.print = print
-- function(string)
-- sens_str = "state*" .. string
-- robot.wifi_net.broadcast(sens_str)
-- end
-- -- Function to be passed assigned to `ahsm.debug`.
M.out = function( action, p1, p2, p3, p4 )
if action == 'step' then
......@@ -49,4 +56,4 @@ M.out = function( action, p1, p2, p3, p4 )
end
return M
\ No newline at end of file
return M
......@@ -65,6 +65,8 @@ do
assert(apds9960r.color.set_ambient_gain(gain))
end
local cont_enables = 0
local color_rgb = {
['red'] = {100,0,0},
['yellow'] = {50,50,0},
......@@ -119,14 +121,24 @@ pio.pin.setdir(pio.OUTPUT, led_pin)
-- @tparam[opt=100] integer period Sampling period in ms, if omitted is
-- read from `nvs.read("color","period")`.
M.enable = function (on, period)
if on then
if on and cont_enables == 0 then
pio.pin.sethigh(led_pin)
period = period or nvs.read("color","period", 100) or 100
apds9960r.color.enable(period)
else
elseif (not on) and cont_enables == 1 then
apds9960r.color.enable(false)
pio.pin.setlow(led_pin)
end
if on then
cont_enables = cont_enables + 1
end
if (not on) and cont_enables ~= 0 then
cont_enables = cont_enables - 1
end
end
return M
local M = {}
M.out = function(string)
sens_str = "state*" .. string
robot.wifi_net.broadcast(sens_str)
end
return M
......@@ -15,6 +15,9 @@ vlring.init(sensors)
local time_budget = nvs.read("laser","time_budget", 5000) or 5000
vlring.set_measurement_timing_budget(time_budget)
local cont_enables = 0
local table_sort = table.sort
--CONSTANTS
......@@ -174,12 +177,22 @@ end
-- @tparam[opt=100] integer period Sampling period in ms, if omitted is read
-- from `nvs.read("laser","period")`
M.enable = function (on, period)
if on then
if on and cont_enables == 0 then
period = period or nvs.read("laser","period", 100) or 100
vlring.get_continuous(period, M.cb.call)
else
elseif (not on) and cont_enables == 1 then
vlring.get_continuous(nil)
end
if on then
cont_enables = cont_enables + 1
end
if (not on) and cont_enables ~= 0 then
cont_enables = cont_enables - 1
end
end
return M
--- Main program using ahsm.
-- Uses state machines to control the robot. This program is intended to
-- be loaded from the @{autorun} script. To achieve this, set `nvs.write("autorun", "main" "main_ahsm.lua")`.
-- This program loads and initalizes @{robot} and ahsm.
--
-- This program loads and initalizes @{robot} and ahsm.
--
-- Configuration is loaded using `nvs.read("ahsm", parameter)` calls, where the
-- available parameters are:
--
--* `"debugger"` the debug output system, like ahsm's "debug_plain". Defaults to nil (disabled)
--
--* `"dot_period"` if positive, a period in sec for printing a dot graph of the root state machine. Defaults to -1 (disabled)
--
--* `"root"` a composite state to be used as root for the state machine. This must be the name of library to be required, which will return an ahsm state. Defaults to "states.test"
--
-- available parameters are:
--
--* `"debugger"` the debug output system, like ahsm's "debug_plain". Defaults to nil (disabled)
--
--* `"dot_period"` if positive, a period in sec for printing a dot graph of the root state machine. Defaults to -1 (disabled)
--
--* `"root"` a composite state to be used as root for the state machine. This must be the name of library to be required, which will return an ahsm state. Defaults to "states.test"
--
--* `"timestep"` time in ms between sweeps to check for timed out transitions. Defaults to 10
-- @script main_ahsm
......@@ -24,8 +24,9 @@ ahsm.get_time = os.gettime
local hsm
-- initialize debugging
do
local debuggername = nvs.read("ahsm", "debugger", nil)
local debuggername = nvs.read("ahsm", "debugger", "debug_plain")
print('main_ahsm debugger:', debuggername)
if debuggername then
local debugger = require( debuggername )
......
--- Omnidirectional platform.
-- Configuration is loaded using `nvs.read("omni", parameter)` calls, where the
-- available parameters are:
-- available parameters are:
--
--* `"maxpower"` Limits the maximum power output of the motors as percentage.
-- Defaults to 80.0
-- Defaults to 80.0
 
--
--* `"kf"` Feed-forward parameter of the motor control in (power% / (tics/s) ).
-- Defaults to 90/1080
-- Defaults to 90/1080
--
--* `"kp"` P parameter of the PID control. Defaults to 0.01
--* `"kp"` P parameter of the PID control. Defaults to 0.01
--
--* `"ki"` I parameter of the PID control. Defaults to 0.05
--* `"ki"` I parameter of the PID control. Defaults to 0.05
--
--* `"kd"` D parameter of the PID control. Defaults to 0.0
-- @module omni
-- @alias M
......@@ -37,7 +37,7 @@ do
-- forward feed parameter
local KF = MAX_SPEED_POWER / MAX_SPEED_TICS
KF = nvs.read("omni","kf", KF) or KF
KF = nvs.read("omni","kf", KF) or KF
local KP = nvs.read("omni","kp", 0.01) or 0.01
local KI = nvs.read("omni","ki", 0.05) or 0.05
local KD = nvs.read("omni","kd", 0.0) or 0.0
......@@ -47,7 +47,7 @@ do
device.set_pid(KP, KI, KD, KF)
device.set_set_rad_per_tick(RAD_PER_TICK)
device.set_set_wheel_diameter(WHEEL_DIAMETER)
local function get_interpolator(x1, y1, x2, y2)
local p = (y2-y1)/(x2-x1)
return function (x)
......@@ -63,8 +63,11 @@ do
-- This is computed from the maxpower setting.
omni.max_speed = normalize(max)
end
local cont_enables = 0
--- The native C firmware module.
-- This can be used to access low level functionality from `omni_hbridge`. FIXME: docs
-- This can be used to access low level functionality from `omni_hbridge`. FIXME: docs
omni.device = device
--- Move the robot.
......@@ -77,7 +80,7 @@ omni.drive = device.drive
--- Enable/disable motors.
-- @function enable
-- @tparam[opt=true] boolean on if true value or omitted, power up.
-- @tparam[opt=true] boolean on if true value or omitted, power up.
-- If false value then power down.
omni.enable = device.set_enable
......@@ -94,14 +97,22 @@ omni.encoder = {}
omni.encoder.cb = require'cb_list'.get_list()
--- Enables the encoder callback.
-- When enabled, wheel movement will trigger @{omni.encoder.cb}.
-- When enabled, wheel movement will trigger @{omni.encoder.cb}.
-- @tparam boolean on true value to enable, false value to disable.
omni.encoder.enable = function (on)
if on then
if on and cont_enables == 0 then
device.set_encoder_callback(omni.encoder.cb.call)
else
elseif (not on) and cont_enables == 1 then
device.set_encoder_callback(nil)
end
if on then
cont_enables = cont_enables + 1
end
if (not on) and cont_enables ~= 0 then
cont_enables = cont_enables - 1
end
end
return omni
\ No newline at end of file
return omni
--- Downward facing proximity sensor.
--- Downward facing proximity sensor.
-- This modeule is used to detect when the robot is picked up.
-- @module proximity
-- @alias M
......@@ -8,8 +8,10 @@ local apds9960r = assert(require('apds9960'))
assert(apds9960r.init())
assert(apds9960r.enable_power())
local cont_enables = 0
--- The native C firmware module.
-- This can be used to access low level functionality from `apds9969.proximity`. FIXME: docs
-- This can be used to access low level functionality from `apds9969.proximity`. FIXME: docs
M.device = apds9960r.proximity
--- The callback module for the proximity sensor.
......@@ -22,24 +24,34 @@ M.cb = require'cb_list'.get_list()
apds9960r.proximity.set_callback(M.cb.call)
--- Enables the proximity callback.
-- When enabled, proximity changes will trigger @{cb}.
-- When enabled, proximity changes will trigger @{cb}.
-- @tparam boolean on true value to enable, false value to disable.
-- @tparam[opt=100] integer period Sampling period in ms, if omitted is read
-- from `nvs.read("proximity","period")`.
-- @tparam[opt=250] integer threshold proximity reference value, if omitted is
-- from `nvs.read("proximity","period")`.
-- @tparam[opt=250] integer threshold proximity reference value, if omitted is
-- read from `nvs.read("proximity","threshold")`
-- @tparam[opt=3] integer hysteresis if omitted is read from
-- `nvs.read("proximity","hysteresis")`
-- `nvs.read("proximity","hysteresis")`
M.enable = function (on, period, threshold, hysteresis)
if on then
if on and cont_enables == 0 then
period = period or nvs.read("proximity","period", 100) or 100
threshold = threshold or nvs.read("proximity","threshold", 250) or 250
hysteresis = hysteresis or nvs.read("proximity","hysteresis", 3) or 3
assert(apds9960r.proximity.enable(period, threshold, hysteresis))
else
elseif (not on) and cont_enables == 1 then
assert(apds9960r.proximity.enable(nil))
end
if on then
cont_enables = cont_enables + 1
end
if (not on) and cont_enables ~= 0 then
cont_enables = cont_enables - 1
end
end
return M
\ No newline at end of file
return M
......@@ -58,27 +58,26 @@ if (filter == "filter") then
end
--local laser_ring_publisher = function (d1,d2,d3,d4,d5,d6)
-- measure_cb(d1,d2,d3,d4,d5,d6)
-- local sens_str = nil
-- if (filter == nil) then
-- sens_str = LASER_CMD .. DELIMITER .. implode(DELIMITER, M.laser_ring.raw_d)
-- else
-- sens_str = LASER_CMD .. DELIMITER .. implode(DELIMITER, M.laser_ring.norm_d)
-- end
-- M.wifi_net.broadcast(sens_str)
--end
--
--local color_cb = function(color_name, h, s, v)
-- local color_str = COLOR_CMD .. DELIMITER .. color_name .. DELIMITER .. h .. DELIMITER .. s .. DELIMITER .. v
-- M.wifi_net.broadcast(color_str)
--end
--
--
--M.laser_ring.cb.append(M.laser_ring.get_filtering_cb())
M.laser_ring.cb.append(M.laser_ring.get_reading_cb())
--M.color.color_cb.append(color_cb)
local laser_ring_publisher = function (d1,d2,d3,d4,d5,d6)
measure_cb(d1,d2,d3,d4,d5,d6)
local sens_str = nil
if (filter == nil) then
sens_str = LASER_CMD .. DELIMITER .. implode(DELIMITER, M.laser_ring.raw_d)
else
sens_str = LASER_CMD .. DELIMITER .. implode(DELIMITER, M.laser_ring.norm_d)
end
--M.wifi_net.broadcast(sens_str)
end
local color_cb = function(color_name, h, s, v)
local color_str = COLOR_CMD .. DELIMITER .. color_name .. DELIMITER .. h .. DELIMITER .. s .. DELIMITER .. v
--M.wifi_net.broadcast(color_str)
end
M.laser_ring.cb.append(laser_ring_publisher)
M.color.color_cb.append(color_cb)
--M.laser_ring.enable(true)
--M.color.enable(true)
......
--- Use color patches to select movement direction.
-- The robot will move in the direction indicated by the led lights when
-- detects a color patch on the floor of the same color. If you pick the
-- robot up it powers down.
--
-- This is a state machine to be run using @{main_ahsm}. For this set
-- `nvs.write("ahsm", "root", "states.colorway")` and then run `dofile'main_ahsm.lua'`,
-- or simply set `nvs.write("autorun", "main", "main_ahsm.lua")` for autorun.
-- @hsm colorway.lua
-- color events
local ahsm = require 'ahsm'
local onoff = require 'states.onoff'
local remotecontrol = require 'states.remotecontrol'
local behavior_name = nvs.read("ahsm", "behavior", nil) or nil
print('behavior loading:', behavior_name)
local behavior = ahsm.state{
entry = function()
print("NO BEHAVIOR .... please load one")
end
}
if (behavior_name ~= nil) then
behavior = require( behavior_name )
end
local t_control = ahsm.transition {
src = behavior, tgt = remotecontrol,
events = { remotecontrol.events.WIFIMESSAGE },
}
local t_behavior = ahsm.transition {
src = remotecontrol, tgt = behavior,
events = { remotecontrol.events.FINCONTROL },
}
local s_on = onoff.states.ON
local s_off = onoff.states.OFF
s_off.entry = function ()
robot.led_ring.clear()
end
s_on.states = { remotecontrol , behavior }
s_on.transitions = {t_control, t_behavior}
s_on.initial = remotecontrol
s_on.entry = function ()
robot.omni.enable(true)
end
s_on.exit = function ()
robot.omni.enable(false)
end
return onoff
......@@ -89,14 +89,14 @@ local s_color = ahsm.state {
ledr.clear()
robot.omni.drive(0, 0, 0)
paint_leds_empty()
s_color.color_cb.append(produce_color_event)
color.color_cb.append(produce_color_event)
s_color.enable(true) -- only enable (no dis. because it could be used by others)
end,
exit = function ()
ledr.clear()
robot.omni.drive(0, 0, 0)
s_color.enable(false)
s_color.color_cb.remove(produce_color_event)
color.color_cb.remove(produce_color_event)
end,
}
......
......@@ -2,7 +2,8 @@ local ahsm = require 'ahsm'
local VEL_CMD = 'speed'
local e_msg = { _name="WIFI_MESSAGE", xdot = nil, ydot = nil, w = nil, }
local e_msg = { _name="WIFI_MESSAGE", cmd = nil,}
local e_fin = { _name="FINCONTROL", }
local offset_led = 2
......@@ -18,20 +19,6 @@ local s_remote_control = ahsm.state {
end,
}
local behavior_name = nvs.read("ahsm", "behavior", nil) or nil
print('behavior loading:', behavior_name)
local s_behavior = ahsm.state{
entry = function()
print("NO BEHAVIOR .... please load one")
end
}
if (behavior_name ~= nil) then
s_behavior = require( behavior_name )
end
function split(s, delimiter)
local result = {};
for match in (s..delimiter):gmatch("(.-)"..delimiter) do
......@@ -43,51 +30,40 @@ end
local t_command = ahsm.transition {
src = s_remote_control, tgt = s_remote_control,
events = { e_msg },
timeout = 5.0,
effect = function (ev)
robot.omni.drive(ev.xdot,ev.ydot,ev.w)
end
}
local t_timeout = ahsm.transition {
src = s_remote_control, tgt = s_behavior,
timeout = 3.0,
}
local t_behavior_to_command = ahsm.transition {
src = s_behavior, tgt = s_remote_control,
events = { e_msg },
effect = function (ev)
robot.omni.drive(ev.xdot,ev.ydot,ev.w)
if (ev == e_msg) then
local data = ev.data
if data[1] == VEL_CMD then
if #data == 5 then
local xdot = data[2]
local ydot = data[3]
local w = data[4]
robot.omni.drive(xdot,ydot,w)
end
end -- Mas comandos con else if
else
robot.hsm.queue_event(e_fin)
end
end
}
local event_message = function(data,ip,port)
local data = split(data, '*')
if data[1] == VEL_CMD then
if #data == 5 then
e_msg.xdot = data[2]
e_msg.ydot = data[3]
e_msg.w = data[4]
robot.hsm.queue_event(e_msg)
else
msg = '[ERROR] Malformed command.'
end
-- TODO WIFI SEND msg
end
e_msg.data = data
robot.hsm.queue_event(e_msg)
end
-- root state
local remote = ahsm.state {
events = { WIFIMESSAGE = e_msg },
states = { REMOTECONTROL=s_remote_control, BEHAVIOUR=s_behavior },
transitions = { TIMEOUT=t_timeout, COMMAND=t_command, BEHAVTOCOMMD=t_behavior_to_command},
initial = s_behavior,
events = { WIFIMESSAGE = e_msg, FINCONTROL = e_fin },
states = { REMOTECONTROL=s_remote_control},
transitions = { COMMAND=t_command},
initial = s_remote_control,
entry = function()
robot.wifi_net.cb.append(event_message)
robot.omni.enable(true)
end,
exit = function()
robot.omni.enable(false)
robot.wifi_net.cb.remove(event_message)
end,
}
......
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