diff --git a/launch/rosbuzz-solo.launch b/launch/rosbuzz-solo.launch
index 3564ce1..054d18b 100644
--- a/launch/rosbuzz-solo.launch
+++ b/launch/rosbuzz-solo.launch
@@ -20,23 +20,35 @@
 	<!-- set streaming rate -->
 	<!-- node pkg="rosservice" type="rosservice" name="freq" args="call /mavros/set_stream_rate 0 10 1" output="screen" / -->
 
-	<!-- run xbee -->
+	<!-- run xbee>
 	<node pkg="xbee_ros_node" type="xbee_mav" name="xbee_mav" args="slave swarm" output="screen" />
 	<param name="Xbee_In_From_Buzz" type="str" value="outMavlink" />
 	<param name="Xbee_Out_To_Buzz" type="str" value="inMavlink" />
+	<param name="Xbee_In_From_Controller" type="str" value="xbee_cmd" / -->
+
+	<!--  xmee_mav Drone type and Commununication Mode -->
+	<!-- node pkg="xbee_ros_node" type="xbee_mav" name="xbee_mav" args="master solo" output="screen" -->
+	<node pkg="xbee_ros_node" type="xbee_mav" name="xbee_mav" args="slave swarm" output="screen" />
+	<!-- node pkg="xbee_ros_node" type="xbee_mav" name="xbee_mav" args="slave solo" output="screen" -->
+	<!--node pkg="xbee_ros_node" type="xbee_mav" name="xbee_mav" args="slave swarm" output="screen" -->
+	<!--  xmee_mav Topics and Services Names -->
+	<param name="Xbee_In_From_Buzz" type="str" value="outMavlink" />
+	<param name="Xbee_Out_To_Buzz" type="str" value="inMavlink" />
 	<param name="Xbee_In_From_Controller" type="str" value="xbee_cmd" />
+	<param name="Xbee_Out_To_Controller" type="str" value="mav_dji_cmd" />
+	
 
 
 	<!-- run rosbuzz -->
 	<node name="rosbuzz_node" pkg="rosbuzz" type="rosbuzz_node" respawn="false" output="screen" >	
 		<rosparam file="/home/pi/ros_catkinKin_ws/src/ROSBuzz/launch/launch_config/solo.yaml"/>
-		<param name="bzzfile_name" value="/home/pi/ros_catkinKin_ws/src/ROSBuzz/script/testsolo.bzz" />
+		<param name="bzzfile_name" value="/home/pi/ros_catkinKin_ws/src/ROSBuzz/script/testflockfev.bzz" />
 		<param name="rcclient" value="true" /> 
-		<param name="rcservice_name" value="/buzzcmd" />
-		<param name="in_payload" value="/inMavlink"/>
-		<param name="out_payload" value="/outMavlink"/>
-		<param name="xbee_status_srv" value="/xbee_status"/>
-           	<param name="xbee_plugged" value="false"/>
+		<param name="rcservice_name" value="buzzcmd" />
+		<param name="in_payload" value="inMavlink"/>
+		<param name="out_payload" value="outMavlink"/>
+		<param name="xbee_status_srv" value="xbee_status"/>
+           	<param name="xbee_plugged" value="true"/>
                 <param name="name" value="solos1"/>
 		<param name="stand_by" value="/home/pi/ros_catkinKin_ws/src/ROSBuzz/script/stand_by.bzz"/>
 	</node>
diff --git a/script/testflockfev.bdb b/script/testflockfev.bdb
index 9eb35cf..76c0075 100644
Binary files a/script/testflockfev.bdb and b/script/testflockfev.bdb differ
diff --git a/script/testflockfev.bo b/script/testflockfev.bo
index 3cb83de..44e24c1 100644
Binary files a/script/testflockfev.bo and b/script/testflockfev.bo differ
diff --git a/script/testflockfev.bzz b/script/testflockfev.bzz
index 463ca52..92a4fc0 100644
--- a/script/testflockfev.bzz
+++ b/script/testflockfev.bzz
@@ -1,6 +1,7 @@
 # We need this for 2D vectors
 # Make sure you pass the correct include path to "bzzc -I <path1:path2> ..."
-include "vec2.bzz"
+#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
@@ -11,12 +12,12 @@ function updated_neigh(){
 neighbors.broadcast(updated, update_no)
 }
 
-TARGET_ALTITUDE = 3.0
+TARGET_ALTITUDE = 5.0
 CURSTATE = "TURNEDOFF"
  
 # Lennard-Jones parameters
-TARGET     = 10.0	#0.000001001
-EPSILON    = 18.0	#0.001
+TARGET     = 12.0	#0.000001001
+EPSILON    = 6.0	#0.001
  
 # Lennard-Jones interaction magnitude
 function lj_magnitude(dist, target, epsilon) {
diff --git a/script/testsolo.bdb b/script/testsolo.bdb
index d25ea48..e3f057f 100644
Binary files a/script/testsolo.bdb and b/script/testsolo.bdb differ
diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp
index 8a45656..d7ae0ed 100644
--- a/src/roscontroller.cpp
+++ b/src/roscontroller.cpp
@@ -30,10 +30,7 @@ namespace rosbzz_node{
 		// set stream rate - wait for the FC to be started
 		SetStreamRate(0, 10, 1);
 		// Get Robot Id - wait for Xbee to be started
-		if(xbeeplugged)
-			GetRobotId();
-		else
-			robot_id= strtol(robot_name.c_str() + 5, NULL, 10);
+
 		setpoint_counter = 0;
 		fcu_timeout = TIMEOUT;
 
@@ -42,6 +39,13 @@ namespace rosbzz_node{
 			ros::Duration(0.5).sleep();
 			ros::spinOnce();
 		}
+
+
+		if(xbeeplugged){
+			GetRobotId();
+		} else {
+			robot_id= strtol(robot_name.c_str() + 5, NULL, 10);
+		}
 	}
 
 	/*---------------------
@@ -59,7 +63,7 @@ namespace rosbzz_node{
 	void roscontroller::GetRobotId()
 	{
 
-		///*
+		
 		mavros_msgs::ParamGet::Request robot_id_srv_request; robot_id_srv_request.param_id="id";
         mavros_msgs::ParamGet::Response robot_id_srv_response;
 		while(!xbeestatus_srv.call(robot_id_srv_request,robot_id_srv_response)){
@@ -167,6 +171,8 @@ namespace rosbzz_node{
                 }else
 			n_c.getParam("xbee_status_srv", xbeesrv_name);
 
+		std::cout<< "////////////////// " << xbeesrv_name;
+
 		GetSubscriptionParameters(n_c);
 		// initialize topics to null?