minor fix to state machine and CA gains tuning

buzz_scripts/include/act/CA.bzz

@@ -1,6 +1,6 @@
 # Lightweight collision avoidance
 function LCA( vel_vec ) {
-	var safety_radius = 2.0
+	var safety_radius = 1.5
 	collide = 0
 
 	var k_v = 4   # x axis gain
@@ -50,8 +50,8 @@ function LCA( vel_vec ) {
 		return vel_vec
 }
 
-robot_radius = 1.0
+robot_radius = 0.75
-safety_radius = 2.0
+safety_radius = 1.5
 combined_radius = 2 * robot_radius + safety_radius
 vel_sample_count = 50
 

buzz_scripts/include/act/states.bzz

@@ -169,16 +169,9 @@ gohomeT=100
 # State function to go back to ROSBuzz recorded home GPS position (at takeoff)
 function goinghome() {
   BVMSTATE = "GOHOME"
-	if(V_TYPE == 0 or V_TYPE == 1) {	# flying vehicle so TAKE_OFF
+	if(V_TYPE == 0 or V_TYPE == 1) {	# vehicle
-		m_navigation = vec_from_gps(homegps.lat, homegps.lng, 0)
+		storegoal(homegps.lat, homegps.lng, pose.position.altitude)
-		#print(" has to move ", math.vec2.length(m_navigation), math.vec2.angle(m_navigation))
+		goto_gps(str2fct(AUTO_LAUNCH_STATE))
-		if(math.vec2.length(m_navigation) < GOTODIST_TOL){ # reached destination
-				BVMSTATE = AUTO_LAUNCH_STATE
-		} else {
-			m_navigation = LimitSpeed(m_navigation, 1.0)
-			#m_navigation = LCA(m_navigation)
-			goto_abs(m_navigation.x, m_navigation.y, cur_goal.altitude - pose.position.altitude, 0.0)
-		}
 	} else
 		BVMSTATE = AUTO_LAUNCH_STATE
 }

buzz_scripts/include/taskallocate/graphformGPS.bzz

@@ -572,7 +572,7 @@ function DoJoined(){
 		}
 		}
 	# using JOINED as the resume state kinds of just reset the barrier timeout, should be IDLE/ALLOCATE...
-	barrier_wait_graph(ROBOTS, "GRAPH_LOCK", "GRAPH_JOINED", -1)
+	barrier_wait_graph(ROBOTS, "GRAPH_LOCK", "GRAPH_JOINED", graph_id)
 	BroadcastGraph()
   }
 }

buzz_scripts/include/utils/table.bzz

@@ -30,6 +30,54 @@ function count(table,value){
 	}
 	return number
 }
+
+#
+# State machine
+#
+function str2fct(value){
+	statef=idle
+	if(value=="TURNEDOFF")
+		statef=turnedoff
+	else if(value=="CUSFUN")
+		statef=cusfun
+	else if(value == "YOLO_DEMO")
+    	statef=yolo_demo
+	else if(value=="STOP")	# ends on turnedoff
+		statef=stop
+	else if(value=="LAUNCH") # ends on AUTO_LAUNCH_STATE
+		if(LAND_AFTER_BARRIER_EXPIRE == 1)
+			statef=launch
+		else
+			statef=launch_switch
+	else if(value=="GOHOME") # ends on AUTO_LAUNCH_STATE
+		statef=goinghome
+	else if(value=="WAYPOINT")
+		statef=indiWP
+	else if(value=="IDLE")
+		statef=idle
+	else if(value=="AGGREGATE")
+		statef=aggregate
+	else if(value=="POTENTIAL")
+		statef=lennardjones
+	else if(value=="PURSUIT")
+		statef=pursuit
+	else if(value=="TASK_ALLOCATE")	# or bidding ?
+		statef=graph_state
+	else if(value=="BIDDING")	# check the absolute path of the waypointlist csv file in bidding.bzz
+		statef=bidding
+	else if(value=="DEPLOY")	# check the absolute path of the waypointlist csv file in bidding.bzz
+		statef=voronoicentroid
+	else if(value=="PATHPLAN")	# ends on navigate, defined in rrtstar
+		statef=rrtstar
+	else if(value=="NAVIGATE")	# ends on idle, defined in rrtstar
+		statef=navigate
+	else if(value == "FOLLOW")	#TODO: not tested in new structure
+		statef=follow
+	else if(value == "PICTURE")	#TODO: not tested in new structure
+		statef=take_picture
+
+	return statef
+}
 #
 # map from int to state - vstig serialization limits to 9....
 #