diff --git a/buzz_scripts/include/act/barrier.bzz b/buzz_scripts/include/act/barrier.bzz index 8b56f3c..23e87d7 100644 --- a/buzz_scripts/include/act/barrier.bzz +++ b/buzz_scripts/include/act/barrier.bzz @@ -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= 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 } diff --git a/buzz_scripts/include/act/states.bzz b/buzz_scripts/include/act/states.bzz index 9668295..cc57742 100644 --- a/buzz_scripts/include/act/states.bzz +++ b/buzz_scripts/include/act/states.bzz @@ -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 + # }) + } } }) } diff --git a/buzz_scripts/main.bzz b/buzz_scripts/main.bzz index e87f015..f560857 100644 --- a/buzz_scripts/main.bzz +++ b/buzz_scripts/main.bzz @@ -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: