diff --git a/src/testflockfev_barrier.bzz b/src/testflockfev_barrier.bzz new file mode 100644 index 0000000..b708a0a --- /dev/null +++ b/src/testflockfev_barrier.bzz @@ -0,0 +1,171 @@ + +# We need this for 2D vectors +# Make sure you pass the correct include path to "bzzc -I ..." +include "/home/ubuntu/buzz/src/include/vec2.bzz" +#################################################################################################### +# Updater related +# This should be here for the updater to work, changing position of code will crash the updater +#################################################################################################### +updated="update_ack" +update_no=0 +function updated_neigh(){ +neighbors.broadcast(updated, update_no) +} + +TARGET_ALTITUDE = 5.0 + +# Lennard-Jones parameters +TARGET = 10.0 #0.000001001 +EPSILON = 10.0 #0.001 + +# Lennard-Jones interaction magnitude +function lj_magnitude(dist, target, epsilon) { + return -(epsilon / dist) * ((target / dist)^4 - (target / dist)^2) +} + +# 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) +} + +# Calculates and actuates the flocking interaction +function hexagon() { + statef=hexagon + # Calculate accumulator + var accum = neighbors.map(lj_vector).reduce(lj_sum, math.vec2.new(0.0, 0.0)) + if(neighbors.count() > 0) + math.vec2.scale(accum, 1.0 / neighbors.count()) + # Move according to vector + print("Robot ", id, "must push ",accum.length, "; ", accum.angle) + uav_moveto(accum.x, accum.y) +} + +######################################## +# +# BARRIER-RELATED FUNCTIONS +# +######################################## + +# +# Constants +# +BARRIER_VSTIG = 1 +ROBOTS = 3 # number of robots in the swarm +barrier_number=0 +barrier_break=0 +# +# Sets a barrier +# +function barrier_set(threshold, transf) { + statef = function() { + barrier_wait(threshold, transf); + } + barrier = stigmergy.create(BARRIER_VSTIG) +} + +# +# Make yourself ready +# +function barrier_ready() { + barrier.put(id, 1) +} + +# +# Executes the barrier +# +function barrier_wait(threshold, transf) { + barrier.get(id) + if ( (barrier.size() >= threshold) or (barrier_break==1) ) { + barrier = nil + transf() + barrier_number=barrier_number+1 + barrier_break=0 + } +} + +# flight status + +function idle() { +statef=idle +neighbors.listen("cmd", + function(vid, value, rid) { + print("Got (", vid, ",", value, ") from robot #", rid) + if(value==22) { + statef=takeoff + } else if(value==21) { + statef=land + } + } + + +) +} + +function takeoff() { + log("TakeOff: ", flight.status) + if( flight.status == 2 and position.altitude >= TARGET_ALTITUDE-TARGET_ALTITUDE/20.0) { + barrier_set(ROBOTS,hexagon) + barrier_ready() + } + else if( flight.status !=3){ + log("Altitude: ", TARGET_ALTITUDE) + neighbors.broadcast("cmd", 22) + uav_takeoff(TARGET_ALTITUDE) + } +} +function land() { + log("Land: ", flight.status) + if(flight.status == 2 or flight.status == 3){ + neighbors.broadcast("cmd", 21) + uav_land() + } + else + statef=idle +} + +# Executed once at init time. +function init() { + statef=idle +} + +# Executed at each time step. +function step() { + neighbors.broadcast("barrier_num", barrier_number) + neighbors.listen("barrier_num", + function(vid, value, rid) { + print("Got (", vid, ",", value, ") from robot #", rid) + if(value > barrier_number) { + barrier_break=1 + } + } + ) + if(flight.rc_cmd==22) { + log("cmd 22") + flight.rc_cmd=0 + statef = takeoff + neighbors.broadcast("cmd", 22) + } else if(flight.rc_cmd==21) { + log("cmd 21") + log("To land") + flight.rc_cmd=0 + statef = land + neighbors.broadcast("cmd", 21) + } else if(flight.rc_cmd==16) { + flight.rc_cmd=0 + uav_goto() + } + statef() +} + +# Executed once when the robot (or the simulator) is reset. +function reset() { +} + +# Executed once at the end of experiment. +function destroy() { +}