ROSBuzz_MISTLab/buzz_scripts/testaloneWP.bzz

include "update.bzz"
# don't use a stigmergy id=11 with this header, for barrier 
# it requires an 'action' function to be defined here.
include "act/states.bzz"
include "vstigenv.bzz"

#State launched after takeoff
AUTO_LAUNCH_STATE = "ACTION"

function action() {
  BVMSTATE = "ACTION"
  uav_storegoal(-1.0,-1.0,-1.0)
  goto_gps(picture)
}

# Executed once at init time.
function init() {
  	init_stig()
	init_swarm()

    # Starting state: TURNEDOFF to wait for user input.
	BVMSTATE = "TURNEDOFF"
}

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

	# update the vstig (status/net/batt/...)
  	# uav_updatestig()

	#
	# State machine
	#
	if(BVMSTATE=="TURNEDOFF")
		statef=turnedoff
	else if(BVMSTATE=="STOP")	# ends on turnedoff
		statef=stop
	else if(BVMSTATE=="LAUNCH") # ends on AUTO_LAUNCH_STATE
		statef=launch
	else if(BVMSTATE=="IDLE")
		statef=idle
	else if(BVMSTATE=="ACTION")
		statef=action

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