Spiri
/
ROSBuzz_MISTLab
12
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

in messages with local buff

This commit is contained in:
vivek-shankar 2017-05-09 19:05:57 -04:00
parent 15b8a911d1
commit b095536db6
8 changed files with 1498 additions and 29 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

208
script/update_sim/testflockfev1.bzz Normal file
View File

@ -0,0 +1,208 @@
# 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() {
}

208
script/update_sim/testflockfev2.bzz Normal file
View File

@ -0,0 +1,208 @@
# 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() {
}

208
script/update_sim/testflockfev3.bzz Normal file
View File

@ -0,0 +1,208 @@
# 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() {
}

208
script/update_sim/testflockfev4.bzz Normal file
View File

@ -0,0 +1,208 @@
# 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() {
}

208
script/update_sim/testflockfev5.bzz Normal file
View File

@ -0,0 +1,208 @@
# 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() {
}

208
script/update_sim/testflockfev6.bzz Normal file
View File

@ -0,0 +1,208 @@
# 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() {
}

208
script/update_sim/testflockfev7.bzz Normal file
View File

@ -0,0 +1,208 @@
# 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() {
}

71
src/buzz_utility.cpp
View File

@ -19,8 +19,8 @@ namespace buzz_utility{
static buzzdebug_t DBG_INFO = 0;
static uint8_t MSG_SIZE = 250; // Only 100 bytes of Buzz messages every step
static int MAX_MSG_SIZE = 10000; // Maximum Msg size for sending update packets
static int Robot_id = 0;
static int Robot_id = 0;
static std::vector<uint8_t*> IN_MSG;
std::map< int, Pos_struct> users_map;
/****************************************/
@ -228,41 +228,55 @@ namespace buzz_utility{
void in_msg_append(uint64_t* payload){
/* Go through messages and add them to the FIFO */
/* Go through messages and append them to the vector */
uint16_t* data= u64_cvt_u16((uint64_t)payload[0]);
/*Size is at first 2 bytes*/
uint16_t size=data[0]*sizeof(uint64_t);
delete[] data;
uint8_t* pl =(uint8_t*)malloc(size);
memset(pl, 0,size);
/* Copy packet into temporary buffer */
memcpy(pl, payload ,size);
/*size and robot id read*/
size_t tot = sizeof(uint32_t);
/* Go through the messages until there's nothing else to read */
uint16_t unMsgSize=0;
/*Obtain Buzz messages only when they are present*/
do {
/* Get payload size */
unMsgSize = *(uint16_t*)(pl + tot);
tot += sizeof(uint16_t);
/* Append message to the Buzz input message queue */
if(unMsgSize > 0 && unMsgSize <= size - tot ) {
buzzinmsg_queue_append(VM,
buzzmsg_payload_frombuffer(pl +tot, unMsgSize));
tot += unMsgSize;
}
}while(size - tot > sizeof(uint16_t) && unMsgSize > 0);
free(pl);
/* Copy packet into temporary buffer */
memcpy(pl, payload ,size);
IN_MSG.push_back(pl);
}
void in_message_process(){
while(!IN_MSG.empty()){
uint8_t* first_INmsg = (uint8_t*)IN_MSG.front();
/* Go through messages and append them to the FIFO */
uint16_t* data= u64_cvt_u16((uint64_t)first_INmsg[0]);
/*Size is at first 2 bytes*/
uint16_t size=data[0]*sizeof(uint64_t);
delete[] data;
/*size and robot id read*/
size_t tot = sizeof(uint32_t);
/* Go through the messages until there's nothing else to read */
uint16_t unMsgSize=0;
/*Obtain Buzz messages push it into queue*/
do {
/* Get payload size */
unMsgSize = *(uint16_t*)(first_INmsg + tot);
tot += sizeof(uint16_t);
/* Append message to the Buzz input message queue */
if(unMsgSize > 0 && unMsgSize <= size - tot ) {
buzzinmsg_queue_append(VM,
buzzmsg_payload_frombuffer(first_INmsg +tot, unMsgSize));
tot += unMsgSize;
}
}while(size - tot > sizeof(uint16_t) && unMsgSize > 0);
IN_MSG.erase(IN_MSG.begin());
free(first_INmsg);
}
/* Process messages VM call*/
buzzvm_process_inmsgs(VM);
}
/***************************************************/
/*Obtains messages from buzz out message Queue*/
/***************************************************/
uint64_t* obt_out_msg(){
/* Process out messages */
buzzvm_process_outmsgs(VM);
uint8_t* buff_send =(uint8_t*)malloc(MAX_MSG_SIZE);
memset(buff_send, 0, MAX_MSG_SIZE);
/*Taking into consideration the sizes included at the end*/
@ -699,8 +713,8 @@ static int create_stig_tables() {
}
void buzz_script_step() {
/* Process messages */
buzzvm_process_inmsgs(VM);
/*Process available messages*/
in_message_process();
/*Update sensors*/
update_sensors();
/* Call Buzz step() function */
@ -710,8 +724,7 @@ static int create_stig_tables() {
buzz_error_info());
buzzvm_dump(VM);
}
/* Process out messages */
buzzvm_process_outmsgs(VM);
/*Print swarm*/
//buzzswarm_members_print(stdout, VM->swarmmembers, VM->robot);
//int SwarmSize = buzzdict_size(VM->swarmmembers)+1;