Merge commit '04bc85be0342fd00fc3aed6210a816c3c96694f1' into ros_drones_ws

buzz_scripts/graphform.bzz

@@ -4,9 +4,10 @@
 include "string.bzz"
 include "vec2.bzz"
 include "update.bzz"
-include "barrier.bzz"	# don't use a stigmergy id=11 with this header.
+
+include "vstigenv.bzz" # reserve stigmergy id=20 and 21 for this header.
+include "barrier.bzz" # reserve stigmergy id=11 for this header.
 include "uavstates.bzz"	# require an 'action' function to be defined here.
-include "vstigenv.bzz"
 
 include "graphs/shapes_Y.bzz"
 
@@ -88,7 +89,7 @@ m_fTargetDistanceTolerance=0
 #step cunt
 step_cunt=0
 
-#virtual stigmergy
+# virtual stigmergy for the LOCK barrier.
 m_lockstig = 1
 
 # Lennard-Jones parameters, may need change
@@ -101,24 +102,6 @@ function FlockInteraction(dist,target,epsilon){
 	return mag
 }
 
-function LimitAngle(angle){
-	if(angle>2*math.pi)
-		return angle-2*math.pi
-	else if (angle<0)
-		return angle+2*math.pi
-	else
-		return angle
-}
-
-#
-# Calculates the angle of the given vector2.
-# PARAM v: The vector2.
-# RETURN: The angle of the vector.
-#
-Angle = function(v) {
-  return math.atan(v.y, v.x)
-}
-
 #
 #return the number of value in table
 #
@@ -205,20 +188,6 @@ function find(table,value){
 	return index
 }
 
-function pow(base,exponent){
-	var i=0
-	var renturn_val=1
-	if(exponent==0)
-		return 1
-	else{
-		while(i<exponent){
-			renturn_val=renturn_val*base
-			i=i+1
-		}
-		return renturn_val
-	}
-}
-
 #
 #pack message into 1 number
 #
@@ -654,7 +623,7 @@ function DoJoining(){
 		#the vector from self to target in global coordinate
 		var S2Target=math.vec2.add(S2Pred,P2Target)
 		#change the vector to local coordinate of self
-		var S2Target_bearing=Angle(S2Target)
+		var S2Target_bearing=math.atan(S2Target.y, S2Target.x)
 		m_bias=m_cMeToPred.Bearing-S2PGlobalBearing
 		S2Target_bearing=S2Target_bearing+m_bias
 

buzz_scripts/include/barrier.bzz

@@ -8,6 +8,7 @@
 # Constants
 #
 BARRIER_VSTIG = 11
+BARRIER_TIMEOUT = 200 # in steps
 
 #
 # Sets a barrier
@@ -29,14 +30,12 @@ function barrier_ready() {
 #
 # Executes the barrier
 #
-BARRIER_TIMEOUT = 200
 timeW=0
 function barrier_wait(threshold, transf, resumef, bdt) {
-  #barrier.get(id)
   barrier.put(id, 1)
   UAVSTATE = "BARRIERWAIT"
-  if(bdt!=-1)
-    neighbors.broadcast("cmd", brd)
+
+  barrier.get(id)
   if(barrier.size() >= threshold) {
 #    getlowest()
     transf()
@@ -44,14 +43,16 @@ function barrier_wait(threshold, transf, resumef, bdt) {
     barrier = nil
     resumef()
     timeW=0
-  }
+  } else if(bdt!=-1)
+    neighbors.broadcast("cmd", bdt)
+
   timeW = timeW+1
 }
 
-# get the lowest id of the fleet, but requires too much bandwidth
+# get the lowest id of the fleet, but requires too much bandwidth...
 function getlowest(){
-  Lid = 20;
-  u=20
+  Lid = 15;
+  u=15
   while(u>=0){
 				tab = barrier.get(u)
 				if(tab!=nil){

buzz_scripts/include/uavstates.bzz

@@ -28,7 +28,7 @@ function takeoff() {
 	statef=takeoff
 	
 	if( flight.status == 2 and position.altitude >= TARGET_ALTITUDE-TARGET_ALTITUDE/20.0) {
-		barrier_set(ROBOTS,action,land,22)
+		barrier_set(ROBOTS,action,land,-1)
 		barrier_ready()
 	}
 	else {

buzz_scripts/include/vstigenv.bzz

@@ -1,16 +1,29 @@
-STATUS_VSTIG = 10
-GROUND_VSTIG = 11
+########################################
+#
+# FLEET V.STIGMERGY-RELATED FUNCTIONS
+#
+########################################
+#
+# Constants
+#
+STATUS_VSTIG = 20
+GROUND_VSTIG = 21
+HIGHEST_ROBOTID = 14
 WAIT4STEP = 10
-v_status = {}
-v_ground = {}
+
+#
+# Init var
+#
+var v_status = {}
+var v_ground = {}
 b_updating = 0
+counter=WAIT4STEP
 
 function uav_initstig() {
 	v_status = stigmergy.create(STATUS_VSTIG)
 	v_ground = stigmergy.create(GROUND_VSTIG)
 }
 
-counter=WAIT4STEP
 function uav_updatestig() {
 	# TODO: Push values on update only.
 	if(counter<=0) {
@@ -78,7 +91,7 @@ function stattab_print() {
 	if(v_status.size()>0) {
 		if(b_updating==0) {
 			u=0
-			while(u<8){
+			while(u<HIGHEST_ROBOTID){
 				tab = v_status.get(u)
 				if(tab!=nil)
 					unpackstatus(tab)
@@ -92,7 +105,7 @@ function stattab_send() {
 	if(v_status.size()>0) {
 		if(b_updating==0) {
 			u=0
-			while(u<8){
+			while(u<HIGHEST_ROBOTID){
 				tab = v_status.get(u)
 				if(tab!=nil){
 					recv_value=tab