Spiri
/
ROSBuzz_MISTLab
12
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

removed state transitions

This commit is contained in:
dave 2018-09-08 19:15:46 -04:00
parent d055d8939f
commit 602d744140
4 changed files with 254 additions and 269 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

23
buzz_scripts/include/act/barrier.bzz
View File

@ -50,19 +50,22 @@ function barrier_ready(bc) {
# Executes the barrier # Executes the barrier
# #
function barrier_wait(threshold, transf, resumef, bc) { function barrier_wait(threshold, transf, resumef, bc) {
if(barrier==nil) #failsafe
barrier_create()
barrier.put(id, bc) barrier.put(id, bc)
barrier.get(id) barrier.get(id)
var allgood = 0
log("--->BS: ", barrier.size(), " / ", threshold, " (", BARRIER_VSTIG, " - ", barrier.get("d"), ") t= ", timeW) log("--->BS: ", barrier.size(), " / ", threshold, " (", BARRIER_VSTIG, " - ", barrier.get("d"), ") t= ", timeW)
if(barrier.size() - 1 >= threshold or barrier.get("d") == 1) { if(barrier.size()-1 >= threshold or barrier.get("d") == 1) {
allgood = barrier_allgood(barrier,bc) if(barrier_allgood(barrier,bc)) {
barrier.put("d", 1)
timeW = 0
BVMSTATE = transf
} else
barrier.put("d", 0)
} }
if(allgood) { if(timeW >= BARRIER_TIMEOUT) {
barrier.put("d", 1)
timeW = 0
BVMSTATE = transf
} else if(timeW >= BARRIER_TIMEOUT) {
log("------> Barrier Timeout !!!!") log("------> Barrier Timeout !!!!")
barrier = nil barrier = nil
timeW = 0 timeW = 0
@ -73,9 +76,7 @@ function barrier_wait(threshold, transf, resumef, bc) {
timeW = timeW+1 timeW = timeW+1
} }
barriergood = 1 # Check all members state
# Barrer check all entries
function barrier_allgood(barrier, bc) { function barrier_allgood(barrier, bc) {
barriergood = 1 barriergood = 1
barrier.foreach( barrier.foreach(

12
buzz_scripts/include/act/states.bzz
View File

@ -17,7 +17,6 @@ GOTO_MAXDIST = 150 # m.
GOTODIST_TOL = 1.0 # m. GOTODIST_TOL = 1.0 # m.
GOTOANG_TOL = 0.1 # rad. GOTOANG_TOL = 0.1 # rad.
path_it = 0 path_it = 0
graphid = 3
pic_time = 0 pic_time = 0
g_it = 0 g_it = 0
@ -103,9 +102,8 @@ function take_picture() {
} }
function goto_gps(transf) { function goto_gps(transf) {
log(" has to move to lat : ", rc_goto.latitude, " long: ", rc_goto.longitude, "Current lat : ", pose.position.latitude," lon ",pose.position.longitude)
m_navigation = vec_from_gps(rc_goto.latitude, rc_goto.longitude, 0) m_navigation = vec_from_gps(rc_goto.latitude, rc_goto.longitude, 0)
print(" has to move ", math.vec2.length(m_navigation), math.vec2.angle(m_navigation)) #print(" has to move ", math.vec2.length(m_navigation), math.vec2.angle(m_navigation))
if(math.vec2.length(m_navigation)>GOTO_MAXDIST) if(math.vec2.length(m_navigation)>GOTO_MAXDIST)
log("Sorry this is too far.") log("Sorry this is too far.")
else if(math.vec2.length(m_navigation) < GOTODIST_TOL and math.vec2.angle(m_navigation) < GOTOANG_TOL) # reached destination else if(math.vec2.length(m_navigation) < GOTODIST_TOL and math.vec2.angle(m_navigation) < GOTOANG_TOL) # reached destination
@ -265,13 +263,13 @@ function nei_cmd_listen() {
neighbors.listen("cmd", neighbors.listen("cmd",
function(vid, value, rid) { function(vid, value, rid) {
print("Got (", vid, ",", value, ") #", rid, "(", BVMSTATE, ")") print("Got (", vid, ",", value, ") #", rid, "(", BVMSTATE, ")")
if(BVMSTATE!="BARRIERWAIT") { #if(BVMSTATE!="BARRIERWAIT") {
if(value==22) { if(value==22 and BVMSTATE=="TURNEDOFF") {
BVMSTATE = "LAUNCH" BVMSTATE = "LAUNCH"
}else if(value==20) { }else if(value==20) {
AUTO_LAUNCH_STATE = "IDLE" AUTO_LAUNCH_STATE = "IDLE"
BVMSTATE = "GOHOME" BVMSTATE = "GOHOME"
} else if(value==21) { } else if(value==21 and BVMSTATE!="TURNEDOFF") {
AUTO_LAUNCH_STATE = "TURNEDOFF" AUTO_LAUNCH_STATE = "TURNEDOFF"
BVMSTATE = "STOP" BVMSTATE = "STOP"
} else if(value==400 and BVMSTATE=="TURNEDOFF") { } else if(value==400 and BVMSTATE=="TURNEDOFF") {
@ -306,6 +304,6 @@ function nei_cmd_listen() {
# # if(gt.id == id) statef=goto # # if(gt.id == id) statef=goto
# }) # })
} }
} #}
}) })
} }

477
buzz_scripts/include/taskallocate/graphformGPS.bzz
View File

@ -11,6 +11,8 @@ ROBOT_RADIUS = 50
ROBOT_DIAMETER = 2.0*ROBOT_RADIUS ROBOT_DIAMETER = 2.0*ROBOT_RADIUS
ROBOT_SAFETYDIST = 2.0*ROBOT_DIAMETER ROBOT_SAFETYDIST = 2.0*ROBOT_DIAMETER
ROOT_ID = 2 ROOT_ID = 2
graph_id = 3
graph_loop = 1
# #
# Global variables # Global variables
@ -347,83 +349,17 @@ function UpdateNodeInfo(){
i=i+1 i=i+1
} }
} }
#
#Transistion to state free
#
function TransitionToFree(){
BVMSTATE="GRAPH_FREE"
m_unWaitCount=m_unLabelSearchWaitTime
m_selfMessage.State=s2i(BVMSTATE)
}
#
#Transistion to state asking
#
function TransitionToAsking(un_label){
BVMSTATE="GRAPH_ASKING"
m_nLabel=un_label
m_unRequestId=id #don't know why the random numbers are the same, add id to make the ReqID different
m_selfMessage.State=s2i(BVMSTATE)
m_selfMessage.ReqLabel=m_nLabel
m_selfMessage.ReqID=m_unRequestId
m_unWaitCount=m_unResponseTimeThreshold
}
#
#Transistion to state joining
#
function TransitionToJoining(){
BVMSTATE="GRAPH_JOINING"
m_selfMessage.State=s2i(BVMSTATE)
m_selfMessage.Label=m_nLabel
m_unWaitCount=m_unJoiningLostPeriod
}
#
#Transistion to state joined
#
function TransitionToJoined(){
BVMSTATE="GRAPH_JOINED"
m_selfMessage.State=s2i(BVMSTATE)
m_selfMessage.Label=m_nLabel
m_vecNodes[m_nLabel].State="ASSIGNED"
#write statues
#v_tag.put(m_nLabel, m_lockstig)
barrier_create()
barrier_ready(900+50)
m_navigation.x=0.0
m_navigation.y=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
#
function TransitionToLock(){
BVMSTATE="GRAPH_LOCK"
m_selfMessage.State=s2i(BVMSTATE)
m_selfMessage.Label=m_nLabel
m_vecNodes[m_nLabel].State="ASSIGNED"
#record neighbor distance
lock_neighbor_id={}
lock_neighbor_dis={}
var i=0
while(i<m_neighbourCount){
lock_neighbor_id[i]=m_messageID[i]
lock_neighbor_dis[i]=m_MessageRange[i]
i=i+1
}
m_navigation.x=0.0
m_navigation.y=0.0
goto_abs(m_navigation.x, m_navigation.y, 0.0, 0.0)
#stop listening
neighbors.ignore("m")
}
# #
# Do free # Do free
# #
function DoFree() { function DoFree() {
UpdateGraph() if(BVMSTATE!="GRAPH_FREE"){
BVMSTATE="GRAPH_FREE"
m_unWaitCount=m_unLabelSearchWaitTime
m_selfMessage.State=s2i(BVMSTATE)
}else{
UpdateGraph()
m_selfMessage.State=s2i(BVMSTATE) m_selfMessage.State=s2i(BVMSTATE)
#wait for a while before looking for a Label #wait for a while before looking for a Label
@ -458,91 +394,108 @@ function DoFree() {
} }
if(unFoundLabel>0){ if(unFoundLabel>0){
TransitionToAsking(unFoundLabel) m_nLabel=unFoundLabel
DoAsking()
return return
} }
#set message #set message
m_selfMessage.State=s2i(BVMSTATE) m_selfMessage.State=s2i(BVMSTATE)
BroadcastGraph() BroadcastGraph()
}
} }
# #
#Do asking #Do asking
# #
function DoAsking(){ function DoAsking(){
UpdateGraph() if(BVMSTATE!="GRAPH_ASKING"){
#look for response from predecessor BVMSTATE="GRAPH_ASKING"
var i=0 m_unRequestId=id #don't know why the random numbers are the same, add id to make the ReqID different
var psResponse=-1 m_selfMessage.State=s2i(BVMSTATE)
while(i<m_neighbourCount and psResponse==-1){ m_selfMessage.ReqLabel=m_nLabel
#the respond robot in joined state m_selfMessage.ReqID=m_unRequestId
#the request Label be the same as requesed m_unWaitCount=m_unResponseTimeThreshold
#get a respond }else {
if(m_MessageState[i]=="GRAPH_JOINED"){ UpdateGraph()
#log("received label = ",m_MessageReqLabel[i]) #look for response from predecessor
if(m_MessageReqLabel[i]==m_nLabel) var i=0
if(m_MessageResponse[i]!="REQ_NONE"){ var psResponse=-1
psResponse=i while(i<m_neighbourCount and psResponse==-1){
}} #the respond robot in joined state
if(m_MessageState[i]=="GRAPH_JOINING" and m_MessageLabel[i]==m_nLabel){ #the request Label be the same as requesed
TransitionToFree() #get a respond
return if(m_MessageState[i]=="GRAPH_JOINED"){
#log("received label = ",m_MessageReqLabel[i])
if(m_MessageReqLabel[i]==m_nLabel)
if(m_MessageResponse[i]!="REQ_NONE"){
psResponse=i
}}
if(m_MessageState[i]=="GRAPH_JOINING" and m_MessageLabel[i]==m_nLabel){
DoFree()
return
}
i=i+1
} }
i=i+1 #analyse response
} if(psResponse==-1){
#analyse response #no response, wait
if(psResponse==-1){
#no response, wait
m_unWaitCount=m_unWaitCount-1 m_unWaitCount=m_unWaitCount-1
m_selfMessage.State=s2i(BVMSTATE) m_selfMessage.State=s2i(BVMSTATE)
m_selfMessage.ReqLabel=m_nLabel m_selfMessage.ReqLabel=m_nLabel
m_selfMessage.ReqID=m_unRequestId m_selfMessage.ReqID=m_unRequestId
#if(m_unWaitCount==0){ #if(m_unWaitCount==0){
#TransitionToFree() #DoFree()
#return #return
#} #}
}
else{
log("respond id=",m_MessageReqID[psResponse])
if(m_MessageReqID[psResponse]!=m_unRequestId){
m_vecNodes[m_nLabel].State="ASSIGNING"
m_vecNodes[m_nLabel].StateAge=m_unJoiningLostPeriod
TransitionToFree()
}
if(m_MessageReqID[psResponse]==m_unRequestId){
if(m_MessageResponse[psResponse]=="REQ_GRANTED"){
TransitionToJoining()
return
} }
else{ else{
TransitionToAsking(m_nLabel) log("respond id=",m_MessageReqID[psResponse])
return if(m_MessageReqID[psResponse]!=m_unRequestId){
m_vecNodes[m_nLabel].State="ASSIGNING"
m_vecNodes[m_nLabel].StateAge=m_unJoiningLostPeriod
DoFree()
}
if(m_MessageReqID[psResponse]==m_unRequestId){
if(m_MessageResponse[psResponse]=="REQ_GRANTED"){
DoJoining()
return
}
else{
DoAsking()
return
}
} }
} }
} m_selfMessage.Label=m_nLabel
m_selfMessage.Label=m_nLabel
m_navigation.x=0.0 m_navigation.x=0.0
m_navigation.y=0.0 m_navigation.y=0.0
goto_abs(m_navigation.x/100.0, m_navigation.y/100.0, 0.0, 0.0) goto_abs(m_navigation.x/100.0, m_navigation.y/100.0, 0.0, 0.0)
BroadcastGraph() BroadcastGraph()
}
} }
# #
#Do joining #Do joining
# #
function DoJoining(){ function DoJoining(){
UpdateGraph() if(BVMSTATE!="GRAPH_JOINING") {
BVMSTATE="GRAPH_JOINING"
if(m_gotjoinedparent!=1) m_selfMessage.State=s2i(BVMSTATE)
set_rc_goto() m_selfMessage.Label=m_nLabel
else m_unWaitCount=m_unJoiningLostPeriod
goto_gps(TransitionToJoined) } else {
#pack the communication package UpdateGraph()
m_selfMessage.State=s2i(BVMSTATE) if(m_gotjoinedparent!=1)
m_selfMessage.Label=m_nLabel set_rc_goto()
BroadcastGraph() else
goto_gps(DoJoined)
#pack the communication package
m_selfMessage.State=s2i(BVMSTATE)
m_selfMessage.Label=m_nLabel
BroadcastGraph()
}
} }
function set_rc_goto() { function set_rc_goto() {
@ -576,122 +529,159 @@ function set_rc_goto() {
#Do joined #Do joined
# #
function DoJoined(){ function DoJoined(){
UpdateGraph() if(BVMSTATE!="GRAPH_JOINED"){
m_selfMessage.State=s2i(BVMSTATE) BVMSTATE="GRAPH_JOINED"
m_selfMessage.Label=m_nLabel m_selfMessage.State=s2i(BVMSTATE)
m_selfMessage.Label=m_nLabel
m_vecNodes[m_nLabel].State="ASSIGNED"
#collect all requests #write statues
var mapRequests={} barrier_create()
var i=0 barrier_ready(-1)
var j=0
var ReqLabel m_navigation.x=0.0
var JoiningLabel m_navigation.y=0.0
var seenPred=0 goto_abs(m_navigation.x/100.0, m_navigation.y/100.0, 0.0, 0.0)
while(i<m_neighbourCount){ } else {
if(m_MessageState[i]=="GRAPH_ASKING"){ UpdateGraph()
ReqLabel=m_MessageReqLabel[i] m_selfMessage.State=s2i(BVMSTATE)
#log("ReqLabel var:",ReqLabel) m_selfMessage.Label=m_nLabel
#log("M_vec var",m_vecNodes[ReqLabel].State)
if(m_vecNodes[ReqLabel].State=="UNASSIGNED") #collect all requests
if(m_nLabel==m_vecNodes[ReqLabel].Pred){ var mapRequests={}
#is a request, store the index var i=0
mapRequests[j]=i var j=0
j=j+1 var ReqLabel
} var JoiningLabel
} var seenPred=0
if(m_MessageState[i]=="GRAPH_JOINING"){ while(i<m_neighbourCount){
JoiningLabel=m_MessageLabel[i] if(m_MessageState[i]=="GRAPH_ASKING"){
if(m_nLabel==m_vecNodes[JoiningLabel].Pred){ ReqLabel=m_MessageReqLabel[i]
##joining wrt this dot,send the global bearing #log("ReqLabel var:",ReqLabel)
var m_messageForJoining={.Label=JoiningLabel,.Bearing=m_MessageBearing[i]-m_bias} #log("M_vec var",m_vecNodes[ReqLabel].State)
if(m_vecNodes[ReqLabel].State=="UNASSIGNED")
if(m_nLabel==m_vecNodes[ReqLabel].Pred){
#is a request, store the index
mapRequests[j]=i
j=j+1
}
}
if(m_MessageState[i]=="GRAPH_JOINING"){
JoiningLabel=m_MessageLabel[i]
if(m_nLabel==m_vecNodes[JoiningLabel].Pred){
##joining wrt this dot,send the global bearing
var m_messageForJoining={.Label=JoiningLabel,.Bearing=m_MessageBearing[i]-m_bias}
}
} }
}
if(m_MessageState[i]=="GRAPH_JOINING" and repeat_assign==1 and m_MessageLabel[i]==assign_label){ if(m_MessageState[i]=="GRAPH_JOINING" and repeat_assign==1 and m_MessageLabel[i]==assign_label){
repeat_assign=0 repeat_assign=0
} }
#if it is the pred #if it is the pred
if((m_MessageState[i]=="GRAPH_JOINED" or m_MessageState[i]=="GRAPH_LOCK") and m_MessageLabel[i]==m_vecNodes[m_nLabel].Pred){ if((m_MessageState[i]=="GRAPH_JOINED" or m_MessageState[i]=="GRAPH_LOCK") and m_MessageLabel[i]==m_vecNodes[m_nLabel].Pred){
seenPred=1 seenPred=1
m_unWaitCount=m_unJoiningLostPeriod m_unWaitCount=m_unJoiningLostPeriod
} }
i=i+1
}
#get request
if(size(mapRequests)!=0){
i=1
var ReqIndex=0
while(i<size(mapRequests)){
#compare the distance
if(m_MessageRange[mapRequests[ReqIndex]]>m_MessageRange[mapRequests[i]])
ReqIndex=i
i=i+1 i=i+1
} }
if(repeat_assign==0){
#get the best index, whose ReqLabel and Reqid are
ReqLabel=m_MessageReqLabel[mapRequests[ReqIndex]]
var ReqID=m_MessageReqID[mapRequests[ReqIndex]]
assign_label=ReqLabel
assign_id=ReqID
repeat_assign=1
}
m_selfMessage.ReqLabel=assign_label
m_selfMessage.ReqID=assign_id
m_selfMessage.Response=r2i("REQ_GRANTED")
#m_vecNodes[ReqLabel].State="ASSIGNING"
log("Label=",assign_label)
log("ID=",assign_id)
m_vecNodes[ReqLabel].StateAge=m_unJoiningLostPeriod
}
#lost pred, wait for some time and transit to free #get request
if(seenPred==0){ if(size(mapRequests)!=0){
m_unWaitCount=m_unWaitCount-1 i=1
if(m_unWaitCount==0){ var ReqIndex=0
TransitionToFree() while(i<size(mapRequests)){
return #compare the distance
} if(m_MessageRange[mapRequests[ReqIndex]]>m_MessageRange[mapRequests[i]])
} ReqIndex=i
barrier_wait(ROBOTS, "GRAPH_TRANSTOLOCK", "GRAPH_JOINED",941) i=i+1
BroadcastGraph() }
if(repeat_assign==0){
#get the best index, whose ReqLabel and Reqid are
ReqLabel=m_MessageReqLabel[mapRequests[ReqIndex]]
var ReqID=m_MessageReqID[mapRequests[ReqIndex]]
assign_label=ReqLabel
assign_id=ReqID
repeat_assign=1
}
m_selfMessage.ReqLabel=assign_label
m_selfMessage.ReqID=assign_id
m_selfMessage.Response=r2i("REQ_GRANTED")
#m_vecNodes[ReqLabel].State="ASSIGNING"
log("Label=",assign_label)
log("ID=",assign_id)
m_vecNodes[ReqLabel].StateAge=m_unJoiningLostPeriod
}
#lost pred, wait for some time and transit to free
if(seenPred==0){
m_unWaitCount=m_unWaitCount-1
if(m_unWaitCount==0){
DoFree()
return
}
}
# using JOINED as the resume state kinds of just reset the barrier timeout, should be IDLE/ALLOCATE...
barrier_wait(ROBOTS, "GRAPH_LOCK", "GRAPH_JOINED", -1)
BroadcastGraph()
}
} }
# #
#Do Lock #Do Lock
# #
timeout_graph = 40 timeout_graph = 40
function DoLock() { function DoLock() {
UpdateGraph() #[transition to lock...
m_selfMessage.State=s2i(BVMSTATE) BVMSTATE="GRAPH_LOCK"
m_selfMessage.Label=m_nLabel m_selfMessage.State=s2i(BVMSTATE)
m_navigation.x=0.0 m_selfMessage.Label=m_nLabel
m_navigation.y=0.0 m_vecNodes[m_nLabel].State="ASSIGNED"
#calculate motion vection #record neighbor distance
if(m_nLabel==0){ lock_neighbor_id={}
m_navigation.x=0.0 #change value so that robot 0 will move lock_neighbor_dis={}
m_navigation.y=0.0 var i=0
} while(i<m_neighbourCount){
if(m_nLabel!=0){ lock_neighbor_id[i]=m_messageID[i]
m_navigation=motion_vector() lock_neighbor_dis[i]=m_MessageRange[i]
} i=i+1
# #move }
m_navigation.x=0.0
m_navigation.y=0.0
goto_abs(m_navigation.x, m_navigation.y, 0.0, 0.0)
# goto_abs(m_navigation.x, m_navigation.y, 0.0, 0.0) #stop listening
BroadcastGraph() neighbors.ignore("m")
#..]
UpdateGraph()
m_selfMessage.State=s2i(BVMSTATE)
m_selfMessage.Label=m_nLabel
m_navigation.x=0.0
m_navigation.y=0.0
#calculate motion vection
if(m_nLabel==0){
m_navigation.x=0.0 #change value so that robot 0 will move
m_navigation.y=0.0
}
if(m_nLabel!=0){
m_navigation=motion_vector()
}
# #move
# if(loop) { # goto_abs(m_navigation.x, m_navigation.y, 0.0, 0.0)
# if(timeout_graph==0) { BroadcastGraph()
# if(graphid < 3)
# graphid = graphid + 1 if(graph_loop) {
# else if(timeout_graph==0) {
# graphid = 0 if(graph_id < 3)
# timeout_graph = 40 graph_id = graph_id + 1
# BVMSTATE="TASK_ALLOCATE" else
# } graph_id = 0
# timeout_graph = timeout_graph - 1 timeout_graph = 40
# } BVMSTATE="TASK_ALLOCATE"
}
timeout_graph = timeout_graph - 1
}
} }
# #
@ -738,7 +728,6 @@ function BroadcastGraph() {
# Executed when reset # Executed when reset
# #
function resetGraph(){ 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_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_selfMessage={.State=s2i("GRAPH_FREE"),.Label=0,.ReqLabel=0,.ReqID=0,.Response=r2i("REQ_NONE")}
m_navigation={.x=0,.y=0} m_navigation={.x=0,.y=0}
@ -755,16 +744,16 @@ function resetGraph(){
assign_id=-1 assign_id=-1
m_gotjoinedparent = 0 m_gotjoinedparent = 0
if(graphid==0){ if(graph_id==0){
log("Loading P graph") log("Loading P graph")
Read_GraphP() Read_GraphP()
} else if(graphid==1) { } else if(graph_id==1) {
log("Loading O graph") log("Loading O graph")
Read_GraphO() Read_GraphO()
} else if(graphid==2) { } else if(graph_id==2) {
log("Loading L graph") log("Loading L graph")
Read_GraphL() Read_GraphL()
} else if(graphid==3) { } else if(graph_id==3) {
log("Loading Y graph") log("Loading Y graph")
Read_GraphY() Read_GraphY()
} }
@ -777,11 +766,9 @@ function resetGraph(){
#[A]The robot used to triger the formation process is defined as joined, #[A]The robot used to triger the formation process is defined as joined,
if(id==ROOT_ID){ if(id==ROOT_ID){
m_nLabel=0 m_nLabel=0
TransitionToJoined() DoJoined()
} }else{ #[B]Other robots are defined as free.
#[B]Other robots are defined as free. DoFree()
else{
TransitionToFree()
} }
} }
# #

9
buzz_scripts/main.bzz
View File

@ -14,6 +14,9 @@ AUTO_LAUNCH_STATE = "TASK_ALLOCATE"
LOWEST_ROBOT_ID = 0 LOWEST_ROBOT_ID = 0
TARGET = 9.0 TARGET = 9.0
EPSILON = 30.0 EPSILON = 30.0
ROOT_ID = 2
graph_id = 3
graph_loop = 0
##### #####
# Vehicule type: # Vehicule type:
@ -34,9 +37,7 @@ function init() {
# initGraph() # initGraph()
TARGET_ALTITUDE = 5 + (id-LOWEST_ROBOT_ID)*4.0 # m TARGET_ALTITUDE = 4 + (id-LOWEST_ROBOT_ID)*3.0 # m
loop = 1
# start the swarm command listener # start the swarm command listener
nei_cmd_listen() nei_cmd_listen()
@ -88,8 +89,6 @@ function step() {
statef=DoJoining statef=DoJoining
else if(BVMSTATE=="GRAPH_JOINED") else if(BVMSTATE=="GRAPH_JOINED")
statef=DoJoined statef=DoJoined
else if(BVMSTATE=="GRAPH_TRANSTOLOCK")
statef=TransitionToLock
else if(BVMSTATE=="GRAPH_LOCK") # move all together (TODO: not tested), but auto-loop in graph list else if(BVMSTATE=="GRAPH_LOCK") # move all together (TODO: not tested), but auto-loop in graph list
statef=DoLock statef=DoLock
else if(BVMSTATE=="PATHPLAN") # ends on navigate, defined in rrtstar else if(BVMSTATE=="PATHPLAN") # ends on navigate, defined in rrtstar