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)
}

#
# 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() {
	if( flight.status == 2) {
		barrier_set(ROBOTS,idle)
		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()
}

# Executed once at init time.
function init() {
	statef=idle
}

# Executed at each time step.
function step() {

	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)
	}

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