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