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

Merge branch 'dev' of https://github.com/MISTLab/ROSBuzz into dev

This commit is contained in:
vivek-shankar 2017-04-02 22:48:09 -04:00
parent 2647f9dc9a 71f72e5e59
commit df6557ff9c
1 changed files with 21 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

38
src/testflockfev.bzz
View File

@ -16,7 +16,7 @@ CURSTATE = "TURNEDOFF"
# Lennard-Jones parameters
TARGET = 10.0 #0.000001001
EPSILON = 22.0 #0.001
EPSILON = 18.0 #0.001
# Lennard-Jones interaction magnitude
function lj_magnitude(dist, target, epsilon) {
@ -78,7 +78,7 @@ function barrier_ready() {
#
# Executes the barrier
#
WAIT_TIMEOUT = 500
WAIT_TIMEOUT = 100
timeW=0
function barrier_wait(threshold, transf) {
barrier.get(id)
@ -99,25 +99,12 @@ function barrier_wait(threshold, transf) {
function idle() {
statef=idle
CURSTATE = "IDLE"
neighbors.listen("cmd",
function(vid, value, rid) {
print("Got (", vid, ",", value, ") from robot #", rid)
if(value==22) {
statef=takeoff
} else if(value==21) {
statef=land
} else if(value==400) {
uav_arm()
} else if(value==401){
uav_disarm()
}
}
)
}
function takeoff() {
CURSTATE = "TAKEOFF"
statef=takeoff
log("TakeOff: ", flight.status)
if( flight.status == 2 and position.altitude >= TARGET_ALTITUDE-TARGET_ALTITUDE/20.0) {
barrier_set(ROBOTS,hexagon)
@ -131,6 +118,8 @@ function takeoff() {
}
}
function land() {
CURSTATE = "LAND"
statef=land
log("Land: ", flight.status)
if(flight.status == 2 or flight.status == 3){
neighbors.broadcast("cmd", 21)
@ -176,6 +165,21 @@ function step() {
uav_disarm()
neighbors.broadcast("cmd", 401)
}
neighbors.listen("cmd",
function(vid, value, rid) {
print("Got (", vid, ",", value, ") from robot #", rid)
if(value==22 and statef==idle) {
statef=takeoff
} else if(value==21) {
statef=land
} else if(value==400 and statef==idle) {
uav_arm()
} else if(value==401 and statef==idle){
uav_disarm()
}
}
)
statef()
log("Current state: ", CURSTATE)
log("Swarm size: ",ROBOTS)