enhanced states switch and zooids rate
This commit is contained in:
dave
parent
8e6319e3a5
commit
cc767a5ac7
|
|
@ -10,12 +10,12 @@ include "utils/conversions.bzz"
|
|||
TARGET_ALTITUDE = 15.0 # m.
|
||||
BVMSTATE = "TURNEDOFF"
|
||||
PICTURE_WAIT = 20 # steps
|
||||
GOTO_MAXVEL = 1.5 # m/steps
|
||||
GOTO_MAXVEL = 2.5 # m/steps
|
||||
GOTO_MAXDIST = 150 # m.
|
||||
GOTODIST_TOL = 0.5 # m.
|
||||
GOTOANG_TOL = 0.1 # rad.
|
||||
path_it = 0
|
||||
rc_State = 0
|
||||
graphid = 0
|
||||
pic_time = 0
|
||||
g_it = 0
|
||||
|
||||
|
|
@ -163,8 +163,8 @@ function formation() {
|
|||
var accum = neighbors.map(lj_vector).reduce(lj_sum, math.vec2.new(0.0, 0.0))
|
||||
if(neighbors.count() > 0)
|
||||
math.vec2.scale(accum, 1.0 / neighbors.count())
|
||||
if(math.vec2.length(accum)>GOTO_MAXVEL*15)
|
||||
accum = math.vec2.scale(accum, 15*GOTO_MAXVEL/math.vec2.length(accum))
|
||||
if(math.vec2.length(accum)>GOTO_MAXVEL*10)
|
||||
accum = math.vec2.scale(accum, 10*GOTO_MAXVEL/math.vec2.length(accum))
|
||||
goto_abs(accum.x, accum.y, 0.0, 0.0)
|
||||
}
|
||||
|
||||
|
|
@ -195,19 +195,22 @@ function rc_cmd_listen() {
|
|||
stattab_send()
|
||||
} else if (flight.rc_cmd==900){
|
||||
flight.rc_cmd=0
|
||||
rc_State = 0
|
||||
BVMSTATE = "TASK_ALLOCATE"
|
||||
neighbors.broadcast("cmd", 900)
|
||||
} else if (flight.rc_cmd==901){
|
||||
flight.rc_cmd=0
|
||||
rc_State = 1
|
||||
destroyGraph()
|
||||
BVMSTATE = "PURSUIT"
|
||||
neighbors.broadcast("cmd", 901)
|
||||
} else if (flight.rc_cmd==902){
|
||||
flight.rc_cmd=0
|
||||
rc_State = 2
|
||||
destroyGraph()
|
||||
BVMSTATE = "AGGREGATE"
|
||||
neighbors.broadcast("cmd", 902)
|
||||
} else if (flight.rc_cmd==903){
|
||||
flight.rc_cmd=0
|
||||
rc_State = 3
|
||||
destroyGraph()
|
||||
BVMSTATE = "FORMATION"
|
||||
neighbors.broadcast("cmd", 903)
|
||||
}
|
||||
}
|
||||
|
|
@ -224,14 +227,17 @@ function nei_cmd_listen() {
|
|||
uav_arm()
|
||||
} else if(value==401 and BVMSTATE=="TURNEDOFF"){
|
||||
uav_disarm()
|
||||
} else if(value==900){
|
||||
rc_State = 0
|
||||
} else if(value==901){
|
||||
rc_State = 1
|
||||
} else if(value==902){
|
||||
rc_State = 2
|
||||
} else if(value==903){
|
||||
rc_State = 3
|
||||
} else if(value==900){ # Shapes
|
||||
BVMSTATE = "TASK_ALLOCATE"
|
||||
} else if(value==901){ # Pursuit
|
||||
destroyGraph()
|
||||
BVMSTATE = "PURSUIT"
|
||||
} else if(value==902){ # Agreggate
|
||||
destroyGraph()
|
||||
BVMSTATE = "AGGREGATE"
|
||||
} else if(value==903){ # Formation
|
||||
destroyGraph()
|
||||
BVMSTATE = "FORMATION"
|
||||
} else if(value==16 and BVMSTATE=="IDLE"){
|
||||
# neighbors.listen("gt",function(vid, value, rid) {
|
||||
# print("Got (", vid, ",", value, ") from robot #", rid)
|
||||
|
|
|
|||
|
|
@ -11,7 +11,6 @@ ROBOT_RADIUS = 50
|
|||
ROBOT_DIAMETER = 2.0*ROBOT_RADIUS
|
||||
ROBOT_SAFETYDIST = 2.0*ROBOT_DIAMETER
|
||||
ROOT_ID = 2
|
||||
old_state = -1
|
||||
|
||||
#
|
||||
# Global variables
|
||||
|
|
@ -401,7 +400,7 @@ function TransitionToJoined(){
|
|||
|
||||
m_navigation.x=0.0
|
||||
m_navigation.y=0.0
|
||||
goto_abs(m_navigation.x/100.0, m_navigation.y/100.0, 0.0)
|
||||
goto_abs(m_navigation.x/100.0, m_navigation.y/100.0, 0.0, 0.0)
|
||||
}
|
||||
#
|
||||
#Transistion to state Lock, lock the current formation
|
||||
|
|
@ -422,10 +421,8 @@ function TransitionToLock(){
|
|||
}
|
||||
m_navigation.x=0.0
|
||||
m_navigation.y=0.0
|
||||
goto_abs(m_navigation.x, m_navigation.y, 0.0)
|
||||
goto_abs(m_navigation.x, m_navigation.y, 0.0, 0.0)
|
||||
|
||||
# prepare to restart a new shape
|
||||
old_state = rc_State
|
||||
#stop listening
|
||||
neighbors.ignore("m")
|
||||
}
|
||||
|
|
@ -536,7 +533,7 @@ function DoAsking(){
|
|||
|
||||
m_navigation.x=0.0
|
||||
m_navigation.y=0.0
|
||||
goto_abs(m_navigation.x/100.0, m_navigation.y/100.0, 0.0)
|
||||
goto_abs(m_navigation.x/100.0, m_navigation.y/100.0, 0.0, 0.0)
|
||||
BroadcastGraph()
|
||||
}
|
||||
#
|
||||
|
|
@ -666,20 +663,14 @@ function DoJoined(){
|
|||
}
|
||||
}
|
||||
|
||||
#check if should to transists to lock
|
||||
#write statues
|
||||
#v_tag.get(m_nLabel)
|
||||
#log(v_tag.size(), " of ", ROBOTS, " ready to lock")
|
||||
#if(v_tag.size()==ROBOTS){
|
||||
# TransitionToLock()
|
||||
#}
|
||||
barrier_wait(ROBOTS, "GRAPH_TRANSTOLOCK", "GRAPH_JOINED")
|
||||
BroadcastGraph()
|
||||
}
|
||||
#
|
||||
#Do Lock
|
||||
#
|
||||
function DoLock(){
|
||||
timeout_graph = 40
|
||||
function DoLock() {
|
||||
UpdateGraph()
|
||||
m_selfMessage.State=s2i(BVMSTATE)
|
||||
m_selfMessage.Label=m_nLabel
|
||||
|
|
@ -693,19 +684,23 @@ function DoLock(){
|
|||
if(m_nLabel!=0){
|
||||
m_navigation=motion_vector()
|
||||
}
|
||||
#move
|
||||
# #move
|
||||
goto_abs(m_navigation.x, m_navigation.y, 0.0, 0.0)
|
||||
BroadcastGraph()
|
||||
}
|
||||
#
|
||||
# Executed after takeoff
|
||||
#
|
||||
function startGraph(){
|
||||
BVMSTATE="GRAPH_FREE"
|
||||
|
||||
# reset the graph
|
||||
statef=resetGraph
|
||||
if(loop) {
|
||||
if(timeout_graph==0) {
|
||||
if(graphid < 3)
|
||||
graphid = graphid + 1
|
||||
else
|
||||
graphid = 0
|
||||
timeout_graph = 40
|
||||
BVMSTATE="TASK_ALLOCATE"
|
||||
}
|
||||
timeout_graph = timeout_graph - 1
|
||||
}
|
||||
}
|
||||
|
||||
#
|
||||
# Executed at init
|
||||
#
|
||||
|
|
@ -750,6 +745,7 @@ function BroadcastGraph() {
|
|||
# Executed when reset
|
||||
#
|
||||
function resetGraph(){
|
||||
BVMSTATE="GRAPH_FREE"
|
||||
m_receivedMessage={.State=s2i("GRAPH_FREE"),.Label=0,.ReqLabel=0,.ReqID=0,.Response=r2i("REQ_NONE"),.Range=0,.Bearing=0}
|
||||
m_selfMessage={.State=s2i("GRAPH_FREE"),.Label=0,.ReqLabel=0,.ReqID=0,.Response=r2i("REQ_NONE")}
|
||||
m_navigation={.x=0,.y=0}
|
||||
|
|
@ -766,16 +762,16 @@ function resetGraph(){
|
|||
assign_id=-1
|
||||
m_gotjoinedparent = 0
|
||||
|
||||
if(rc_State==0){
|
||||
if(graphid==0){
|
||||
log("Loading P graph")
|
||||
Read_GraphP()
|
||||
} else if(rc_State==1) {
|
||||
} else if(graphid==1) {
|
||||
log("Loading O graph")
|
||||
Read_GraphO()
|
||||
} else if(rc_State==2) {
|
||||
} else if(graphid==2) {
|
||||
log("Loading L graph")
|
||||
Read_GraphL()
|
||||
} else if(rc_State==3) {
|
||||
} else if(graphid==3) {
|
||||
log("Loading Y graph")
|
||||
Read_GraphY()
|
||||
}
|
||||
|
|
@ -802,7 +798,7 @@ function destroyGraph() {
|
|||
#clear neighbour message
|
||||
m_navigation.x=0.0
|
||||
m_navigation.y=0.0
|
||||
goto_abs(m_navigation.x/100.0, m_navigation.y/100.0, 0.0)
|
||||
goto_abs(m_navigation.x/100.0, m_navigation.y/100.0, 0.0, 0.0)
|
||||
m_vecNodes={}
|
||||
#stop listening
|
||||
neighbors.ignore("m")
|
||||
|
|
|
|||
|
|
@ -7,7 +7,7 @@ include "taskallocate/graphformGPS.bzz"
|
|||
include "vstigenv.bzz"
|
||||
|
||||
#State launched after takeoff
|
||||
AUTO_LAUNCH_STATE = "PURSUIT"
|
||||
AUTO_LAUNCH_STATE = "TASK_ALLOCATE"
|
||||
|
||||
#####
|
||||
# Vehicule type:
|
||||
|
|
@ -27,6 +27,7 @@ function init() {
|
|||
init_swarm()
|
||||
|
||||
TARGET_ALTITUDE = 10 + id*2.0 # m
|
||||
loop = 1
|
||||
|
||||
# start the swarm command listener
|
||||
nei_cmd_listen()
|
||||
|
|
@ -60,21 +61,19 @@ function step() {
|
|||
statef=formation
|
||||
else if(BVMSTATE=="PURSUIT")
|
||||
statef=pursuit
|
||||
else if(BVMSTATE=="TASK_ALLOCATE") { # or bidding ?
|
||||
startGraph()
|
||||
} else if(BVMSTATE=="GRAPH_FREE") {
|
||||
else if(BVMSTATE=="TASK_ALLOCATE") # or bidding ?
|
||||
statef=resetGraph
|
||||
else if(BVMSTATE=="GRAPH_FREE")
|
||||
statef=DoFree
|
||||
} else if(BVMSTATE=="GRAPH_ASKING") {
|
||||
else if(BVMSTATE=="GRAPH_ASKING")
|
||||
statef=DoAsking
|
||||
} else if(BVMSTATE=="GRAPH_JOINING") {
|
||||
else if(BVMSTATE=="GRAPH_JOINING")
|
||||
statef=DoJoining
|
||||
} else if(BVMSTATE=="GRAPH_JOINED") {
|
||||
else if(BVMSTATE=="GRAPH_JOINED")
|
||||
statef=DoJoined
|
||||
} else if(BVMSTATE=="GRAPH_TRANSTOLOCK")
|
||||
else if(BVMSTATE=="GRAPH_TRANSTOLOCK")
|
||||
statef=TransitionToLock
|
||||
else if(BVMSTATE=="GRAPH_LOCK" and old_state!=rc_State) #switch to a new graph
|
||||
startGraph()
|
||||
else if(BVMSTATE=="GRAPH_LOCK" and old_state==rc_State) # move all together (TODO: not tested)
|
||||
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
|
||||
|
|
|
|||
Loading…
Reference in New Issue