270 lines
7.8 KiB
Plaintext
270 lines
7.8 KiB
Plaintext
include "vec2.bzz"
|
|
include "update.bzz"
|
|
include "barrier.bzz" # don't use a stigmergy id=11 with this header.
|
|
include "uavstates.bzz" # require an 'action' function to be defined here.
|
|
|
|
# Lennard-Jones parameters
|
|
TARGET = 12.0
|
|
EPSILON = 10.0
|
|
|
|
#################################################
|
|
### UTILITY FUNCTIONS ###########################
|
|
#################################################
|
|
|
|
# Write a table as if it was a matrix
|
|
function write_knowledge(k, row, col, val) {
|
|
var key = string.concat(string.tostring(row),"-",string.tostring(col))
|
|
k[key] = val
|
|
log("Writing knowledge:", val, " to ", row, " ", col)
|
|
}
|
|
|
|
# Read a table as if it was a matrix
|
|
function read_knowledge(k, row, col) {
|
|
var key = string.concat(string.tostring(row),"-",string.tostring(col))
|
|
if (k[key] == nil) {
|
|
log("Warning: reading 'nil' value from the knowledge table at", row, " ", col, ", returning -1")
|
|
return -1
|
|
} else {
|
|
return k[key]
|
|
}
|
|
}
|
|
|
|
# Int to String
|
|
function itos(i) {
|
|
|
|
log("Use 'string.tostring(OJB)' instead")
|
|
|
|
if (i==0) { return "0" }
|
|
if (i==1) { return "1" }
|
|
if (i==2) { return "2" }
|
|
if (i==3) { return "3" }
|
|
if (i==4) { return "4" }
|
|
if (i==5) { return "5" }
|
|
if (i==6) { return "6" }
|
|
if (i==7) { return "7" }
|
|
if (i==8) { return "8" }
|
|
if (i==9) { return "9" }
|
|
|
|
log("Function 'itos' out of bounds, returning the answer (42)")
|
|
return "42"
|
|
}
|
|
|
|
# String to Int
|
|
function stoi(s) {
|
|
if (s=='0') { return 0 }
|
|
if (s=='1') { return 1 }
|
|
if (s=='2') { return 2 }
|
|
if (s=='3') { return 3 }
|
|
if (s=='4') { return 4 }
|
|
if (s=='5') { return 5 }
|
|
if (s=='6') { return 6 }
|
|
if (s=='7') { return 7 }
|
|
if (s=='8') { return 8 }
|
|
if (s=='9') { return 9 }
|
|
|
|
log("Function 'stoi' out of bounds, returning the answer (42)")
|
|
return 42
|
|
|
|
}
|
|
|
|
# Rads to degrees
|
|
function rtod(r) {
|
|
return (r*(180.0/math.pi))
|
|
}
|
|
|
|
# Degrees to rads
|
|
function dtor(d) {
|
|
return (math.pi*(d/180.0))
|
|
}
|
|
|
|
# Force angles in the (-180,180) interval
|
|
function degrees_interval(a) {
|
|
var temp = a
|
|
while ((temp>360.0) or (temp<0.0)) {
|
|
if (temp > 360.0) {
|
|
temp = temp - 360.0
|
|
} else if (temp < 0.0){
|
|
temp = temp + 360.0
|
|
}
|
|
}
|
|
if (temp > 180.0) {
|
|
temp = temp - 360.0
|
|
}
|
|
return temp
|
|
}
|
|
|
|
# Force angles in the (-pi,pi) interval
|
|
function radians_interval(a) {
|
|
var temp = a
|
|
while ((temp>2.0*math.pi) or (temp<0.0)) {
|
|
if (temp > 2.0*math.pi) {
|
|
temp = temp - 2.0*math.pi
|
|
} else if (temp < 0.0){
|
|
temp = temp + 2.0*math.pi
|
|
}
|
|
}
|
|
if (temp > math.pi) {
|
|
temp = temp - 2.0*math.pi
|
|
}
|
|
return temp
|
|
}
|
|
|
|
#################################################
|
|
### MOVEMENT/COMMUNICATION PRIMITIVES ###########
|
|
#################################################
|
|
|
|
# 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() {
|
|
# 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
|
|
moveto(accum.x, accum.y)
|
|
}
|
|
|
|
function inform_your_neighborhood() {
|
|
# Reset to 0 the visibility of all neighbors
|
|
foreach(knowledge, function(key, value) {
|
|
column = string.sub(key, string.length(key)-1,string.length(key))
|
|
if (column=='3') {
|
|
knowledge[key] = 0
|
|
}
|
|
})
|
|
neighbors.foreach( function(rid, data) {
|
|
# For each neighbor, send a message with its azimuth, as seen by the broadcasting robot
|
|
message_id = string.tostring(rid)
|
|
neighbors.broadcast(message_id, rtod(data.azimuth))
|
|
# Record the neighbor azimuth in my own knowledge table
|
|
write_knowledge(knowledge, rid, 0, rtod(data.azimuth))
|
|
# Record the neighbor distance in my own knowledge table
|
|
write_knowledge(knowledge, rid, 2, data.distance)
|
|
# Set neighbor as visible
|
|
write_knowledge(knowledge, rid, 3, 1)
|
|
})
|
|
# Send a message with the desired direction, as seen by the broadcasting robot
|
|
neighbors.broadcast("direction", local_dir)
|
|
|
|
}
|
|
|
|
function listen_to_your_neighborhood() {
|
|
# For all "senders" in my neighborhood, record my azimuth, as seen by them
|
|
message_id = string.tostring(id)
|
|
neighbors.listen(message_id, function(vid, value, rid) {
|
|
write_knowledge(knowledge, rid, 1, value)
|
|
})
|
|
}
|
|
|
|
#################################################
|
|
### ACTUAL CONTROLLERS ##########################
|
|
#################################################
|
|
|
|
function zero() {
|
|
# Do not move
|
|
moveto(0.0,0.0)
|
|
# Tell the neighbors of the center where to go
|
|
inform_your_neighborhood()
|
|
}
|
|
|
|
function onetwo() {
|
|
if (id == 1) {
|
|
angle = 45
|
|
} else {
|
|
angle = -135
|
|
}
|
|
# Broadcast information
|
|
inform_your_neighborhood()
|
|
# If the center 0 is in sight
|
|
if (read_knowledge(knowledge, 0, 3) == 1) {
|
|
arm_offset = degrees_interval(read_knowledge(knowledge, 0, 1) - angle)
|
|
if (arm_offset<3 and arm_offset>(-3)) {
|
|
hexagon() # Underlying Lennard-Jones potential behavior
|
|
} else {
|
|
|
|
local_rotation = degrees_interval( read_knowledge(knowledge, 0, 1) + (180.0 - read_knowledge(knowledge, 0, 0)) )
|
|
local_arm = degrees_interval(angle - local_rotation)
|
|
|
|
if (read_knowledge(knowledge, 0, 2) > 250.0) {
|
|
x_mov = math.cos(dtor(read_knowledge(knowledge, 0, 0)))
|
|
y_mov = math.sin(dtor(read_knowledge(knowledge, 0, 0)))
|
|
} else if (read_knowledge(knowledge, 0, 2) < 30.0) {
|
|
x_mov = -math.cos(dtor(read_knowledge(knowledge, 0, 0)))
|
|
y_mov = -math.sin(dtor(read_knowledge(knowledge, 0, 0)))
|
|
} else {
|
|
spiraling = 2.0+(id/10.0) # Fun stuff but be careful with this, it affects how a robots turns around a central node, use random number generation, eventually
|
|
if (arm_offset > 0) { # Clockwise
|
|
|
|
x_mov = -math.sin(dtor(read_knowledge(knowledge, 0, 0)))
|
|
y_mov = math.cos(dtor(read_knowledge(knowledge, 0, 0))) * spiraling
|
|
} else { # Counterclockwise
|
|
x_mov = math.sin(dtor(read_knowledge(knowledge, 0, 0)))
|
|
y_mov = -math.cos(dtor(read_knowledge(knowledge, 0, 0))) * spiraling
|
|
}
|
|
}
|
|
speed = 100
|
|
moveto(speed * x_mov,speed * y_mov)
|
|
}
|
|
} else {
|
|
hexagon()
|
|
}
|
|
}
|
|
|
|
#################################################
|
|
### BUZZ FUNCTIONS ##############################
|
|
#################################################
|
|
|
|
function action(){
|
|
if (id == 0)
|
|
statef=zero
|
|
else
|
|
statef=onetwo
|
|
|
|
UAVSTATE="TENTACLES"
|
|
}
|
|
|
|
# Executed at init time
|
|
function init() {
|
|
uav_initswarm()
|
|
|
|
# Local knowledge table
|
|
knowledge = {}
|
|
# Update local knowledge with information from the neighbors
|
|
listen_to_your_neighborhood()
|
|
# Variables initialization
|
|
iteration = 0
|
|
|
|
}
|
|
|
|
# Executed every time step
|
|
function step() {
|
|
uav_rccmd()
|
|
uav_neicmd()
|
|
|
|
statef()
|
|
log("Current state: ", CURSTATE)
|
|
log("Swarm size: ",ROBOTS)
|
|
|
|
# Count the number of steps
|
|
iteration = iteration + 1
|
|
}
|
|
|
|
# Executed once when the robot (or the simulator) is reset.
|
|
function reset() {
|
|
}
|
|
# Execute at exit
|
|
function destroy() {
|
|
}
|