121 lines
3.0 KiB
Plaintext
121 lines
3.0 KiB
Plaintext
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 "utils/table.bzz"
|
|
#include "plan/rrtstar.bzz"
|
|
include "taskallocate/graphformGPS.bzz"
|
|
#include "taskallocate/bidding.bzz"
|
|
include "vstigenv.bzz"
|
|
#include "timesync.bzz"
|
|
include "utils/takeoff_heights.bzz"
|
|
|
|
#State launched after takeoff
|
|
|
|
AUTO_LAUNCH_STATE = "IDLE"
|
|
LAND_AFTER_BARRIER_EXPIRE = 1 # if set to be 1 , the robots will land after barrier expire; if set to be 0, the robots will carry on to AUTO_LAUNCH_STATE.
|
|
|
|
#####
|
|
# Vehicule type:
|
|
# 0 -> outdoor flying vehicle
|
|
# 1 -> indoor flying vehicle
|
|
# 2 -> outdoor wheeled vehicle
|
|
# 3 -> indoor wheeled vehicle
|
|
if(id==0) # No network ID=0, it's the groundstation/charging station.
|
|
V_TYPE = 2
|
|
else
|
|
V_TYPE = 0
|
|
|
|
|
|
# Executed once at init time.
|
|
function init() {
|
|
init_stig()
|
|
init_swarm()
|
|
#init_bidding()
|
|
|
|
TARGET_ALTITUDE = takeoff_heights[id]
|
|
|
|
# start the swarm command listener
|
|
nei_cmd_listen()
|
|
|
|
# Starting state: TURNEDOFF to wait for user input.
|
|
BVMSTATE = "TURNEDOFF"
|
|
}
|
|
|
|
# Executed at each time step.
|
|
function step() {
|
|
|
|
# listen to Remote Controller
|
|
rc_cmd_listen()
|
|
|
|
# update the vstig (status/net/batt/...)
|
|
uav_updatestig()
|
|
|
|
#
|
|
# State machine
|
|
#
|
|
if(BVMSTATE=="TURNEDOFF")
|
|
statef=turnedoff
|
|
else if(BVMSTATE=="CUSFUN")
|
|
statef=cusfun
|
|
else if(BVMSTATE=="STOP") # ends on turnedoff
|
|
statef=stop
|
|
else if(BVMSTATE=="LAUNCH") # ends on AUTO_LAUNCH_STATE
|
|
if(LAND_AFTER_BARRIER_EXPIRE == 1)
|
|
statef=launch
|
|
else
|
|
statef=launch_switch
|
|
else if(BVMSTATE=="GOHOME") # ends on AUTO_LAUNCH_STATE
|
|
statef=goinghome
|
|
else if(BVMSTATE=="WAYPOINT")
|
|
statef=indiWP
|
|
else if(BVMSTATE=="IDLE")
|
|
statef=idle
|
|
else if(BVMSTATE=="AGGREGATE")
|
|
statef=aggregate
|
|
else if(BVMSTATE=="POTENTIAL")
|
|
statef=lennardjones
|
|
else if(BVMSTATE=="PURSUIT")
|
|
statef=pursuit
|
|
else if(BVMSTATE=="TASK_ALLOCATE") # or bidding ?
|
|
statef=resetGraph
|
|
else if(BVMSTATE=="BIDDING") # check the absolute path of the waypointlist csv file in bidding.bzz
|
|
statef=bidding
|
|
else if(BVMSTATE=="DEPLOY") # check the absolute path of the waypointlist csv file in bidding.bzz
|
|
statef=voronoicentroid
|
|
else if(BVMSTATE=="GRAPH_FREE")
|
|
statef=DoFree
|
|
else if(BVMSTATE=="GRAPH_ASKING")
|
|
statef=DoAsking
|
|
else if(BVMSTATE=="GRAPH_JOINING")
|
|
statef=DoJoining
|
|
else if(BVMSTATE=="GRAPH_JOINED")
|
|
statef=DoJoined
|
|
else if(BVMSTATE=="GRAPH_LOCK") # move all together (TODO: not tested), but auto-loop in graph list
|
|
statef=DoLock
|
|
else if(BVMSTATE=="PATHPLAN") # ends on navigate, defined in rrtstar
|
|
statef=rrtstar
|
|
else if(BVMSTATE=="NAVIGATE") # ends on idle, defined in rrtstar
|
|
statef=navigate
|
|
else if(BVMSTATE == "FOLLOW") #TODO: not tested in new structure
|
|
statef=follow
|
|
else if(BVMSTATE == "PICTURE") #TODO: not tested in new structure
|
|
statef=take_picture
|
|
|
|
|
|
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() {
|
|
close_bidding()
|
|
discomp_destroy()
|
|
}
|