ROSBuzz_MISTLab/src/test1.bzz

139 lines
2.8 KiB
Plaintext
Raw Normal View History

2017-01-25 15:08:15 -04:00
# We need this for 2D vectors
# Make sure you pass the correct include path to "bzzc -I <path1:path2> ..."
include "/home/ubuntu/buzz/src/include/vec2.bzz"
2017-01-25 22:10:07 -04:00
TARGET_ALTITUDE = 10.0
2017-01-25 15:08:15 -04:00
# Lennard-Jones parameters
2017-01-25 22:10:07 -04:00
TARGET = 0.000000100005
EPSILON = 0.1
2017-01-25 15:08:15 -04:00
# Lennard-Jones interaction magnitude
function lj_magnitude(dist, target, epsilon) {
return -(epsilon / dist) * ((target / dist)^4 - (target / dist)^2)
}
# Neighbor data to LJ interaction vector
function lj_vector(rid, data) {
return math.vec2.newp(lj_magnitude(data.distance, TARGET, EPSILON), data.azimuth)
}
# Accumulator of neighbor LJ interactions
function lj_sum(rid, data, accum) {
return math.vec2.add(data, accum)
}
# Calculates and actuates the flocking interaction
function hexagon() {
statef=hexagon
# Calculate accumulator
var accum = neighbors.map(lj_vector).reduce(lj_sum, math.vec2.new(0.0, 0.0))
if(neighbors.count() > 0)
math.vec2.scale(accum, 1.0 / neighbors.count())
# Move according to vector
2017-01-25 22:54:35 -04:00
print("Robot ", id, "must push ",accum.x, "; ", accum.y)
2017-01-25 22:10:07 -04:00
uav_goto(accum.x+position.latitude, accum.y+position.longitude, TARGET_ALTITUDE)
2017-01-25 15:08:15 -04:00
}
2016-12-23 15:21:52 -04:00
########################################
#
# BARRIER-RELATED FUNCTIONS
#
########################################
#
# Constants
#
BARRIER_VSTIG = 1
2017-01-25 15:08:15 -04:00
ROBOTS = 3 # number of robots in the swarm
2016-12-23 15:21:52 -04:00
#
# Sets a barrier
#
function barrier_set(threshold, transf) {
statef = function() {
barrier_wait(threshold, transf);
}
barrier = stigmergy.create(BARRIER_VSTIG)
}
#
# Make yourself ready
#
function barrier_ready() {
barrier.put(id, 1)
}
#
# Executes the barrier
#
function barrier_wait(threshold, transf) {
barrier.get(id)
if(barrier.size() >= threshold) {
barrier = nil
transf()
}
}
# flight status
function idle() {
2016-12-23 18:48:01 -04:00
statef=idle
2016-12-23 17:36:31 -04:00
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
}
}
)
2016-12-23 15:21:52 -04:00
}
function takeoff() {
2017-01-25 22:10:07 -04:00
if( flight.status == 2 and position.altitude >= TARGET_ALTITUDE-TARGET_ALTITUDE/50.0) {
2017-01-25 15:08:15 -04:00
barrier_set(ROBOTS,hexagon)
2016-12-23 15:21:52 -04:00
barrier_ready()
}
2017-01-25 18:30:11 -04:00
else if( flight.status !=3)
2017-01-25 22:10:07 -04:00
uav_takeoff(TARGET_ALTITUDE)
2016-12-23 15:21:52 -04:00
}
function land() {
if( flight.status == 1) {
barrier_set(ROBOTS,idle)
barrier_ready()
}
else if(flight.status!=0 and flight.status!=4)
2016-12-23 15:21:52 -04:00
uav_land()
}
# Executed once at init time.
function init() {
2017-01-09 18:15:33 -04:00
statef=idle
}
2017-01-09 18:15:33 -04:00
# Executed at each time step.
function step() {
2016-12-23 18:48:01 -04:00
if(flight.rc_cmd==22 and flight.status==1) {
flight.rc_cmd=0
2016-12-23 15:21:52 -04:00
statef=takeoff
2016-12-06 13:02:39 -04:00
neighbors.broadcast("cmd", 22)
2016-12-23 18:48:01 -04:00
} else if(flight.rc_cmd==21 and flight.status==2) {
flight.rc_cmd=0
2016-12-23 15:21:52 -04:00
statef=land
2016-12-06 13:02:39 -04:00
neighbors.broadcast("cmd", 21)
2016-12-05 18:57:20 -04:00
}
2016-12-23 15:21:52 -04:00
statef()
}
# Executed once when the robot (or the simulator) is reset.
function reset() {
}
# Executed once at the end of experiment.
function destroy() {
}