merge

script/testflockfevbzz

@@ -8,19 +8,20 @@ include "vec2.bzz"
 updated="update_ack"
 update_no=0
 function updated_neigh(){
-neighbors.broadcast(updated, update_no)
+	neighbors.broadcast(updated, update_no)
 }
 
-TARGET_ALTITUDE = 3.0
+TARGET_ALTITUDE = 5.0
 CURSTATE = "TURNEDOFF"
  
 # Lennard-Jones parameters
-TARGET     = 12.0	#0.000001001
+TARGET     = 12.0
-EPSILON    = 6.0	#0.001
+EPSILON    = 14.0
  
 # Lennard-Jones interaction magnitude
 function lj_magnitude(dist, target, epsilon) {
   return -(epsilon / dist) * ((target / dist)^4 - (target / dist)^2)
+  #return -(4 * epsilon) * ((target / dist)^12 - (target / dist)^6)
 }
  
 # Neighbor data to LJ interaction vector
@@ -33,27 +34,50 @@ function lj_sum(rid, data, accum) {
   return math.vec2.add(data, accum)
 }
 
+function user_attract(t) {
+	fus = math.vec2.new(0.0, 0.0)
+	if(size(t)>0) {
+		foreach(t, function(u, tab) {
+				#log("id: ",u," Range ", tab.r, "Bearing ", tab.b)
+				fus = math.vec2.add(fus, math.vec2.newp(lj_magnitude(tab.r, 3 * TARGET / 4.0, EPSILON * 2.0), tab.b))
+			})
+		math.vec2.scale(fus, 1.0 / size(t))
+	}
+	#print("User attract:", fus.x," ", fus.y, " [", size(t), "]")
+	return fus
+}
+ 
 # Calculates and actuates the flocking interaction
 function hexagon() {
   statef=hexagon
-  CURSTATE = "HEXAGON"
   # Calculate accumulator
   var accum = neighbors.map(lj_vector).reduce(lj_sum, math.vec2.new(0.0, 0.0))
   if(neighbors.count() > 0)
-    math.vec2.scale(accum, 1.0 / neighbors.count())
+    accum = math.vec2.scale(accum, 1.0 / neighbors.count())
+
+  accum = math.vec2.add(accum, user_attract(users.dataL))
+  accum = math.vec2.scale(accum, 1.0 / 2.0)
+
+  if(math.vec2.length(accum) > 1.0) {
+  	  accum = math.vec2.scale(accum, 1.0 / math.vec2.length(accum))
+  }
+
   # Move according to vector
-  #print("Robot ", id, "must push ",accum.length, "; ", accum.angle)
+  print("Robot ", id, "must push ", math.vec2.length(accum) )#, "; ", math.vec2.angle(accum))
-  uav_moveto(accum.x,accum.y)
+  uav_moveto(accum.x, accum.y)
+  CURSTATE = "LENNARDJONES"
 
 #  if(timeW>=WAIT_TIMEOUT) {	#FOR MOVETO TESTS
 #  	timeW =0
 #  	statef=land
 #  	} else if(timeW>=WAIT_TIMEOUT/2) {
+#		  CURSTATE ="GOEAST"
 #  	timeW = timeW+1
-#  	uav_moveto(0.06,0.0)
+#  	uav_moveto(0.0,5.0)
 #  	} else {
+#		  CURSTATE ="GONORTH"
 #  	timeW = timeW+1
-#  	uav_moveto(0.0,0.06)
+#  	uav_moveto(5.0,0.0)
 #  	}
 }
  
@@ -67,7 +91,6 @@ function hexagon() {
 # Constants
 #
 BARRIER_VSTIG = 1
-# ROBOTS = 3 # number of robots in the swarm
 
 #
 # Sets a barrier
@@ -93,9 +116,10 @@ WAIT_TIMEOUT = 200
 timeW=0
 function barrier_wait(threshold, transf) {
   barrier.get(id)
+  barrier.put(id, 1)
   CURSTATE = "BARRIERWAIT"
   if(barrier.size() >= threshold) {
-    barrier = nil
+    #barrier = nil
     transf()
   } else if(timeW>=WAIT_TIMEOUT) {
     barrier = nil
@@ -116,8 +140,8 @@ CURSTATE = "IDLE"
 function takeoff() {
 	CURSTATE = "TAKEOFF"
 	statef=takeoff
-	log("TakeOff: ", flight.status)
+	#log("TakeOff: ", flight.status)
-	log("Relative position: ", position.altitude)
+	#log("Relative position: ", position.altitude)
 	
 	if( flight.status == 2 and position.altitude >= TARGET_ALTITUDE-TARGET_ALTITUDE/20.0) {
 		barrier_set(ROBOTS,hexagon)
@@ -133,27 +157,53 @@ function takeoff() {
 function land() {
 	CURSTATE = "LAND"
 	statef=land
-	log("Land: ", flight.status)
+	#log("Land: ", flight.status)
 	if(flight.status == 2 or flight.status == 3){
 		neighbors.broadcast("cmd", 21)
 		uav_land()
 	}
 	else {
+		barrier_set(ROBOTS,idle)
+		barrier_ready()
 		timeW=0
-		barrier = nil
+		#barrier = nil
-		statef=idle
+		#statef=idle
 	}
 }
 
+function users_save(t) {
+	if(size(t)>0) {
+		foreach(t, function(id, tab) {
+				#log("id: ",id," Latitude ", tab.la, "Longitude ", tab.lo)
+				add_user_rb(id,tab.la,tab.lo)
+			})
+	}
+}
+
+# printing the contents of a table: a custom function
+function table_print(t) {
+  if(size(t)>0) {
+		foreach(t, function(u, tab) {
+				log("id: ",u," Range ", tab.r, "Bearing ", tab.b)
+			})
+	}
+}
+
+########################################
+#
+# MAIN FUNCTIONS
+#
+########################################
+
 # Executed once at init time.
 function init() {
 	s = swarm.create(1)
 	s.join()
-	statef=idle
+	vt = stigmergy.create(5)
-	CURSTATE = "IDLE"
-	vt = stigmergy.cerate(5)
 	t = {}
 	vt.put("p",t)
+	statef=idle
+	CURSTATE = "IDLE"
 }
 
 # Executed at each time step.
@@ -174,7 +224,8 @@ function step() {
 	} else if(flight.rc_cmd==16) {
 		flight.rc_cmd=0
 		statef = idle
-		uav_goto()
+		#uav_goto()
+		add_user_rb(10,rc_goto.latitude,rc_goto.longitude)
 	} else if(flight.rc_cmd==400) {
 		flight.rc_cmd=0
 		uav_arm()
@@ -202,8 +253,16 @@ neighbors.listen("cmd",
 	statef()
 	log("Current state: ", CURSTATE)
 	log("Swarm size: ",ROBOTS)
-	if(users.dataG)
+	
-		vt.put("p",users.dataG)
+	# Read a value from the structure
+		#		log(users)      
+				#users_print(users.dataG)
+				if(size(users.dataG)>0)
+					vt.put("p", users.dataG)
+				
+        # Get the number of keys in the structure
+        #log("The vstig has ", vt.size(), " elements")
+				users_save(vt.get("p"))
 				table_print(users.dataL)
 }
  

script/testflocksim.bzz

@@ -39,7 +39,7 @@ function user_attract(t) {
 	if(size(t)>0) {
 		foreach(t, function(u, tab) {
 				#log("id: ",u," Range ", tab.r, "Bearing ", tab.b)
-				fus = math.vec2.add(fus, math.vec2.newp(lj_magnitude(tab.r, TARGET / 2.0, EPSILON * 2.0), tab.b))
+				fus = math.vec2.add(fus, math.vec2.newp(lj_magnitude(tab.r, 3 * TARGET / 4.0, EPSILON * 2.0), tab.b))
 			})
 		math.vec2.scale(fus, 1.0 / size(t))
 	}
@@ -58,8 +58,8 @@ function hexagon() {
   accum = math.vec2.add(accum, user_attract(users.dataL))
   accum = math.vec2.scale(accum, 1.0 / 2.0)
 
-  if(math.vec2.length(accum) > 0.75) {
+  if(math.vec2.length(accum) > 1.0) {
-  	  accum = math.vec2.scale(accum, 0.75 / math.vec2.length(accum))
+  	  accum = math.vec2.scale(accum, 1.0 / math.vec2.length(accum))
   }
 
   # Move according to vector