adapted bzz files from sim.

buzz_scripts/include/act/barrier.bzz

@@ -7,10 +7,11 @@
 #
 # Constants
 #
-BARRIER_TIMEOUT = 1200 # in steps
+BARRIER_TIMEOUT = 200 # in steps
 BARRIER_VSTIG = 80
 timeW = 0
 barrier = nil
+bc = 0;
 
 #
 # Sets a barrier
@@ -22,15 +23,15 @@ function barrier_create() {
   #log("---> Prev. br. ", barrier, " ", BARRIER_VSTIG)
   if(barrier!=nil) {
     barrier=nil
-    BARRIER_VSTIG = BARRIER_VSTIG +1
+  #  BARRIER_VSTIG = BARRIER_VSTIG +1
   }
   #log("---> New. br. ", barrier, " ", BARRIER_VSTIG)
   barrier = stigmergy.create(BARRIER_VSTIG)
 }
 
-function barrier_set(threshold, transf, resumef) {
+function barrier_set(threshold, transf, resumef, bc) {
   statef = function() {
-    barrier_wait(threshold, transf, resumef);
+    barrier_wait(threshold, transf, resumef, bc);
   }
   BVMSTATE = "BARRIERWAIT"
   barrier_create()
@@ -39,30 +40,41 @@ function barrier_set(threshold, transf, resumef) {
 #
 # Make yourself ready
 #
-function barrier_ready() {
+function barrier_ready(bc) {
   #log("BARRIER READY -------")
-  barrier.put(id, 1)
+  barrier.put(id, bc)
   barrier.put("d", 0)
 }
 
 #
 # Executes the barrier
 #
-function barrier_wait(threshold, transf, resumef) {
+function barrier_wait(threshold, transf, resumef, bc) {
-  barrier.put(id, 1)
+  barrier.put(id, bc)
-
   barrier.get(id)
-  #log("--->BS: ", barrier.size(), " (", BARRIER_VSTIG, ")")
+  var allgood = 0
+  log("--->BS: ", barrier.size(), " / ", threshold, " (", BARRIER_VSTIG, " - ", barrier.get("d"), ") t= ", timeW)
   if(barrier.size() - 1 >= threshold or barrier.get("d") == 1) {
+    var bi = LOWEST_ROBOT_ID
+    allgood = 1
+    while (bi<LOWEST_ROBOT_ID+threshold) {
+      if(barrier.get(bi) != bc)
+        allgood = 0
+      bi = bi + 1
+    }
+  }
+  
+  if(allgood) {
     barrier.put("d", 1)
     timeW = 0
     BVMSTATE = transf
   } else if(timeW >= BARRIER_TIMEOUT) {
     log("------> Barrier Timeout !!!!")
-    barrier=nil
+    barrier = nil
     timeW = 0
     BVMSTATE = resumef
-  }
+  } else if(timeW % 10 == 0 and bc > 0)
+    neighbors.broadcast("cmd", bc)
 
   timeW = timeW+1
 }

buzz_scripts/include/act/states.bzz

@@ -27,44 +27,60 @@ function idle() {
     BVMSTATE = "IDLE"
 }
 
+# Custom function for the user.
+function cusfun(){
+	BVMSTATE="CUSFUN"
+	log("cusfun: yay!!!")
+}
+
 function launch() {
 	BVMSTATE = "LAUNCH"
-	if(V_TYPE == 0 or V_TYPE == 1) {	# flying vehicle so LAND
+	if(V_TYPE == 0 or V_TYPE == 1) {	# flying vehicle so TAKE_OFF
 		homegps = {.lat=pose.position.latitude, .long=pose.position.longitude}
 		if( flight.status == 2 and pose.position.altitude >= TARGET_ALTITUDE-TARGET_ALTITUDE/20.0) {
-			barrier_set(ROBOTS, AUTO_LAUNCH_STATE, "LAUNCH")
+			barrier_set(ROBOTS, AUTO_LAUNCH_STATE, "STOP", 22)
-			barrier_ready()
+			barrier_ready(22)
 		} else {
 			log("Altitude: ", pose.position.altitude)
 			neighbors.broadcast("cmd", 22)
 			uav_takeoff(TARGET_ALTITUDE)
 		}
 	} else {
-		barrier_set(ROBOTS, AUTO_LAUNCH_STATE, "LAUNCH")
+		barrier_set(ROBOTS, AUTO_LAUNCH_STATE, "STOP", 22)
-		barrier_ready()
+		barrier_ready(22)
 	}
 }
 
+gohomeT=100
+function goinghome() {
+  BVMSTATE = "GOHOME"
+	if(gohomeT > 0) {	# TODO: Make a real check if home is reached
+		gohome()
+		gohomeT = gohomeT - 1
+	} else
+  	BVMSTATE = AUTO_LAUNCH_STATE
+}
+
 function stop() {
-    BVMSTATE = "STOP"
+  BVMSTATE = "STOP"
 	if(V_TYPE == 0 or V_TYPE == 1) {	# flying vehicle so LAND
 		neighbors.broadcast("cmd", 21)
 		uav_land()
 		if(flight.status != 2 and flight.status != 3) {
-			barrier_set(ROBOTS,"TURNEDOFF","STOP")
+			barrier_set(ROBOTS,"TURNEDOFF","STOP", 21)
-			barrier_ready()
+			barrier_ready(21)
 		}
 	} else {
-			barrier_set(ROBOTS,"TURNEDOFF","STOP")
+			barrier_set(ROBOTS,"TURNEDOFF","STOP", 21)
-			barrier_ready()
+			barrier_ready(21)
-	}	
+	}
 }
 
 function take_picture() {
   BVMSTATE="PICTURE"
-  uav_setgimbal(0.0, 0.0, -90.0, 20.0)
+  setgimbal(0.0, 0.0, -90.0, 20.0)
 	if(pic_time==PICTURE_WAIT/2) { # wait for the drone to stabilize
-	  uav_takepicture()
+	  takepicture()
   } else if(pic_time>=PICTURE_WAIT) { # wait for the picture
     BVMSTATE="IDLE"
     pic_time=0 
@@ -117,7 +133,7 @@ function aggregate() {
 # circle all together around centroid
 function pursuit() {
   BVMSTATE="PURSUIT"
-	rd = 15.0
+	rd = 20.0
 	pc = 3.2
 	vd = 15.0
   r_vec = neighbors.reduce(function(rid, data, r_vec) {
@@ -127,8 +143,7 @@ function pursuit() {
   if(neighbors.count() > 0)
     r_vec = math.vec2.scale(r_vec, 1.0 / neighbors.count())
 	r = math.vec2.length(r_vec)
-  sqr = (r-rd)*(r-rd)+pc*pc*r*r
+	gamma = vd / math.sqrt((r-rd)*(r-rd)+pc*pc*r*r)
-  gamma = vd / math.sqrt(sqr)
 	dr = -gamma * (r-rd)
 	dT = gamma * pc
   vfg = math.vec2.newp(r+dr*0.1, math.vec2.angle(r_vec)+dT*0.1)
@@ -138,9 +153,9 @@ function pursuit() {
 
 # Lennard-Jones interaction magnitude
 TARGET     = 8.0
-EPSILON    = 3.0
+EPSILON    = 30.0
 function lj_magnitude(dist, target, epsilon) {
-	lj = -(epsilon / dist) * ((target / dist)^4 - 1.2 * (target / dist)^2)
+	lj = -(epsilon / dist) * ((target / dist)^4 - (target / dist)^2)
   return lj
 }
  
@@ -155,92 +170,119 @@ function lj_sum(rid, data, accum) {
 }
  
 # Calculates and actuates the flocking interaction
-old_lj = math.vec2.newp(0.0, 0.0)	# 1-step filter, to smoothen dir. changes
 function formation() {
   BVMSTATE="FORMATION"
   # Calculate accumulator
   accum_lj = neighbors.map(lj_vector).reduce(lj_sum, math.vec2.new(0.0, 0.0))
   if(neighbors.count() > 0)
     accum_lj = math.vec2.scale(accum_lj, 1.0 / neighbors.count())
-	old_lj = LimitSpeed(math.vec2.add(old_lj, accum_lj), 1.0/4.0)
+	accum_lj = LimitSpeed(accum_lj, 1.0/3.0)
-  goto_abs(old_lj.x, old_lj.y, 0.0, 0.0)
+  goto_abs(accum_lj.x, accum_lj.y, 0.0, 0.0)
 }
 
+# listens to commands from the remote control (web, commandline, etc)
 function rc_cmd_listen() {
-    if(flight.rc_cmd==22) {
+  if(flight.rc_cmd==22) {
 		log("cmd 22")
 		flight.rc_cmd=0
 		BVMSTATE = "LAUNCH"		
 		neighbors.broadcast("cmd", 22)
 	} else if(flight.rc_cmd==21) {
-		log("cmd 21")
 		flight.rc_cmd=0
+		AUTO_LAUNCH_STATE = "TURNEDOFF"
+		barrier_set(ROBOTS, "GOHOME", BVMSTATE, 21)
+		barrier_ready(21)
 		BVMSTATE = "STOP"
 		neighbors.broadcast("cmd", 21)
+	} else if(flight.rc_cmd==20) {
+		flight.rc_cmd=0
+		AUTO_LAUNCH_STATE = "IDLE"
+		barrier_set(ROBOTS, "GOHOME", BVMSTATE, 20)
+		barrier_ready(20)
+		neighbors.broadcast("cmd", 20)
 	} else if(flight.rc_cmd==16) {
 		flight.rc_cmd=0
 		BVMSTATE = "PATHPLAN"
 	} else if(flight.rc_cmd==400) {
 		flight.rc_cmd=0
-		uav_arm()
+		arm()
 		neighbors.broadcast("cmd", 400)
 	} else if (flight.rc_cmd==401){
 		flight.rc_cmd=0
-		uav_disarm()
+		disarm()
 		neighbors.broadcast("cmd", 401)
 	} else if (flight.rc_cmd==666){
 		flight.rc_cmd=0
 		stattab_send()
 	} else if (flight.rc_cmd==900){
 		flight.rc_cmd=0
-		BVMSTATE = "TASK_ALLOCATE"
+		barrier_set(ROBOTS, "TASK_ALLOCATE", BVMSTATE, 900)
+		barrier_ready(900)
 		neighbors.broadcast("cmd", 900)
 	} else if (flight.rc_cmd==901){
 		flight.rc_cmd=0
 		destroyGraph()
-		BVMSTATE = "PURSUIT"
+		barrier_set(ROBOTS, "PURSUIT", BVMSTATE, 901)
+		barrier_ready(901)
 		neighbors.broadcast("cmd", 901)
 	} else if (flight.rc_cmd==902){
 		flight.rc_cmd=0
 		destroyGraph()
-		BVMSTATE = "AGGREGATE"
+		barrier_set(ROBOTS, "AGGREGATE", BVMSTATE, 902)
+		barrier_ready(902)
 		neighbors.broadcast("cmd", 902)
 	} else if (flight.rc_cmd==903){
 		flight.rc_cmd=0
 		destroyGraph()
-		BVMSTATE = "FORMATION"
+		barrier_set(ROBOTS, "FORMATION", BVMSTATE, 903)
+		barrier_ready(903)
 		neighbors.broadcast("cmd", 903)
 	}
 }
 
+# listens to other fleet members broadcasting commands
 function nei_cmd_listen() {
     neighbors.listen("cmd",
 			function(vid, value, rid) {
 				print("Got (", vid, ",", value, ") #", rid, "(", BVMSTATE, ")")
-				if(value==22 and BVMSTATE!="BARRIERWAIT") {
+				if(BVMSTATE!="BARRIERWAIT") {
-		      		BVMSTATE = "LAUNCH"
+					if(value==22) {
-				} else if(value==21 and BVMSTATE!="BARRIERWAIT") {
+						BVMSTATE = "LAUNCH"
-		      		BVMSTATE = "STOP"
+					}else if(value==20) {
-				} else if(value==400 and BVMSTATE=="TURNEDOFF") {
+						AUTO_LAUNCH_STATE = "IDLE"
-					uav_arm()
+						BVMSTATE = "GOHOME"
-				} else if(value==401 and BVMSTATE=="TURNEDOFF"){
+					} else if(value==21) {
-					uav_disarm()
+						AUTO_LAUNCH_STATE = "TURNEDOFF"
-				} else if(value==900){	# Shapes
+						BVMSTATE = "STOP"
-					BVMSTATE = "TASK_ALLOCATE"
+					} else if(value==400 and BVMSTATE=="TURNEDOFF") {
-				} else if(value==901){	# Pursuit
+						arm()
-					destroyGraph()
+					} else if(value==401 and BVMSTATE=="TURNEDOFF"){
-					BVMSTATE = "PURSUIT"
+						disarm()
-				} else if(value==902){	# Agreggate
+					} else if(value==900){	# Shapes
-					destroyGraph()
+						barrier_set(ROBOTS, "TASK_ALLOCATE", BVMSTATE, 900)
-					BVMSTATE = "AGGREGATE"
+						#barrier_ready(900)
-				} else if(value==903){	# Formation
+						neighbors.broadcast("cmd", 900)
-					destroyGraph()
+					} else if(value==901){	# Pursuit
-					BVMSTATE = "FORMATION"
+						destroyGraph()
-				} else if(value==16 and BVMSTATE=="IDLE"){
+						barrier_set(ROBOTS, "PURSUIT", BVMSTATE, 901)
-				#  neighbors.listen("gt",function(vid, value, rid) {
+						#barrier_ready(901)
-				#    print("Got (", vid, ",", value, ") from robot #", rid)
+						neighbors.broadcast("cmd", 901)
-				#    # if(gt.id == id) statef=goto
+					} else if(value==902){	# Agreggate
-				#  }) 
+						destroyGraph()
+						barrier_set(ROBOTS, "AGGREGATE", BVMSTATE, 902)
+						#barrier_ready(902)
+						neighbors.broadcast("cmd", 902)
+					} else if(value==903){	# Formation
+						destroyGraph()
+						barrier_set(ROBOTS, "FORMATION", BVMSTATE, 903)
+						#barrier_ready(903)
+						neighbors.broadcast("cmd", 903)
+					} else if(value==16 and BVMSTATE=="IDLE"){
+					#  neighbors.listen("gt",function(vid, value, rid) {
+					#    print("Got (", vid, ",", value, ") from robot #", rid)
+					#    # if(gt.id == id) statef=goto
+					#  }) 
+					}
 				}
     })
 }

buzz_scripts/include/plan/rrtstar.bzz

@@ -20,7 +20,7 @@ mapgoal = {}
 function navigate() {
   BVMSTATE="NAVIGATE"
 	if(V_TYPE == 0) { # TODO: place these in a generic function in conversion.bzz
-		uav_storegoal(goal_list[g_it].x, goal_list[g_it].y, pose.position.altitude)
+		storegoal(goal_list[g_it].x, goal_list[g_it].y, pose.position.altitude)
 		cur_goal = vec_from_gps(rc_goto.latitude, rc_goto.longitude, 0)
 		m_pos = math.vec2.scale(vec_from_gps(homegps.lat, homegps.long, 0),-1)
 	}

buzz_scripts/include/update.bzz

@@ -4,6 +4,7 @@
 ####################################################################################################
 updated="update_ack"
 update_no=0
-function updated_neigh(){
+updates_active = 1
+function updated_no_bct(){
 	neighbors.broadcast(updated, update_no)
 }

buzz_scripts/main.bzz

@@ -7,7 +7,11 @@ include "taskallocate/graphformGPS.bzz"
 include "vstigenv.bzz"
 
 #State launched after takeoff
-AUTO_LAUNCH_STATE = "PURSUIT"
+AUTO_LAUNCH_STATE = "CUSFUN"
+#Lowest robot id in the network
+LOWEST_ROBOT_ID = 97
+TARGET     = 9.0
+EPSILON    = 30.0
 
 #####
 # Vehicule type:
@@ -21,12 +25,13 @@ goal_list = {
 	.0={.x = 45.5088103899, .y = -73.1540826153, .z = 2.5}
 }
 
+
 # Executed once at init time.
 function init() {
   	init_stig()
 	init_swarm()
 
-	TARGET_ALTITUDE = 10 + id*2.0 # m
+	TARGET_ALTITUDE = 5 + (id-LOWEST_ROBOT_ID)*3.0 # m
 	loop = 1
 
 	# start the swarm command listener
@@ -49,10 +54,14 @@ function step() {
 	#
 	if(BVMSTATE=="TURNEDOFF")
 		statef=turnedoff
+	else if(BVMSTATE=="CUSFUN")
+		statef=cusfun
 	else if(BVMSTATE=="STOP")	# ends on turnedoff
 		statef=stop
 	else if(BVMSTATE=="LAUNCH") # ends on AUTO_LAUNCH_STATE
 		statef=launch
+	else if(BVMSTATE=="GOHOME") # ends on AUTO_LAUNCH_STATE
+		statef=goinghome
 	else if(BVMSTATE=="IDLE")
 		statef=idle
 	else if(BVMSTATE=="AGGREGATE")

buzz_scripts/minimal.bzz

@@ -51,7 +51,6 @@ function step() {
 		statef=action
 
 	statef()
-
 	log("Current state: ", BVMSTATE)
 }
  

buzz_scripts/stand_by.bzz

@@ -1,35 +1,38 @@
+include "act/states.bzz"
+include "vstigenv.bzz"
+
 updated="update_ack"
 update_no=0
-function init(){
+BVMSTATE = "UPDATESTANDBY"
-barrier = stigmergy.create(101)
-barrier.put(id,1)
-barrier_val=0
-barrier.onconflict(function (k,l,r) {
-if(r. data < l. data or (r. data == l. data )) return l
-else return r
-})
 
-s = swarm.create(1)
+# Executed once at init time.
-s.join()
+function init(){
+	barrier = stigmergy.create(101)
+	barrier.put(id,1)
+	barrier_val=0
+	barrier.onconflict(function (k,l,r) {
+	if(r. data < l. data or (r. data == l. data )) return l
+	else return r
+	})
+	init_swarm()
+	# start the swarm command listener
+	nei_cmd_listen()
 }
 
 function stand_by(){
+	barrier.get(id)
+	barrier_val = barrier.size()
 
-barrier.get(id)
+	neighbors.listen(updated,
-barrier_val = barrier.size()
+	   function(vid, value, rid) {
-
+	      if(value==update_no) barrier.put(rid,1)
-neighbors.listen(updated,
+	   }
-   function(vid, value, rid) {
-	print("  got from",vid,"  ", " value = ",value," ","rid"," " )
-      if(value==update_no) barrier.put(rid,1)
-   }
 )
 
 }
 
-
+# Executed at each time step.
 function step() {
-
+	stand_by()
-stand_by()
 
 }

buzz_scripts/testLJ.bzz

@@ -0,0 +1,72 @@
+include "update.bzz"
+# don't use a stigmergy id=11 with this header, for barrier 
+# it requires an 'action' function to be defined here.
+include "act/states.bzz"
+include "vstigenv.bzz"
+
+V_TYPE = 0
+
+#State launched after takeoff
+AUTO_LAUNCH_STATE = "FORMATION"
+
+
+TARGET     = 8.0
+EPSILON    = 3.0
+GOTO_MAXVEL = 2.5 # m/steps
+
+# Executed once at init time.
+function init() {
+  	init_stig()
+	init_swarm()
+
+	# start the swarm command listener
+	nei_cmd_listen()
+
+    # Starting state: TURNEDOFF to wait for user input.
+	BVMSTATE = "TURNEDOFF"
+	TAKEOFF_COUNTER = 20
+}
+
+# Executed at each time step.
+function step() {
+	rc_cmd_listen()
+
+	# update the vstig (status/net/batt/...)
+  	# uav_updatestig()
+
+	#
+	# State machine
+	#
+	if(BVMSTATE=="TURNEDOFF")
+		statef=turnedoff
+	else if(BVMSTATE=="STOP")	# ends on turnedoff
+		statef=stop
+	else if(BVMSTATE=="LAUNCH") # ends on AUTO_LAUNCH_STATE
+		statef=launch
+	else if(BVMSTATE=="IDLE")
+		statef=idle
+	else if(BVMSTATE=="FORMATION")
+		statef=formation
+
+	statef()
+
+	log("Current state: ", BVMSTATE)
+
+	# Auto-takeoff (delayed for simulator boot)
+	if(id == 0) {
+		if(TAKEOFF_COUNTER>0)
+			TAKEOFF_COUNTER = TAKEOFF_COUNTER - 1
+		else if(TAKEOFF_COUNTER == 0) {
+			BVMSTATE="LAUNCH"
+			TAKEOFF_COUNTER = -1
+		}
+	}
+}
+ 
+# Executed once when the robot (or the simulator) is reset.
+function reset() {
+}
+ 
+# Executed once at the end of experiment.
+function destroy() {
+}