From 94ead5f496d6c97c6feec6356a475f395b88c713 Mon Sep 17 00:00:00 2001 From: dave Date: Thu, 10 Aug 2017 20:38:26 -0400 Subject: [PATCH] fix MP gps and add status --- buzz_scripts/flock.bzz | 1 + buzz_scripts/include/utilities.bzz | 152 ++++++++ buzz_scripts/tree_centroid.bzz | 537 +++++++++++++++++++++++++++++ src/buzz_utility.cpp | 2 +- 4 files changed, 691 insertions(+), 1 deletion(-) create mode 100644 buzz_scripts/include/utilities.bzz create mode 100644 buzz_scripts/tree_centroid.bzz diff --git a/buzz_scripts/flock.bzz b/buzz_scripts/flock.bzz index c4246ab..3db9d25 100644 --- a/buzz_scripts/flock.bzz +++ b/buzz_scripts/flock.bzz @@ -65,6 +65,7 @@ function action() { function init() { uav_initswarm() uav_initstig() + TARGET_ALTITUDE = 2.5 + id * 5 } # Executed at each time step. diff --git a/buzz_scripts/include/utilities.bzz b/buzz_scripts/include/utilities.bzz new file mode 100644 index 0000000..6c4dab6 --- /dev/null +++ b/buzz_scripts/include/utilities.bzz @@ -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 diff --git a/buzz_scripts/tree_centroid.bzz b/buzz_scripts/tree_centroid.bzz new file mode 100644 index 0000000..d14277f --- /dev/null +++ b/buzz_scripts/tree_centroid.bzz @@ -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 +} + +################ \ No newline at end of file diff --git a/src/buzz_utility.cpp b/src/buzz_utility.cpp index 6039dc7..1d296c9 100644 --- a/src/buzz_utility.cpp +++ b/src/buzz_utility.cpp @@ -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 IN_MSG;