ROSBuzz_MISTLab/buzz_scripts/testflocksim.bzz

120 lines
2.9 KiB
Plaintext
Raw Normal View History

2017-05-09 14:28:32 -03:00
include "vec2.bzz"
2017-06-19 17:15:18 -03:00
include "update.bzz"
include "barrier.bzz" # don't use a stigmergy id=11 with this header.
include "uavstates.bzz" # require an 'action' function to be defined here.
include "vstigenv.bzz"
2017-04-21 14:29:53 -03:00
2017-04-24 15:20:59 -03:00
TARGET_ALTITUDE = 10.0
2017-04-21 14:29:53 -03:00
# Lennard-Jones parameters
2017-04-24 15:20:59 -03:00
TARGET = 12.0
2017-05-24 00:32:25 -03:00
EPSILON = 14.0
2017-04-21 14:29:53 -03:00
# Lennard-Jones interaction magnitude
function lj_magnitude(dist, target, epsilon) {
return -(epsilon / dist) * ((target / dist)^4 - (target / dist)^2)
2017-05-24 00:32:25 -03:00
#return -(4 * epsilon) * ((target / dist)^12 - (target / dist)^6)
2017-04-21 14:29:53 -03:00
}
# Neighbor data to LJ interaction vector
function lj_vector(rid, data) {
return math.vec2.newp(lj_magnitude(data.distance, TARGET, EPSILON), data.azimuth)
}
# Accumulator of neighbor LJ interactions
function lj_sum(rid, data, accum) {
return math.vec2.add(data, accum)
}
2017-05-24 00:32:25 -03:00
function user_attract(t) {
fus = math.vec2.new(0.0, 0.0)
if(size(t)>0) {
foreach(t, function(u, tab) {
#log("id: ",u," Range ", tab.r, "Bearing ", tab.b)
2017-05-24 00:52:49 -03:00
fus = math.vec2.add(fus, math.vec2.newp(lj_magnitude(tab.r, 3 * TARGET / 4.0, EPSILON * 2.0), tab.b))
2017-05-24 00:32:25 -03:00
})
math.vec2.scale(fus, 1.0 / size(t))
}
#print("User attract:", fus.x," ", fus.y, " [", size(t), "]")
return fus
}
2017-04-21 14:29:53 -03:00
# Calculates and actuates the flocking interaction
2017-06-19 17:15:18 -03:00
function action() {
statef=action
2017-04-21 14:29:53 -03:00
# Calculate accumulator
var accum = neighbors.map(lj_vector).reduce(lj_sum, math.vec2.new(0.0, 0.0))
if(neighbors.count() > 0)
2017-05-24 00:32:25 -03:00
accum = math.vec2.scale(accum, 1.0 / neighbors.count())
accum = math.vec2.add(accum, user_attract(users.dataL))
accum = math.vec2.scale(accum, 1.0 / 2.0)
2017-05-24 00:52:49 -03:00
if(math.vec2.length(accum) > 1.0) {
accum = math.vec2.scale(accum, 1.0 / math.vec2.length(accum))
2017-05-24 00:32:25 -03:00
}
2017-04-21 14:29:53 -03:00
# Move according to vector
2017-05-24 00:32:25 -03:00
print("Robot ", id, "must push ", math.vec2.length(accum) )#, "; ", math.vec2.angle(accum))
uav_moveto(accum.x, accum.y)
2017-06-19 17:15:18 -03:00
UAVSTATE = "LENNARDJONES"
2017-04-21 14:29:53 -03:00
# if(timeW>=WAIT_TIMEOUT) { #FOR MOVETO TESTS
# timeW =0
# statef=land
2017-05-24 00:32:25 -03:00
# } else if(timeW>=WAIT_TIMEOUT/2) {
2017-06-19 17:15:18 -03:00
# UAVSTATE ="GOEAST"
2017-05-24 00:32:25 -03:00
# timeW = timeW+1
# uav_moveto(0.0,5.0)
2017-04-21 14:29:53 -03:00
# } else {
2017-06-19 17:15:18 -03:00
# UAVSTATE ="GONORTH"
2017-04-21 14:29:53 -03:00
# timeW = timeW+1
2017-05-24 00:32:25 -03:00
# uav_moveto(5.0,0.0)
2017-04-21 14:29:53 -03:00
# }
}
2017-05-10 08:34:57 -03:00
########################################
#
# MAIN FUNCTIONS
#
########################################
2017-04-21 14:29:53 -03:00
# Executed once at init time.
function init() {
2017-06-19 17:15:18 -03:00
uav_initswarm()
vt = stigmergy.create(5)
t = {}
vt.put("p",t)
2017-04-21 14:29:53 -03:00
}
# Executed at each time step.
function step() {
2017-06-19 17:15:18 -03:00
uav_rccmd()
uav_neicmd()
2017-04-21 14:29:53 -03:00
statef()
2017-06-19 17:15:18 -03:00
log("Current state: ", UAVSTATE)
2017-04-21 14:29:53 -03:00
log("Swarm size: ",ROBOTS)
2017-04-24 15:20:59 -03:00
# Read a value from the structure
2017-05-17 17:23:51 -03:00
# log(users)
2017-05-10 08:34:57 -03:00
#users_print(users.dataG)
if(size(users.dataG)>0)
vt.put("p", users.dataG)
2017-05-07 17:02:23 -03:00
2017-04-24 15:20:59 -03:00
# Get the number of keys in the structure
2017-05-17 17:23:51 -03:00
#log("The vstig has ", vt.size(), " elements")
2017-05-10 08:34:57 -03:00
users_save(vt.get("p"))
table_print(users.dataL)
2017-04-21 14:29:53 -03:00
}
# Executed once when the robot (or the simulator) is reset.
function reset() {
}
# Executed once at the end of experiment.
function destroy() {
}