update simulation script delete
This commit is contained in:
vivek-shankar
parent
99aaee0e71
commit
4170697dec
|
|
@ -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;
|
||||
}
|
||||
|
|
@ -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;
|
||||
}
|
||||
|
|
@ -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
|
||||
}
|
||||
|
|
@ -1,209 +0,0 @@
|
|||
# We need this for 2D vectors
|
||||
|
||||
# Make sure you pass the correct include path to "bzzc -I <path1:path2> ..."
|
||||
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() {
|
||||
}
|
||||
|
|
@ -1,208 +0,0 @@
|
|||
# We need this for 2D vectors
|
||||
# Make sure you pass the correct include path to "bzzc -I <path1:path2> ..."
|
||||
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() {
|
||||
}
|
||||
|
|
@ -1,240 +0,0 @@
|
|||
# We need this for 2D vectors
|
||||
# Make sure you pass the correct include path to "bzzc -I <path1:path2> ..."
|
||||
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() {
|
||||
}
|
||||
|
|
@ -1,240 +0,0 @@
|
|||
# We need this for 2D vectors
|
||||
# Make sure you pass the correct include path to "bzzc -I <path1:path2> ..."
|
||||
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() {
|
||||
}
|
||||
107
script/vec2.bzz
107
script/vec2.bzz
|
|
@ -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
|
||||
}
|
||||
Loading…
Reference in New Issue