fix MP gps and add status

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.

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

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
+}
+
+################

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<uint8_t*> IN_MSG;