ROSBuzz_MISTLab/script/testflockfev.bzz.old

# We need this for 2D vectors
# Make sure you pass the correct include path to "bzzc -I <path1:path2> ..."
#include "vec2.bzz"
include "/home/ubuntu/buzz/src/include/vec2.bzz"
####################################################################################################
# Updater related
# This should be here for the updater to work, changing position of code will crash the updater
####################################################################################################
updated="update_ack"
update_no=0
function updated_neigh(){
neighbors.broadcast(updated, update_no)
}

TARGET_ALTITUDE = 5.0
CURSTATE = "TURNEDOFF"
 
# Lennard-Jones parameters
TARGET     = 12.0	#0.000001001
EPSILON    = 3.0	#0.001
 
# Lennard-Jones interaction magnitude
function lj_magnitude(dist, target, epsilon) {
  return -(epsilon / dist) * ((target / dist)^4 - (target / dist)^2)
}
 
# Neighbor data to LJ interaction vector
function lj_vector(rid, data) {
  return math.vec2.newp(lj_magnitude(data.distance, TARGET, EPSILON), data.azimuth)
}
 
# Accumulator of neighbor LJ interactions
function lj_sum(rid, data, accum) {
  return math.vec2.add(data, accum)
}
 
# 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())
  # Move according to vector
  #print("Robot ", id, "must push ",accum.length, "; ", accum.angle)
  uav_moveto(accum.x,accum.y)

#  if(timeW>=WAIT_TIMEOUT) {	#FOR MOVETO TESTS
#  	timeW =0
#  	statef=land
#  	} else if(timeW>=WAIT_TIMEOUT/2) {
#  	timeW = timeW+1
#  	uav_moveto(0.06,0.0)
#  	} else {
#  	timeW = timeW+1
#  	uav_moveto(0.0,0.06)
#  	}
}
 
########################################
#
# BARRIER-RELATED FUNCTIONS
#
########################################

#
# Constants
#
BARRIER_VSTIG = 1
# ROBOTS = 3 # number of robots in the swarm

#
# Sets a barrier
#
function barrier_set(threshold, transf) {
  statef = function() {
    barrier_wait(threshold, transf);
  }
  barrier = stigmergy.create(BARRIER_VSTIG)
}

#
# Make yourself ready
#
function barrier_ready() {
  barrier.put(id, 1)
}

#
# Executes the barrier
#
WAIT_TIMEOUT = 200
timeW=0
function barrier_wait(threshold, transf) {
  barrier.put(id, 1)
  barrier.get(id)
  CURSTATE = "BARRIERWAIT"
  if(barrier.size() >= threshold) {
#    barrier = nil
    transf()
  } else if(timeW>=WAIT_TIMEOUT) {
    barrier = nil
    statef=land
    timeW=0
  }
  timeW = timeW+1
}

# flight status

function idle() {
statef=idle
CURSTATE = "IDLE"
   
}

function takeoff() {
	CURSTATE = "TAKEOFF"
	statef=takeoff
	log("TakeOff: ", flight.status)
	log("Relative position: ", position.altitude)
	
	if( flight.status == 2 and position.altitude >= TARGET_ALTITUDE-TARGET_ALTITUDE/20.0) {
		barrier_set(ROBOTS,hexagon)
		barrier_ready()
		#statef=hexagon
	}
	else {
		log("Altitude: ", TARGET_ALTITUDE)
		neighbors.broadcast("cmd", 22)
		uav_takeoff(TARGET_ALTITUDE)
	}
}
function land() {
	CURSTATE = "LAND"
	statef=land
	log("Land: ", flight.status)
	if(flight.status == 2 or flight.status == 3){
		neighbors.broadcast("cmd", 21)
		uav_land()
	}
	else {
		timeW=0
		barrier = nil
		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)
			})
	}
}

function table_print(t) {
  if(size(t)>0) {
		foreach(t, function(u, tab) {
				log("id: ",u," Range ", tab.r, "Bearing ", tab.b)
			})
	}
}

# Executed once at init time.
function init() {
	s = swarm.create(1)
	s.join()

	vt = stigmergy.create(5)
	t = {}
	vt.put("p",t)

	statef=idle
	CURSTATE = "IDLE"
}

# Executed at each time step.
function step() {
	if(flight.rc_cmd==22) {
		log("cmd 22")
		flight.rc_cmd=0
		statef = takeoff
		CURSTATE = "TAKEOFF"		
		neighbors.broadcast("cmd", 22)
	} else if(flight.rc_cmd==21) {
		log("cmd 21")
		log("To land")
		flight.rc_cmd=0
		statef = land
		CURSTATE = "LAND"
		neighbors.broadcast("cmd", 21)
	} else if(flight.rc_cmd==16) {
		flight.rc_cmd=0
		statef = idle
		uav_goto()
	} else if(flight.rc_cmd==400) {
		flight.rc_cmd=0
		uav_arm()
		neighbors.broadcast("cmd", 400)
	} else if (flight.rc_cmd==401){
		flight.rc_cmd=0
		uav_disarm()
		neighbors.broadcast("cmd", 401)
	}
neighbors.listen("cmd",
   function(vid, value, rid) {
      print("Got (", vid, ",", value, ") from robot #", rid)
	if(value==22 and CURSTATE=="IDLE") {
		statef=takeoff
	} else if(value==21) {
		statef=land
	} else if(value==400 and CURSTATE=="IDLE") {
		uav_arm()
	} else if(value==401 and CURSTATE=="IDLE"){
		uav_disarm()
	}
   }

)
	statef()
	log("Current state: ", CURSTATE)
	log("Swarm size: ",ROBOTS)

	# Check local users and push to v.stig
	if(size(users.dataG)>0)
		vt.put("p", users.dataG)
				
        # Save locally the users and print RG
	users_save(vt.get("p"))
	table_print(users.dataL)
}
 
# Executed once when the robot (or the simulator) is reset.
function reset() {
}
 
# Executed once at the end of experiment.
function destroy() {
}