135 lines
2.9 KiB
Plaintext
135 lines
2.9 KiB
Plaintext
########################################
|
|
#
|
|
# BARRIER-RELATED FUNCTIONS
|
|
#
|
|
########################################
|
|
|
|
#
|
|
# Constants
|
|
#
|
|
BARRIER_TIMEOUT = 600 # in steps
|
|
BARRIER_VSTIG = 80
|
|
timeW = 1
|
|
barrier = nil
|
|
hvs = 0;
|
|
|
|
|
|
#
|
|
# Sets a barrier
|
|
#
|
|
function barrier_create() {
|
|
timeW = 1
|
|
if(barrier==nil)
|
|
barrier = stigmergy.create(BARRIER_VSTIG)
|
|
}
|
|
|
|
function barrier_set(threshold, transf, resumef, bc) {
|
|
statef = function() {
|
|
barrier_wait(threshold, transf, resumef, bc);
|
|
}
|
|
BVMSTATE = "BARRIERWAIT"
|
|
barrier_create()
|
|
}
|
|
|
|
#
|
|
# Make yourself ready
|
|
#
|
|
function barrier_ready(bc) {
|
|
#log("BARRIER READY -------")
|
|
barrier.put(id, bc)
|
|
barrier.put("d", 0)
|
|
barrier.put("n", 1)
|
|
}
|
|
|
|
#
|
|
# Executes the barrier
|
|
#
|
|
function barrier_wait(threshold, transf, resumef, bc) {
|
|
if(barrier==nil) #failsafe
|
|
barrier_create()
|
|
|
|
barrier.put(id, bc)
|
|
if(barrier.get("n")<threshold)
|
|
barrier.put("n", threshold)
|
|
if(threshold>1) {
|
|
neighbors.foreach(function (nei,data){
|
|
barrier.get(nei)
|
|
})
|
|
}
|
|
|
|
# Not to miss any RC inputs, for waypoints, potential and deploy states
|
|
check_rc_wp()
|
|
|
|
#log("--->BS: ", barrier.size(), " / ", threshold, " (", BARRIER_VSTIG, " - ", barrier.get("d"), ") t= ", timeW)
|
|
if(barrier.size()-2 >= barrier.get("n")) {
|
|
if(barrier_allgood(barrier,bc)) {
|
|
barrier.put("d", 1)
|
|
timeW = 0
|
|
BVMSTATE = transf
|
|
} else
|
|
barrier.put("d", 0)
|
|
}
|
|
|
|
if(timeW >= BARRIER_TIMEOUT) {
|
|
log("------> Barrier Timeout !!!!")
|
|
#barrier = nil
|
|
timeW = 0
|
|
BVMSTATE = resumef
|
|
} else if(timeW % 10 == 0 and bc > 0)
|
|
neighbors.broadcast("cmd", bc)
|
|
|
|
timeW = timeW+1
|
|
}
|
|
|
|
function barrier_wait_graph(threshold, transf, resumef, bc) {
|
|
if(barrier==nil) #failsafe
|
|
barrier_create()
|
|
|
|
barrier.put(id, bc)
|
|
if(barrier.get("n")<threshold)
|
|
barrier.put("n", threshold)
|
|
if(threshold>1) {
|
|
neighbors.foreach(function (nei,data){
|
|
barrier.get(nei)
|
|
})
|
|
}
|
|
|
|
# Not to miss any RC inputs, for waypoints, potential and deploy states
|
|
check_rc_wp()
|
|
|
|
#log("--->BS: ", barrier.size(), " / ", threshold, " (", BARRIER_VSTIG, " - ", barrier.get("d"), ") t= ", timeW)
|
|
if(barrier.size()-2 >= barrier.get("n")) {
|
|
if(barrier_allgood(barrier,bc)) {
|
|
barrier.put("d", 1)
|
|
timeW = 0
|
|
GRAPHSTATE = transf
|
|
} else
|
|
barrier.put("d", 0)
|
|
}
|
|
|
|
if(timeW >= BARRIER_TIMEOUT) {
|
|
log("------> Barrier Timeout !!!!")
|
|
#barrier = nil
|
|
timeW = 0
|
|
GRAPHSTATE = resumef
|
|
} else if(timeW % 10 == 0 and bc > 0)
|
|
neighbors.broadcast("cmd", bc)
|
|
|
|
timeW = timeW+1
|
|
}
|
|
|
|
# Check all members state
|
|
function barrier_allgood(barrier, bc) {
|
|
barriergood = 1
|
|
barrier.foreach(function(key, value, robot){
|
|
barrier.get(key)
|
|
#log("VS entry : ", key, " ", value, " ", robot)
|
|
if(key == "d"){
|
|
if(value == 1)
|
|
return 1
|
|
} else if(key!="n" and value != bc)
|
|
barriergood = 0
|
|
})
|
|
return barriergood
|
|
}
|