From 4170697dec2f0fa2ef4511f8483d630f88a384bf Mon Sep 17 00:00:00 2001 From: vivek-shankar Date: Mon, 15 May 2017 20:07:40 -0400 Subject: [PATCH] update simulation script delete --- script/string.bzz | 92 --------- script/update_sim/include/string.bzz | 92 --------- script/update_sim/include/vec2.bzz | 107 ----------- script/update_sim/old_testflockfev1.bzz | 209 --------------------- script/update_sim/old_testflockfev2.bzz | 208 -------------------- script/update_sim/testflockfev1.bzz | 240 ------------------------ script/update_sim/testflockfev2.bzz | 240 ------------------------ script/vec2.bzz | 107 ----------- 8 files changed, 1295 deletions(-) delete mode 100644 script/string.bzz delete mode 100644 script/update_sim/include/string.bzz delete mode 100644 script/update_sim/include/vec2.bzz delete mode 100644 script/update_sim/old_testflockfev1.bzz delete mode 100644 script/update_sim/old_testflockfev2.bzz delete mode 100644 script/update_sim/testflockfev1.bzz delete mode 100644 script/update_sim/testflockfev2.bzz delete mode 100644 script/vec2.bzz diff --git a/script/string.bzz b/script/string.bzz deleted file mode 100644 index 4140a1b..0000000 --- a/script/string.bzz +++ /dev/null @@ -1,92 +0,0 @@ -# -# Returns the string character at the given position. -# PARAM s: The string -# PARAM n: The position of the wanted character -# RETURN The character at the wanted position, or nil -# -string.charat = function(s, n) { - return string.sub(s, n, n+1) -} - -# -# Returns the index of the first occurrence of the given string m -# within another string s. If none is found, this function returns -# nil. -# PARAM s: The string -# PARAM m: The string to match -# RETURN: The position of the first match, or nil -# -string.indexoffirst = function(s, m) { - var ls = string.length(s) - var lm = string.length(m) - var i = 0 - while(i < ls-lm+1) { - if(string.sub(s, i, i+lm) == m) return i - i = i + 1 - } - return nil -} - -# -# Returns the index of the last occurrence of the given string m -# within another string s. If none is found, this function returns -# nil. -# PARAM s: The string -# PARAM m: The string to match -# RETURN: The position of the last match, or nil -# -string.indexoflast = function(s, m) { - var ls = string.length(s) - var lm = string.length(m) - var i = ls - lm + 1 - while(i >= 0) { - if(string.sub(s, i, i+lm) == m) return i - i = i - 1 - } - return nil -} - -# Splits a string s using the delimiters in d. The string list is -# returned in a table indexed by value (starting at 0). -# PARAM s: The string -# PARAM d: A string containing the delimiters -# RETURN: A table containing the tokens -string.split = function(s, d) { - var i1 = 0 # index to move along s (token start) - var i2 = 0 # index to move along s (token end) - var c = 0 # token count - var t = {} # token list - var ls = string.length(s) - var ld = string.length(d) - # Go through string s - while(i2 < ls) { - # Try every delimiter - var j = 0 # index to move along d - var f = nil # whether the delimiter was found or not - while(j < ld and (not f)) { - if(string.charat(s, i2) == string.charat(d, j)) { - # Delimiter found - f = 1 - # Is it worth adding a new token? - if(i2 > i1) { - t[c] = string.sub(s, i1, i2) - c = c + 1 - } - # Start new token - i1 = i2 + 1 - } - else { - # Next delimiter - j = j + 1 - } - } - # Next string character - i2 = i2 + 1 - } - # Is it worth adding a new token? - if(i2 > i1) { - t[c] = string.sub(s, i1, i2) - } - # Return token list - return t; -} diff --git a/script/update_sim/include/string.bzz b/script/update_sim/include/string.bzz deleted file mode 100644 index 4140a1b..0000000 --- a/script/update_sim/include/string.bzz +++ /dev/null @@ -1,92 +0,0 @@ -# -# Returns the string character at the given position. -# PARAM s: The string -# PARAM n: The position of the wanted character -# RETURN The character at the wanted position, or nil -# -string.charat = function(s, n) { - return string.sub(s, n, n+1) -} - -# -# Returns the index of the first occurrence of the given string m -# within another string s. If none is found, this function returns -# nil. -# PARAM s: The string -# PARAM m: The string to match -# RETURN: The position of the first match, or nil -# -string.indexoffirst = function(s, m) { - var ls = string.length(s) - var lm = string.length(m) - var i = 0 - while(i < ls-lm+1) { - if(string.sub(s, i, i+lm) == m) return i - i = i + 1 - } - return nil -} - -# -# Returns the index of the last occurrence of the given string m -# within another string s. If none is found, this function returns -# nil. -# PARAM s: The string -# PARAM m: The string to match -# RETURN: The position of the last match, or nil -# -string.indexoflast = function(s, m) { - var ls = string.length(s) - var lm = string.length(m) - var i = ls - lm + 1 - while(i >= 0) { - if(string.sub(s, i, i+lm) == m) return i - i = i - 1 - } - return nil -} - -# Splits a string s using the delimiters in d. The string list is -# returned in a table indexed by value (starting at 0). -# PARAM s: The string -# PARAM d: A string containing the delimiters -# RETURN: A table containing the tokens -string.split = function(s, d) { - var i1 = 0 # index to move along s (token start) - var i2 = 0 # index to move along s (token end) - var c = 0 # token count - var t = {} # token list - var ls = string.length(s) - var ld = string.length(d) - # Go through string s - while(i2 < ls) { - # Try every delimiter - var j = 0 # index to move along d - var f = nil # whether the delimiter was found or not - while(j < ld and (not f)) { - if(string.charat(s, i2) == string.charat(d, j)) { - # Delimiter found - f = 1 - # Is it worth adding a new token? - if(i2 > i1) { - t[c] = string.sub(s, i1, i2) - c = c + 1 - } - # Start new token - i1 = i2 + 1 - } - else { - # Next delimiter - j = j + 1 - } - } - # Next string character - i2 = i2 + 1 - } - # Is it worth adding a new token? - if(i2 > i1) { - t[c] = string.sub(s, i1, i2) - } - # Return token list - return t; -} diff --git a/script/update_sim/include/vec2.bzz b/script/update_sim/include/vec2.bzz deleted file mode 100644 index e2fb9b0..0000000 --- a/script/update_sim/include/vec2.bzz +++ /dev/null @@ -1,107 +0,0 @@ -# -# Create a new namespace for vector2 functions -# -math.vec2 = {} - -# -# Creates a new vector2. -# PARAM x: The x coordinate. -# PARAM y: The y coordinate. -# RETURN: A new vector2. -# -math.vec2.new = function(x, y) { - return { .x = x, .y = y } -} - -# -# Creates a new vector2 from polar coordinates. -# PARAM l: The length of the vector2. -# PARAM a: The angle of the vector2. -# RETURN: A new vector2. -# -math.vec2.newp = function(l, a) { - return { - .x = l * math.cos(a), - .y = l * math.sin(a) - } -} - -# -# Calculates the length of the given vector2. -# PARAM v: The vector2. -# RETURN: The length of the vector. -# -math.vec2.length = function(v) { - return math.sqrt(v.x * v.x + v.y * v.y) -} - -# -# Calculates the angle of the given vector2. -# PARAM v: The vector2. -# RETURN: The angle of the vector. -# -math.vec2.angle = function(v) { - return math.atan2(v.y, v.x) -} - -# -# Returns the normalized form of a vector2. -# PARAM v: The vector2. -# RETURN: The normalized form. -# -math.vec2.norm = function(v) { - var l = math.length(v) - return { - .x = v.x / l, - .y = v.y / l - } -} - -# -# Calculates v1 + v2. -# PARAM v1: A vector2. -# PARAM v2: A vector2. -# RETURN: v1 + v2 -# -math.vec2.add = function(v1, v2) { - return { - .x = v1.x + v2.x, - .y = v1.y + v2.y - } -} - -# -# Calculates v1 - v2. -# PARAM v1: A vector2. -# PARAM v2: A vector2. -# RETURN: v1 + v2 -# -math.vec2.sub = function(v1, v2) { - return { - .x = v1.x - v2.x, - .y = v1.y - v2.y - } -} - -# -# Scales a vector by a numeric constant. -# PARAM v: A vector2. -# PARAM s: A number (float or int). -# RETURN: s * v -# -math.vec2.scale = function(v, s) { - return { - .x = v.x * s, - .y = v.y * s - } -} - -# -# Calculates v1 . v2 (the dot product) -# PARAM v1: A vector2. -# PARAM v2: A vector2. -# RETURN: v1 . v2 -# -math.vec2.dot = function(v1, v2) { - return v1.x * v2.x + v1.y * v2.y -} diff --git a/script/update_sim/old_testflockfev1.bzz b/script/update_sim/old_testflockfev1.bzz deleted file mode 100644 index 02abd4c..0000000 --- a/script/update_sim/old_testflockfev1.bzz +++ /dev/null @@ -1,209 +0,0 @@ -# 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 = 3.0 -CURSTATE = "TURNEDOFF" - -# Lennard-Jones parameters -TARGET = 10.0 #0.000001001 -EPSILON = 18.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 { -# timeW = timeW+1 -# uav_moveto(0.0,0.0) -# } -} - -######################################## -# -# 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.select(1) - s.join() - statef=idle - CURSTATE = "IDLE" -} - -# 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) -} - -# Executed once when the robot (or the simulator) is reset. -function reset() { -} - -# Executed once at the end of experiment. -function destroy() { -} diff --git a/script/update_sim/old_testflockfev2.bzz b/script/update_sim/old_testflockfev2.bzz deleted file mode 100644 index 1845eb9..0000000 --- a/script/update_sim/old_testflockfev2.bzz +++ /dev/null @@ -1,208 +0,0 @@ -# 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 = 3.0 -CURSTATE = "TURNEDOFF" - -# Lennard-Jones parameters -TARGET = 10.0 #0.000001001 -EPSILON = 18.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 { -# timeW = timeW+1 -# uav_moveto(0.0,0.0) -# } -} - -######################################## -# -# 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.select(1) - s.join() - statef=idle - CURSTATE = "IDLE" -} - -# 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) -} - -# Executed once when the robot (or the simulator) is reset. -function reset() { -} - -# Executed once at the end of experiment. -function destroy() { -} diff --git a/script/update_sim/testflockfev1.bzz b/script/update_sim/testflockfev1.bzz deleted file mode 100644 index 06b3e32..0000000 --- a/script/update_sim/testflockfev1.bzz +++ /dev/null @@ -1,240 +0,0 @@ -# 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 = 3.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 - } -} - -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" -} - -# 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) - - # 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. -function reset() { -} - -# Executed once at the end of experiment. -function destroy() { -} diff --git a/script/update_sim/testflockfev2.bzz b/script/update_sim/testflockfev2.bzz deleted file mode 100644 index 06b3e32..0000000 --- a/script/update_sim/testflockfev2.bzz +++ /dev/null @@ -1,240 +0,0 @@ -# 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 = 3.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 - } -} - -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" -} - -# 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) - - # 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. -function reset() { -} - -# Executed once at the end of experiment. -function destroy() { -} diff --git a/script/vec2.bzz b/script/vec2.bzz deleted file mode 100644 index e2fb9b0..0000000 --- a/script/vec2.bzz +++ /dev/null @@ -1,107 +0,0 @@ -# -# Create a new namespace for vector2 functions -# -math.vec2 = {} - -# -# Creates a new vector2. -# PARAM x: The x coordinate. -# PARAM y: The y coordinate. -# RETURN: A new vector2. -# -math.vec2.new = function(x, y) { - return { .x = x, .y = y } -} - -# -# Creates a new vector2 from polar coordinates. -# PARAM l: The length of the vector2. -# PARAM a: The angle of the vector2. -# RETURN: A new vector2. -# -math.vec2.newp = function(l, a) { - return { - .x = l * math.cos(a), - .y = l * math.sin(a) - } -} - -# -# Calculates the length of the given vector2. -# PARAM v: The vector2. -# RETURN: The length of the vector. -# -math.vec2.length = function(v) { - return math.sqrt(v.x * v.x + v.y * v.y) -} - -# -# Calculates the angle of the given vector2. -# PARAM v: The vector2. -# RETURN: The angle of the vector. -# -math.vec2.angle = function(v) { - return math.atan2(v.y, v.x) -} - -# -# Returns the normalized form of a vector2. -# PARAM v: The vector2. -# RETURN: The normalized form. -# -math.vec2.norm = function(v) { - var l = math.length(v) - return { - .x = v.x / l, - .y = v.y / l - } -} - -# -# Calculates v1 + v2. -# PARAM v1: A vector2. -# PARAM v2: A vector2. -# RETURN: v1 + v2 -# -math.vec2.add = function(v1, v2) { - return { - .x = v1.x + v2.x, - .y = v1.y + v2.y - } -} - -# -# Calculates v1 - v2. -# PARAM v1: A vector2. -# PARAM v2: A vector2. -# RETURN: v1 + v2 -# -math.vec2.sub = function(v1, v2) { - return { - .x = v1.x - v2.x, - .y = v1.y - v2.y - } -} - -# -# Scales a vector by a numeric constant. -# PARAM v: A vector2. -# PARAM s: A number (float or int). -# RETURN: s * v -# -math.vec2.scale = function(v, s) { - return { - .x = v.x * s, - .y = v.y * s - } -} - -# -# Calculates v1 . v2 (the dot product) -# PARAM v1: A vector2. -# PARAM v2: A vector2. -# RETURN: v1 . v2 -# -math.vec2.dot = function(v1, v2) { - return v1.x * v2.x + v1.y * v2.y -}