clean bzz includes
This commit is contained in:
dave
parent
956b282ad3
commit
78edc3c9f0
|
|
@ -1,5 +1,4 @@
|
||||||
0 -1 -1 -1
|
0 -1 -1 -1
|
||||||
1 0 10.0 0.0
|
1 0 1000.0 0.0
|
||||||
2 0 10.0 1.57
|
2 0 1000.0 1.57
|
||||||
3 0 10.0 3.14
|
3 0 1000.0 3.14
|
||||||
4 0 10.0 4.71
|
|
||||||
|
|
@ -1,5 +1,5 @@
|
||||||
0 -1 -1 -1 -1
|
0 -1 -1 -1 -1
|
||||||
1 0 10.0 -1 -1
|
1 0 1000.0 -1 -1
|
||||||
2 0 10.0 1 14.0
|
2 0 1000.0 1 1414.2
|
||||||
3 0 10.0 2 14.0
|
3 0 1000.0 2 1414.2
|
||||||
4 0 10.0 1 14.0
|
4 0 1000.0 1 1414.2
|
||||||
|
|
@ -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() {
|
||||||
|
}
|
||||||
|
|
@ -4,8 +4,8 @@
|
||||||
include "string.bzz"
|
include "string.bzz"
|
||||||
include "vec2.bzz"
|
include "vec2.bzz"
|
||||||
include "update.bzz"
|
include "update.bzz"
|
||||||
include "barrier.bzz" #don't use a stigmergy id=11 with this header.
|
include "barrier.bzz" # don't use a stigmergy id=11 with this header.
|
||||||
include "uavstates.bzz"
|
include "uavstates.bzz" # require an 'action' function to be defined here.
|
||||||
|
|
||||||
#
|
#
|
||||||
#Constant parameters, need to be adjust
|
#Constant parameters, need to be adjust
|
||||||
|
|
@ -275,7 +275,7 @@ function Get_DisAndAzi(id){
|
||||||
neighbors.foreach(
|
neighbors.foreach(
|
||||||
function(rid, data) {
|
function(rid, data) {
|
||||||
if(rid==id){
|
if(rid==id){
|
||||||
m_receivedMessage.Range=data.distance
|
m_receivedMessage.Range=data.distance*100.0
|
||||||
m_receivedMessage.Bearing=data.azimuth
|
m_receivedMessage.Bearing=data.azimuth
|
||||||
}
|
}
|
||||||
})
|
})
|
||||||
|
|
@ -370,7 +370,7 @@ v_tag.put(m_nLabel, 1)
|
||||||
|
|
||||||
m_navigation.x=0.0
|
m_navigation.x=0.0
|
||||||
m_navigation.y=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.x=0.0
|
||||||
m_navigation.y=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_P=math.vec2.scale(tempvec_P,size(setJoinedIndexes))
|
||||||
tempvec_N=math.vec2.scale(tempvec_N,size(setJoinedIndexes))
|
tempvec_N=math.vec2.scale(tempvec_N,size(setJoinedIndexes))
|
||||||
m_navigation=math.vec2.add(tempvec_P,tempvec_N)
|
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
|
}else{ #no joined robots in sight
|
||||||
i=0
|
i=0
|
||||||
var tempvec={.x=0.0,.y=0.0}
|
var tempvec={.x=0.0,.y=0.0}
|
||||||
|
|
@ -454,28 +454,10 @@ function DoFree() {
|
||||||
i=i+1
|
i=i+1
|
||||||
}
|
}
|
||||||
m_navigation=math.vec2.scale(tempvec,1.0/i)
|
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
|
#jump the first step
|
||||||
if(step_cunt<=1){
|
if(step_cunt<=1){
|
||||||
|
|
@ -577,37 +559,21 @@ function DoJoining(){
|
||||||
m_bias=m_cMeToPred.Bearing-S2PGlobalBearing
|
m_bias=m_cMeToPred.Bearing-S2PGlobalBearing
|
||||||
S2Target_bearing=S2Target_bearing+m_bias
|
S2Target_bearing=S2Target_bearing+m_bias
|
||||||
m_navigation=math.vec2.newp(S2Target_dis,S2Target_bearing)
|
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
|
#test if is already in desired position
|
||||||
if(math.abs(S2Target.x)<m_fTargetDistanceTolerance and math.abs(S2Target.y)<m_fTargetDistanceTolerance){
|
if(math.abs(S2Target.x)<m_fTargetDistanceTolerance and math.abs(S2Target.y)<m_fTargetDistanceTolerance){
|
||||||
TransitionToJoined()
|
log(S2Target_dis,S2Target_bearing)
|
||||||
|
#TransitionToJoined()
|
||||||
return
|
return
|
||||||
}
|
}
|
||||||
} else{ #miss pred, there is a change the another robot block the sight, keep moving as before for sometime
|
} else{ #miss pred, there is a change the another robot block the sight, keep moving as before for sometime
|
||||||
m_unWaitCount=m_unWaitCount-1
|
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){
|
if(m_unWaitCount==0){
|
||||||
TransitionToFree()
|
TransitionToFree()
|
||||||
return
|
return
|
||||||
|
|
@ -689,14 +655,14 @@ function DoJoined(){
|
||||||
|
|
||||||
m_navigation.x=0.0
|
m_navigation.x=0.0
|
||||||
m_navigation.y=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
|
#check if should to transists to lock
|
||||||
|
|
||||||
|
|
||||||
if(v_tag.size()==ROBOTS){
|
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)
|
log(";",m_nLabel,";",mypred1.range-m_vecNodes_fixed[m_nLabel].d1)
|
||||||
}
|
}
|
||||||
#move
|
#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_unResponseTimeThreshold=10
|
||||||
m_unLabelSearchWaitTime=10
|
m_unLabelSearchWaitTime=10
|
||||||
m_fTargetDistanceTolerance=1.5
|
m_fTargetDistanceTolerance=150
|
||||||
m_unJoiningLostPeriod=100
|
m_unJoiningLostPeriod=100
|
||||||
|
|
||||||
#
|
#
|
||||||
# Join Swarm
|
# Join Swarm
|
||||||
#
|
#
|
||||||
s = swarm.create(1)
|
uav_initswarm()
|
||||||
s.join()
|
|
||||||
#ROBOT_NUM=5
|
|
||||||
Reset();
|
Reset();
|
||||||
statef=turnedoff
|
|
||||||
}
|
}
|
||||||
|
|
||||||
#
|
#
|
||||||
# Executed every step
|
# Executed every step
|
||||||
#
|
#
|
||||||
function step(){
|
function step(){
|
||||||
uavcmd()
|
uav_rccmd()
|
||||||
|
uav_neicmd()
|
||||||
#update the graph
|
#update the graph
|
||||||
UpdateNodeInfo()
|
UpdateNodeInfo()
|
||||||
#reset message package to be sent
|
#reset message package to be sent
|
||||||
|
|
|
||||||
|
|
@ -6,6 +6,13 @@
|
||||||
TARGET_ALTITUDE = 5.0
|
TARGET_ALTITUDE = 5.0
|
||||||
UAVSTATE = "TURNEDOFF"
|
UAVSTATE = "TURNEDOFF"
|
||||||
|
|
||||||
|
function uav_initswarm(){
|
||||||
|
s = swarm.create(1)
|
||||||
|
s.join()
|
||||||
|
statef=turnedoff
|
||||||
|
UAVSTATE = "TURNEDOFF"
|
||||||
|
}
|
||||||
|
|
||||||
function turnedoff() {
|
function turnedoff() {
|
||||||
statef=turnedoff
|
statef=turnedoff
|
||||||
UAVSTATE = "TURNEDOFF"
|
UAVSTATE = "TURNEDOFF"
|
||||||
|
|
@ -23,7 +30,7 @@ function takeoff() {
|
||||||
#log("Relative position: ", position.altitude)
|
#log("Relative position: ", position.altitude)
|
||||||
|
|
||||||
if( flight.status == 2 and position.altitude >= TARGET_ALTITUDE-TARGET_ALTITUDE/20.0) {
|
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()
|
barrier_ready()
|
||||||
#statef=hexagon
|
#statef=hexagon
|
||||||
}
|
}
|
||||||
|
|
@ -51,7 +58,7 @@ function land() {
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
function uavcmd() {
|
function uav_rccmd() {
|
||||||
if(flight.rc_cmd==22) {
|
if(flight.rc_cmd==22) {
|
||||||
log("cmd 22")
|
log("cmd 22")
|
||||||
flight.rc_cmd=0
|
flight.rc_cmd=0
|
||||||
|
|
@ -79,7 +86,9 @@ function uavcmd() {
|
||||||
uav_disarm()
|
uav_disarm()
|
||||||
neighbors.broadcast("cmd", 401)
|
neighbors.broadcast("cmd", 401)
|
||||||
}
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
function uav_neicmd() {
|
||||||
neighbors.listen("cmd",
|
neighbors.listen("cmd",
|
||||||
function(vid, value, rid) {
|
function(vid, value, rid) {
|
||||||
print("Got (", vid, ",", value, ") from robot #", rid)
|
print("Got (", vid, ",", value, ") from robot #", rid)
|
||||||
|
|
|
||||||
|
|
@ -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)
|
||||||
|
})
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
@ -1,11 +1,7 @@
|
||||||
include "vec2.bzz"
|
include "vec2.bzz"
|
||||||
updated="update_ack"
|
include "update.bzz"
|
||||||
update_no=0
|
include "barrier.bzz" # don't use a stigmergy id=11 with this header.
|
||||||
function updated_neigh(){
|
include "uavstates.bzz" # require an 'action' function to be defined here.
|
||||||
neighbors.broadcast(updated, update_no)
|
|
||||||
}
|
|
||||||
TARGET_ALTITUDE = 5.0
|
|
||||||
CURSTATE = "TURNEDOFF"
|
|
||||||
|
|
||||||
# Lennard-Jones parameters
|
# Lennard-Jones parameters
|
||||||
TARGET = 12.0
|
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 ##############################
|
### BUZZ FUNCTIONS ##############################
|
||||||
#################################################
|
#################################################
|
||||||
|
|
||||||
|
function action(){
|
||||||
|
if (id == 0)
|
||||||
|
statef=zero
|
||||||
|
else
|
||||||
|
statef=onetwo
|
||||||
|
|
||||||
|
UAVSTATE="TENTACLES"
|
||||||
|
}
|
||||||
|
|
||||||
# Executed at init time
|
# Executed at init time
|
||||||
function init() {
|
function init() {
|
||||||
s = swarm.create(0)
|
uav_initswarm()
|
||||||
s.join()
|
|
||||||
# Local knowledge table
|
# Local knowledge table
|
||||||
knowledge = {}
|
knowledge = {}
|
||||||
# Update local knowledge with information from the neighbors
|
# Update local knowledge with information from the neighbors
|
||||||
|
|
@ -352,71 +246,16 @@ function init() {
|
||||||
# Variables initialization
|
# Variables initialization
|
||||||
iteration = 0
|
iteration = 0
|
||||||
|
|
||||||
vt = stigmergy.create(5)
|
|
||||||
t = {}
|
|
||||||
vt.put("p",t)
|
|
||||||
statef=idle
|
|
||||||
CURSTATE = "IDLE"
|
|
||||||
}
|
}
|
||||||
|
|
||||||
# Executed every time step
|
# Executed every time step
|
||||||
function step() {
|
function step() {
|
||||||
if(flight.rc_cmd==22) {
|
uav_rccmd()
|
||||||
log("cmd 22")
|
uav_neicmd()
|
||||||
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()
|
statef()
|
||||||
log("Current state: ", CURSTATE)
|
log("Current state: ", CURSTATE)
|
||||||
log("Swarm size: ",ROBOTS)
|
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
|
# Count the number of steps
|
||||||
iteration = iteration + 1
|
iteration = iteration + 1
|
||||||
|
|
|
||||||
|
|
@ -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
159
script/test1.bzz
|
|
@ -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() {
|
|
||||||
}
|
|
||||||
|
|
@ -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
|
function action() {
|
||||||
# Make sure you pass the correct include path to "bzzc -I <path1:path2> ..."
|
statef=action
|
||||||
#include "/home/ubuntu/buzz/src/include/vec2.bzz"
|
# test moveto cmd dx, dy
|
||||||
#include "vec2.bzz"
|
# uav_moveto(0.5, 0.5)
|
||||||
####################################################################################################
|
|
||||||
# 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
|
|
||||||
|
|
||||||
# Executed once at init time.
|
# Executed once at init time.
|
||||||
function init() {
|
function init() {
|
||||||
}
|
}
|
||||||
|
|
@ -22,19 +17,7 @@ function init() {
|
||||||
# Executed at each time step.
|
# Executed at each time step.
|
||||||
function step() {
|
function step() {
|
||||||
log("Altitude: ", position.altitude)
|
log("Altitude: ", position.altitude)
|
||||||
if(flight.rc_cmd==22) {
|
uav_rccmd()
|
||||||
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)
|
|
||||||
}
|
}
|
||||||
|
|
||||||
# Executed once when the robot (or the simulator) is reset.
|
# Executed once when the robot (or the simulator) is reset.
|
||||||
|
|
|
||||||
|
|
@ -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() {
|
|
||||||
}
|
|
||||||
|
|
@ -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() {
|
|
||||||
}
|
|
||||||
|
|
@ -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() {
|
|
||||||
}
|
|
||||||
|
|
@ -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"
|
include "vec2.bzz"
|
||||||
####################################################################################################
|
include "update.bzz"
|
||||||
# Updater related
|
include "barrier.bzz" # don't use a stigmergy id=11 with this header.
|
||||||
# This should be here for the updater to work, changing position of code will crash the updater
|
include "uavstates.bzz" # require an 'action' function to be defined here.
|
||||||
####################################################################################################
|
include "vstigenv.bzz"
|
||||||
updated="update_ack"
|
|
||||||
update_no=0
|
|
||||||
function updated_neigh(){
|
|
||||||
neighbors.broadcast(updated, update_no)
|
|
||||||
}
|
|
||||||
|
|
||||||
TARGET_ALTITUDE = 10.0
|
TARGET_ALTITUDE = 10.0
|
||||||
CURSTATE = "TURNEDOFF"
|
|
||||||
|
|
||||||
# Lennard-Jones parameters
|
# Lennard-Jones parameters
|
||||||
TARGET = 12.0
|
TARGET = 12.0
|
||||||
|
|
@ -48,8 +40,8 @@ function user_attract(t) {
|
||||||
}
|
}
|
||||||
|
|
||||||
# Calculates and actuates the flocking interaction
|
# Calculates and actuates the flocking interaction
|
||||||
function hexagon() {
|
function action() {
|
||||||
statef=hexagon
|
statef=action
|
||||||
# Calculate accumulator
|
# Calculate accumulator
|
||||||
var accum = neighbors.map(lj_vector).reduce(lj_sum, math.vec2.new(0.0, 0.0))
|
var accum = neighbors.map(lj_vector).reduce(lj_sum, math.vec2.new(0.0, 0.0))
|
||||||
if(neighbors.count() > 0)
|
if(neighbors.count() > 0)
|
||||||
|
|
@ -65,129 +57,22 @@ function hexagon() {
|
||||||
# Move according to vector
|
# Move according to vector
|
||||||
print("Robot ", id, "must push ", math.vec2.length(accum) )#, "; ", math.vec2.angle(accum))
|
print("Robot ", id, "must push ", math.vec2.length(accum) )#, "; ", math.vec2.angle(accum))
|
||||||
uav_moveto(accum.x, accum.y)
|
uav_moveto(accum.x, accum.y)
|
||||||
CURSTATE = "LENNARDJONES"
|
UAVSTATE = "LENNARDJONES"
|
||||||
|
|
||||||
# if(timeW>=WAIT_TIMEOUT) { #FOR MOVETO TESTS
|
# if(timeW>=WAIT_TIMEOUT) { #FOR MOVETO TESTS
|
||||||
# timeW =0
|
# timeW =0
|
||||||
# statef=land
|
# statef=land
|
||||||
# } else if(timeW>=WAIT_TIMEOUT/2) {
|
# } else if(timeW>=WAIT_TIMEOUT/2) {
|
||||||
# CURSTATE ="GOEAST"
|
# UAVSTATE ="GOEAST"
|
||||||
# timeW = timeW+1
|
# timeW = timeW+1
|
||||||
# uav_moveto(0.0,5.0)
|
# uav_moveto(0.0,5.0)
|
||||||
# } else {
|
# } else {
|
||||||
# CURSTATE ="GONORTH"
|
# UAVSTATE ="GONORTH"
|
||||||
# timeW = timeW+1
|
# timeW = timeW+1
|
||||||
# uav_moveto(5.0,0.0)
|
# 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.
|
# Executed once at init time.
|
||||||
function init() {
|
function init() {
|
||||||
s = swarm.create(1)
|
uav_initswarm()
|
||||||
s.join()
|
|
||||||
vt = stigmergy.create(5)
|
vt = stigmergy.create(5)
|
||||||
t = {}
|
t = {}
|
||||||
vt.put("p",t)
|
vt.put("p",t)
|
||||||
statef=idle
|
|
||||||
CURSTATE = "IDLE"
|
|
||||||
}
|
}
|
||||||
|
|
||||||
# Executed at each time step.
|
# Executed at each time step.
|
||||||
function step() {
|
function step() {
|
||||||
if(flight.rc_cmd==22) {
|
uav_rccmd()
|
||||||
log("cmd 22")
|
uav_neicmd()
|
||||||
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()
|
statef()
|
||||||
log("Current state: ", CURSTATE)
|
log("Current state: ", UAVSTATE)
|
||||||
log("Swarm size: ",ROBOTS)
|
log("Swarm size: ",ROBOTS)
|
||||||
|
|
||||||
# Read a value from the structure
|
# Read a value from the structure
|
||||||
|
|
|
||||||
1156
script/testsolo.basm
1156
script/testsolo.basm
File diff suppressed because it is too large
Load Diff
Binary file not shown.
|
|
@ -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() {
|
|
||||||
}
|
|
||||||
|
|
@ -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() {
|
|
||||||
}
|
|
||||||
Loading…
Reference in New Issue