ROSBuzz_MISTLab/src/test1.bzz

########################################
#
# 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)
  statestr = "barrier"
}

#
# Make yourself ready
#
function barrier_ready() {
 
  barrier.put(id, 1)
  print ("before put")
  barrier.get(id)
  print("barrier put : ")
}

#
# Executes the barrier
#
function table_print(t) {
  foreach(t, function(key, value) {
      log(key, " -> ", value)
    })
}
function barrier_wait(threshold, transf) {
 #table_print(barrier)
  barrier.get(id)
  extradbg = barrier.size()
  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() {
	if( flight.status == 2) {
		barrier_set(ROBOTS,transition_to_land)
		barrier_ready()
	}
	else if(flight.status!=3)
		uav_takeoff()
}
function land() {
	if( flight.status == 1) {
		barrier_set(ROBOTS,idle)
		barrier_ready()
	}
	else if(flight.status!=0 and flight.status!=4)
		uav_land()
}


function transition_to_land() {
	statef=transition_to_land
	if(battery.capacity<50){
		print("Low battery! Landing the fleet")
		statef=land
		neighbors.broadcast("cmd", 21)
	}
}

# Executed once at init time.
function init() {
i = 0
a = 0
val = 0
oldcmd = 0
tim=0
statef=idle
}
 
# Executed at each time step.
function step() {

#if(flight.rc_cmd!=oldcmd) {
	
	if(flight.rc_cmd==22 and flight.status==1) {
		flight.rc_cmd=0
		statef=takeoff		
		neighbors.broadcast("cmd", 22)
	} else if(flight.rc_cmd==21 and flight.status==2) {
		flight.rc_cmd=0
		statef=land
		neighbors.broadcast("cmd", 21)
	}
#	oldcmd=flight.rc_cmd
#}

statef()
}
 
# Executed once when the robot (or the simulator) is reset.
function reset() {
}
 
# Executed once at the end of experiment.
function destroy() {
}