ROSBuzz_MISTLab/buzz_scripts/testRRT.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"

function action() {
  statef=action
  uav_storegoal(45.5088103899,-73.1540826153,25.0)
  set_goto(idle)
}

# Executed once at init time.
function init() {
  	uav_initstig()
	uav_initswarm()
	#statef=turnedoff
	#UAVSTATE = "TURNEDOFF"
	statef = takeoff
	BVMSTATE = "TAKEOFF"
}

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

	statef()

	log("Current state: ", BVMSTATE)
}
 
# Executed once when the robot (or the simulator) is reset.
function reset() {
}
 
# Executed once at the end of experiment.
function destroy() {
}