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

clean bzz includes

This commit is contained in:
dave 2017-06-19 16:15:18 -04:00
parent 956b282ad3
commit 78edc3c9f0
18 changed files with 215 additions and 2739 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
script/Graph_drone.graph
View File

@ -1,5 +1,4 @@
0 -1 -1 -1
1 0 10.0 0.0
2 0 10.0 1.57
3 0 10.0 3.14
4 0 10.0 4.71
1 0 1000.0 0.0
2 0 1000.0 1.57
3 0 1000.0 3.14

8
script/Graph_fixed.graph
View File

@ -1,5 +1,5 @@
0 -1 -1 -1 -1
1 0 10.0 -1 -1
2 0 10.0 1 14.0
3 0 10.0 2 14.0
4 0 10.0 1 14.0
1 0 1000.0 -1 -1
2 0 1000.0 1 1414.2
3 0 1000.0 2 1414.2
4 0 1000.0 1 1414.2

102
script/flock.bzz Normal file
View File

@ -0,0 +1,102 @@
include "vec2.bzz"
include "update.bzz"
include "barrier.bzz" # don't use a stigmergy id=11 with this header.
include "uavstates.bzz" # require an 'action' function to be defined here.
include "vstigenv.bzz"
# Lennard-Jones parameters
TARGET = 12.0
EPSILON = 14.0
# Lennard-Jones interaction magnitude
function lj_magnitude(dist, target, epsilon) {
return -(epsilon / dist) * ((target / dist)^4 - (target / dist)^2)
#return -(4 * epsilon) * ((target / dist)^12 - (target / dist)^6)
}
# Neighbor data to LJ interaction vector
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)
}
function user_attract(t) {
fus = math.vec2.new(0.0, 0.0)
if(size(t)>0) {
foreach(t, function(u, tab) {
#log("id: ",u," Range ", tab.r, "Bearing ", tab.b)
fus = math.vec2.add(fus, math.vec2.newp(lj_magnitude(tab.r, 3 * TARGET / 4.0, EPSILON * 2.0), tab.b))
})
math.vec2.scale(fus, 1.0 / size(t))
}
#print("User attract:", fus.x," ", fus.y, " [", size(t), "]")
return fus
}
# Calculates and actuates the flocking interaction
function action() {
statef=action
# Calculate accumulator
var accum = neighbors.map(lj_vector).reduce(lj_sum, math.vec2.new(0.0, 0.0))
if(neighbors.count() > 0)
accum = math.vec2.scale(accum, 1.0 / neighbors.count())
#accum = math.vec2.add(accum, user_attract(users.dataL))
#accum = math.vec2.scale(accum, 1.0 / 2.0)
if(math.vec2.length(accum) > 1.0) {
accum = math.vec2.scale(accum, 1.0 / math.vec2.length(accum))
}
# Move according to vector
print("Robot ", id, "must push ", math.vec2.length(accum) )#, "; ", math.vec2.angle(accum))
uav_moveto(accum.x, accum.y)
UAVSTATE = "LENNARDJONES"
# if(timeW>=WAIT_TIMEOUT) { #FOR MOVETO TESTS
# timeW =0
# statef=land
# } else if(timeW>=WAIT_TIMEOUT/2) {
# UAVSTATE ="GOEAST"
# timeW = timeW+1
# uav_moveto(0.0,5.0)
# } else {
# UAVSTATE ="GONORTH"
# timeW = timeW+1
# uav_moveto(5.0,0.0)
# }
}
########################################
#
# MAIN FUNCTIONS
#
########################################
# Executed once at init time.
function init() {
uav_initswarm()
}
# Executed at each time step.
function step() {
uav_rccmd()
uav_neicmd()
statef()
log("Current state: ", UAVSTATE)
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() {
}

76
script/graphform.bzz
View File

@ -4,8 +4,8 @@
include "string.bzz"
include "vec2.bzz"
include "update.bzz"
include "barrier.bzz" #don't use a stigmergy id=11 with this header.
include "uavstates.bzz"
include "barrier.bzz" # don't use a stigmergy id=11 with this header.
include "uavstates.bzz" # require an 'action' function to be defined here.
#
#Constant parameters, need to be adjust
@ -275,7 +275,7 @@ function Get_DisAndAzi(id){
neighbors.foreach(
function(rid, data) {
if(rid==id){
m_receivedMessage.Range=data.distance
m_receivedMessage.Range=data.distance*100.0
m_receivedMessage.Bearing=data.azimuth
}
})
@ -370,7 +370,7 @@ v_tag.put(m_nLabel, 1)
m_navigation.x=0.0
m_navigation.y=0.0
uav_moveto(m_navigation.x,m_navigation.y)
uav_moveto(m_navigation.x/100.0,m_navigation.y/100.0)
}
#
@ -384,7 +384,7 @@ m_vecNodes[m_nLabel].State="ASSIGNED"
m_navigation.x=0.0
m_navigation.y=0.0
uav_moveto(m_navigation.x,m_navigation.y)
uav_moveto(m_navigation.x/100.0,m_navigation.y/100.0)
}
#
@ -444,7 +444,7 @@ function DoFree() {
tempvec_P=math.vec2.scale(tempvec_P,size(setJoinedIndexes))
tempvec_N=math.vec2.scale(tempvec_N,size(setJoinedIndexes))
m_navigation=math.vec2.add(tempvec_P,tempvec_N)
uav_moveto(m_navigation.x,m_navigation.y)
uav_moveto(m_navigation.x/100.0,m_navigation.y/100.0)
}else{ #no joined robots in sight
i=0
var tempvec={.x=0.0,.y=0.0}
@ -454,28 +454,10 @@ function DoFree() {
i=i+1
}
m_navigation=math.vec2.scale(tempvec,1.0/i)
uav_moveto(m_navigation.x,m_navigation.y)
uav_moveto(m_navigation.x/100.0,m_navigation.y/100.0)
}
#collision avoidence
i=0
var turnAngle=0.0
var needHide=0
while(i<m_neighbourCunt){
#if there is a robot within tolerance before, turn 90 degree to hide
if(m_MessageRange[i]<ROBOT_SAFETYDIST){
turnAngle=m_MessageBearing[i]+math.pi/2.0
needHide=1
}
i=i+1
}
if(needHide==1){
m_navigation.x=0.0
m_navigation.y=0.0
m_navigation=math.vec2.newp(m_sWheelTurningParams.MaxSpeed,turnAngle)
uav_moveto(m_navigation.x,m_navigation.y)
}
#jump the first step
if(step_cunt<=1){
@ -577,37 +559,21 @@ function DoJoining(){
m_bias=m_cMeToPred.Bearing-S2PGlobalBearing
S2Target_bearing=S2Target_bearing+m_bias
m_navigation=math.vec2.newp(S2Target_dis,S2Target_bearing)
uav_moveto(m_navigation.x,m_navigation.y)
uav_moveto(m_navigation.x/100.0,m_navigation.y/100.0)
#test if is already in desired position
if(math.abs(S2Target.x)<m_fTargetDistanceTolerance and math.abs(S2Target.y)<m_fTargetDistanceTolerance){
TransitionToJoined()
log(S2Target_dis,S2Target_bearing)
#TransitionToJoined()
return
}
} else{ #miss pred, there is a change the another robot block the sight, keep moving as before for sometime
m_unWaitCount=m_unWaitCount-1
}
#avoide collision
i=0
var turnAngle=0.0
var needHide=0
while(i<m_neighbourCunt){
#if there is a robot within tolerance before, turn 90 degree to hide
if(m_MessageRange[i]<ROBOT_SAFETYDIST){
turnAngle=m_MessageBearing[i]+math.pi/2.0
needHide=1
}
i=i+1
}
if(needHide==1){
m_navigation.x=0.0
m_navigation.y=0.0
m_navigation=math.vec2.newp(m_sWheelTurningParams.MaxSpeed,turnAngle)
uav_moveto(m_navigation.x,m_navigation.y)
}
if(m_unWaitCount==0){
TransitionToFree()
return
@ -689,14 +655,14 @@ function DoJoined(){
m_navigation.x=0.0
m_navigation.y=0.0
uav_moveto(m_navigation.x,m_navigation.y)
uav_moveto(m_navigation.x/100.0,m_navigation.y/100.0)
#check if should to transists to lock
if(v_tag.size()==ROBOTS){
TransitionToLock()
#TransitionToLock()
}
}
@ -762,7 +728,11 @@ if(m_nLabel>1){
log(";",m_nLabel,";",mypred1.range-m_vecNodes_fixed[m_nLabel].d1)
}
#move
uav_moveto(m_navigation.x,m_navigation.y)
uav_moveto(m_navigation.x/100.0,m_navigation.y/100.0)
}
function action(){
statef=action
}
#
@ -774,24 +744,22 @@ function init() {
#
m_unResponseTimeThreshold=10
m_unLabelSearchWaitTime=10
m_fTargetDistanceTolerance=1.5
m_fTargetDistanceTolerance=150
m_unJoiningLostPeriod=100
#
# Join Swarm
#
s = swarm.create(1)
s.join()
#ROBOT_NUM=5
uav_initswarm()
Reset();
statef=turnedoff
}
#
# Executed every step
#
function step(){
uavcmd()
uav_rccmd()
uav_neicmd()
#update the graph
UpdateNodeInfo()
#reset message package to be sent

13
script/include/uavstates.bzz
View File

@ -6,6 +6,13 @@
TARGET_ALTITUDE = 5.0
UAVSTATE = "TURNEDOFF"
function uav_initswarm(){
s = swarm.create(1)
s.join()
statef=turnedoff
UAVSTATE = "TURNEDOFF"
}
function turnedoff() {
statef=turnedoff
UAVSTATE = "TURNEDOFF"
@ -23,7 +30,7 @@ function takeoff() {
#log("Relative position: ", position.altitude)
if( flight.status == 2 and position.altitude >= TARGET_ALTITUDE-TARGET_ALTITUDE/20.0) {
barrier_set(ROBOTS,idle,land)
barrier_set(ROBOTS,action,land)
barrier_ready()
#statef=hexagon
}
@ -51,7 +58,7 @@ function land() {
}
}
function uavcmd() {
function uav_rccmd() {
if(flight.rc_cmd==22) {
log("cmd 22")
flight.rc_cmd=0
@ -79,7 +86,9 @@ function uavcmd() {
uav_disarm()
neighbors.broadcast("cmd", 401)
}
}
function uav_neicmd() {
neighbors.listen("cmd",
function(vid, value, rid) {
print("Got (", vid, ",", value, ") from robot #", rid)

33
script/include/vstigenv.bzz Normal file
View File

@ -0,0 +1,33 @@
function checkusers() {
# Read a value from the structure
if(size(users)>0)
log("Got a user!")
# log(users)
#users_print(users.dataG)
# if(size(users.dataG)>0)
# vt.put("p", users.dataG)
# Get the number of keys in the structure
#log("The vstig has ", vt.size(), " elements")
# users_save(vt.get("p"))
# table_print(users.dataL)
}
function users_save(t) {
if(size(t)>0) {
foreach(t, function(id, tab) {
#log("id: ",id," Latitude ", tab.la, "Longitude ", tab.lo)
add_user_rb(id,tab.la,tab.lo)
})
}
}
# printing the contents of a table: a custom function
function table_print(t) {
if(size(t)>0) {
foreach(t, function(u, tab) {
log("id: ",u," Range ", tab.r, "Bearing ", tab.b)
})
}
}

193
script/tentacle.bzz
View File

@ -1,11 +1,7 @@
include "vec2.bzz"
updated="update_ack"
update_no=0
function updated_neigh(){
neighbors.broadcast(updated, update_no)
}
TARGET_ALTITUDE = 5.0
CURSTATE = "TURNEDOFF"
include "update.bzz"
include "barrier.bzz" # don't use a stigmergy id=11 with this header.
include "uavstates.bzz" # require an 'action' function to be defined here.
# Lennard-Jones parameters
TARGET = 12.0
@ -226,125 +222,23 @@ function onetwo() {
}
}
########################################
#
# BARRIER-RELATED FUNCTIONS
#
########################################
#
# Constants
#
BARRIER_VSTIG = 1
#
# 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)
barrier.put(id, 1)
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) {
if(id==0)
barrier_set(ROBOTS, zero)
else
barrier_set(ROBOTS, onetwo)
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 {
barrier_set(ROBOTS,idle)
barrier_ready()
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)
})
}
}
# printing the contents of a table: a custom function
function table_print(t) {
if(size(t)>0) {
foreach(t, function(u, tab) {
log("id: ",u," Range ", tab.r, "Bearing ", tab.b)
})
}
}
#################################################
### BUZZ FUNCTIONS ##############################
#################################################
function action(){
if (id == 0)
statef=zero
else
statef=onetwo
UAVSTATE="TENTACLES"
}
# Executed at init time
function init() {
s = swarm.create(0)
s.join()
uav_initswarm()
# Local knowledge table
knowledge = {}
# Update local knowledge with information from the neighbors
@ -352,71 +246,16 @@ function init() {
# Variables initialization
iteration = 0
vt = stigmergy.create(5)
t = {}
vt.put("p",t)
statef=idle
CURSTATE = "IDLE"
}
# Executed every 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()
add_user_rb(10,rc_goto.latitude,rc_goto.longitude)
} 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()
}
}
uav_rccmd()
uav_neicmd()
)
statef()
log("Current state: ", CURSTATE)
log("Swarm size: ",ROBOTS)
# Read a value from the structure
# log(users)
#users_print(users.dataG)
if(size(users.dataG)>0)
vt.put("p", users.dataG)
# Get the number of keys in the structure
#log("The vstig has ", vt.size(), " elements")
users_save(vt.get("p"))
#table_print(users.dataL)
# Count the number of steps
iteration = iteration + 1

45
script/test.bzz
View File

@ -1,45 +0,0 @@
# Executed once at init time.
function init() {
i = 1
a = 0
val = 0
}
# Executed at each time step.
function step() {
if (i == 0) {
neighbors.listen("Take",
function(vid, value, rid) {
print("Got (", vid, ",", value, ") from robot #", rid)
}
)
neighbors.listen("key",
function(vid, value, rid) {
print("Got (", vid, ",", value, ") from robot #", rid)
val = value
}
)
print(val)
if ((val == 23) and (a == 0)) {
uav_takeoff()
a=1
}
if (a == 10) uav_land()
if (a != 0) a = a+1
}
else{
neighbors.broadcast("key", 23)
neighbors.broadcast("Take", "no")
}
}
# Executed once when the robot (or the simulator) is reset.
function reset() {
}
# Executed once at the end of experiment.
function destroy() {
}

159
script/test1.bzz
View File

@ -1,159 +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 = 2.0
# Lennard-Jones parameters
TARGET = 10.0 #0.000001001
EPSILON = 10.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
# 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)
}
########################################
#
# BARRIER-RELATED FUNCTIONS
#
########################################
#
# Constants
#
BARRIER_VSTIG = 1
ROBOTS = 2 # 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
#
function barrier_wait(threshold, transf) {
barrier.get(id)
if(barrier.size() >= threshold) {
barrier = nil
transf()
}
}
# flight status
function idle() {
statef=idle
neighbors.listen("cmd",
function(vid, value, rid) {
print("Got (", vid, ",", value, ") from robot #", rid)
if(value==22) {
statef=takeoff
} else if(value==21) {
statef=land
}
}
)
}
function takeoff() {
log("TakeOff: ", flight.status)
if( flight.status == 2 and position.altitude >= TARGET_ALTITUDE-TARGET_ALTITUDE/20.0) {
barrier_set(ROBOTS,hexagon)
barrier_ready()
}
else if( flight.status !=3){
log("Altitude: ", TARGET_ALTITUDE)
neighbors.broadcast("cmd", 22)
uav_takeoff(TARGET_ALTITUDE)
}
}
function land() {
log("Land: ", flight.status)
if(flight.status == 2 or flight.status == 3){
neighbors.broadcast("cmd", 21)
uav_land()
}
else
statef=idle
}
# Executed once at init time.
function init() {
statef=idle
}
# Executed at each time step.
function step() {
if(flight.rc_cmd==22) {
log("cmd 22")
flight.rc_cmd=0
statef = takeoff
neighbors.broadcast("cmd", 22)
} else if(flight.rc_cmd==21) {
log("cmd 21")
log("To land")
flight.rc_cmd=0
statef = land
neighbors.broadcast("cmd", 21)
} else if(flight.rc_cmd==16) {
flight.rc_cmd=0
uav_goto()
}
statef()
}
# Executed once when the robot (or the simulator) is reset.
function reset() {
}
# Executed once at the end of experiment.
function destroy() {
}

37
script/testalone.bzz
View File

@ -1,20 +1,15 @@
include "vec2.bzz"
include "update.bzz"
include "barrier.bzz" # don't use a stigmergy id=11 with this header.
include "uavstates.bzz" # require an 'action' function to be defined here.
include "vstigenv.bzz"
# 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"
#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)
function action() {
statef=action
# test moveto cmd dx, dy
# uav_moveto(0.5, 0.5)
}
TARGET_ALTITUDE = 5.0
# Executed once at init time.
function init() {
}
@ -22,19 +17,7 @@ function init() {
# Executed at each time step.
function step() {
log("Altitude: ", position.altitude)
if(flight.rc_cmd==22) {
flight.rc_cmd=0
uav_takeoff(TARGET_ALTITUDE)
} else if(flight.rc_cmd==21) {
flight.rc_cmd=0
uav_land()
} else if(flight.rc_cmd==16) {
flight.rc_cmd=0
uav_goto()
}
# test moveto cmd
#if( flight.status == 2 and position.altitude >= TARGET_ALTITUDE-TARGET_ALTITUDE/20.0)
# uav_moveto(0.5, 0.5)
uav_rccmd()
}
# Executed once when the robot (or the simulator) is reset.

275
script/testflockfev.bzz
View File

@ -1,275 +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 = 5.0
CURSTATE = "TURNEDOFF"
# Lennard-Jones parameters
TARGET = 12.0
EPSILON = 14.0
# Lennard-Jones interaction magnitude
function lj_magnitude(dist, target, epsilon) {
return -(epsilon / dist) * ((target / dist)^4 - (target / dist)^2)
#return -(4 * epsilon) * ((target / dist)^12 - (target / dist)^6)
}
# Neighbor data to LJ interaction vector
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)
}
function user_attract(t) {
fus = math.vec2.new(0.0, 0.0)
if(size(t)>0) {
foreach(t, function(u, tab) {
#log("id: ",u," Range ", tab.r, "Bearing ", tab.b)
fus = math.vec2.add(fus, math.vec2.newp(lj_magnitude(tab.r, 3 * TARGET / 4.0, EPSILON * 2.0), tab.b))
})
math.vec2.scale(fus, 1.0 / size(t))
}
#print("User attract:", fus.x," ", fus.y, " [", size(t), "]")
return fus
}
# Calculates and actuates the flocking interaction
function hexagon() {
statef=hexagon
# Calculate accumulator
var accum = neighbors.map(lj_vector).reduce(lj_sum, math.vec2.new(0.0, 0.0))
if(neighbors.count() > 0)
accum = math.vec2.scale(accum, 1.0 / neighbors.count())
accum = math.vec2.add(accum, user_attract(users.dataL))
accum = math.vec2.scale(accum, 1.0 / 2.0)
if(math.vec2.length(accum) > 1.0) {
accum = math.vec2.scale(accum, 1.0 / math.vec2.length(accum))
}
# Move according to vector
print("Robot ", id, "must push ", math.vec2.length(accum) )#, "; ", math.vec2.angle(accum))
uav_moveto(accum.x, accum.y)
CURSTATE = "LENNARDJONES"
# if(timeW>=WAIT_TIMEOUT) { #FOR MOVETO TESTS
# timeW =0
# statef=land
# } else if(timeW>=WAIT_TIMEOUT/2) {
# CURSTATE ="GOEAST"
# timeW = timeW+1
# uav_moveto(0.0,5.0)
# } else {
# CURSTATE ="GONORTH"
# timeW = timeW+1
# uav_moveto(5.0,0.0)
# }
}
########################################
#
# BARRIER-RELATED FUNCTIONS
#
########################################
#
# Constants
#
BARRIER_VSTIG = 1
#
# 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)
barrier.put(id, 1)
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 {
barrier_set(ROBOTS,idle)
barrier_ready()
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)
})
}
}
# printing the contents of a table: a custom function
function table_print(t) {
if(size(t)>0) {
foreach(t, function(u, tab) {
log("id: ",u," Range ", tab.r, "Bearing ", tab.b)
})
}
}
########################################
#
# MAIN FUNCTIONS
#
########################################
# Executed once at init time.
function init() {
s = swarm.create(1)
s.join()
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()
add_user_rb(10,rc_goto.latitude,rc_goto.longitude)
} 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)
# Read a value from the structure
# log(users)
#users_print(users.dataG)
# if(size(users.dataG)>0)
# vt.put("p", users.dataG)
# Get the number of keys in the structure
#log("The vstig has ", vt.size(), " elements")
# users_save(vt.get("p"))
# table_print(users.dataL)
}
# Executed once when the robot (or the simulator) is reset.
function reset() {
}
# Executed once at the end of experiment.
function destroy() {
}

242
script/testflockfev.bzz.old
View File

@ -1,242 +0,0 @@
# We need this for 2D vectors
# Make sure you pass the correct include path to "bzzc -I <path1:path2> ..."
#include "vec2.bzz"
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 = 5.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.put(id, 1)
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() {
}

171
script/testflockfev_barrier.bzz
View File

@ -1,171 +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 = 5.0
# Lennard-Jones parameters
TARGET = 10.0 #0.000001001
EPSILON = 10.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
# 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)
}
########################################
#
# BARRIER-RELATED FUNCTIONS
#
########################################
#
# Constants
#
BARRIER_VSTIG = 1
ROBOTS = 3 # number of robots in the swarm
barrier_number=0
barrier_break=0
#
# 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
#
function barrier_wait(threshold, transf) {
barrier.get(id)
if ( (barrier.size() >= threshold) or (barrier_break==1) ) {
barrier = nil
transf()
barrier_number=barrier_number+1
barrier_break=0
}
}
# flight status
function idle() {
statef=idle
neighbors.listen("cmd",
function(vid, value, rid) {
print("Got (", vid, ",", value, ") from robot #", rid)
if(value==22) {
statef=takeoff
} else if(value==21) {
statef=land
}
}
)
}
function takeoff() {
log("TakeOff: ", flight.status)
if( flight.status == 2 and position.altitude >= TARGET_ALTITUDE-TARGET_ALTITUDE/20.0) {
barrier_set(ROBOTS,hexagon)
barrier_ready()
}
else if( flight.status !=3){
log("Altitude: ", TARGET_ALTITUDE)
neighbors.broadcast("cmd", 22)
uav_takeoff(TARGET_ALTITUDE)
}
}
function land() {
log("Land: ", flight.status)
if(flight.status == 2 or flight.status == 3){
neighbors.broadcast("cmd", 21)
uav_land()
}
else
statef=idle
}
# Executed once at init time.
function init() {
statef=idle
}
# Executed at each time step.
function step() {
neighbors.broadcast("barrier_num", barrier_number)
neighbors.listen("barrier_num",
function(vid, value, rid) {
print("Got (", vid, ",", value, ") from robot #", rid)
if(value > barrier_number) {
barrier_break=1
}
}
)
if(flight.rc_cmd==22) {
log("cmd 22")
flight.rc_cmd=0
statef = takeoff
neighbors.broadcast("cmd", 22)
} else if(flight.rc_cmd==21) {
log("cmd 21")
log("To land")
flight.rc_cmd=0
statef = land
neighbors.broadcast("cmd", 21)
} else if(flight.rc_cmd==16) {
flight.rc_cmd=0
uav_goto()
}
statef()
}
# Executed once when the robot (or the simulator) is reset.
function reset() {
}
# Executed once at the end of experiment.
function destroy() {
}

184
script/testflocksim.bzz
View File

@ -1,18 +1,10 @@
# 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)
}
include "update.bzz"
include "barrier.bzz" # don't use a stigmergy id=11 with this header.
include "uavstates.bzz" # require an 'action' function to be defined here.
include "vstigenv.bzz"
TARGET_ALTITUDE = 10.0
CURSTATE = "TURNEDOFF"
# Lennard-Jones parameters
TARGET = 12.0
@ -48,8 +40,8 @@ function user_attract(t) {
}
# Calculates and actuates the flocking interaction
function hexagon() {
statef=hexagon
function action() {
statef=action
# Calculate accumulator
var accum = neighbors.map(lj_vector).reduce(lj_sum, math.vec2.new(0.0, 0.0))
if(neighbors.count() > 0)
@ -65,129 +57,22 @@ function hexagon() {
# Move according to vector
print("Robot ", id, "must push ", math.vec2.length(accum) )#, "; ", math.vec2.angle(accum))
uav_moveto(accum.x, accum.y)
CURSTATE = "LENNARDJONES"
UAVSTATE = "LENNARDJONES"
# if(timeW>=WAIT_TIMEOUT) { #FOR MOVETO TESTS
# timeW =0
# statef=land
# } else if(timeW>=WAIT_TIMEOUT/2) {
# CURSTATE ="GOEAST"
# UAVSTATE ="GOEAST"
# timeW = timeW+1
# uav_moveto(0.0,5.0)
# } else {
# CURSTATE ="GONORTH"
# UAVSTATE ="GONORTH"
# timeW = timeW+1
# uav_moveto(5.0,0.0)
# }
}
########################################
#
# BARRIER-RELATED FUNCTIONS
#
########################################
#
# Constants
#
BARRIER_VSTIG = 1
#
# 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)
barrier.put(id, 1)
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 {
barrier_set(ROBOTS,idle)
barrier_ready()
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)
})
}
}
# printing the contents of a table: a custom function
function table_print(t) {
if(size(t)>0) {
foreach(t, function(u, tab) {
log("id: ",u," Range ", tab.r, "Bearing ", tab.b)
})
}
}
########################################
#
@ -197,61 +82,20 @@ function table_print(t) {
# Executed once at init time.
function init() {
s = swarm.create(1)
s.join()
uav_initswarm()
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()
add_user_rb(10,rc_goto.latitude,rc_goto.longitude)
} 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()
}
}
uav_rccmd()
uav_neicmd()
)
statef()
log("Current state: ", CURSTATE)
log("Current state: ", UAVSTATE)
log("Swarm size: ",ROBOTS)
# Read a value from the structure

1156
script/testsolo.basm
View File

File diff suppressed because it is too large Load Diff

BIN
script/testsolo.bdbg
View File

Binary file not shown.

214
script/testsolo.bzz
View File

@ -1,214 +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)
log("Time: ", timeW)
if(timeW>=WAIT_TIMEOUT) { #FOR MOVETO TESTS
timeW =0
statef=land
} else {
if(timeW >= (WAIT_TIMEOUT / 2)){
uav_moveto(0.03,0.0)
} else {
uav_moveto(0.0,0.03)
}
timeW = timeW+1
}
}
########################################
#
# 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 = 300
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_set(ROBOTS, land);
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() {
}

39
script/teststig.bzz
View File

@ -1,39 +0,0 @@
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)
}
function init(){
s = swarm.create(1)
s.join()
v = stigmergy.create(5)
t= {}
v.put("p",t)
v.put("u",1)
}
function step() {
log("Swarm size: ",ROBOTS)
log("The vstig has ", v.size(), " elements")
log(v.get("u"))
if (id==1) {
tmp = { .x=3}
v.put("p",tmp)
v.put("u",2)
}
log(v.get("p"))
}
# Executed once when the robot (or the simulator) is reset.
function reset() {
}
# Executed once at the end of experiment.
function destroy() {
}