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

enhanced states switch and zooids rate

This commit is contained in:
dave 2018-03-03 22:43:12 -05:00
parent 8e6319e3a5
commit cc767a5ac7
3 changed files with 56 additions and 55 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -10,12 +10,12 @@ include "utils/conversions.bzz"
TARGET_ALTITUDE = 15.0 # m. TARGET_ALTITUDE = 15.0 # m.
BVMSTATE = "TURNEDOFF" BVMSTATE = "TURNEDOFF"
PICTURE_WAIT = 20 # steps PICTURE_WAIT = 20 # steps
GOTO_MAXVEL = 1.5 # m/steps GOTO_MAXVEL = 2.5 # m/steps
GOTO_MAXDIST = 150 # m. GOTO_MAXDIST = 150 # m.
GOTODIST_TOL = 0.5 # m. GOTODIST_TOL = 0.5 # m.
GOTOANG_TOL = 0.1 # rad. GOTOANG_TOL = 0.1 # rad.
path_it = 0 path_it = 0
rc_State = 0 graphid = 0
pic_time = 0 pic_time = 0
g_it = 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)) var accum = neighbors.map(lj_vector).reduce(lj_sum, math.vec2.new(0.0, 0.0))
if(neighbors.count() > 0) if(neighbors.count() > 0)
math.vec2.scale(accum, 1.0 / neighbors.count()) math.vec2.scale(accum, 1.0 / neighbors.count())
if(math.vec2.length(accum)>GOTO_MAXVEL*15) if(math.vec2.length(accum)>GOTO_MAXVEL*10)
accum = math.vec2.scale(accum, 15*GOTO_MAXVEL/math.vec2.length(accum)) accum = math.vec2.scale(accum, 10*GOTO_MAXVEL/math.vec2.length(accum))
goto_abs(accum.x, accum.y, 0.0, 0.0) goto_abs(accum.x, accum.y, 0.0, 0.0)
} }
@ -195,19 +195,22 @@ function rc_cmd_listen() {
stattab_send() stattab_send()
} else if (flight.rc_cmd==900){ } else if (flight.rc_cmd==900){
flight.rc_cmd=0 flight.rc_cmd=0
rc_State = 0 BVMSTATE = "TASK_ALLOCATE"
neighbors.broadcast("cmd", 900) neighbors.broadcast("cmd", 900)
} else if (flight.rc_cmd==901){ } else if (flight.rc_cmd==901){
flight.rc_cmd=0 flight.rc_cmd=0
rc_State = 1 destroyGraph()
BVMSTATE = "PURSUIT"
neighbors.broadcast("cmd", 901) neighbors.broadcast("cmd", 901)
} else if (flight.rc_cmd==902){ } else if (flight.rc_cmd==902){
flight.rc_cmd=0 flight.rc_cmd=0
rc_State = 2 destroyGraph()
BVMSTATE = "AGGREGATE"
neighbors.broadcast("cmd", 902) neighbors.broadcast("cmd", 902)
} else if (flight.rc_cmd==903){ } else if (flight.rc_cmd==903){
flight.rc_cmd=0 flight.rc_cmd=0
rc_State = 3 destroyGraph()
BVMSTATE = "FORMATION"
neighbors.broadcast("cmd", 903) neighbors.broadcast("cmd", 903)
} }
} }
@ -224,14 +227,17 @@ function nei_cmd_listen() {
uav_arm() uav_arm()
} else if(value==401 and BVMSTATE=="TURNEDOFF"){ } else if(value==401 and BVMSTATE=="TURNEDOFF"){
uav_disarm() uav_disarm()
} else if(value==900){ } else if(value==900){ # Shapes
rc_State = 0 BVMSTATE = "TASK_ALLOCATE"
} else if(value==901){ } else if(value==901){ # Pursuit
rc_State = 1 destroyGraph()
} else if(value==902){ BVMSTATE = "PURSUIT"
rc_State = 2 } else if(value==902){ # Agreggate
} else if(value==903){ destroyGraph()
rc_State = 3 BVMSTATE = "AGGREGATE"
} else if(value==903){ # Formation
destroyGraph()
BVMSTATE = "FORMATION"
} else if(value==16 and BVMSTATE=="IDLE"){ } else if(value==16 and BVMSTATE=="IDLE"){
# neighbors.listen("gt",function(vid, value, rid) { # neighbors.listen("gt",function(vid, value, rid) {
# print("Got (", vid, ",", value, ") from robot #", rid) # print("Got (", vid, ",", value, ") from robot #", rid)

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

@ -11,7 +11,6 @@ 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
old_state = -1
# #
# Global variables # Global variables
@ -401,7 +400,7 @@ function TransitionToJoined(){
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) goto_abs(m_navigation.x/100.0, m_navigation.y/100.0, 0.0, 0.0)
} }
# #
#Transistion to state Lock, lock the current formation #Transistion to state Lock, lock the current formation
@ -422,10 +421,8 @@ function TransitionToLock(){
} }
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, 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 #stop listening
neighbors.ignore("m") neighbors.ignore("m")
} }
@ -536,7 +533,7 @@ function DoAsking(){
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) goto_abs(m_navigation.x/100.0, m_navigation.y/100.0, 0.0, 0.0)
BroadcastGraph() 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") barrier_wait(ROBOTS, "GRAPH_TRANSTOLOCK", "GRAPH_JOINED")
BroadcastGraph() BroadcastGraph()
} }
# #
#Do Lock #Do Lock
# #
function DoLock(){ timeout_graph = 40
function DoLock() {
UpdateGraph() UpdateGraph()
m_selfMessage.State=s2i(BVMSTATE) m_selfMessage.State=s2i(BVMSTATE)
m_selfMessage.Label=m_nLabel m_selfMessage.Label=m_nLabel
@ -693,19 +684,23 @@ function DoLock(){
if(m_nLabel!=0){ if(m_nLabel!=0){
m_navigation=motion_vector() m_navigation=motion_vector()
} }
#move # #move
goto_abs(m_navigation.x, m_navigation.y, 0.0, 0.0) goto_abs(m_navigation.x, m_navigation.y, 0.0, 0.0)
BroadcastGraph() BroadcastGraph()
}
#
# Executed after takeoff
#
function startGraph(){
BVMSTATE="GRAPH_FREE"
# reset the graph if(loop) {
statef=resetGraph 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 # Executed at init
# #
@ -750,6 +745,7 @@ 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}
@ -766,16 +762,16 @@ function resetGraph(){
assign_id=-1 assign_id=-1
m_gotjoinedparent = 0 m_gotjoinedparent = 0
if(rc_State==0){ if(graphid==0){
log("Loading P graph") log("Loading P graph")
Read_GraphP() Read_GraphP()
} else if(rc_State==1) { } else if(graphid==1) {
log("Loading O graph") log("Loading O graph")
Read_GraphO() Read_GraphO()
} else if(rc_State==2) { } else if(graphid==2) {
log("Loading L graph") log("Loading L graph")
Read_GraphL() Read_GraphL()
} else if(rc_State==3) { } else if(graphid==3) {
log("Loading Y graph") log("Loading Y graph")
Read_GraphY() Read_GraphY()
} }
@ -802,7 +798,7 @@ function destroyGraph() {
#clear neighbour message #clear neighbour message
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) goto_abs(m_navigation.x/100.0, m_navigation.y/100.0, 0.0, 0.0)
m_vecNodes={} m_vecNodes={}
#stop listening #stop listening
neighbors.ignore("m") neighbors.ignore("m")

21
buzz_scripts/main.bzz
View File

@ -7,7 +7,7 @@ include "taskallocate/graphformGPS.bzz"
include "vstigenv.bzz" include "vstigenv.bzz"
#State launched after takeoff #State launched after takeoff
AUTO_LAUNCH_STATE = "PURSUIT" AUTO_LAUNCH_STATE = "TASK_ALLOCATE"
##### #####
# Vehicule type: # Vehicule type:
@ -27,6 +27,7 @@ function init() {
init_swarm() init_swarm()
TARGET_ALTITUDE = 10 + id*2.0 # m TARGET_ALTITUDE = 10 + id*2.0 # m
loop = 1
# start the swarm command listener # start the swarm command listener
nei_cmd_listen() nei_cmd_listen()
@ -60,21 +61,19 @@ function step() {
statef=formation statef=formation
else if(BVMSTATE=="PURSUIT") else if(BVMSTATE=="PURSUIT")
statef=pursuit statef=pursuit
else if(BVMSTATE=="TASK_ALLOCATE") { # or bidding ? else if(BVMSTATE=="TASK_ALLOCATE") # or bidding ?
startGraph() statef=resetGraph
} else if(BVMSTATE=="GRAPH_FREE") { else if(BVMSTATE=="GRAPH_FREE")
statef=DoFree statef=DoFree
} else if(BVMSTATE=="GRAPH_ASKING") { else if(BVMSTATE=="GRAPH_ASKING")
statef=DoAsking statef=DoAsking
} else if(BVMSTATE=="GRAPH_JOINING") { else if(BVMSTATE=="GRAPH_JOINING")
statef=DoJoining statef=DoJoining
} else if(BVMSTATE=="GRAPH_JOINED") { else if(BVMSTATE=="GRAPH_JOINED")
statef=DoJoined statef=DoJoined
} else if(BVMSTATE=="GRAPH_TRANSTOLOCK") else if(BVMSTATE=="GRAPH_TRANSTOLOCK")
statef=TransitionToLock statef=TransitionToLock
else if(BVMSTATE=="GRAPH_LOCK" and old_state!=rc_State) #switch to a new graph else if(BVMSTATE=="GRAPH_LOCK") # move all together (TODO: not tested), but auto-loop in graph list
startGraph()
else if(BVMSTATE=="GRAPH_LOCK" and old_state==rc_State) # move all together (TODO: not tested)
statef=DoLock statef=DoLock
else if(BVMSTATE=="PATHPLAN") # ends on navigate, defined in rrtstar else if(BVMSTATE=="PATHPLAN") # ends on navigate, defined in rrtstar
statef=rrtstar statef=rrtstar