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

test

Browse Source
This commit is contained in:
David St-Onge 2017-02-22 17:14:27 -05:00
parent 9aee21a4ef
commit 271e4e5004
1 changed files with 8 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
src/testflockfev.bzz
View File

@ -13,6 +13,7 @@ neighbors.broadcast(updated, update_no)
} }
TARGET_ALTITUDE = 5.0 TARGET_ALTITUDE = 5.0
CURSTATE = TURNEDOFF
# Lennard-Jones parameters # Lennard-Jones parameters
TARGET = 10.0 #0.000001001 TARGET = 10.0 #0.000001001
@ -36,6 +37,7 @@ function lj_sum(rid, data, accum) {
# Calculates and actuates the flocking interaction # Calculates and actuates the flocking interaction
function hexagon() { function hexagon() {
statef=hexagon statef=hexagon
CURSTATE=HEXAGON
# Calculate accumulator # Calculate accumulator
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)
@ -89,6 +91,7 @@ function barrier_wait(threshold, transf) {
function idle() { function idle() {
statef=idle statef=idle
CURSTATE=IDLE
neighbors.listen("cmd", neighbors.listen("cmd",
function(vid, value, rid) { function(vid, value, rid) {
print("Got (", vid, ",", value, ") from robot #", rid) print("Got (", vid, ",", value, ") from robot #", rid)
@ -129,6 +132,7 @@ function land() {
# Executed once at init time. # Executed once at init time.
function init() { function init() {
statef=idle statef=idle
CURSTATE=IDLE
} }
# Executed at each time step. # Executed at each time step.
@ -137,19 +141,22 @@ function step() {
if(flight.rc_cmd==22) { if(flight.rc_cmd==22) {
log("cmd 22") log("cmd 22")
flight.rc_cmd=0 flight.rc_cmd=0
statef = takeoff statef = takeoff
CURSTATE = TAKEOFF
neighbors.broadcast("cmd", 22) neighbors.broadcast("cmd", 22)
} else if(flight.rc_cmd==21) { } else if(flight.rc_cmd==21) {
log("cmd 21") log("cmd 21")
log("To land") log("To land")
flight.rc_cmd=0 flight.rc_cmd=0
statef = land statef = land
CURSTATE = LAND
neighbors.broadcast("cmd", 21) neighbors.broadcast("cmd", 21)
} else if(flight.rc_cmd==16) { } else if(flight.rc_cmd==16) {
flight.rc_cmd=0 flight.rc_cmd=0
uav_goto() uav_goto()
} }
statef() statef()
log("Current state: ", CURSTATE)
} }
# Executed once when the robot (or the simulator) is reset. # Executed once when the robot (or the simulator) is reset.