fix MP gps and add status
This commit is contained in:
dave
parent
9a0332aed9
commit
94ead5f496
|
|
@ -65,6 +65,7 @@ function action() {
|
|||
function init() {
|
||||
uav_initswarm()
|
||||
uav_initstig()
|
||||
TARGET_ALTITUDE = 2.5 + id * 5
|
||||
}
|
||||
|
||||
# Executed at each time step.
|
||||
|
|
|
|||
|
|
@ -0,0 +1,152 @@
|
|||
# Utilities
|
||||
|
||||
# Rads to degrees
|
||||
function rtod(r) {
|
||||
return (r*(180.0/math.pi))
|
||||
}
|
||||
|
||||
# Copy a table
|
||||
function table_deep_copy(new_t, old_t, depth) {
|
||||
depth = depth - 1
|
||||
if (old_t != nil) {
|
||||
foreach(old_t, function(key, value) {
|
||||
new_t[key] = value
|
||||
if(depth != 0) {
|
||||
new_t[key] = {}
|
||||
table_deep_copy(new_t[key], value, depth)
|
||||
}
|
||||
})
|
||||
}
|
||||
}
|
||||
|
||||
function table_copy(old_t, depth) {
|
||||
var t = {};
|
||||
table_deep_copy(t, old_t, depth);
|
||||
return t;
|
||||
}
|
||||
|
||||
# Print the contents of a table
|
||||
function table_print(t, depth) {
|
||||
depth = depth - 1
|
||||
if (t != nil) {
|
||||
foreach(t, function(key, value) {
|
||||
log(key, " -> ", value)
|
||||
if(depth != 0) {
|
||||
table_print(t[key], depth)
|
||||
}
|
||||
})
|
||||
}
|
||||
}
|
||||
|
||||
# Write a table as if it was a vector
|
||||
function write_vector(k, index, val) {
|
||||
var key = string.tostring(index)
|
||||
k[key] = val
|
||||
}
|
||||
|
||||
# Read a table as if it was a vector
|
||||
function read_vector(k, index) {
|
||||
var key = string.tostring(index)
|
||||
if (k[key] == nil) {
|
||||
return -1
|
||||
} else {
|
||||
return k[key]
|
||||
}
|
||||
}
|
||||
|
||||
# Write a table as if it was a matrix
|
||||
function write_matrix(k, row, col, val) {
|
||||
var key = string.concat(string.tostring(row),"-",string.tostring(col))
|
||||
k[key] = val
|
||||
}
|
||||
|
||||
# Read a table as if it was a matrix
|
||||
function read_matrix(k, row, col) {
|
||||
var key = string.concat(string.tostring(row),"-",string.tostring(col))
|
||||
if (k[key] == nil) {
|
||||
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
|
||||
|
||||
}
|
||||
|
||||
# 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
|
||||
}
|
||||
|
||||
############################################
|
||||
|
||||
#base = {}
|
||||
|
||||
#base.create = function() {
|
||||
# return {
|
||||
# .method = function(a,b) {
|
||||
# return a + b
|
||||
# }
|
||||
# }
|
||||
# }
|
||||
|
||||
#x = base.create()
|
||||
#x.method(3,4) # returns 7
|
||||
|
||||
#derived = {}
|
||||
|
||||
#derived.create = function() {
|
||||
# b = base.create()
|
||||
# b.method = function(a,b) {
|
||||
# return a * b
|
||||
# }
|
||||
#}
|
||||
|
||||
#x = derived.create()
|
||||
#x.method(3,4) # returns 12
|
||||
|
|
@ -0,0 +1,537 @@
|
|||
# Include files
|
||||
include "string.bzz"
|
||||
include "vec2.bzz"
|
||||
include "utilities.bzz"
|
||||
include "barrier.bzz"
|
||||
|
||||
############################################
|
||||
|
||||
# Global variables
|
||||
|
||||
RANGE = 200 # rab range in cm, get from argos file
|
||||
|
||||
N_SONS = 10 # Maximum number of sons
|
||||
|
||||
TRIGGER_VSTIG = 101 # ID of trigger virtual stigmergy
|
||||
BARRIER_VSTIG = 102 # ID of barrier virtual stigmergy
|
||||
|
||||
################################################################
|
||||
################################################################
|
||||
|
||||
# Tree utility functions
|
||||
|
||||
function parent_select() {
|
||||
|
||||
|
||||
# Selects potential parents
|
||||
var candidates = {}
|
||||
candidates = neighbors.filter(function(rid, data) {
|
||||
return ((knowledge.level[rid] > 0) and (data.distance < (RANGE - 10.0)) and (knowledge.free[rid] == 1))})
|
||||
# and (data.distance > 10.0)
|
||||
|
||||
# Selects closest parent candidate as parent
|
||||
var temp = {}
|
||||
temp = candidates.reduce(function(rid, data, accum) {
|
||||
accum.min = math.min(accum.min, data.distance)
|
||||
return accum
|
||||
}, {.min = RANGE})
|
||||
var min = temp.min
|
||||
|
||||
|
||||
var flag = 0
|
||||
foreach(knowledge.distance, function(key, value) {
|
||||
if ((flag == 0) and (candidates.data[key] != nil)) {
|
||||
if (value == min) {
|
||||
tree.parent.id = key
|
||||
tree.parent.distance = value
|
||||
tree.parent.azimuth = knowledge.azimuth[key]
|
||||
flag = 1
|
||||
}
|
||||
}
|
||||
#break (when implemented)
|
||||
})
|
||||
|
||||
}
|
||||
|
||||
####################################
|
||||
|
||||
function count() {
|
||||
|
||||
if (nb_sons == 0) {
|
||||
eq_level = level
|
||||
}
|
||||
|
||||
else if (nb_sons >= 1) {
|
||||
var temp = {}
|
||||
temp = sons.reduce(function(rid, data, accum) {
|
||||
accum.sum = accum.sum + tree.sons[rid].eq_level
|
||||
return accum
|
||||
}, {.sum = 0})
|
||||
eq_level = temp.sum - (nb_sons - 1) * level
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
####################################
|
||||
|
||||
function change_frame(p01, p1, theta) {
|
||||
|
||||
var p0 = {
|
||||
.x = math.cos(theta) * p1.x + math.sin(theta) * p1.y + p01.x,
|
||||
.y = -math.sin(theta) * p1.x + math.cos(theta) * p1.y + p01.y
|
||||
}
|
||||
return p0
|
||||
|
||||
}
|
||||
|
||||
####################################
|
||||
|
||||
transform_accum = function(rid, data) {
|
||||
|
||||
# Son contribution in frame F1
|
||||
var p1 = {
|
||||
.x = tree.sons[rid].accum_x,
|
||||
.y = tree.sons[rid].accum_y
|
||||
}
|
||||
|
||||
# Rotation angle between F0 and F1
|
||||
var theta = tree.sons[rid].angle_parent - data.azimuth - math.pi
|
||||
|
||||
# Translation vector from F0 to F1
|
||||
var p01 = {
|
||||
.x = 0,
|
||||
.y = 0
|
||||
}
|
||||
|
||||
var p0 = {}
|
||||
|
||||
if (tree.sons[rid].angle_parent != nil) {
|
||||
var rot_angle = radians_interval(theta)
|
||||
p0 = change_frame(p01, p1, rot_angle)
|
||||
}
|
||||
else {
|
||||
p0.x = p1.x
|
||||
p0.y = p1.y
|
||||
}
|
||||
|
||||
return p0
|
||||
}
|
||||
|
||||
####################################
|
||||
|
||||
function centroid() {
|
||||
|
||||
# If the robot has a parent
|
||||
if ((tree.parent != nil) or (root == 1)) {
|
||||
var sum_F1 = { .x = 0, .y = 0}
|
||||
|
||||
# If the robot has at least one son
|
||||
if (nb_sons > 0) {
|
||||
var temp = {}
|
||||
# Expresses son contrib (in F1) in its own reference frame (F0)
|
||||
temp = sons.map(transform_accum)
|
||||
# Sums son contributions expressed in F0 frame
|
||||
sum_F1 = temp.reduce(function(rid, data, accum) {
|
||||
accum.x = accum.x + data.x
|
||||
accum.y = accum.y + data.y
|
||||
#log("id ", rid, " sum_x ", accum.x, " sum_y ", accum.y)
|
||||
return accum
|
||||
}, {.x = 0, .y = 0})
|
||||
}
|
||||
|
||||
# If the robot is not the root
|
||||
if ((root == 0) and (tree.parent.id != nil)) {
|
||||
#var nb_descendants = eq_level - level
|
||||
var p0 = knowledge.distance[tree.parent.id]#tree.parent.distance
|
||||
var theta = knowledge.azimuth[tree.parent.id]#tree.parent.azimuth
|
||||
# Adds current robot contribution to centroid sum
|
||||
accum_x = sum_F1.x - (nb_descendants + 1) * p0 * math.cos(theta)
|
||||
accum_y = sum_F1.y - (nb_descendants + 1) * p0 * math.sin(theta)
|
||||
}
|
||||
# If the robot is the root
|
||||
else if ((root == 1) and (robot_count != 0)) {
|
||||
# Centroid coordinates in root ref frame
|
||||
accum_x = sum_F1.x / robot_count
|
||||
accum_y = sum_F1.y / robot_count
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
################################################################
|
||||
################################################################
|
||||
|
||||
# Tree reconfiguration functions
|
||||
|
||||
function tree_config() {
|
||||
statef()
|
||||
}
|
||||
|
||||
function end_fun() {
|
||||
if (root == 1) {
|
||||
log("centroid X: ", accum_x, " Y ", accum_y)
|
||||
}
|
||||
}
|
||||
|
||||
####################################
|
||||
|
||||
function root_select() {
|
||||
|
||||
log(id," root_select")
|
||||
if (tree.parent.id != nil) {
|
||||
if(neighbors.data[tree.parent.id] != nil) {
|
||||
angle_parent = neighbors.data[tree.parent.id].azimuth
|
||||
}
|
||||
}
|
||||
|
||||
if (root == 1) {
|
||||
|
||||
# Listens for new root acknowledgment
|
||||
|
||||
foreach(knowledge.ackn, function(key, value) {
|
||||
if (value == better_root) {
|
||||
#log(id, " got it")
|
||||
root = 0
|
||||
level = 0
|
||||
setleds(0,0,0)
|
||||
}
|
||||
})
|
||||
|
||||
if (ack == 1) {
|
||||
# Triggers transition to new state
|
||||
trigger.put("a", 1)
|
||||
}
|
||||
else if ((root == 1) and (better_root != id) and (trigger.get("a") != 1)) {
|
||||
setleds(255,0,0)
|
||||
better_root = id
|
||||
|
||||
# Centroid position in root reference frame (F0)
|
||||
var c0 = math.vec2.new(accum_x, accum_y)
|
||||
|
||||
# Distance from current root to centroid
|
||||
var dist_rc = math.vec2.length(c0)
|
||||
#log("root ", id, " dist_centr ", dist_rc)
|
||||
|
||||
# Distances from neighbors to centroid
|
||||
var dist_centroid = {}
|
||||
dist_centroid = neighbors.map(function(rid, data) {
|
||||
# Neighbour position in frame F0
|
||||
var p0 = math.vec2.newp(data.distance, data.azimuth)
|
||||
# Difference vector between p0 and c0
|
||||
var v = math.vec2.sub(p0, c0)
|
||||
# Distance between robot and centroid
|
||||
return math.vec2.length(v)
|
||||
})
|
||||
|
||||
# Minimal distance to centroid
|
||||
var temp = {}
|
||||
temp = dist_centroid.reduce(function(rid, data, accum) {
|
||||
accum.min = math.min(accum.min, data)
|
||||
return accum
|
||||
}, {.min = dist_rc})
|
||||
var min = temp.min
|
||||
#log("min ", min)
|
||||
|
||||
# If the current root is the closest to the centroid
|
||||
if(dist_rc == min) {
|
||||
ack = 1
|
||||
}
|
||||
# Otherwise
|
||||
else {
|
||||
var flag = 0
|
||||
# Selects closest robot to centroid as better root
|
||||
foreach(dist_centroid.data, function(key, value) {
|
||||
if(flag == 0) {
|
||||
if(value == min) {
|
||||
better_root = key
|
||||
flag = 1
|
||||
}
|
||||
# break (when implemented)
|
||||
}
|
||||
})
|
||||
|
||||
|
||||
#log(id, " better root : ", better_root)
|
||||
#log("X : ", accum_x, "Y : ", accum_y)
|
||||
var angle = neighbors.data[better_root].azimuth
|
||||
# Broadcasts
|
||||
var message = {
|
||||
.better_root = better_root,
|
||||
.centroid_x = accum_x,
|
||||
.centroid_y = accum_y,
|
||||
.angle_old_root = angle
|
||||
}
|
||||
neighbors.broadcast("msg1", message)
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
else if (better_root == nil) {
|
||||
# Listen for old root message
|
||||
foreach(knowledge.better_root, function(rid, value) {
|
||||
if(value == id) {
|
||||
|
||||
var theta = neighbors.data[rid].azimuth
|
||||
|
||||
var p1 = {
|
||||
.x = knowledge.centroid_x[rid],
|
||||
.y = knowledge.centroid_y[rid]
|
||||
}
|
||||
|
||||
var p01 = {
|
||||
.x = neighbors.data[rid].distance * math.cos(theta),
|
||||
.y = neighbors.data[rid].distance * math.sin(theta)
|
||||
}
|
||||
|
||||
var p0 = {}
|
||||
|
||||
if (knowledge.angle_old_root[rid] != nil) {
|
||||
var rot_angle = radians_interval(knowledge.angle_old_root[rid] - theta - math.pi)
|
||||
p0 = change_frame(p01, p1, rot_angle)
|
||||
}
|
||||
else {
|
||||
p0.x = p1.x
|
||||
p0.y = p1.y
|
||||
}
|
||||
|
||||
accum_x = p0.x
|
||||
accum_y = p0.y
|
||||
|
||||
centroid_x = accum_x
|
||||
centroid_y = accum_y
|
||||
|
||||
root = 1
|
||||
neighbors.broadcast("got_it", id)
|
||||
}
|
||||
})
|
||||
}
|
||||
|
||||
# Transitions to new state when all robots are ready
|
||||
if (trigger.get("a") == 1) {
|
||||
barrier_set(ROBOTS, end_fun)
|
||||
barrier_ready()
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
####################################
|
||||
|
||||
function tree_select() {
|
||||
|
||||
log(id, " tree_select")
|
||||
|
||||
neighbors.map(function(rid, data) {
|
||||
knowledge.distance[rid] = data.distance
|
||||
knowledge.azimuth[rid] = data.azimuth
|
||||
return 1
|
||||
})
|
||||
|
||||
if (level == 0) {
|
||||
# Finds a parent
|
||||
parent_select()
|
||||
# Updates robot level
|
||||
if (tree.parent.id != nil) {
|
||||
#log(" ")
|
||||
#log("selected")
|
||||
#log("son ", id)
|
||||
#log("parent ", tree.parent.id)
|
||||
#log(" ")
|
||||
|
||||
level = knowledge.level[tree.parent.id] + 1
|
||||
angle_parent = neighbors.data[tree.parent.id].azimuth
|
||||
}
|
||||
}
|
||||
else {
|
||||
# Updates list of sons
|
||||
foreach(knowledge.parent, function(key, value) {
|
||||
if(value == id) {
|
||||
#log(value)
|
||||
if(tree.sons[key] == nil) {
|
||||
# Updates robot nb_sons
|
||||
nb_sons = nb_sons + 1
|
||||
# Updates robot free status
|
||||
if (nb_sons >= N_SONS) {
|
||||
free = 0
|
||||
}
|
||||
}
|
||||
tree.sons[key] = {
|
||||
.distance = knowledge.distance[key],
|
||||
.azimuth = knowledge.azimuth[key],
|
||||
.eq_level = knowledge.eq_level[key],
|
||||
.accum_x = knowledge.accum_x[key],
|
||||
.accum_y = knowledge.accum_y[key],
|
||||
.angle_parent = knowledge.angle_parent[key]
|
||||
}
|
||||
}
|
||||
})
|
||||
}
|
||||
|
||||
# Creates a subset of neighbors to get the sons
|
||||
# and parent
|
||||
sons = {}
|
||||
sons = neighbors.filter(function(rid, data) {
|
||||
return (tree.sons[rid] != nil)
|
||||
})
|
||||
parent = {}
|
||||
parent = neighbors.filter(function(rid, data) {
|
||||
return (tree.parent.id == rid)
|
||||
})
|
||||
|
||||
# Counts robots (updates eq_level)
|
||||
count()
|
||||
|
||||
# Updates count of robots in the tree
|
||||
if (root == 1) {
|
||||
robot_count = eq_level
|
||||
}
|
||||
|
||||
nb_descendants = eq_level - level
|
||||
|
||||
|
||||
# Computes centroid (updates accum_x, accum_y)
|
||||
centroid()
|
||||
|
||||
# Broadcast information to other robots
|
||||
var message = {
|
||||
.level = level,
|
||||
.parent = tree.parent.id,
|
||||
.eq_level = eq_level,
|
||||
.free = free,
|
||||
.accum_x = accum_x,
|
||||
.accum_y = accum_y,
|
||||
.angle_parent = angle_parent
|
||||
}
|
||||
neighbors.broadcast("msg2", message)
|
||||
|
||||
# Triggers transition to new state if root count = ROBOTS
|
||||
if (root == 1) {
|
||||
if(robot_count == ROBOTS) {
|
||||
log("centroid X: ", accum_x, " Y ", accum_y)
|
||||
trigger.put("b", 1)
|
||||
}
|
||||
}
|
||||
|
||||
# Transitions to new state when all robots are ready
|
||||
if (trigger.get("b") == 1) {
|
||||
barrier_set(ROBOTS, root_select)
|
||||
barrier_ready()
|
||||
}
|
||||
}
|
||||
|
||||
################################################################
|
||||
################################################################
|
||||
|
||||
|
||||
# This function is executed every time you press the 'execute' button
|
||||
function init() {
|
||||
|
||||
trigger = stigmergy.create(TRIGGER_VSTIG)
|
||||
barrier = stigmergy.create(BARRIER_VSTIG)
|
||||
|
||||
# Trees
|
||||
old_tree = {}
|
||||
tree = old_tree
|
||||
old_tree.parent = {}
|
||||
old_tree.sons = {}
|
||||
|
||||
# Boolean variables
|
||||
root = 0 # Root status
|
||||
free = 1 # Node status (true if accepts sons)
|
||||
ack = 0
|
||||
|
||||
# Tree variables
|
||||
level = 0
|
||||
eq_level = 0
|
||||
nb_sons = 0
|
||||
nb_descendants = 0
|
||||
|
||||
# Update root status
|
||||
if (id == 0) {
|
||||
root = 1 # True if robot is the ro ot
|
||||
level = 1
|
||||
robot_count = 0
|
||||
}
|
||||
|
||||
statef = tree_select
|
||||
|
||||
knowledge = {
|
||||
.level = {},
|
||||
.parent = {},
|
||||
.eq_level = {},
|
||||
.free = {},
|
||||
.accum_x = {},
|
||||
.accum_y = {},
|
||||
.angle_parent = {},
|
||||
.distance = {},
|
||||
.azimuth = {},
|
||||
.better_root = {},
|
||||
.centroid_x = {},
|
||||
.centroid_y = {},
|
||||
.angle_old_root = {},
|
||||
.ackn = {}
|
||||
}
|
||||
|
||||
# Broadcast information to other robots
|
||||
var message = {
|
||||
.level = level,
|
||||
.parent = old_tree.parent.id,
|
||||
.eq_level = eq_level,
|
||||
.free = free,
|
||||
.accum_x = accum_x,
|
||||
.accum_y = accum_y,
|
||||
.angle_parent = 0.0
|
||||
}
|
||||
neighbors.broadcast("msg2", message)
|
||||
|
||||
# Listen to information from other robots for root_select
|
||||
neighbors.listen("msg1", function(vid, value, rid) {
|
||||
knowledge.better_root[rid] = value.better_root
|
||||
knowledge.centroid_x[rid] = value.centroid_x
|
||||
knowledge.centroid_y[rid] = value.centroid_y
|
||||
knowledge.angle_old_root[rid] = value.angle_old_root
|
||||
knowledge.angle_parent[rid] = value.angle_parent
|
||||
})
|
||||
|
||||
# Listen to information from other robots for tree_select
|
||||
neighbors.listen("msg2", function(vid, value, rid) {
|
||||
knowledge.level[rid] = value.level
|
||||
knowledge.parent[rid] = value.parent
|
||||
knowledge.eq_level[rid] = value.eq_level
|
||||
knowledge.free[rid] = value.free
|
||||
knowledge.accum_x[rid] = value.accum_x
|
||||
knowledge.accum_y[rid] = value.accum_y
|
||||
knowledge.angle_parent[rid] = value.angle_parent
|
||||
})
|
||||
|
||||
neighbors.listen("got_it", function(vid, value, rid) {
|
||||
knowledge.ackn[rid] = value
|
||||
})
|
||||
|
||||
}
|
||||
|
||||
############################################
|
||||
|
||||
# This function is executed at each time step
|
||||
function step() {
|
||||
|
||||
tree_config()
|
||||
goal = math.vec2.new(0.0, 0.0)
|
||||
|
||||
}
|
||||
############################################
|
||||
|
||||
# This function is executed every time you press the 'reset'
|
||||
# button in the GUI.
|
||||
function reset() {
|
||||
# put your code here
|
||||
}
|
||||
|
||||
############################################
|
||||
|
||||
# This function is executed only once, when the robot is removed
|
||||
# from the simulation
|
||||
function destroy() {
|
||||
# put your code here
|
||||
}
|
||||
|
||||
################
|
||||
|
|
@ -17,7 +17,7 @@ namespace buzz_utility{
|
|||
static char* BO_FNAME = 0;
|
||||
static uint8_t* BO_BUF = 0;
|
||||
static buzzdebug_t DBG_INFO = 0;
|
||||
static uint32_t MSG_SIZE = 500; // Only 250 bytes of Buzz messages every step (limited to Xbee frame size)
|
||||
static uint32_t MSG_SIZE = 250; // Only 250 bytes of Buzz messages every step (limited to Xbee frame size)
|
||||
static uint32_t MAX_MSG_SIZE = 10000; // Maximum Msg size for sending update packets
|
||||
static uint8_t Robot_id = 0;
|
||||
static std::vector<uint8_t*> IN_MSG;
|
||||
|
|
|
|||
Loading…
Reference in New Issue