From 1bfeada346c627833264314a884ef51feb5a59d8 Mon Sep 17 00:00:00 2001 From: vivek-shankar Date: Wed, 4 Jul 2018 21:17:42 -0400 Subject: [PATCH] adapted bzz files from sim. --- buzz_scripts/include/act/barrier.bzz | 36 ++++--- buzz_scripts/include/act/states.bzz | 148 +++++++++++++++++--------- buzz_scripts/include/plan/rrtstar.bzz | 2 +- buzz_scripts/include/update.bzz | 3 +- buzz_scripts/main.bzz | 13 ++- buzz_scripts/minimal.bzz | 1 - buzz_scripts/stand_by.bzz | 45 ++++---- buzz_scripts/testLJ.bzz | 72 +++++++++++++ 8 files changed, 229 insertions(+), 91 deletions(-) create mode 100755 buzz_scripts/testLJ.bzz diff --git a/buzz_scripts/include/act/barrier.bzz b/buzz_scripts/include/act/barrier.bzz index 8b56f3c..a87bed7 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,41 @@ 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 = LOWEST_ROBOT_ID + 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 0b80cea..cda775b 100644 --- a/buzz_scripts/include/act/states.bzz +++ b/buzz_scripts/include/act/states.bzz @@ -27,44 +27,60 @@ function idle() { BVMSTATE = "IDLE" } +# Custom function for the user. +function cusfun(){ + BVMSTATE="CUSFUN" + log("cusfun: yay!!!") +} + function launch() { BVMSTATE = "LAUNCH" - if(V_TYPE == 0 or V_TYPE == 1) { # flying vehicle so LAND + if(V_TYPE == 0 or V_TYPE == 1) { # flying vehicle so TAKE_OFF 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, "STOP", 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, "STOP", 22) + barrier_ready(22) } } +gohomeT=100 +function goinghome() { + BVMSTATE = "GOHOME" + if(gohomeT > 0) { # TODO: Make a real check if home is reached + gohome() + gohomeT = gohomeT - 1 + } else + BVMSTATE = AUTO_LAUNCH_STATE +} + 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) + } } function take_picture() { BVMSTATE="PICTURE" - uav_setgimbal(0.0, 0.0, -90.0, 20.0) + setgimbal(0.0, 0.0, -90.0, 20.0) if(pic_time==PICTURE_WAIT/2) { # wait for the drone to stabilize - uav_takepicture() + takepicture() } else if(pic_time>=PICTURE_WAIT) { # wait for the picture BVMSTATE="IDLE" pic_time=0 @@ -117,7 +133,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) { @@ -127,8 +143,7 @@ function pursuit() { if(neighbors.count() > 0) r_vec = math.vec2.scale(r_vec, 1.0 / neighbors.count()) r = math.vec2.length(r_vec) - sqr = (r-rd)*(r-rd)+pc*pc*r*r - gamma = vd / math.sqrt(sqr) + gamma = vd / math.sqrt((r-rd)*(r-rd)+pc*pc*r*r) dr = -gamma * (r-rd) dT = gamma * pc vfg = math.vec2.newp(r+dr*0.1, math.vec2.angle(r_vec)+dT*0.1) @@ -138,9 +153,9 @@ function pursuit() { # Lennard-Jones interaction magnitude TARGET = 8.0 -EPSILON = 3.0 +EPSILON = 30.0 function lj_magnitude(dist, target, epsilon) { - lj = -(epsilon / dist) * ((target / dist)^4 - 1.2 * (target / dist)^2) + lj = -(epsilon / dist) * ((target / dist)^4 - (target / dist)^2) return lj } @@ -155,92 +170,119 @@ function lj_sum(rid, data, accum) { } # Calculates and actuates the flocking interaction -old_lj = math.vec2.newp(0.0, 0.0) # 1-step filter, to smoothen dir. changes function formation() { BVMSTATE="FORMATION" # Calculate accumulator 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) - goto_abs(old_lj.x, old_lj.y, 0.0, 0.0) + accum_lj = LimitSpeed(accum_lj, 1.0/3.0) + goto_abs(accum_lj.x, accum_lj.y, 0.0, 0.0) } +# listens to commands from the remote control (web, commandline, etc) function rc_cmd_listen() { - if(flight.rc_cmd==22) { + if(flight.rc_cmd==22) { log("cmd 22") flight.rc_cmd=0 BVMSTATE = "LAUNCH" neighbors.broadcast("cmd", 22) } else if(flight.rc_cmd==21) { - log("cmd 21") flight.rc_cmd=0 + AUTO_LAUNCH_STATE = "TURNEDOFF" + barrier_set(ROBOTS, "GOHOME", BVMSTATE, 21) + barrier_ready(21) BVMSTATE = "STOP" neighbors.broadcast("cmd", 21) + } else if(flight.rc_cmd==20) { + flight.rc_cmd=0 + AUTO_LAUNCH_STATE = "IDLE" + barrier_set(ROBOTS, "GOHOME", BVMSTATE, 20) + barrier_ready(20) + neighbors.broadcast("cmd", 20) } else if(flight.rc_cmd==16) { flight.rc_cmd=0 BVMSTATE = "PATHPLAN" } else if(flight.rc_cmd==400) { flight.rc_cmd=0 - uav_arm() + arm() neighbors.broadcast("cmd", 400) } else if (flight.rc_cmd==401){ flight.rc_cmd=0 - uav_disarm() + disarm() neighbors.broadcast("cmd", 401) } else if (flight.rc_cmd==666){ flight.rc_cmd=0 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) } } +# listens to other fleet members broadcasting commands 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==20) { + AUTO_LAUNCH_STATE = "IDLE" + BVMSTATE = "GOHOME" + } else if(value==21) { + AUTO_LAUNCH_STATE = "TURNEDOFF" + BVMSTATE = "STOP" + } else if(value==400 and BVMSTATE=="TURNEDOFF") { + arm() + } else if(value==401 and BVMSTATE=="TURNEDOFF"){ + 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/include/plan/rrtstar.bzz b/buzz_scripts/include/plan/rrtstar.bzz index 54c69ab..a55aff1 100644 --- a/buzz_scripts/include/plan/rrtstar.bzz +++ b/buzz_scripts/include/plan/rrtstar.bzz @@ -20,7 +20,7 @@ mapgoal = {} function navigate() { BVMSTATE="NAVIGATE" if(V_TYPE == 0) { # TODO: place these in a generic function in conversion.bzz - uav_storegoal(goal_list[g_it].x, goal_list[g_it].y, pose.position.altitude) + storegoal(goal_list[g_it].x, goal_list[g_it].y, pose.position.altitude) cur_goal = vec_from_gps(rc_goto.latitude, rc_goto.longitude, 0) m_pos = math.vec2.scale(vec_from_gps(homegps.lat, homegps.long, 0),-1) } diff --git a/buzz_scripts/include/update.bzz b/buzz_scripts/include/update.bzz index f49fd7c..4a5c699 100644 --- a/buzz_scripts/include/update.bzz +++ b/buzz_scripts/include/update.bzz @@ -4,6 +4,7 @@ #################################################################################################### updated="update_ack" update_no=0 -function updated_neigh(){ +updates_active = 1 +function updated_no_bct(){ neighbors.broadcast(updated, update_no) } \ No newline at end of file diff --git a/buzz_scripts/main.bzz b/buzz_scripts/main.bzz index e87f015..d52fd95 100644 --- a/buzz_scripts/main.bzz +++ b/buzz_scripts/main.bzz @@ -7,7 +7,11 @@ include "taskallocate/graphformGPS.bzz" include "vstigenv.bzz" #State launched after takeoff -AUTO_LAUNCH_STATE = "PURSUIT" +AUTO_LAUNCH_STATE = "CUSFUN" +#Lowest robot id in the network +LOWEST_ROBOT_ID = 97 +TARGET = 9.0 +EPSILON = 30.0 ##### # Vehicule type: @@ -21,12 +25,13 @@ goal_list = { .0={.x = 45.5088103899, .y = -73.1540826153, .z = 2.5} } + # Executed once at init time. function init() { init_stig() init_swarm() - TARGET_ALTITUDE = 10 + id*2.0 # m + TARGET_ALTITUDE = 5 + (id-LOWEST_ROBOT_ID)*3.0 # m loop = 1 # start the swarm command listener @@ -49,10 +54,14 @@ function step() { # if(BVMSTATE=="TURNEDOFF") statef=turnedoff + else if(BVMSTATE=="CUSFUN") + statef=cusfun else if(BVMSTATE=="STOP") # ends on turnedoff statef=stop else if(BVMSTATE=="LAUNCH") # ends on AUTO_LAUNCH_STATE statef=launch + else if(BVMSTATE=="GOHOME") # ends on AUTO_LAUNCH_STATE + statef=goinghome else if(BVMSTATE=="IDLE") statef=idle else if(BVMSTATE=="AGGREGATE") diff --git a/buzz_scripts/minimal.bzz b/buzz_scripts/minimal.bzz index 8f0494b..a49329b 100644 --- a/buzz_scripts/minimal.bzz +++ b/buzz_scripts/minimal.bzz @@ -51,7 +51,6 @@ function step() { statef=action statef() - log("Current state: ", BVMSTATE) } diff --git a/buzz_scripts/stand_by.bzz b/buzz_scripts/stand_by.bzz index 1349089..c605089 100644 --- a/buzz_scripts/stand_by.bzz +++ b/buzz_scripts/stand_by.bzz @@ -1,35 +1,38 @@ +include "act/states.bzz" +include "vstigenv.bzz" + updated="update_ack" update_no=0 -function init(){ -barrier = stigmergy.create(101) -barrier.put(id,1) -barrier_val=0 -barrier.onconflict(function (k,l,r) { -if(r. data < l. data or (r. data == l. data )) return l -else return r -}) +BVMSTATE = "UPDATESTANDBY" -s = swarm.create(1) -s.join() +# Executed once at init time. +function init(){ + barrier = stigmergy.create(101) + barrier.put(id,1) + barrier_val=0 + barrier.onconflict(function (k,l,r) { + if(r. data < l. data or (r. data == l. data )) return l + else return r + }) + init_swarm() + # start the swarm command listener + nei_cmd_listen() } function stand_by(){ + barrier.get(id) + barrier_val = barrier.size() -barrier.get(id) -barrier_val = barrier.size() - -neighbors.listen(updated, - function(vid, value, rid) { - print(" got from",vid," ", " value = ",value," ","rid"," " ) - if(value==update_no) barrier.put(rid,1) - } + neighbors.listen(updated, + function(vid, value, rid) { + if(value==update_no) barrier.put(rid,1) + } ) } - +# Executed at each time step. function step() { - -stand_by() + stand_by() } diff --git a/buzz_scripts/testLJ.bzz b/buzz_scripts/testLJ.bzz new file mode 100755 index 0000000..5240618 --- /dev/null +++ b/buzz_scripts/testLJ.bzz @@ -0,0 +1,72 @@ +include "update.bzz" +# don't use a stigmergy id=11 with this header, for barrier +# it requires an 'action' function to be defined here. +include "act/states.bzz" +include "vstigenv.bzz" + +V_TYPE = 0 + +#State launched after takeoff +AUTO_LAUNCH_STATE = "FORMATION" + + +TARGET = 8.0 +EPSILON = 3.0 +GOTO_MAXVEL = 2.5 # m/steps + +# Executed once at init time. +function init() { + init_stig() + init_swarm() + + # start the swarm command listener + nei_cmd_listen() + + # Starting state: TURNEDOFF to wait for user input. + BVMSTATE = "TURNEDOFF" + TAKEOFF_COUNTER = 20 +} + +# Executed at each time step. +function step() { + rc_cmd_listen() + + # update the vstig (status/net/batt/...) + # uav_updatestig() + + # + # State machine + # + if(BVMSTATE=="TURNEDOFF") + statef=turnedoff + else if(BVMSTATE=="STOP") # ends on turnedoff + statef=stop + else if(BVMSTATE=="LAUNCH") # ends on AUTO_LAUNCH_STATE + statef=launch + else if(BVMSTATE=="IDLE") + statef=idle + else if(BVMSTATE=="FORMATION") + statef=formation + + statef() + + log("Current state: ", BVMSTATE) + + # Auto-takeoff (delayed for simulator boot) + if(id == 0) { + if(TAKEOFF_COUNTER>0) + TAKEOFF_COUNTER = TAKEOFF_COUNTER - 1 + else if(TAKEOFF_COUNTER == 0) { + BVMSTATE="LAUNCH" + TAKEOFF_COUNTER = -1 + } + } +} + +# Executed once when the robot (or the simulator) is reset. +function reset() { +} + +# Executed once at the end of experiment. +function destroy() { +}