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

Hub: made changes to allow arbitrary robot id's and Rosbuzz: changed bzzfiles barrier, states and main to fit arbitrary id's.

This commit is contained in:
vivek-shankar 2018-07-04 21:10:57 -04:00
parent 66cb2df693
commit 1cb390f595
3 changed files with 22 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -53,12 +53,11 @@ function barrier_wait(threshold, transf, resumef, bc) {
barrier.put(id, bc) barrier.put(id, bc)
barrier.get(id) barrier.get(id)
var allgood = 0 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) {
var bi = 0 var bi = LOWEST_ROBOT_ID
allgood = 1 allgood = 1
while (bi<threshold) { while (bi<LOWEST_ROBOT_ID+threshold) {
if(barrier.get(bi) != bc) if(barrier.get(bi) != bc)
allgood = 0 allgood = 0
bi = bi + 1 bi = bi + 1

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

@ -27,12 +27,18 @@ function idle() {
BVMSTATE = "IDLE" BVMSTATE = "IDLE"
} }
# Custom function for the user.
function cusfun(){
BVMSTATE="CUSFUN"
log("cusfun: yay!!!")
}
function launch() { function launch() {
BVMSTATE = "LAUNCH" BVMSTATE = "LAUNCH"
if(V_TYPE == 0 or V_TYPE == 1) { # flying vehicle so LAND if(V_TYPE == 0 or V_TYPE == 1) { # flying vehicle so TAKE_OFF
homegps = {.lat=pose.position.latitude, .long=pose.position.longitude} homegps = {.lat=pose.position.latitude, .long=pose.position.longitude}
if( flight.status == 2 and pose.position.altitude >= TARGET_ALTITUDE-TARGET_ALTITUDE/20.0) { if( flight.status == 2 and pose.position.altitude >= TARGET_ALTITUDE-TARGET_ALTITUDE/20.0) {
barrier_set(ROBOTS, AUTO_LAUNCH_STATE, "LAUNCH", 22) barrier_set(ROBOTS, AUTO_LAUNCH_STATE, "STOP", 22)
barrier_ready(22) barrier_ready(22)
} else { } else {
log("Altitude: ", pose.position.altitude) log("Altitude: ", pose.position.altitude)
@ -40,7 +46,7 @@ function launch() {
uav_takeoff(TARGET_ALTITUDE) uav_takeoff(TARGET_ALTITUDE)
} }
} else { } else {
barrier_set(ROBOTS, AUTO_LAUNCH_STATE, "LAUNCH", 22) barrier_set(ROBOTS, AUTO_LAUNCH_STATE, "STOP", 22)
barrier_ready(22) barrier_ready(22)
} }
} }
@ -183,10 +189,10 @@ function rc_cmd_listen() {
neighbors.broadcast("cmd", 22) neighbors.broadcast("cmd", 22)
} else if(flight.rc_cmd==21) { } else if(flight.rc_cmd==21) {
flight.rc_cmd=0 flight.rc_cmd=0
AUTO_LAUNCH_STATE = "STOP" AUTO_LAUNCH_STATE = "TURNEDOFF"
barrier_set(ROBOTS, "GOHOME", BVMSTATE, 21) barrier_set(ROBOTS, "GOHOME", BVMSTATE, 21)
barrier_ready(21) barrier_ready(21)
BVMSTATE = "GOHOME" BVMSTATE = "STOP"
neighbors.broadcast("cmd", 21) neighbors.broadcast("cmd", 21)
} else if(flight.rc_cmd==20) { } else if(flight.rc_cmd==20) {
flight.rc_cmd=0 flight.rc_cmd=0
@ -246,8 +252,8 @@ function nei_cmd_listen() {
AUTO_LAUNCH_STATE = "IDLE" AUTO_LAUNCH_STATE = "IDLE"
BVMSTATE = "GOHOME" BVMSTATE = "GOHOME"
} else if(value==21) { } else if(value==21) {
AUTO_LAUNCH_STATE = "STOP" AUTO_LAUNCH_STATE = "TURNEDOFF"
BVMSTATE = "GOHOME" BVMSTATE = "STOP"
} else if(value==400 and BVMSTATE=="TURNEDOFF") { } else if(value==400 and BVMSTATE=="TURNEDOFF") {
arm() arm()
} else if(value==401 and BVMSTATE=="TURNEDOFF"){ } else if(value==401 and BVMSTATE=="TURNEDOFF"){

9
buzz_scripts/main.bzz
View File

@ -7,7 +7,9 @@ include "taskallocate/graphformGPS.bzz"
include "vstigenv.bzz" include "vstigenv.bzz"
#State launched after takeoff #State launched after takeoff
AUTO_LAUNCH_STATE = "FORMATION" AUTO_LAUNCH_STATE = "CUSFUN"
#Lowest robot id in the network
LOWEST_ROBOT_ID = 97
TARGET = 9.0 TARGET = 9.0
EPSILON = 30.0 EPSILON = 30.0
@ -23,12 +25,13 @@ goal_list = {
.0={.x = 45.5088103899, .y = -73.1540826153, .z = 2.5} .0={.x = 45.5088103899, .y = -73.1540826153, .z = 2.5}
} }
# Executed once at init time. # Executed once at init time.
function init() { function init() {
init_stig() init_stig()
init_swarm() init_swarm()
TARGET_ALTITUDE = 10 + id*2.0 # m TARGET_ALTITUDE = 5 + (id-LOWEST_ROBOT_ID)*3.0 # m
loop = 1 loop = 1
# start the swarm command listener # start the swarm command listener
@ -51,6 +54,8 @@ function step() {
# #
if(BVMSTATE=="TURNEDOFF") if(BVMSTATE=="TURNEDOFF")
statef=turnedoff statef=turnedoff
else if(BVMSTATE=="CUSFUN")
statef=cusfun
else if(BVMSTATE=="STOP") # ends on turnedoff else if(BVMSTATE=="STOP") # ends on turnedoff
statef=stop statef=stop
else if(BVMSTATE=="LAUNCH") # ends on AUTO_LAUNCH_STATE else if(BVMSTATE=="LAUNCH") # ends on AUTO_LAUNCH_STATE