Spiri
/
ROSBuzz_MISTLab
12
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

added states to webcontrol and enhance barrier for state machine

This commit is contained in:
dave 2018-06-01 13:10:59 -04:00
parent d3214b9cf6
commit db452bd063
3 changed files with 82 additions and 55 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

37
buzz_scripts/include/act/barrier.bzz
View File

@ -7,10 +7,11 @@
#
# Constants
#
BARRIER_TIMEOUT = 1200 # in steps
BARRIER_TIMEOUT = 200 # in steps
BARRIER_VSTIG = 80
timeW = 0
barrier = nil
bc = 0;
#
# Sets a barrier
@ -22,15 +23,15 @@ function barrier_create() {
#log("---> Prev. br. ", barrier, " ", BARRIER_VSTIG)
if(barrier!=nil) {
barrier=nil
BARRIER_VSTIG = BARRIER_VSTIG +1
# BARRIER_VSTIG = BARRIER_VSTIG +1
}
#log("---> New. br. ", barrier, " ", BARRIER_VSTIG)
barrier = stigmergy.create(BARRIER_VSTIG)
}
function barrier_set(threshold, transf, resumef) {
function barrier_set(threshold, transf, resumef, bc) {
statef = function() {
barrier_wait(threshold, transf, resumef);
barrier_wait(threshold, transf, resumef, bc);
}
BVMSTATE = "BARRIERWAIT"
barrier_create()
@ -39,30 +40,42 @@ function barrier_set(threshold, transf, resumef) {
#
# Make yourself ready
#
function barrier_ready() {
function barrier_ready(bc) {
#log("BARRIER READY -------")
barrier.put(id, 1)
barrier.put(id, bc)
barrier.put("d", 0)
}
#
# Executes the barrier
#
function barrier_wait(threshold, transf, resumef) {
barrier.put(id, 1)
function barrier_wait(threshold, transf, resumef, bc) {
barrier.put(id, bc)
barrier.get(id)
#log("--->BS: ", barrier.size(), " (", BARRIER_VSTIG, ")")
var allgood = 0
log("--->BS: ", barrier.size(), " / ", threshold, " (", BARRIER_VSTIG, " - ", barrier.get("d"), ") t= ", timeW)
if(barrier.size() - 1 >= threshold or barrier.get("d") == 1) {
var bi = 0
allgood = 1
while (bi<threshold) {
if(barrier.get(bi) != bc)
allgood = 0
bi = bi + 1
}
}
if(allgood) {
barrier.put("d", 1)
timeW = 0
BVMSTATE = transf
} else if(timeW >= BARRIER_TIMEOUT) {
log("------> Barrier Timeout !!!!")
barrier=nil
barrier = nil
timeW = 0
BVMSTATE = resumef
}
} else if(timeW % 10 == 0 and bc > 0)
neighbors.broadcast("cmd", bc)
timeW = timeW+1
}

98
buzz_scripts/include/act/states.bzz
View File

@ -32,31 +32,31 @@ function launch() {
if(V_TYPE == 0 or V_TYPE == 1) { # flying vehicle so LAND
homegps = {.lat=pose.position.latitude, .long=pose.position.longitude}
if( flight.status == 2 and pose.position.altitude >= TARGET_ALTITUDE-TARGET_ALTITUDE/20.0) {
barrier_set(ROBOTS, AUTO_LAUNCH_STATE, "LAUNCH")
barrier_ready()
barrier_set(ROBOTS, AUTO_LAUNCH_STATE, "LAUNCH", 22)
barrier_ready(22)
} else {
log("Altitude: ", pose.position.altitude)
neighbors.broadcast("cmd", 22)
uav_takeoff(TARGET_ALTITUDE)
}
} else {
barrier_set(ROBOTS, AUTO_LAUNCH_STATE, "LAUNCH")
barrier_ready()
barrier_set(ROBOTS, AUTO_LAUNCH_STATE, "LAUNCH", 22)
barrier_ready(22)
}
}
function stop() {
BVMSTATE = "STOP"
BVMSTATE = "STOP"
if(V_TYPE == 0 or V_TYPE == 1) { # flying vehicle so LAND
neighbors.broadcast("cmd", 21)
uav_land()
if(flight.status != 2 and flight.status != 3) {
barrier_set(ROBOTS,"TURNEDOFF","STOP")
barrier_ready()
barrier_set(ROBOTS,"TURNEDOFF","STOP", 21)
barrier_ready(21)
}
} else {
barrier_set(ROBOTS,"TURNEDOFF","STOP")
barrier_ready()
barrier_set(ROBOTS,"TURNEDOFF","STOP", 21)
barrier_ready(21)
}
}
@ -117,7 +117,7 @@ function aggregate() {
# circle all together around centroid
function pursuit() {
BVMSTATE="PURSUIT"
rd = 15.0
rd = 20.0
pc = 3.2
vd = 15.0
r_vec = neighbors.reduce(function(rid, data, r_vec) {
@ -137,9 +137,9 @@ function pursuit() {
# Lennard-Jones interaction magnitude
TARGET = 8.0
EPSILON = 3.0
EPSILON = 0.1
function lj_magnitude(dist, target, epsilon) {
lj = -(epsilon / dist) * ((target / dist)^4 - 1.2 * (target / dist)^2)
lj = -(epsilon / dist) * ((target / dist)^4 - 1.02 * (target / dist)^2)
return lj
}
@ -161,12 +161,12 @@ function formation() {
accum_lj = neighbors.map(lj_vector).reduce(lj_sum, math.vec2.new(0.0, 0.0))
if(neighbors.count() > 0)
accum_lj = math.vec2.scale(accum_lj, 1.0 / neighbors.count())
old_lj = LimitSpeed(math.vec2.add(old_lj, accum_lj), 1.0/4.0)
old_lj = LimitSpeed(math.vec2.add(old_lj, accum_lj), 1.0/2.0)
goto_abs(old_lj.x, old_lj.y, 0.0, 0.0)
}
function rc_cmd_listen() {
if(flight.rc_cmd==22) {
if(flight.rc_cmd==22) {
log("cmd 22")
flight.rc_cmd=0
BVMSTATE = "LAUNCH"
@ -192,22 +192,26 @@ function rc_cmd_listen() {
stattab_send()
} else if (flight.rc_cmd==900){
flight.rc_cmd=0
BVMSTATE = "TASK_ALLOCATE"
barrier_set(ROBOTS, "TASK_ALLOCATE", BVMSTATE, 900)
barrier_ready(900)
neighbors.broadcast("cmd", 900)
} else if (flight.rc_cmd==901){
flight.rc_cmd=0
destroyGraph()
BVMSTATE = "PURSUIT"
barrier_set(ROBOTS, "PURSUIT", BVMSTATE, 901)
barrier_ready(901)
neighbors.broadcast("cmd", 901)
} else if (flight.rc_cmd==902){
flight.rc_cmd=0
destroyGraph()
BVMSTATE = "AGGREGATE"
barrier_set(ROBOTS, "AGGREGATE", BVMSTATE, 902)
barrier_ready(902)
neighbors.broadcast("cmd", 902)
} else if (flight.rc_cmd==903){
flight.rc_cmd=0
destroyGraph()
BVMSTATE = "FORMATION"
barrier_set(ROBOTS, "FORMATION", BVMSTATE, 903)
barrier_ready(903)
neighbors.broadcast("cmd", 903)
}
}
@ -216,30 +220,40 @@ function nei_cmd_listen() {
neighbors.listen("cmd",
function(vid, value, rid) {
print("Got (", vid, ",", value, ") #", rid, "(", BVMSTATE, ")")
if(value==22 and BVMSTATE!="BARRIERWAIT") {
BVMSTATE = "LAUNCH"
} else if(value==21 and BVMSTATE!="BARRIERWAIT") {
BVMSTATE = "STOP"
} else if(value==400 and BVMSTATE=="TURNEDOFF") {
uav_arm()
} else if(value==401 and BVMSTATE=="TURNEDOFF"){
uav_disarm()
} else if(value==900){ # Shapes
BVMSTATE = "TASK_ALLOCATE"
} else if(value==901){ # Pursuit
destroyGraph()
BVMSTATE = "PURSUIT"
} else if(value==902){ # Agreggate
destroyGraph()
BVMSTATE = "AGGREGATE"
} else if(value==903){ # Formation
destroyGraph()
BVMSTATE = "FORMATION"
} else if(value==16 and BVMSTATE=="IDLE"){
# neighbors.listen("gt",function(vid, value, rid) {
# print("Got (", vid, ",", value, ") from robot #", rid)
# # if(gt.id == id) statef=goto
# })
if(BVMSTATE!="BARRIERWAIT") {
if(value==22) {
BVMSTATE = "LAUNCH"
} else if(value==21) {
BVMSTATE = "STOP"
} else if(value==400 and BVMSTATE=="TURNEDOFF") {
uav_arm()
} else if(value==401 and BVMSTATE=="TURNEDOFF"){
uav_disarm()
} else if(value==900){ # Shapes
barrier_set(ROBOTS, "TASK_ALLOCATE", BVMSTATE, 900)
#barrier_ready(900)
neighbors.broadcast("cmd", 900)
} else if(value==901){ # Pursuit
destroyGraph()
barrier_set(ROBOTS, "PURSUIT", BVMSTATE, 901)
#barrier_ready(901)
neighbors.broadcast("cmd", 901)
} else if(value==902){ # Agreggate
destroyGraph()
barrier_set(ROBOTS, "AGGREGATE", BVMSTATE, 902)
#barrier_ready(902)
neighbors.broadcast("cmd", 902)
} else if(value==903){ # Formation
destroyGraph()
barrier_set(ROBOTS, "FORMATION", BVMSTATE, 903)
#barrier_ready(903)
neighbors.broadcast("cmd", 903)
} else if(value==16 and BVMSTATE=="IDLE"){
# neighbors.listen("gt",function(vid, value, rid) {
# print("Got (", vid, ",", value, ") from robot #", rid)
# # if(gt.id == id) statef=goto
# })
}
}
})
}

2
buzz_scripts/main.bzz
View File

@ -7,7 +7,7 @@ include "taskallocate/graphformGPS.bzz"
include "vstigenv.bzz"
#State launched after takeoff
AUTO_LAUNCH_STATE = "PURSUIT"
AUTO_LAUNCH_STATE = "IDLE"
#####
# Vehicule type: