added states to webcontrol and enhance barrier for state machine

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,42 @@ 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 = 0
+    allgood = 1
+    while (bi<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

@@ -32,31 +32,31 @@ function launch() {
 	if(V_TYPE == 0 or V_TYPE == 1) {	# flying vehicle so LAND
 		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, "LAUNCH", 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, "LAUNCH", 22)
-		barrier_ready()
+		barrier_ready(22)
 	}
 }
 
 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)
 	}	
 }
 
@@ -117,7 +117,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) {
@@ -137,9 +137,9 @@ function pursuit() {
 
 # Lennard-Jones interaction magnitude
 TARGET     = 8.0
-EPSILON    = 3.0
+EPSILON    = 0.1
 function lj_magnitude(dist, target, epsilon) {
-	lj = -(epsilon / dist) * ((target / dist)^4 - 1.2 * (target / dist)^2)
+	lj = -(epsilon / dist) * ((target / dist)^4 - 1.02 * (target / dist)^2)
   return lj
 }
  
@@ -161,12 +161,12 @@ function formation() {
   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)
+	old_lj = LimitSpeed(math.vec2.add(old_lj, accum_lj), 1.0/2.0)
   goto_abs(old_lj.x, old_lj.y, 0.0, 0.0)
 }
 
 function rc_cmd_listen() {
-    if(flight.rc_cmd==22) {
+  if(flight.rc_cmd==22) {
 		log("cmd 22")
 		flight.rc_cmd=0
 		BVMSTATE = "LAUNCH"		
@@ -192,22 +192,26 @@ function rc_cmd_listen() {
 		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)
 	}
 }
@@ -216,30 +220,40 @@ 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==21) {
-				} else if(value==400 and BVMSTATE=="TURNEDOFF") {
+						BVMSTATE = "STOP"
-					uav_arm()
+					} else if(value==400 and BVMSTATE=="TURNEDOFF") {
-				} else if(value==401 and BVMSTATE=="TURNEDOFF"){
+						uav_arm()
-					uav_disarm()
+					} else if(value==401 and BVMSTATE=="TURNEDOFF"){
-				} else if(value==900){	# Shapes
+						uav_disarm()
-					BVMSTATE = "TASK_ALLOCATE"
+					} else if(value==900){	# Shapes
-				} else if(value==901){	# Pursuit
+						barrier_set(ROBOTS, "TASK_ALLOCATE", BVMSTATE, 900)
-					destroyGraph()
+						#barrier_ready(900)
-					BVMSTATE = "PURSUIT"
+						neighbors.broadcast("cmd", 900)
-				} else if(value==902){	# Agreggate
+					} else if(value==901){	# Pursuit
-					destroyGraph()
+						destroyGraph()
-					BVMSTATE = "AGGREGATE"
+						barrier_set(ROBOTS, "PURSUIT", BVMSTATE, 901)
-				} else if(value==903){	# Formation
+						#barrier_ready(901)
-					destroyGraph()
+						neighbors.broadcast("cmd", 901)
-					BVMSTATE = "FORMATION"
+					} else if(value==902){	# Agreggate
-				} else if(value==16 and BVMSTATE=="IDLE"){
+						destroyGraph()
-				#  neighbors.listen("gt",function(vid, value, rid) {
+						barrier_set(ROBOTS, "AGGREGATE", BVMSTATE, 902)
-				#    print("Got (", vid, ",", value, ") from robot #", rid)
+						#barrier_ready(902)
-				#    # if(gt.id == id) statef=goto
+						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/main.bzz

@@ -7,7 +7,7 @@ include "taskallocate/graphformGPS.bzz"
 include "vstigenv.bzz"
 
 #State launched after takeoff
-AUTO_LAUNCH_STATE = "PURSUIT"
+AUTO_LAUNCH_STATE = "IDLE"
 
 #####
 # Vehicule type: