From 15a7acf5f643cfdab32b0b5d580147cc06127af7 Mon Sep 17 00:00:00 2001 From: dave Date: Tue, 23 May 2017 23:52:49 -0400 Subject: [PATCH] merge --- script/Update.log | 0 ...{testflockfev.bzz => testflockfev.bzz.old} | 0 ...{testflockfev.bzz.save => testflockfevbzz} | 107 ++++++++++++++---- script/testflocksim.bzz | 6 +- 4 files changed, 86 insertions(+), 27 deletions(-) create mode 100644 script/Update.log rename script/{testflockfev.bzz => testflockfev.bzz.old} (100%) rename script/{testflockfev.bzz.save => testflockfevbzz} (64%) diff --git a/script/Update.log b/script/Update.log new file mode 100644 index 0000000..e69de29 diff --git a/script/testflockfev.bzz b/script/testflockfev.bzz.old similarity index 100% rename from script/testflockfev.bzz rename to script/testflockfev.bzz.old diff --git a/script/testflockfev.bzz.save b/script/testflockfevbzz similarity index 64% rename from script/testflockfev.bzz.save rename to script/testflockfevbzz index 4de66e5..57850d8 100644 --- a/script/testflockfev.bzz.save +++ b/script/testflockfevbzz @@ -8,19 +8,20 @@ include "vec2.bzz" updated="update_ack" update_no=0 function updated_neigh(){ -neighbors.broadcast(updated, update_no) + neighbors.broadcast(updated, update_no) } -TARGET_ALTITUDE = 3.0 +TARGET_ALTITUDE = 5.0 CURSTATE = "TURNEDOFF" # Lennard-Jones parameters -TARGET = 12.0 #0.000001001 -EPSILON = 6.0 #0.001 +TARGET = 12.0 +EPSILON = 14.0 # Lennard-Jones interaction magnitude function lj_magnitude(dist, target, epsilon) { return -(epsilon / dist) * ((target / dist)^4 - (target / dist)^2) + #return -(4 * epsilon) * ((target / dist)^12 - (target / dist)^6) } # Neighbor data to LJ interaction vector @@ -32,28 +33,51 @@ function lj_vector(rid, data) { function lj_sum(rid, data, accum) { return math.vec2.add(data, accum) } + +function user_attract(t) { + fus = math.vec2.new(0.0, 0.0) + if(size(t)>0) { + foreach(t, function(u, tab) { + #log("id: ",u," Range ", tab.r, "Bearing ", tab.b) + fus = math.vec2.add(fus, math.vec2.newp(lj_magnitude(tab.r, 3 * TARGET / 4.0, EPSILON * 2.0), tab.b)) + }) + math.vec2.scale(fus, 1.0 / size(t)) + } + #print("User attract:", fus.x," ", fus.y, " [", size(t), "]") + return fus +} # 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()) + accum = math.vec2.scale(accum, 1.0 / neighbors.count()) + + accum = math.vec2.add(accum, user_attract(users.dataL)) + accum = math.vec2.scale(accum, 1.0 / 2.0) + + if(math.vec2.length(accum) > 1.0) { + accum = math.vec2.scale(accum, 1.0 / math.vec2.length(accum)) + } + # Move according to vector - #print("Robot ", id, "must push ",accum.length, "; ", accum.angle) - uav_moveto(accum.x,accum.y) + print("Robot ", id, "must push ", math.vec2.length(accum) )#, "; ", math.vec2.angle(accum)) + uav_moveto(accum.x, accum.y) + CURSTATE = "LENNARDJONES" # if(timeW>=WAIT_TIMEOUT) { #FOR MOVETO TESTS # timeW =0 # statef=land # } else if(timeW>=WAIT_TIMEOUT/2) { +# CURSTATE ="GOEAST" # timeW = timeW+1 -# uav_moveto(0.06,0.0) +# uav_moveto(0.0,5.0) # } else { +# CURSTATE ="GONORTH" # timeW = timeW+1 -# uav_moveto(0.0,0.06) +# uav_moveto(5.0,0.0) # } } @@ -67,7 +91,6 @@ function hexagon() { # Constants # BARRIER_VSTIG = 1 -# ROBOTS = 3 # number of robots in the swarm # # Sets a barrier @@ -93,9 +116,10 @@ WAIT_TIMEOUT = 200 timeW=0 function barrier_wait(threshold, transf) { barrier.get(id) + barrier.put(id, 1) CURSTATE = "BARRIERWAIT" if(barrier.size() >= threshold) { - barrier = nil + #barrier = nil transf() } else if(timeW>=WAIT_TIMEOUT) { barrier = nil @@ -116,8 +140,8 @@ CURSTATE = "IDLE" function takeoff() { CURSTATE = "TAKEOFF" statef=takeoff - log("TakeOff: ", flight.status) - log("Relative position: ", position.altitude) + #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) @@ -133,27 +157,53 @@ function takeoff() { function land() { CURSTATE = "LAND" statef=land - log("Land: ", flight.status) + #log("Land: ", flight.status) if(flight.status == 2 or flight.status == 3){ neighbors.broadcast("cmd", 21) uav_land() } else { + barrier_set(ROBOTS,idle) + barrier_ready() timeW=0 - barrier = nil - statef=idle + #barrier = nil + #statef=idle } } +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) + }) + } +} + +# printing the contents of a table: a custom function +function table_print(t) { + if(size(t)>0) { + foreach(t, function(u, tab) { + log("id: ",u," Range ", tab.r, "Bearing ", tab.b) + }) + } +} + +######################################## +# +# MAIN FUNCTIONS +# +######################################## + # Executed once at init time. function init() { s = swarm.create(1) s.join() - statef=idle - CURSTATE = "IDLE" - vt = stigmergy.cerate(5) + vt = stigmergy.create(5) t = {} vt.put("p",t) + statef=idle + CURSTATE = "IDLE" } # Executed at each time step. @@ -174,7 +224,8 @@ function step() { } else if(flight.rc_cmd==16) { flight.rc_cmd=0 statef = idle - uav_goto() + #uav_goto() + add_user_rb(10,rc_goto.latitude,rc_goto.longitude) } else if(flight.rc_cmd==400) { flight.rc_cmd=0 uav_arm() @@ -202,9 +253,17 @@ neighbors.listen("cmd", statef() log("Current state: ", CURSTATE) log("Swarm size: ",ROBOTS) - if(users.dataG) - vt.put("p",users.dataG) - table_print(users.dataL) + + # Read a value from the structure + # log(users) + #users_print(users.dataG) + if(size(users.dataG)>0) + vt.put("p", users.dataG) + + # Get the number of keys in the structure + #log("The vstig has ", vt.size(), " elements") + users_save(vt.get("p")) + table_print(users.dataL) } # Executed once when the robot (or the simulator) is reset. diff --git a/script/testflocksim.bzz b/script/testflocksim.bzz index dceec19..bbecbc3 100644 --- a/script/testflocksim.bzz +++ b/script/testflocksim.bzz @@ -39,7 +39,7 @@ function user_attract(t) { if(size(t)>0) { foreach(t, function(u, tab) { #log("id: ",u," Range ", tab.r, "Bearing ", tab.b) - fus = math.vec2.add(fus, math.vec2.newp(lj_magnitude(tab.r, TARGET / 2.0, EPSILON * 2.0), tab.b)) + fus = math.vec2.add(fus, math.vec2.newp(lj_magnitude(tab.r, 3 * TARGET / 4.0, EPSILON * 2.0), tab.b)) }) math.vec2.scale(fus, 1.0 / size(t)) } @@ -58,8 +58,8 @@ function hexagon() { accum = math.vec2.add(accum, user_attract(users.dataL)) accum = math.vec2.scale(accum, 1.0 / 2.0) - if(math.vec2.length(accum) > 0.75) { - accum = math.vec2.scale(accum, 0.75 / math.vec2.length(accum)) + if(math.vec2.length(accum) > 1.0) { + accum = math.vec2.scale(accum, 1.0 / math.vec2.length(accum)) } # Move according to vector