From 1cb390f5954a489eee22dc44f04b8c5de1550ce2 Mon Sep 17 00:00:00 2001
From: vivek-shankar <vivek-shankar.varadharajan@polymtl.ca>
Date: Wed, 4 Jul 2018 21:10:57 -0400
Subject: [PATCH] Hub: made changes to allow arbitrary robot id's and Rosbuzz:
 changed bzzfiles barrier, states and main to fit arbitrary id's.

---
 buzz_scripts/include/act/barrier.bzz |  5 ++---
 buzz_scripts/include/act/states.bzz  | 20 +++++++++++++-------
 buzz_scripts/main.bzz                |  9 +++++++--
 3 files changed, 22 insertions(+), 12 deletions(-)

diff --git a/buzz_scripts/include/act/barrier.bzz b/buzz_scripts/include/act/barrier.bzz
index 23e87d7..a87bed7 100644
--- a/buzz_scripts/include/act/barrier.bzz
+++ b/buzz_scripts/include/act/barrier.bzz
@@ -53,12 +53,11 @@ function barrier_wait(threshold, transf, resumef, bc) {
   barrier.put(id, bc)
   barrier.get(id)
   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
+    var bi = LOWEST_ROBOT_ID
     allgood = 1
-    while (bi<threshold) {
+    while (bi<LOWEST_ROBOT_ID+threshold) {
       if(barrier.get(bi) != bc)
         allgood = 0
       bi = bi + 1
diff --git a/buzz_scripts/include/act/states.bzz b/buzz_scripts/include/act/states.bzz
index a082cb7..cda775b 100644
--- a/buzz_scripts/include/act/states.bzz
+++ b/buzz_scripts/include/act/states.bzz
@@ -27,12 +27,18 @@ 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", 22)
+			barrier_set(ROBOTS, AUTO_LAUNCH_STATE, "STOP", 22)
 			barrier_ready(22)
 		} else {
 			log("Altitude: ", pose.position.altitude)
@@ -40,7 +46,7 @@ function launch() {
 			uav_takeoff(TARGET_ALTITUDE)
 		}
 	} else {
-		barrier_set(ROBOTS, AUTO_LAUNCH_STATE, "LAUNCH", 22)
+		barrier_set(ROBOTS, AUTO_LAUNCH_STATE, "STOP", 22)
 		barrier_ready(22)
 	}
 }
@@ -183,10 +189,10 @@ function rc_cmd_listen() {
 		neighbors.broadcast("cmd", 22)
 	} else if(flight.rc_cmd==21) {
 		flight.rc_cmd=0
-		AUTO_LAUNCH_STATE = "STOP"
+		AUTO_LAUNCH_STATE = "TURNEDOFF"
 		barrier_set(ROBOTS, "GOHOME", BVMSTATE, 21)
 		barrier_ready(21)
-		BVMSTATE = "GOHOME"
+		BVMSTATE = "STOP"
 		neighbors.broadcast("cmd", 21)
 	} else if(flight.rc_cmd==20) {
 		flight.rc_cmd=0
@@ -246,8 +252,8 @@ function nei_cmd_listen() {
 						AUTO_LAUNCH_STATE = "IDLE"
 						BVMSTATE = "GOHOME"
 					} else if(value==21) {
-						AUTO_LAUNCH_STATE = "STOP"
-						BVMSTATE = "GOHOME"
+						AUTO_LAUNCH_STATE = "TURNEDOFF"
+						BVMSTATE = "STOP"
 					} else if(value==400 and BVMSTATE=="TURNEDOFF") {
 						arm()
 					} else if(value==401 and BVMSTATE=="TURNEDOFF"){
diff --git a/buzz_scripts/main.bzz b/buzz_scripts/main.bzz
index e4a33b3..d52fd95 100644
--- a/buzz_scripts/main.bzz
+++ b/buzz_scripts/main.bzz
@@ -7,7 +7,9 @@ include "taskallocate/graphformGPS.bzz"
 include "vstigenv.bzz"
 
 #State launched after takeoff
-AUTO_LAUNCH_STATE = "FORMATION"
+AUTO_LAUNCH_STATE = "CUSFUN"
+#Lowest robot id in the network
+LOWEST_ROBOT_ID = 97
 TARGET     = 9.0
 EPSILON    = 30.0
 
@@ -23,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
@@ -51,6 +54,8 @@ 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