diff --git a/script/testflockfev.bzz b/script/testflockfev.bzz index 8dc3d2a..06b3e32 100644 --- a/script/testflockfev.bzz +++ b/script/testflockfev.bzz @@ -16,7 +16,7 @@ CURSTATE = "TURNEDOFF" # Lennard-Jones parameters TARGET = 12.0 #0.000001001 -EPSILON = 6.0 #0.001 +EPSILON = 3.0 #0.001 # Lennard-Jones interaction magnitude function lj_magnitude(dist, target, epsilon) { @@ -145,10 +145,32 @@ function land() { } } +function users_save(t) { + if(size(t)>0) { + foreach(t, function(id, tab) { + #log("id: ",id," Latitude ", tab.la, "Longitude ", tab.lo) + add_user_rb(id,tab.la,tab.lo) + }) + } +} + +function table_print(t) { + if(size(t)>0) { + foreach(t, function(u, tab) { + log("id: ",u," Range ", tab.r, "Bearing ", tab.b) + }) + } +} + # Executed once at init time. function init() { s = swarm.create(1) s.join() + + vt = stigmergy.create(5) + t = {} + vt.put("p",t) + statef=idle CURSTATE = "IDLE" } @@ -199,6 +221,14 @@ neighbors.listen("cmd", statef() log("Current state: ", CURSTATE) log("Swarm size: ",ROBOTS) + + # Check local users and push to v.stig + if(size(users.dataG)>0) + vt.put("p", users.dataG) + + # Save locally the users and print RG + users_save(vt.get("p")) + table_print(users.dataL) } # Executed once when the robot (or the simulator) is reset. diff --git a/script/testflockfev.bzz.save b/script/testflockfev.bzz.save new file mode 100644 index 0000000..4de66e5 --- /dev/null +++ b/script/testflockfev.bzz.save @@ -0,0 +1,216 @@ +# We need this for 2D vectors +# Make sure you pass the correct include path to "bzzc -I ..." +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 = 3.0 +CURSTATE = "TURNEDOFF" + +# Lennard-Jones parameters +TARGET = 12.0 #0.000001001 +EPSILON = 6.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 + CURSTATE = "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) + +# if(timeW>=WAIT_TIMEOUT) { #FOR MOVETO TESTS +# timeW =0 +# statef=land +# } else if(timeW>=WAIT_TIMEOUT/2) { +# timeW = timeW+1 +# uav_moveto(0.06,0.0) +# } else { +# timeW = timeW+1 +# uav_moveto(0.0,0.06) +# } +} + +######################################## +# +# BARRIER-RELATED FUNCTIONS +# +######################################## + +# +# Constants +# +BARRIER_VSTIG = 1 +# ROBOTS = 3 # number of robots in the swarm + +# +# 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 +# +WAIT_TIMEOUT = 200 +timeW=0 +function barrier_wait(threshold, transf) { + barrier.get(id) + CURSTATE = "BARRIERWAIT" + if(barrier.size() >= threshold) { + barrier = nil + transf() + } else if(timeW>=WAIT_TIMEOUT) { + barrier = nil + statef=land + timeW=0 + } + timeW = timeW+1 +} + +# flight status + +function idle() { +statef=idle +CURSTATE = "IDLE" + +} + +function takeoff() { + CURSTATE = "TAKEOFF" + statef=takeoff + log("TakeOff: ", flight.status) + log("Relative position: ", position.altitude) + + if( flight.status == 2 and position.altitude >= TARGET_ALTITUDE-TARGET_ALTITUDE/20.0) { + barrier_set(ROBOTS,hexagon) + barrier_ready() + #statef=hexagon + } + else { + log("Altitude: ", TARGET_ALTITUDE) + neighbors.broadcast("cmd", 22) + uav_takeoff(TARGET_ALTITUDE) + } +} +function land() { + CURSTATE = "LAND" + statef=land + log("Land: ", flight.status) + if(flight.status == 2 or flight.status == 3){ + neighbors.broadcast("cmd", 21) + uav_land() + } + else { + timeW=0 + barrier = nil + statef=idle + } +} + +# Executed once at init time. +function init() { + s = swarm.create(1) + s.join() + statef=idle + CURSTATE = "IDLE" + vt = stigmergy.cerate(5) + t = {} + vt.put("p",t) +} + +# Executed at each time step. +function step() { + if(flight.rc_cmd==22) { + log("cmd 22") + flight.rc_cmd=0 + statef = takeoff + CURSTATE = "TAKEOFF" + neighbors.broadcast("cmd", 22) + } else if(flight.rc_cmd==21) { + log("cmd 21") + log("To land") + flight.rc_cmd=0 + statef = land + CURSTATE = "LAND" + neighbors.broadcast("cmd", 21) + } else if(flight.rc_cmd==16) { + flight.rc_cmd=0 + statef = idle + uav_goto() + } else if(flight.rc_cmd==400) { + flight.rc_cmd=0 + uav_arm() + neighbors.broadcast("cmd", 400) + } else if (flight.rc_cmd==401){ + flight.rc_cmd=0 + uav_disarm() + neighbors.broadcast("cmd", 401) + } +neighbors.listen("cmd", + function(vid, value, rid) { + print("Got (", vid, ",", value, ") from robot #", rid) + if(value==22 and CURSTATE=="IDLE") { + statef=takeoff + } else if(value==21) { + statef=land + } else if(value==400 and CURSTATE=="IDLE") { + uav_arm() + } else if(value==401 and CURSTATE=="IDLE"){ + uav_disarm() + } + } + +) + statef() + log("Current state: ", CURSTATE) + log("Swarm size: ",ROBOTS) + if(users.dataG) + vt.put("p",users.dataG) + table_print(users.dataL) +} + +# Executed once when the robot (or the simulator) is reset. +function reset() { +} + +# Executed once at the end of experiment. +function destroy() { +}